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),
_distance_to_target(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);
@ -841,8 +841,7 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler)
if (_flags.reset_accel_to_lean_xy) {
_accel_target_jerk_limited.x = _accel_target.x;
_accel_target_jerk_limited.y = _accel_target.y;
_accel_target_filtered.x = _accel_target.x;
_accel_target_filtered.y = _accel_target.y;
_accel_target_filter.reset(Vector2f(_accel_target.x, _accel_target.y));
_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;
// lowpass filter on NE accel
float freq_cut = 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);
_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_filter.set_cutoff_frequency(min(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler));
Vector2f accel_target_filtered = _accel_target_filter.apply(_accel_target_jerk_limited, dt);
// rotate accelerations into body forward-right frame
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_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();
// 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);

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
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