Correct nested if else for USE_MAG and USE_GPS (#5414)
If USE_MAG was undefined but USE_GPS was defined, then the "else if" would incorrectly apply to a condition above making the USE_GPS section unlikely to execute.
This commit is contained in:
parent
0430fcb67d
commit
18b857de65
|
@ -428,7 +428,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
|
|||
}
|
||||
#endif
|
||||
#if defined(USE_GPS)
|
||||
else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) {
|
||||
if (!useMag && STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) {
|
||||
// In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
|
||||
rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - gpsSol.groundCourse);
|
||||
useYaw = true;
|
||||
|
|
Loading…
Reference in New Issue