Plane: add guided radius to mavlink DO_REPOSITION

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;

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

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;
+}