1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +03:00

Moved updateGPSRescueState to imuUpdateAttitude. gps_rescue.c tidy.

This commit is contained in:
Curtis Bangert 2018-06-13 11:17:21 -04:00
parent 156b71182f
commit 8950eb1f68
4 changed files with 84 additions and 83 deletions

View file

@ -394,11 +394,11 @@ float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage)
if ((fabsf(gyroAverage[X]) > ATTITUDE_RESET_GYRO_LIMIT)
|| (fabsf(gyroAverage[Y]) > ATTITUDE_RESET_GYRO_LIMIT)
|| (fabsf(gyroAverage[Z]) > ATTITUDE_RESET_GYRO_LIMIT)
|| (!useAcc)) {
|| (!useAcc)) {
gyroQuietPeriodTimeEnd = currentTimeUs + ATTITUDE_RESET_QUIET_TIME;
attitudeResetTimeEnd = 0;
}
}
}
if (attitudeResetTimeEnd > 0) { // Resetting the attitude estimation
if (currentTimeUs >= attitudeResetTimeEnd) {
@ -408,14 +408,14 @@ float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage)
} else {
attitudeResetActive = true;
}
} else if ((gyroQuietPeriodTimeEnd > 0) && (currentTimeUs >= gyroQuietPeriodTimeEnd)) {
} else if ((gyroQuietPeriodTimeEnd > 0) && (currentTimeUs >= gyroQuietPeriodTimeEnd)) {
// Start the high gain period to bring the estimation into convergence
attitudeResetTimeEnd = currentTimeUs + ATTITUDE_RESET_ACTIVE_TIME;
gyroQuietPeriodTimeEnd = 0;
}
}
lastArmState = armState;
if (attitudeResetActive) {
ret = ATTITUDE_RESET_KP_GAIN;
} else {
@ -451,7 +451,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
useCOG = true;
} else {
// If GPS rescue mode is active and we can use it, go for it. When we're close to home we will
// If GPS rescue mode is active and we can use it, go for it. When we're close to home we will
// probably stop re calculating GPS heading data. Other future modes can also use this extern
if (canUseGPSHeading) {
@ -532,7 +532,7 @@ bool shouldInitializeGPSHeading()
return true;
}
return false;
}