mirror of https://github.com/ArduPilot/ardupilot
AC_PosControl: add comments and defines for jerk limits
This commit is contained in:
parent
4408c1b935
commit
965db2c7f7
|
@ -802,7 +802,8 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler)
|
|||
float accel_right, accel_forward;
|
||||
float lean_angle_max = _attitude_control.lean_angle_max();
|
||||
|
||||
float max_delta_accel = dt * 1700.0f;
|
||||
// apply jerk limit of 17 m/s^3 - equates to a worst case of about 100 deg/sec/sec
|
||||
float max_delta_accel = dt * POSCONTROL_JERK_LIMIT_CMSSS;
|
||||
|
||||
Vector2f accel_in(_accel_target.x, _accel_target.y);
|
||||
Vector2f accel_change = accel_in-_accel_target_jerk_limited;
|
||||
|
@ -814,7 +815,7 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler)
|
|||
}
|
||||
|
||||
// 5Hz lowpass filter on NE accel
|
||||
float freq_cut = 5.0f * ekfNavVelGainScaler;
|
||||
float freq_cut = POSCONTROL_ACCEL_FILTER_HZ * ekfNavVelGainScaler;
|
||||
float alpha = constrain_float(dt/(dt + 1.0f/(2.0f*(float)M_PI*freq_cut)),0.0f,1.0f);
|
||||
_accel_target_filtered.x += alpha * (_accel_target_jerk_limited.x - _accel_target_filtered.x);
|
||||
_accel_target_filtered.y += alpha * (_accel_target_jerk_limited.y - _accel_target_filtered.y);
|
||||
|
|
|
@ -40,6 +40,8 @@
|
|||
|
||||
#define POSCONTROL_VEL_ERROR_CUTOFF_FREQ 4.0 // 4hz low-pass filter on velocity error
|
||||
#define POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ 2.0 // 2hz low-pass filter on accel error
|
||||
#define POSCONTROL_JERK_LIMIT_CMSSS 1700.0f // 17m/s/s/s jerk limit on horizontal acceleration
|
||||
#define POSCONTROL_ACCEL_FILTER_HZ 5.0f // 5hz low-pass filter on acceleration
|
||||
|
||||
class AC_PosControl
|
||||
{
|
||||
|
@ -370,7 +372,7 @@ private:
|
|||
LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error
|
||||
LowPassFilterFloat _accel_error_filter; // low-pass-filter on z-axis accelerometer error
|
||||
|
||||
Vector2f _accel_target_jerk_limited;
|
||||
Vector2f _accel_target_filtered;
|
||||
Vector2f _accel_target_jerk_limited; // acceleration target jerk limited to 100deg/s/s
|
||||
Vector2f _accel_target_filtered; // acceleration target filtered with 5hz low pass filter
|
||||
};
|
||||
#endif // AC_POSCONTROL_H
|
||||
|
|
Loading…
Reference in New Issue