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

Remove the rest of the code

Browse source
This commit is contained in:
Julio Cesar 2024-02-09 19:33:16 -03:00
parent a14dcb1fda
commit 7fa7bd272e
3 changed files with 1 additions and 77 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/navigation/navigation.c
View file

@ -3722,9 +3722,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1);
if (posControl.flags.estPosStatus == EST_TRUSTED) navFlags |= (1 << 2);
if (posControl.flags.isTerrainFollowEnabled) navFlags |= (1 << 3);
#if defined(NAV_GPS_GLITCH_DETECTION)
if (isGPSGlitchDetected()) navFlags |= (1 << 4);
#endif
// naFlags |= (1 << 4); // Old NAV GPS Glitch Detection flag
if (posControl.flags.estHeadingStatus == EST_TRUSTED) navFlags |= (1 << 5);
// Reset all navigation requests - NAV controllers will set them if necessary

70
src/main/navigation/navigation_pos_estimator.c
View file

@ -162,56 +162,6 @@ static timeUs_t getGPSDeltaTimeFilter(timeUs_t dTus)
return dTus; // Filter failed. Set GPS Hz by measurement
}
#if defined(NAV_GPS_GLITCH_DETECTION)
static bool detectGPSGlitch(timeUs_t currentTimeUs)
{
static timeUs_t previousTime = 0;
static fpVector3_t lastKnownGoodPosition;
static fpVector3_t lastKnownGoodVelocity;
bool isGlitching = false;
#ifdef USE_GPS_FIX_ESTIMATION
if (STATE(GPS_ESTIMATED_FIX)) {
//disable sanity checks in GPS estimation mode
//when estimated GPS fix is replaced with real fix, coordinates may jump
previousTime = 0;
return true;
}
#endif
if (previousTime == 0) {
isGlitching = false;
}
else {
fpVector3_t predictedGpsPosition;
float gpsDistance;
float dT = US2S(currentTimeUs - previousTime);
/* We predict new position based on previous GPS velocity and position */
predictedGpsPosition.x = lastKnownGoodPosition.x + lastKnownGoodVelocity.x * dT;
predictedGpsPosition.y = lastKnownGoodPosition.y + lastKnownGoodVelocity.y * dT;
/* New pos is within predefined radius of predicted pos, radius is expanded exponentially */
gpsDistance = calc_length_pythagorean_2D(predictedGpsPosition.x - lastKnownGoodPosition.x, predictedGpsPosition.y - lastKnownGoodPosition.y);
if (gpsDistance <= (INAV_GPS_GLITCH_RADIUS + 0.5f * INAV_GPS_GLITCH_ACCEL * dT * dT)) {
isGlitching = false;
}
else {
isGlitching = true;
}
}
if (!isGlitching) {
previousTime = currentTimeUs;
lastKnownGoodPosition = posEstimator.gps.pos;
lastKnownGoodVelocity = posEstimator.gps.vel;
}
return isGlitching;
}
#endif
/**
* Update GPS topic
* Function is called on each GPS update
@ -295,19 +245,6 @@ void onNewGPSData(void)
posEstimator.gps.vel.z = (posEstimator.gps.vel.z + (gpsSol.llh.alt - previousAlt) / dT) / 2.0f;
}
#if defined(NAV_GPS_GLITCH_DETECTION)
/* GPS glitch protection. We have local coordinates and local velocity for current GPS update. Check if they are sane */
if (detectGPSGlitch(currentTimeUs)) {
posEstimator.gps.glitchRecovery = false;
posEstimator.gps.glitchDetected = true;
}
else {
/* Store previous glitch flag in glitchRecovery to indicate a valid reading after a glitch */
posEstimator.gps.glitchRecovery = posEstimator.gps.glitchDetected;
posEstimator.gps.glitchDetected = false;
}
#endif
/* FIXME: use HDOP/VDOP */
if (gpsSol.flags.validEPE) {
posEstimator.gps.eph = gpsSol.eph;
@ -895,13 +832,6 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
}
}
#if defined(NAV_GPS_GLITCH_DETECTION)
bool isGPSGlitchDetected(void)
{
return posEstimator.gps.glitchDetected;
}
#endif
float getEstimatedAglPosition(void) {
return posEstimator.est.aglAlt;
}

4
src/main/navigation/navigation_pos_estimator_private.h
View file

@ -65,10 +65,6 @@ typedef struct {
typedef struct {
timeUs_t lastUpdateTime; // Last update time (us)
#if defined(NAV_GPS_GLITCH_DETECTION)
bool glitchDetected;
bool glitchRecovery;
#endif
fpVector3_t pos; // GPS position in NEU coordinate system (cm)
fpVector3_t vel; // GPS velocity (cms)
float eph;