From dd6a69f588bf2da4fd8cf8e3136fee815535585c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 13 Sep 2013 17:43:00 +1000
Subject: [PATCH] Plane: added WP_MAX_RADIUS parameter
useful when using AUTO_FBW_STEER for manual steering in competitions
---
ArduPlane/Parameters.h | 2 ++
ArduPlane/Parameters.pde | 11 ++++++++++-
ArduPlane/commands_logic.pde | 9 +++++++++
3 files changed, 21 insertions(+), 1 deletion(-)
diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index 7a8ad1d0e..0c4daf0d7 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -93,6 +93,7 @@ public:
k_param_takeoff_throttle_delay,
k_param_skip_gyro_cal,
k_param_auto_fbw_steer,
+ k_param_waypoint_max_radius,
// 110: Telemetry control
//
@@ -295,6 +296,7 @@ public:
AP_Int8 command_total;
AP_Int8 command_index;
AP_Int16 waypoint_radius;
+ AP_Int16 waypoint_max_radius;
AP_Int16 loiter_radius;
#if GEOFENCE_ENABLED == ENABLED
diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde
index e7ba77878..49a91cb0e 100644
--- a/ArduPlane/Parameters.pde
+++ b/ArduPlane/Parameters.pde
@@ -195,13 +195,22 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: WP_RADIUS
// @DisplayName: Waypoint Radius
- // @Description: Defines the distance from a waypoint, that when crossed indicates the wp has been hit.
+ // @Description: Defines the distance from a waypoint that when crossed indicates the waypoint has been completed. To avoid the aircraft looping around the waypoint in case it misses by more than the WP_RADIUS an additional check is made to see if the aircraft has crossed a "finish line" passing through the waypoint and perpendicular to the flight path from the previous waypoint. If that finish line is crossed then the waypoint is considered complete.
// @Units: Meters
// @Range: 1 32767
// @Increment: 1
// @User: Standard
GSCALAR(waypoint_radius, "WP_RADIUS", WP_RADIUS_DEFAULT),
+ // @Param: WP_MAX_RADIUS
+ // @DisplayName: Waypoint Maximum Radius
+ // @Description: Sets the maximum distance to a waypoint for the waypoint to be considered complete. This overrides the "cross the finish line" logic that is normally used to consider a waypoint complete. For normal AUTO behaviour this parameter should be set to zero. Using a non-zero value is only recommended when it is critical that the aircraft does approach within the given radius, and should loop around until it has done so. This can cause the aircraft to loop forever if its turn radius is greater than the maximum radius set.
+ // @Units: Meters
+ // @Range: 0 32767
+ // @Increment: 1
+ // @User: Standard
+ GSCALAR(waypoint_max_radius, "WP_MAX_RADIUS", 0),
+
// @Param: WP_LOITER_RAD
// @DisplayName: Waypoint Loiter Radius
// @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.
diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde
index 0f59ff011..bdee929e9 100644
--- a/ArduPlane/commands_logic.pde
+++ b/ArduPlane/commands_logic.pde
@@ -389,6 +389,15 @@ static bool verify_nav_wp()
hold_course_cd = -1;
nav_controller->update_waypoint(prev_WP, next_WP);
+
+ // see if the user has specified a maximum distance to waypoint
+ if (g.waypoint_max_radius > 0 && wp_distance > (uint16_t)g.waypoint_max_radius) {
+ if (location_passed_point(current_loc, prev_WP, next_WP)) {
+ // this is needed to ensure completion of the waypoint
+ prev_WP = current_loc;
+ }
+ return false;
+ }
if (wp_distance <= nav_controller->turn_distance(g.waypoint_radius)) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
--
GitLab