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

AC_PosControl: use LowPassFilterVector2f

This commit is contained in:
Jonathan Challinger 2015-04-16 12:57:28 -07:00 committed by Randy Mackay
parent 8f667b3d1c
commit 738b1967ad
2 changed files with 7 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -54,7 +54,7 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
_alt_max(0.0f), _alt_max(0.0f),
_distance_to_target(0.0f), _distance_to_target(0.0f),
_accel_target_jerk_limited(0.0f,0.0f), _accel_target_jerk_limited(0.0f,0.0f),
_accel_target_filtered(0.0f,0.0f) _accel_target_filter(POSCONTROL_ACCEL_FILTER_HZ)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
@ -841,8 +841,7 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler)
if (_flags.reset_accel_to_lean_xy) { if (_flags.reset_accel_to_lean_xy) {
_accel_target_jerk_limited.x = _accel_target.x; _accel_target_jerk_limited.x = _accel_target.x;
_accel_target_jerk_limited.y = _accel_target.y; _accel_target_jerk_limited.y = _accel_target.y;
_accel_target_filtered.x = _accel_target.x; _accel_target_filter.reset(Vector2f(_accel_target.x, _accel_target.y));
_accel_target_filtered.y = _accel_target.y;
_flags.reset_accel_to_lean_xy = false; _flags.reset_accel_to_lean_xy = false;
} }
@ -859,14 +858,12 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler)
_accel_target_jerk_limited += accel_change; _accel_target_jerk_limited += accel_change;
// lowpass filter on NE accel // lowpass filter on NE accel
float freq_cut = min(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler); _accel_target_filter.set_cutoff_frequency(min(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler));
float alpha = constrain_float(dt/(dt + 1.0f/(2.0f*(float)M_PI*freq_cut)),0.0f,1.0f); Vector2f accel_target_filtered = _accel_target_filter.apply(_accel_target_jerk_limited, dt);
_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);
// rotate accelerations into body forward-right frame // rotate accelerations into body forward-right frame
accel_forward = _accel_target_filtered.x*_ahrs.cos_yaw() + _accel_target_filtered.y*_ahrs.sin_yaw(); accel_forward = accel_target_filtered.x*_ahrs.cos_yaw() + accel_target_filtered.y*_ahrs.sin_yaw();
accel_right = -_accel_target_filtered.x*_ahrs.sin_yaw() + _accel_target_filtered.y*_ahrs.cos_yaw(); accel_right = -accel_target_filtered.x*_ahrs.sin_yaw() + accel_target_filtered.y*_ahrs.cos_yaw();
// update angle targets that will be passed to stabilize controller // update angle targets that will be passed to stabilize controller
_pitch_target = constrain_float(fast_atan(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI),-lean_angle_max, lean_angle_max); _pitch_target = constrain_float(fast_atan(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI),-lean_angle_max, lean_angle_max);

2
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -383,6 +383,6 @@ 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
Vector2f _accel_target_jerk_limited; // acceleration target jerk limited to 100deg/s/s 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 LowPassFilterVector2f _accel_target_filter; // acceleration target filter
}; };
#endif // AC_POSCONTROL_H #endif // AC_POSCONTROL_H