From a5708acaef6e2dadb2b85ce273bbd74ad20a176c Mon Sep 17 00:00:00 2001 From: giacomo892 Date: Fri, 9 Apr 2021 08:33:28 +0200 Subject: [PATCH] AC_Circle: Restore radius every time --- libraries/AC_WPNav/AC_Circle.cpp | 7 ++++--- libraries/AC_WPNav/AC_Circle.h | 5 +++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index 785bb93d98..7e264ab26d 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -13,7 +13,7 @@ const AP_Param::GroupInfo AC_Circle::var_info[] = { // @Range: 0 200000 // @Increment: 100 // @User: Standard - AP_GROUPINFO("RADIUS", 0, AC_Circle, _radius, AC_CIRCLE_RADIUS_DEFAULT), + AP_GROUPINFO("RADIUS", 0, AC_Circle, _radius_parm, AC_CIRCLE_RADIUS_DEFAULT), // @Param: RATE // @DisplayName: Circle rate @@ -62,7 +62,6 @@ void AC_Circle::init(const Vector3f& center, bool terrain_alt) { _center = center; _terrain_alt = terrain_alt; - // initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles) _pos_control.set_desired_accel_xy(0.0f,0.0f); _pos_control.set_desired_velocity_xy(0.0f,0.0f); @@ -82,7 +81,9 @@ void AC_Circle::init(const Vector3f& center, bool terrain_alt) /// 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 void AC_Circle::init() -{ +{ + //initialize radius from params + _radius =_radius_parm; // initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles) _pos_control.set_desired_accel_xy(0.0f,0.0f); _pos_control.set_desired_velocity_xy(0.0f,0.0f); diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h index 4285bcaef1..5d12e7a1d1 100644 --- a/libraries/AC_WPNav/AC_Circle.h +++ b/libraries/AC_WPNav/AC_Circle.h @@ -42,7 +42,7 @@ public: bool center_is_terrain_alt() const { return _terrain_alt; } /// get_radius - returns radius of circle in cm - float get_radius() const { return _radius; } + float get_radius() const { return is_positive(_radius)?_radius:_radius_parm; } /// set_radius - sets circle radius in cm void set_radius(float radius_cm); @@ -135,12 +135,13 @@ private: }; // parameters - AP_Float _radius; // maximum horizontal speed in cm/s during missions + AP_Float _radius_parm; // radius of circle in cm loaded from params AP_Float _rate; // rotation speed in deg/sec AP_Int16 _options; // stick control enable/disable // internal variables Vector3f _center; // center of circle in cm from home + float _radius; // radius of circle in cm 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