diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 96dddf74c7..6cfa8f16a9 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1301,9 +1301,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_NAV_LOITER_TIME: case MAV_CMD_NAV_LOITER_TURNS: if (tell_command.options & MASK_OPTIONS_LOITER_DIRECTION) { - param3 = -g.loiter_radius; + param3 = -abs(g.loiter_radius); } else { - param3 = g.loiter_radius; + param3 = abs(g.loiter_radius); } case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_DO_SET_HOME: @@ -1312,9 +1312,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_NAV_LOITER_UNLIM: if (tell_command.options & MASK_OPTIONS_LOITER_DIRECTION) { - param3 = -g.loiter_radius;; + param3 = -abs(g.loiter_radius); } else { - param3 = g.loiter_radius; + param3 = abs(g.loiter_radius); } break; case MAV_CMD_CONDITION_CHANGE_ALT: diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 433262e034..ac2d037f06 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -184,7 +184,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: WP_LOITER_RAD // @DisplayName: Waypoint Loiter Radius - // @Description: Defines the distance from the waypoint center, the plane will maintain during a loiter + // @Description: Defines the distance from the waypoint center, the plane will maintain during a loiter. If you set this value to a negative number then the default loiter direction will be counter-clockwise instead of clockwise. // @Units: Meters // @Range: 1 32767 // @Increment: 1 diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index ffbd8bdb61..1993c54f6b 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -230,7 +230,12 @@ static void do_RTL(void) control_mode = RTL; crash_timer = 0; next_WP = home; - loiter_direction = 1; + + if (g.loiter_radius < 0) { + loiter_direction = -1; + } else { + loiter_direction = 1; + } // Altitude to hold over home // Set by configuration tool @@ -268,7 +273,7 @@ static void loiter_set_direction_wp(const struct Location *nav_command) if (nav_command->options & MASK_OPTIONS_LOITER_DIRECTION) { loiter_direction = -1; } else { - loiter_direction=1; + loiter_direction = 1; } } @@ -507,7 +512,11 @@ static bool verify_within_distance() static void do_loiter_at_location() { - loiter_direction = 1; + if (g.loiter_radius < 0) { + loiter_direction = -1; + } else { + loiter_direction = 1; + } next_WP = current_loc; } diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index e3bed5caac..7a66d95c7d 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -142,6 +142,6 @@ static void calc_altitude_error() static void update_loiter() { - nav_controller->update_loiter(next_WP, g.loiter_radius, loiter_direction); + nav_controller->update_loiter(next_WP, abs(g.loiter_radius), loiter_direction); }