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

AC_PosControl: add comments and defines for jerk limits

Browse Source
This commit is contained in:
Randy Mackay 2015-03-10 22:04:31 +09:00
parent 4408c1b935
commit 965db2c7f7
2 changed files with 7 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -802,7 +802,8 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler)
float accel_right, accel_forward; float accel_right, accel_forward;
float lean_angle_max = _attitude_control.lean_angle_max(); 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_in(_accel_target.x, _accel_target.y);
Vector2f accel_change = accel_in-_accel_target_jerk_limited; 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 // 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); 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.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); _accel_target_filtered.y += alpha * (_accel_target_jerk_limited.y - _accel_target_filtered.y);

6
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -40,6 +40,8 @@
#define POSCONTROL_VEL_ERROR_CUTOFF_FREQ 4.0 // 4hz low-pass filter on velocity error #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_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 class AC_PosControl
{ {
@ -370,7 +372,7 @@ private:
LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error
LowPassFilterFloat _accel_error_filter; // low-pass-filter on z-axis accelerometer error LowPassFilterFloat _accel_error_filter; // low-pass-filter on z-axis accelerometer error
Vector2f _accel_target_jerk_limited; Vector2f _accel_target_jerk_limited; // acceleration target jerk limited to 100deg/s/s
Vector2f _accel_target_filtered; Vector2f _accel_target_filtered; // acceleration target filtered with 5hz low pass filter
}; };
#endif // AC_POSCONTROL_H #endif // AC_POSCONTROL_H