From 46bc30a40c9aa0be3e96ef0005bcf7f73b36b564 Mon Sep 17 00:00:00 2001
From: Tom Pittenger <tom.pittenger@krausaerospace.com>
Date: Mon, 10 Oct 2022 14:58:30 -0700
Subject: [PATCH] Plane: add guided radius to mavlink DO_REPOSITION

---
 ArduPlane/GCS_Mavlink.cpp |  5 +++++
 ArduPlane/mode.h          |  5 +++++
 ArduPlane/mode_guided.cpp | 13 +++++++++++--
 3 files changed, 21 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index c311361b2c..1171add4ad 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/ArduPlane/GCS_Mavlink.cpp
@@ -725,6 +725,11 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com
 
         plane.set_guided_WP(requested_position);
 
+        // Loiter radius for planes. Positive radius in meters, direction is controlled by Yaw (param4) value, parsed above
+        if (!isnan(packet.param3) && packet.param3 > 0) {
+            plane.mode_guided.set_radius_and_direction(packet.param3, requested_position.loiter_ccw);
+        }
+
         return MAV_RESULT_ACCEPTED;
     }
     return MAV_RESULT_FAILED;
diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
index c8e696a1db..1f2a0272dc 100644
--- a/ArduPlane/mode.h
+++ b/ArduPlane/mode.h
@@ -224,9 +224,14 @@ public:
     // handle a guided target request from GCS
     bool handle_guided_request(Location target_loc) override;
 
+    void set_radius_and_direction(const float radius, const bool direction_is_ccw);
+
 protected:
 
     bool _enter() override;
+
+private:
+    float active_radius_m;
 };
 
 class ModeCircle: public Mode
diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp
index b80e72dd02..9d354db2e6 100644
--- a/ArduPlane/mode_guided.cpp
+++ b/ArduPlane/mode_guided.cpp
@@ -20,6 +20,9 @@ bool ModeGuided::_enter()
     }
 #endif
 
+    // set guided radius to WP_LOITER_RAD on mode change.
+    active_radius_m = 0;
+
     plane.set_guided_WP(loc);
     return true;
 }
@@ -39,8 +42,7 @@ void ModeGuided::update()
 
 void ModeGuided::navigate()
 {
-    // Zero indicates to use WP_LOITER_RAD
-    plane.update_loiter(0);
+    plane.update_loiter(active_radius_m);
 }
 
 bool ModeGuided::handle_guided_request(Location target_loc)
@@ -55,3 +57,10 @@ bool ModeGuided::handle_guided_request(Location target_loc)
 
     return true;
 }
+
+void ModeGuided::set_radius_and_direction(const float radius, const bool direction_is_ccw)
+{
+    // constrain to (uint16_t) range for update_loiter()
+    active_radius_m = constrain_int32(fabsf(radius), 0, UINT16_MAX);
+    plane.loiter.direction = direction_is_ccw ? -1 : 1;
+}