Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: use EKF yaw bias to correct fixed wing yaw damper

Browse Source
This commit is contained in:
Andrew Tridgell 2021-12-06 20:54:33 +11:00
parent 304b7df8c2
commit 639447142a
1 changed files with 4 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/APM_Control/AP_YawController.cpp
View File

@ -196,6 +196,10 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator)
// Get the accln vector (m/s^2)
float accel_y = AP::ins().get_accel().y;
// subtract current bias estimate from EKF
const Vector3f &abias = _ahrs.get_accel_bias();
accel_y -= abias.y;
// Subtract the steady turn component of rate from the measured rate
// to calculate the rate relative to the turn requirement in degrees/sec
float rate_hp_in = ToDeg(omega_z - rate_offset);