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

AC_AttitudeControl: apply EKF Z gain scaler

Browse Source 
for flying with DCM
This commit is contained in:
Andrew Tridgell 2021-08-14 16:06:34 +10:00
parent 53e7436525
commit 285798446a
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -625,8 +625,8 @@ void AC_PosControl::update_xy_controller()
} }
_last_update_xy_us = AP_HAL::micros64(); _last_update_xy_us = AP_HAL::micros64();
float ekfGndSpdLimit, ekfNavVelGainScaler; float ahrsGndSpdLimit, ahrsControlScaleXY;
AP::ahrs().getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); AP::ahrs().getControlLimits(ahrsGndSpdLimit, ahrsControlScaleXY);
// Position Controller // Position Controller
@ -634,7 +634,7 @@ void AC_PosControl::update_xy_controller()
Vector2f vel_target = _p_pos_xy.update_all(_pos_target.x, _pos_target.y, curr_pos, _limit.pos_xy); Vector2f vel_target = _p_pos_xy.update_all(_pos_target.x, _pos_target.y, curr_pos, _limit.pos_xy);
// add velocity feed-forward scaled to compensate for optical flow measurement induced EKF noise // add velocity feed-forward scaled to compensate for optical flow measurement induced EKF noise
vel_target *= ekfNavVelGainScaler; vel_target *= ahrsControlScaleXY;
_vel_target.x = vel_target.x; _vel_target.x = vel_target.x;
_vel_target.y = vel_target.y; _vel_target.y = vel_target.y;
_vel_target.x += _vel_desired.x; _vel_target.x += _vel_desired.x;
@ -652,7 +652,7 @@ void AC_PosControl::update_xy_controller()
} }
Vector2f accel_target = _pid_vel_xy.update_all(Vector2f{_vel_target.x, _vel_target.y}, _vehicle_horiz_vel, Vector2f(_limit_vector.x, _limit_vector.y)); Vector2f accel_target = _pid_vel_xy.update_all(Vector2f{_vel_target.x, _vel_target.y}, _vehicle_horiz_vel, Vector2f(_limit_vector.x, _limit_vector.y));
// acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise // acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
accel_target *= ekfNavVelGainScaler; accel_target *= ahrsControlScaleXY;
// pass the correction acceleration to the target acceleration output // pass the correction acceleration to the target acceleration output
_accel_target.x = accel_target.x; _accel_target.x = accel_target.x;
@ -962,6 +962,7 @@ void AC_PosControl::update_z_controller()
float pos_target_zf = _pos_target.z; float pos_target_zf = _pos_target.z;
_vel_target.z = _p_pos_z.update_all(pos_target_zf, curr_alt, _limit.pos_down, _limit.pos_up); _vel_target.z = _p_pos_z.update_all(pos_target_zf, curr_alt, _limit.pos_down, _limit.pos_up);
_vel_target.z *= AP::ahrs().getControlScaleZ();
_pos_target.z = pos_target_zf; _pos_target.z = pos_target_zf;
@ -972,6 +973,7 @@ void AC_PosControl::update_z_controller()
const Vector3f& curr_vel = _inav.get_velocity(); const Vector3f& curr_vel = _inav.get_velocity();
_accel_target.z = _pid_vel_z.update_all(_vel_target.z, curr_vel.z, _motors.limit.throttle_lower, _motors.limit.throttle_upper); _accel_target.z = _pid_vel_z.update_all(_vel_target.z, curr_vel.z, _motors.limit.throttle_lower, _motors.limit.throttle_upper);
_accel_target.z *= AP::ahrs().getControlScaleZ();
// add feed forward component // add feed forward component
_accel_target.z += _accel_desired.z; _accel_target.z += _accel_desired.z;