Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: AC_PosControl: Auto Terain following update

Browse Source
This commit is contained in:
Leonard Hall 2021-07-14 23:48:45 +09:30 committed by Randy Mackay
parent 5dcfee07d9
commit ff1843a79c
2 changed files with 22 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -336,6 +336,9 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav,
/// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time. /// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z, float pos_offset_z_buffer) void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z, float pos_offset_z_buffer)
{ {
// Terrain following velocity scalar must be calculated before we remove the position offset
const float offset_z_scaler = pos_offset_z_scaler(pos_offset_z, pos_offset_z_buffer);
// remove terrain offsets for flat earth assumption // remove terrain offsets for flat earth assumption
_pos_target.z -= _pos_offset_z; _pos_target.z -= _pos_offset_z;
_vel_desired.z -= _vel_offset_z; _vel_desired.z -= _vel_offset_z;
@ -369,7 +372,7 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z, float
} }
// reduce speed if we are reaching the edge of our vertical buffer // reduce speed if we are reaching the edge of our vertical buffer
vel_max_xy_cms *= pos_offset_z_scaler(pos_offset_z, pos_offset_z_buffer); vel_max_xy_cms *= offset_z_scaler;
Vector2f vel; Vector2f vel;
Vector2f accel; Vector2f accel;
@ -394,13 +397,13 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z, float
/// pos_offset_z_scaler - calculates a multiplier used to reduce the horizontal velocity to allow the z position controller to stay within the provided buffer range /// pos_offset_z_scaler - calculates a multiplier used to reduce the horizontal velocity to allow the z position controller to stay within the provided buffer range
float AC_PosControl::pos_offset_z_scaler(float pos_offset_z, float pos_offset_z_buffer) float AC_PosControl::pos_offset_z_scaler(float pos_offset_z, float pos_offset_z_buffer) const
{ {
if (is_zero(pos_offset_z_buffer)) { if (is_zero(pos_offset_z_buffer)) {
return 1.0; return 1.0;
} }
const Vector3f curr_pos = _inav.get_position(); const Vector3f curr_pos = _inav.get_position();
float pos_offset_error_z = curr_pos.z - (_pos_target.z + pos_offset_z); float pos_offset_error_z = curr_pos.z - (_pos_target.z - _pos_offset_z + pos_offset_z);
return constrain_float((1.0 - (fabsf(pos_offset_error_z) - 0.5 * pos_offset_z_buffer) / (0.5 * pos_offset_z_buffer)), 0.01, 1.0); return constrain_float((1.0 - (fabsf(pos_offset_error_z) - 0.5 * pos_offset_z_buffer) / (0.5 * pos_offset_z_buffer)), 0.01, 1.0);
} }
@ -906,9 +909,9 @@ void AC_PosControl::set_alt_target_with_slew(const float& pos)
void AC_PosControl::update_pos_offset_z(float pos_offset_z) void AC_PosControl::update_pos_offset_z(float pos_offset_z)
{ {
postype_t posp_z = _pos_offset_z; postype_t p_offset_z = _pos_offset_z;
update_pos_vel_accel(posp_z, _vel_offset_z, _accel_offset_z, _dt, MIN(_limit_vector.z, 0.0f)); update_pos_vel_accel(p_offset_z, _vel_offset_z, _accel_offset_z, _dt, MIN(_limit_vector.z, 0.0f));
_pos_offset_z = posp_z; _pos_offset_z = p_offset_z;
// input shape the terrain offset // input shape the terrain offset
shape_pos_vel_accel(pos_offset_z, 0.0f, 0.0f, shape_pos_vel_accel(pos_offset_z, 0.0f, 0.0f,

14
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -46,6 +46,9 @@ public:
/// get_dt - gets time delta in seconds for all position controllers /// get_dt - gets time delta in seconds for all position controllers
float get_dt() const { return _dt; } float get_dt() const { return _dt; }
/// get_shaping_tc_xy_s - gets the time constant of the xy kinimatic path generation in seconds
float get_shaping_tc_xy_s() const { return _shaping_tc_xy_s; }
/// get_shaping_tc_z_s - gets the time constant of the z kinimatic path generation in seconds /// get_shaping_tc_z_s - gets the time constant of the z kinimatic path generation in seconds
float get_shaping_tc_z_s() const { return _shaping_tc_z_s; } float get_shaping_tc_z_s() const { return _shaping_tc_z_s; }
@ -60,7 +63,7 @@ public:
void input_pos_xyz(const Vector3p& pos, float pos_offset_z, float pos_offset_z_buffer); void input_pos_xyz(const Vector3p& pos, float pos_offset_z, float pos_offset_z_buffer);
/// pos_offset_z_scaler - calculates a multiplier used to reduce the horizontal velocity to allow the z position controller to stay within the provided buffer range /// pos_offset_z_scaler - calculates a multiplier used to reduce the horizontal velocity to allow the z position controller to stay within the provided buffer range
float pos_offset_z_scaler(float pos_offset_z, float pos_offset_z_buffer); float pos_offset_z_scaler(float pos_offset_z, float pos_offset_z_buffer) const;
/// ///
/// Lateral position controller /// Lateral position controller
@ -294,6 +297,15 @@ public:
/// set_pos_offset_z_cm - set altitude offset in cm above home /// set_pos_offset_z_cm - set altitude offset in cm above home
void set_pos_offset_z_cm(float pos_offset_z) { _pos_offset_z = pos_offset_z; } void set_pos_offset_z_cm(float pos_offset_z) { _pos_offset_z = pos_offset_z; }
/// get_pos_offset_z_cm - returns altitude offset in cm above home
float get_pos_offset_z_cm() const { return _pos_offset_z; }
/// get_vel_offset_z_cm - returns current vertical offset speed in cm/s
float get_vel_offset_z_cms() const { return _vel_offset_z; }
/// get_accel_offset_z_cm - returns current vertical offset acceleration in cm/s/s
float get_accel_offset_z_cmss() const { return _accel_offset_z; }
/// Outputs /// Outputs