rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Port of OP-754 On Revo. Fixed signs on bias calculation. Added rate zeroing at startup to yaw

Browse Source
This commit is contained in:
Alessio Morale 2013-05-22 20:27:12 +02:00
parent 294a295510
commit 697874b315
1 changed files with 7 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
flight/modules/Attitude/revolution/attitude.c
View File

@ -499,11 +499,14 @@ static int32_t updateAttitudeComplementary(bool first_run)
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
if(revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi + (gyrosData.x + gyrosBias.x) * rollPitchBiasRate;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi + (gyrosData.y + gyrosBias.y) * rollPitchBiasRate;
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi - gyrosData.x * rollPitchBiasRate;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi - gyrosData.y * rollPitchBiasRate;
gyrosBias.z -= - gyrosData.z * rollPitchBiasRate;
} else {
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi + gyrosData.x * rollPitchBiasRate;;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi + gyrosData.y * rollPitchBiasRate;
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi - (gyrosData.x - gyrosBias.x) * rollPitchBiasRate;;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi - (gyrosData.y - gyrosBias.y) * rollPitchBiasRate;
gyrosBias.z -= - (gyrosData.z - gyrosBias.z) * rollPitchBiasRate;
// if the raw values are not adjusted, we need to adjust here.
gyrosData.x -= gyrosBias.x;