diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-10-11 16:22:08 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-10-11 16:22:08 +0200 |
commit | e770f84858491591cd07554989fbc0708b9e0cd3 (patch) | |
tree | 723d690aa3759fdfe0776888f5c22bddf1d2d40c /src/modules/mavlink/missionlib.c | |
parent | 87e1ffe0ba293c62b882a8ae9729878e36a95c4c (diff) | |
parent | 1306c9de7b946783ff1143bb42a33734e9380e2c (diff) | |
download | px4-firmware-e770f84858491591cd07554989fbc0708b9e0cd3.tar.gz px4-firmware-e770f84858491591cd07554989fbc0708b9e0cd3.tar.bz2 px4-firmware-e770f84858491591cd07554989fbc0708b9e0cd3.zip |
Merge branch 'master' into px4io_fix
Diffstat (limited to 'src/modules/mavlink/missionlib.c')
-rw-r--r-- | src/modules/mavlink/missionlib.c | 33 |
1 files changed, 25 insertions, 8 deletions
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index 3175e64ce..e8d707948 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -170,6 +170,28 @@ bool set_special_fields(float param1, float param2, float param3, float param4, sp->param2 = param2; sp->param3 = param3; sp->param4 = param4; + + + /* define the turn distance */ + float orbit = 15.0f; + + if (command == (int)MAV_CMD_NAV_WAYPOINT) { + + orbit = param2; + + } else if (command == (int)MAV_CMD_NAV_LOITER_TURNS || + command == (int)MAV_CMD_NAV_LOITER_TIME || + command == (int)MAV_CMD_NAV_LOITER_UNLIM) { + + orbit = param3; + } else { + + // XXX set default orbit via param + // 15 initialized above + } + + sp->turn_distance_xy = orbit; + sp->turn_distance_z = orbit; } /** @@ -223,10 +245,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, int last_setpoint_index = -1; bool last_setpoint_valid = false; - /* at first waypoint, but cycled once through mission */ - if (index == 0 && last_waypoint_index > 0) { - last_setpoint_index = last_waypoint_index; - } else { + if (index > 0) { last_setpoint_index = index - 1; } @@ -251,10 +270,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, int next_setpoint_index = -1; bool next_setpoint_valid = false; - /* at last waypoint, try to re-loop through mission as default */ - if (index == (wpm->size - 1) && wpm->size > 1) { - next_setpoint_index = 0; - } else if (wpm->size > 1) { + /* next waypoint */ + if (wpm->size > 1) { next_setpoint_index = index + 1; } |