aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AttitudeControl: Scale angle demand noise filter

Browse Source 
Scales filter frequency using EKF nav gain scaler to take advantage of the reduced nav frequency whichcan tolerate a slower angle response.
This is required to reduce the effect of EKF optical flow noise increase with height.
This commit is contained in:
priseborough 2014-11-18 05:54:33 +11:00 committed by Andrew Tridgell
parent a905432ffe
commit 12ea1d6e85
2 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -571,7 +571,7 @@ void AC_PosControl::update_xy_controller(bool use_desired_velocity, float ekfNav
break;
case 3:
// run position controller's acceleration to lean angle step
accel_to_lean_angles();
accel_to_lean_angles(ekfNavVelGainScaler);
_xy_step++;
break;
}
@ -642,7 +642,7 @@ void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler)
rate_to_accel_xy(dt_xy, ekfNavVelGainScaler);
// run acceleration to lean angle step
accel_to_lean_angles();
accel_to_lean_angles(ekfNavVelGainScaler);
}
// update altitude target
@ -821,7 +821,7 @@ void AC_PosControl::rate_to_accel_xy(float dt, float ekfNavVelGainScaler)
/// accel_to_lean_angles - horizontal desired acceleration to lean angles
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void AC_PosControl::accel_to_lean_angles()
void AC_PosControl::accel_to_lean_angles(float ekfNavVelGainScaler)
{
// catch if we've just been started
uint32_t now = hal.scheduler->millis();
@ -861,7 +861,7 @@ void AC_PosControl::accel_to_lean_angles()
lastPitchDem = _pitch_target;
// 5Hz lowpass filter on angles - required due to optical flow noise
float freq_cut = 5.0f;
float freq_cut = 5.0f * ekfNavVelGainScaler;
float alpha = constrain_float(dt_xy/(dt_xy + 1.0f/(2.0f*(float)M_PI*freq_cut)),0.0f,1.0f);
static float roll_target_filtered = 0.0f;
static float pitch_target_filtered = 0.0f;

2
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -317,7 +317,7 @@ private:
/// accel_to_lean_angles - horizontal desired acceleration to lean angles
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void accel_to_lean_angles();
void accel_to_lean_angles(float ekfNavVelGainScaler);
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
float calc_leash_length(float speed_cms, float accel_cms, float kP) const;