diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index 9b41d53461..36900b12d5 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -24,7 +24,7 @@ const AP_Param::GroupInfo AC_Circle::var_info[] = { // @Range: -90 90 // @Increment: 1 // @User: Standard - AP_GROUPINFO("RATE", 1, AC_Circle, _rate, AC_CIRCLE_RATE_DEFAULT), + AP_GROUPINFO("RATE", 1, AC_Circle, _rate_parm, AC_CIRCLE_RATE_DEFAULT), // @Param: OPTIONS // @DisplayName: Circle options @@ -55,15 +55,18 @@ AC_Circle::AC_Circle(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_Po // init flags _flags.panorama = false; + _rate = _rate_parm; } /// init - initialise circle controller setting center specifically -/// set terrain_alt to true if center.z should be interpreted as an alt-above-terrain +/// set terrain_alt to true if center.z should be interpreted as an alt-above-terrain. Rate should be +ve in deg/sec for cw turn /// caller should set the position controller's x,y and z speeds and accelerations before calling this -void AC_Circle::init(const Vector3p& center, bool terrain_alt) +void AC_Circle::init(const Vector3p& center, bool terrain_alt, float rate_deg_per_sec) { _center = center; _terrain_alt = terrain_alt; + _rate = rate_deg_per_sec; + // initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles) _pos_control.init_xy_controller_stopping_point(); _pos_control.init_z_controller_stopping_point(); @@ -79,9 +82,10 @@ void AC_Circle::init(const Vector3p& center, bool terrain_alt) /// caller should set the position controller's x,y and z speeds and accelerations before calling this void AC_Circle::init() { - // initialize radius from params + // initialize radius and rate from params _radius = _radius_parm; _last_radius_param = _radius_parm; + _rate = _rate_parm; // initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles) _pos_control.init_xy_controller_stopping_point(); @@ -134,8 +138,8 @@ void AC_Circle::set_center(const Location& center) /// set_circle_rate - set circle rate in degrees per second void AC_Circle::set_rate(float deg_per_sec) { - if (!is_equal(deg_per_sec, _rate.get())) { - _rate.set(deg_per_sec); + if (!is_equal(deg_per_sec, _rate)) { + _rate = deg_per_sec; } } diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h index f0ce123753..94f578042c 100644 --- a/libraries/AC_WPNav/AC_Circle.h +++ b/libraries/AC_WPNav/AC_Circle.h @@ -20,9 +20,9 @@ public: AC_Circle(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control); /// init - initialise circle controller setting center specifically - /// set terrain_alt to true if center.z should be interpreted as an alt-above-terrain + /// set terrain_alt to true if center.z should be interpreted as an alt-above-terrain. Rate should be +ve in deg/sec for cw turn /// caller should set the position controller's x,y and z speeds and accelerations before calling this - void init(const Vector3p& center, bool terrain_alt); + void init(const Vector3p& center, bool terrain_alt, float rate_deg_per_sec); /// init - initialise circle controller setting center using stopping point and projecting out based on the copter's heading /// caller should set the position controller's x,y and z speeds and accelerations before calling this @@ -139,12 +139,13 @@ private: // parameters AP_Float _radius_parm; // radius of circle in cm loaded from params - AP_Float _rate; // rotation speed in deg/sec + AP_Float _rate_parm; // rotation speed in deg/sec AP_Int16 _options; // stick control enable/disable // internal variables Vector3p _center; // center of circle in cm from home float _radius; // radius of circle in cm + float _rate; // rotation speed of circle in deg/sec. +ve for cw turn float _yaw; // yaw heading (normally towards circle center) float _angle; // current angular position around circle in radians (0=directly north of the center of the circle) float _angle_total; // total angle traveled in radians