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

gps fix estimation squashed commit

Browse source
This commit is contained in:
Roman Lut 2022-08-21 22:06:53 +03:00
parent 2e0783bed6
commit f87c081475
22 changed files with 273 additions and 29 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -2371,8 +2371,17 @@ bool validateRTHSanityChecker(void)
return true;
}
if (STATE(GPS_ESTIMATED_FIX)) {
//disable sanity checks in GPS estimation mode
//when estimated GPS fix is replaced with real fix, coordinates may jump
posControl.rthSanityChecker.minimalDistanceToHome = 1e10f;
//schedule check 5 seconds after apperange of real GPS fix, when position estimation coords stabilise after jump
posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000;
return true;
}
// Check at 10Hz rate
if ((currentTimeMs - posControl.rthSanityChecker.lastCheckTime) > 100) {
if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) {
const float currentDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
posControl.rthSanityChecker.lastCheckTime = currentTimeMs;
@ -3889,7 +3898,7 @@ void updateWpMissionPlanner(void)
{
static timeMs_t resetTimerStart = 0;
if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) {
const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && STATE(GPS_FIX);
const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX));
posControl.flags.wpMissionPlannerActive = true;
if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) {