AC_WPNav: Circle mode pilot control of rate & radius

libraries/AC_WPNav/AC_Circle.cpp

@@ -9,7 +9,7 @@ const AP_Param::GroupInfo AC_Circle::var_info[] = {
     // @DisplayName: Circle Radius
     // @Description: Defines the radius of the circle the vehicle will fly when in Circle flight mode
     // @Units: cm
-    // @Range: 0 10000
+    // @Range: 0 200000
     // @Increment: 100
     // @User: Standard
     AP_GROUPINFO("RADIUS",  0,  AC_Circle, _radius, AC_CIRCLE_RADIUS_DEFAULT),
@@ -23,6 +23,13 @@ const AP_Param::GroupInfo AC_Circle::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("RATE",    1, AC_Circle, _rate,    AC_CIRCLE_RATE_DEFAULT),
 
+    // @Param: CONTROL
+    // @DisplayName: Circle control
+    // @Description: Enable or disable using the pitch/roll stick control circle mode's radius and rate
+    // @Values: 0:Disable,1:Enable
+    // @User: Standard
+    AP_GROUPINFO("CONTROL", 2, AC_Circle, _control, 1),
+
     AP_GROUPEND
 };
 
@@ -106,6 +113,13 @@ void AC_Circle::set_rate(float deg_per_sec)
     }
 }
 
+/// set_circle_rate - set circle rate in degrees per second
+void AC_Circle::set_radius(float radius_cm)
+{
+    _radius = constrain_float(radius_cm, 0, AC_CIRCLE_RADIUS_MAX);
+    calc_velocities(false);
+}
+
 /// update - update circle controller
 void AC_Circle::update()
 {
@@ -167,7 +181,7 @@ void AC_Circle::update()
 //  closest point on the circle will be placed in result
 //  result's altitude (i.e. z) will be set to the circle_center's altitude
 //  if vehicle is at the center of the circle, the edge directly behind vehicle will be returned
-void AC_Circle::get_closest_point_on_circle(Vector3f &result)
+void AC_Circle::get_closest_point_on_circle(Vector3f &result) const
 {
     // return center if radius is zero
     if (_radius <= 0) {

libraries/AC_WPNav/AC_Circle.h

@@ -10,6 +10,7 @@
 #define AC_CIRCLE_RADIUS_DEFAULT    1000.0f     // radius of the circle in cm that the vehicle will fly
 #define AC_CIRCLE_RATE_DEFAULT      20.0f       // turn rate in deg/sec.  Positive to turn clockwise, negative for counter clockwise
 #define AC_CIRCLE_ANGULAR_ACCEL_MIN 2.0f        // angular acceleration should never be less than 2deg/sec
+#define AC_CIRCLE_RADIUS_MAX        200000.0f   // maximum allowed circle radius of 2km
 
 class AC_Circle
 {
@@ -33,11 +34,18 @@ public:
     const Vector3f& get_center() const { return _center; }
 
     /// get_radius - returns radius of circle in cm
-    float get_radius() { return _radius; }
+    float get_radius() const { return _radius; }
-    /// set_radius - sets circle radius in cm
-    void set_radius(float radius_cm) { _radius = radius_cm; }
 
-    /// set_circle_rate - set circle rate in degrees per second
+    /// set_radius - sets circle radius in cm
+    void set_radius(float radius_cm);
+
+    /// get_rate - returns target rate in deg/sec held in RATE parameter
+    float get_rate() const { return _rate; }
+
+    /// get_rate_current - returns actual calculated rate target in deg/sec, which may be less than _rate
+    float get_rate_current() const { return ToDeg(_angular_vel); }
+
+    /// set_rate - set circle rate in degrees per second
     void set_rate(float deg_per_sec);
 
     /// get_angle_total - return total angle in radians that vehicle has circled
@@ -56,7 +64,7 @@ public:
     //  closest point on the circle will be placed in result
     //  result's altitude (i.e. z) will be set to the circle_center's altitude
     //  if vehicle is at the center of the circle, the edge directly behind vehicle will be returned
-    void get_closest_point_on_circle(Vector3f &result);
+    void get_closest_point_on_circle(Vector3f &result) const;
 
     /// get horizontal distance to loiter target in cm
     float get_distance_to_target() const { return _pos_control.get_distance_to_target(); }
@@ -64,6 +72,9 @@ public:
     /// get bearing to target in centi-degrees
     int32_t get_bearing_to_target() const { return _pos_control.get_bearing_to_target(); }
 
+    /// true if pilot control of radius and turn rate is enabled
+    bool pilot_control_enabled() const { return _control > 0; }
+
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
@@ -92,6 +103,7 @@ private:
     // parameters
     AP_Float    _radius;        // maximum horizontal speed in cm/s during missions
     AP_Float    _rate;          // rotation speed in deg/sec
+    AP_Int16    _control;       // stick control enable/disable
 
     // internal variables
     Vector3f    _center;        // center of circle in cm from home