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

AC_WPNav: Allow changing circle rate without changing parameter

This commit is contained in:
rishabsingh3003 2023-02-15 03:47:06 +05:30 committed by Andrew Tridgell
parent eb752a8397
commit 5cb77d9f81
2 changed files with 14 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AC_WPNav/AC_Circle.cpp
View File

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

7
libraries/AC_WPNav/AC_Circle.h
View File

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