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

AC_Circle: add function to check for radius param change

This commit is contained in:
Iampete1 2021-05-02 01:41:30 +01:00 committed by Andrew Tridgell
parent 7cc11e6856
commit e995e8873c
2 changed files with 17 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -81,9 +81,11 @@ 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 /// 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 /// caller should set the position controller's x,y and z speeds and accelerations before calling this
void AC_Circle::init() void AC_Circle::init()
{ {
//initialize radius from params // initialize radius from params
_radius =_radius_parm; _radius = _radius_parm;
_last_radius_param = _radius_parm;
// initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles) // 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_accel_xy(0.0f,0.0f);
_pos_control.set_desired_velocity_xy(0.0f,0.0f); _pos_control.set_desired_velocity_xy(0.0f,0.0f);
@ -382,3 +384,11 @@ bool AC_Circle::get_terrain_offset(float& offset_cm)
// we should never get here but just in case // we should never get here but just in case
return false; return false;
} }
void AC_Circle::check_param_change()
{
if (!is_equal(_last_radius_param,_radius_parm.get())) {
_radius = _radius_parm;
_last_radius_param = _radius_parm;
}
}

4
libraries/AC_WPNav/AC_Circle.h
View File

@ -92,6 +92,9 @@ public:
/// provide rangefinder altitude /// provide rangefinder altitude
void set_rangefinder_alt(bool use, bool healthy, float alt_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_alt_cm = alt_cm; } void set_rangefinder_alt(bool use, bool healthy, float alt_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_alt_cm = alt_cm; }
/// check for a change in the radius params
void check_param_change();
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
private: private:
@ -149,6 +152,7 @@ private:
float _angular_vel_max; // maximum velocity in radians/sec float _angular_vel_max; // maximum velocity in radians/sec
float _angular_accel; // angular acceleration in radians/sec/sec float _angular_accel; // angular acceleration in radians/sec/sec
uint32_t _last_update_ms; // system time of last update uint32_t _last_update_ms; // system time of last update
float _last_radius_param; // last value of radius param, used to update radius on param change
// terrain following variables // terrain following variables
bool _terrain_alt; // true if _center.z is alt-above-terrain, false if alt-above-ekf-origin bool _terrain_alt; // true if _center.z is alt-above-terrain, false if alt-above-ekf-origin