From 2326eee9a0e3587c4395d95f115d4feedff9bdf2 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Mon, 30 Jan 2017 09:44:35 -0800 Subject: [PATCH] Plane: Move loiter_radius to vehicle --- ArduPlane/Parameters.cpp | 2 +- ArduPlane/Parameters.h | 1 - ArduPlane/commands.cpp | 2 +- ArduPlane/commands_logic.cpp | 8 ++++---- ArduPlane/navigation.cpp | 4 ++-- 5 files changed, 8 insertions(+), 9 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index de88ca1aaf..fcf801a6a5 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -292,7 +292,7 @@ const AP_Param::Info Plane::var_info[] = { // @Range: -32767 32767 // @Increment: 1 // @User: Standard - GSCALAR(loiter_radius, "WP_LOITER_RAD", LOITER_RADIUS_DEFAULT), + ASCALAR(loiter_radius, "WP_LOITER_RAD", LOITER_RADIUS_DEFAULT), // @Param: RTL_RADIUS // @DisplayName: RTL loiter radius diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 5f97779b6f..73646f3a38 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -401,7 +401,6 @@ public: AP_Int8 waypoint_mode; AP_Int16 waypoint_radius; AP_Int16 waypoint_max_radius; - AP_Int16 loiter_radius; AP_Int16 rtl_radius; #if GEOFENCE_ENABLED == ENABLED diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index c91490369a..0cef7fe650 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -70,7 +70,7 @@ void Plane::set_next_WP(const struct Location &loc) void Plane::set_guided_WP(void) { - if (g.loiter_radius < 0 || guided_WP_loc.flags.loiter_ccw) { + if (aparm.loiter_radius < 0 || guided_WP_loc.flags.loiter_ccw) { loiter.direction = -1; } else { loiter.direction = 1; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 4a208d1b85..0f87c5bb41 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -337,7 +337,7 @@ void Plane::do_RTL(int32_t rtl_altitude) setup_terrain_target_alt(next_WP_loc); set_target_altitude_location(next_WP_loc); - if (g.loiter_radius < 0) { + if (aparm.loiter_radius < 0) { loiter.direction = -1; } else { loiter.direction = 1; @@ -653,7 +653,7 @@ bool Plane::verify_loiter_unlim() bool Plane::verify_loiter_time() { bool result = false; - // mission radius is always g.loiter_radius + // mission radius is always aparm.loiter_radius update_loiter(0); if (loiter.start_time_ms == 0) { @@ -861,7 +861,7 @@ bool Plane::verify_within_distance() void Plane::do_loiter_at_location() { - if (g.loiter_radius < 0) { + if (aparm.loiter_radius < 0) { loiter.direction = -1; } else { loiter.direction = 1; @@ -1031,7 +1031,7 @@ bool Plane::verify_loiter_heading(bool init) return true; } - if (get_distance(next_WP_loc, next_nav_cmd.content.location) < labs(g.loiter_radius)) { + if (get_distance(next_WP_loc, next_nav_cmd.content.location) < labs(aparm.loiter_radius)) { /* Whenever next waypoint is within the loiter radius, maintaining loiter would prevent us from ever pointing toward the next waypoint. Hence break out of loiter immediately diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index fa26ea86cb..b9e6e15709 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -143,11 +143,11 @@ void Plane::update_loiter(uint16_t radius) { if (radius <= 1) { // if radius is <=1 then use the general loiter radius. if it's small, use default - radius = (abs(g.loiter_radius) <= 1) ? LOITER_RADIUS_DEFAULT : abs(g.loiter_radius); + radius = (abs(aparm.loiter_radius) <= 1) ? LOITER_RADIUS_DEFAULT : abs(aparm.loiter_radius); if (next_WP_loc.flags.loiter_ccw == 1) { loiter.direction = -1; } else { - loiter.direction = (g.loiter_radius < 0) ? -1 : 1; + loiter.direction = (aparm.loiter_radius < 0) ? -1 : 1; } }