diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index 31af41de00..493046c255 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/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) { diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h index 7a73cb1e2d..b6fe5668fa 100644 --- a/libraries/AC_WPNav/AC_Circle.h +++ b/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; } - /// set_radius - sets circle radius in cm - void set_radius(float radius_cm) { _radius = radius_cm; } + float get_radius() const { return _radius; } - /// 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