Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 01:35:35 +03:00
Code Issues Wiki Activity

GPS glitch detection fix. Logging GPS glitch status to blackbox

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-12-08 23:03:36 +10:00
parent fb647b181e
commit 77b53404c3
3 changed files with 9 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/flight/navigation_rewrite.c
View file

@ -1851,6 +1851,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
if (posControl.flags.hasValidSurfaceSensor) navFlags |= (1 << 1);
if (posControl.flags.hasValidPositionSensor) navFlags |= (1 << 2);
if ((STATE(GPS_FIX) && GPS_numSat >= posControl.navConfig->inav.gps_min_sats)) navFlags |= (1 << 3);
if (isGPSGlitchDetected()) navFlags |= (1 << 4);
#endif
// No navigation when disarmed

8
src/main/flight/navigation_rewrite_pos_estimator.c
View file

@ -201,7 +201,7 @@ static bool detectGPSGlitch(t_fp_vector * newLocalPos, float dT)
/* We predict new position based on previous GPS velocity and position */
predictedGpsPosition.V.X = posEstimator.gps.pos.V.X + posEstimator.gps.vel.V.X * dT;
predictedGpsPosition.V.Y = posEstimator.gps.pos.V.Y + posEstimator.gps.vel.V.Y * dT - newLocalPos->V.Y;
predictedGpsPosition.V.Y = posEstimator.gps.pos.V.Y + posEstimator.gps.vel.V.Y * dT;
/* Calculate position error */
gpsDistance = sqrtf(sq(predictedGpsPosition.V.X - newLocalPos->V.X) + sq(predictedGpsPosition.V.Y - newLocalPos->V.Y));
@ -578,7 +578,6 @@ static void updateEstimatedTopic(uint32_t currentTime)
posEstimator.est.surface = -1;
posEstimator.est.surfaceVel = 0;
#endif
}
/**
@ -628,6 +627,11 @@ static void publishEstimatedTopic(uint32_t currentTime)
}
}
bool isGPSGlitchDetected(void)
{
return posEstimator.gps.glitchDetected;
}
/**
* Initialize position estimator
* Should be called once before any update occurs

2
src/main/flight/navigation_rewrite_private.h
View file

@ -286,6 +286,8 @@ void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX,
void updateActualAltitudeAndClimbRate(bool hasValidSensor, float newAltitude, float newVelocity);
void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, float surfaceVelocity);
bool isGPSGlitchDetected(void);
/* Autonomous navigation functions */
void setupAutonomousControllerRTH(void);
void resetAutonomousControllerForWP(void);