Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_Circle: Restore radius every time

This commit is contained in:
giacomo892 2021-04-09 08:33:28 +02:00 committed by Peter Hall
parent e4c405b77b
commit a5708acaef
2 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -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);
@ -83,6 +82,8 @@ void AC_Circle::init(const Vector3f& 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
_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);

5
libraries/AC_WPNav/AC_Circle.h
View File

@ -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