diff options
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; } |