Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: faster z-axis slowdown when over speed

Browse Source
This commit is contained in:
Leonard Hall 2015-10-27 22:45:16 +09:00 committed by Randy Mackay
parent 245f7ce268
commit aec66c5db6
2 changed files with 15 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -178,10 +178,20 @@ void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float d
/// set force_descend to true during landing to allow target to move low enough to slow the motors
void AC_PosControl::set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend)
{
// jerk_z is calculated to reach full acceleration in 1000ms.
float jerk_z = _accel_z_cms * POSCONTROL_JERK_RATIO;
// calculated increased maximum acceleration if over speed
float accel_z_cms = _accel_z_cms;
if (_vel_desired.z < _speed_down_cms && !is_zero(_speed_down_cms)) {
accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_down_cms;
}
if (_vel_desired.z > _speed_up_cms && !is_zero(_speed_up_cms)) {
accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_up_cms;
}
accel_z_cms = constrain_float(accel_z_cms, 0.0f, 750.0f);
float accel_z_max = min(_accel_z_cms, safe_sqrt(2.0f*fabsf(_vel_desired.z - climb_rate_cms)*jerk_z));
// jerk_z is calculated to reach full acceleration in 1000ms.
float jerk_z = accel_z_cms * POSCONTROL_JERK_RATIO;
float accel_z_max = min(accel_z_cms, safe_sqrt(2.0f*fabsf(_vel_desired.z - climb_rate_cms)*jerk_z));
_accel_last_z_cms += jerk_z * dt;
_accel_last_z_cms = min(accel_z_max, _accel_last_z_cms);

2
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -43,6 +43,8 @@
#define POSCONTROL_ACCEL_FILTER_HZ 2.0f // low-pass filter on acceleration (unit: hz)
#define POSCONTROL_JERK_RATIO 1.0f // Defines the time it takes to reach the requested acceleration
#define POSCONTROL_OVERSPEED_GAIN_Z 2.0f // gain controlling rate at which z-axis speed is brought back within SPEED_UP and SPEED_DOWN range
class AC_PosControl
{
public: