From 26deae54bbdf08085e85cace5d5d203ba7fb7fa1 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 21 Aug 2023 23:28:11 +0100 Subject: [PATCH 01/10] first build --- src/main/fc/fc_core.c | 20 ++++++++++++++------ src/main/fc/fc_core.h | 1 + src/main/navigation/navigation.c | 20 +++++++++++++++----- src/main/navigation/navigation.h | 1 + src/main/navigation/navigation_fixedwing.c | 2 +- 5 files changed, 32 insertions(+), 12 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index af8d1202b17..46ef140f84a 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -118,6 +118,7 @@ uint8_t motorControlEnable = false; static bool isRXDataNew; static disarmReason_t lastDisarmReason = DISARM_NONE; timeUs_t lastDisarmTimeUs = 0; +timeMs_t emergInflightRearmTimeout = 0; static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible static timeMs_t prearmActivationTime = 0; @@ -176,7 +177,7 @@ int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband) { int16_t stickDeflection = 0; -#if defined(SITL_BUILD) // Workaround due to strange bug in GCC > 10.2 https://gcc.gnu.org/bugzilla/show_bug.cgi?id=108914 +#if defined(SITL_BUILD) // Workaround due to strange bug in GCC > 10.2 https://gcc.gnu.org/bugzilla/show_bug.cgi?id=108914 const int16_t value = rawData - PWM_RANGE_MIDDLE; if (value < -500) { stickDeflection = -500; @@ -186,9 +187,9 @@ int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband) stickDeflection = value; } #else - stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500); + stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500); #endif - + stickDeflection = applyDeadbandRescaled(stickDeflection, deadband, -500, 500); return rcLookup(stickDeflection, rate); } @@ -432,6 +433,7 @@ void disarm(disarmReason_t disarmReason) if (ARMING_FLAG(ARMED)) { lastDisarmReason = disarmReason; lastDisarmTimeUs = micros(); + emergInflightRearmTimeout = US2MS(lastDisarmTimeUs) + (isProbablyStillFlying() ? 5000 : 0); DISABLE_ARMING_FLAG(ARMED); #ifdef USE_BLACKBOX @@ -509,6 +511,11 @@ void releaseSharedTelemetryPorts(void) { } } +bool emergInflightRearmEnabled(void) +{ + return millis() < emergInflightRearmTimeout; +} + void tryArm(void) { updateArmingStatus(); @@ -529,9 +536,10 @@ void tryArm(void) #endif #ifdef USE_PROGRAMMING_FRAMEWORK - if (!isArmingDisabled() || emergencyArmingIsEnabled() || LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) { + if (!isArmingDisabled() || emergencyArmingIsEnabled() || emergInflightRearmEnabled() || + LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) { #else - if (!isArmingDisabled() || emergencyArmingIsEnabled()) { + if (!isArmingDisabled() || emergencyArmingIsEnabled() || emergInflightRearmEnabled()) { #endif // If nav_extra_arming_safety was bypassed we always // allow bypassing it even without the sticks set @@ -837,7 +845,7 @@ static float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompens void taskMainPidLoop(timeUs_t currentTimeUs) { - + cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 37d0bbda79a..1d34e31742b 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -43,6 +43,7 @@ void tryArm(void); disarmReason_t getDisarmReason(void); bool emergencyArmingUpdate(bool armingSwitchIsOn); +bool emergInflightRearmEnabled(void); bool areSensorsCalibrating(void); float getFlightTime(void); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index d8bc20554cc..04c910aa63a 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -228,6 +228,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, static navWapointHeading_t wpHeadingControl; navigationPosControl_t posControl; navSystemStatus_t NAV_Status; +static bool landingDetectorIsActive; EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients; @@ -2803,14 +2804,14 @@ void updateLandingStatus(timeMs_t currentTimeMs) } lastUpdateTimeMs = currentTimeMs; - static bool landingDetectorIsActive; - DEBUG_SET(DEBUG_LANDING, 0, landingDetectorIsActive); DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED)); if (!ARMING_FLAG(ARMED)) { - resetLandingDetector(); - landingDetectorIsActive = false; + if (!emergInflightRearmEnabled()) { + resetLandingDetector(); + landingDetectorIsActive = false; + } if (!IS_RC_MODE_ACTIVE(BOXARM)) { DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); } @@ -2852,6 +2853,15 @@ bool isFlightDetected(void) return STATE(AIRPLANE) ? isFixedWingFlying() : isMulticopterFlying(); } +bool isProbablyStillFlying(void) +{ + bool inFlightSanityCheck = false; + if (STATE(AIRPLANE)) { + inFlightSanityCheck = isGPSHeadingValid(); + } + return landingDetectorIsActive && !STATE(LANDING_DETECTED) && inFlightSanityCheck; +} + /*----------------------------------------------------------- * Z-position controller *-----------------------------------------------------------*/ @@ -3801,7 +3811,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) canActivateWaypoint = false; // Launch mode can be activated if feature FW_LAUNCH is enabled or BOX is turned on prior to arming (avoid switching to LAUNCH in flight) - canActivateLaunchMode = isNavLaunchEnabled(); + canActivateLaunchMode = isNavLaunchEnabled() && (!sensors(SENSOR_GPS) || (sensors(SENSOR_GPS) && !isGPSHeadingValid())); } return NAV_FSM_EVENT_SWITCH_TO_IDLE; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index cf4289e14a1..9ffe532e6c5 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -610,6 +610,7 @@ const char * fixedWingLaunchStateMessage(void); float calculateAverageSpeed(void); void updateLandingStatus(timeMs_t currentTimeMs); +bool isProbablyStillFlying(void); const navigationPIDControllers_t* getNavigationPIDControllers(void); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 91354d97aed..7b55a48e6a0 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -692,7 +692,7 @@ bool isFixedWingFlying(void) bool velCondition = posControl.actualState.velXY > 250.0f || airspeed > 250.0f; bool launchCondition = isNavLaunchEnabled() && fixedWingLaunchStatus() == FW_LAUNCH_FLYING; - return (isImuHeadingValid() && throttleCondition && velCondition) || launchCondition; + return (isGPSHeadingValid() && throttleCondition && velCondition) || launchCondition; } /*----------------------------------------------------------- From 3b9e2a24ff26e697af412c000741bf3998466989 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 23 Aug 2023 15:07:20 +0100 Subject: [PATCH 02/10] add MR flight sanity and forced Angle --- src/main/fc/fc_core.c | 69 +++++++++++++++++++------------- src/main/navigation/navigation.c | 7 ++-- 2 files changed, 45 insertions(+), 31 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 46ef140f84a..187613a9ee4 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -104,6 +104,7 @@ enum { #define EMERGENCY_ARMING_TIME_WINDOW_MS 10000 #define EMERGENCY_ARMING_COUNTER_STEP_MS 1000 #define EMERGENCY_ARMING_MIN_ARM_COUNT 10 +#define EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS 5000 timeDelta_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop static timeUs_t flightTime = 0; @@ -119,6 +120,7 @@ static bool isRXDataNew; static disarmReason_t lastDisarmReason = DISARM_NONE; timeUs_t lastDisarmTimeUs = 0; timeMs_t emergInflightRearmTimeout = 0; +timeMs_t mcEmergRearmStabiliseTimeout = 0; static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible static timeMs_t prearmActivationTime = 0; @@ -314,11 +316,11 @@ static void updateArmingStatus(void) /* CHECK: Do not allow arming if Servo AutoTrim is enabled */ if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) { - ENABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); - } + ENABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); + } else { - DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); - } + DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); + } #ifdef USE_DSHOT /* CHECK: Don't arm if the DShot beeper was used recently, as there is a minimum delay before sending the next DShot command */ @@ -433,7 +435,7 @@ void disarm(disarmReason_t disarmReason) if (ARMING_FLAG(ARMED)) { lastDisarmReason = disarmReason; lastDisarmTimeUs = micros(); - emergInflightRearmTimeout = US2MS(lastDisarmTimeUs) + (isProbablyStillFlying() ? 5000 : 0); + emergInflightRearmTimeout = US2MS(lastDisarmTimeUs) + (isProbablyStillFlying() ? EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS : 0); DISABLE_ARMING_FLAG(ARMED); #ifdef USE_BLACKBOX @@ -501,19 +503,20 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn) return counter >= EMERGENCY_ARMING_MIN_ARM_COUNT; } -#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) - -void releaseSharedTelemetryPorts(void) { - serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); - while (sharedPort) { - mspSerialReleasePortIfAllocated(sharedPort); - sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); - } -} - bool emergInflightRearmEnabled(void) { - return millis() < emergInflightRearmTimeout; + /* Emergency rearm allowed within emergInflightRearmTimeout window. + * On MR emergency rearm only allowed after 1.5s if MR dropping after disarm, i.e. still in flight */ + timeMs_t currentTimeMs = millis(); + if (STATE(MULTIROTOR)) { + uint16_t mcFlightSanityCheckTime = EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS - 1500; // check MR vertical speed at least 2 sec after disarm + if (getEstimatedActualVelocity(Z) > -100.0f && (emergInflightRearmTimeout - currentTimeMs < mcFlightSanityCheckTime)) { + emergInflightRearmTimeout = currentTimeMs; // MR doesn't appear to be flying so don't allow emergency rearm + } else { + mcEmergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise MR + } + } + return currentTimeMs < emergInflightRearmTimeout; } void tryArm(void) @@ -594,6 +597,16 @@ void tryArm(void) } } +#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) + +void releaseSharedTelemetryPorts(void) { + serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); + while (sharedPort) { + mspSerialReleasePortIfAllocated(sharedPort); + sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); + } +} + void processRx(timeUs_t currentTimeUs) { // Calculate RPY channel data @@ -644,9 +657,12 @@ void processRx(timeUs_t currentTimeUs) processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData); } + // Angle mode forced on briefly after multirotor emergency in flight rearm to help stabilise attitude + bool mcEmergRearmAngleEnforce = STATE(MULTIROTOR) && mcEmergRearmStabiliseTimeout > millis(); + bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || mcEmergRearmAngleEnforce; bool canUseHorizonMode = true; - if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode()) && sensors(SENSOR_ACC)) { + if (sensors(SENSOR_ACC) && (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle)) { // bumpless transfer to Level mode canUseHorizonMode = false; @@ -813,7 +829,6 @@ void processRx(timeUs_t currentTimeUs) } } #endif - } // Function for loop trigger @@ -929,15 +944,15 @@ void taskMainPidLoop(timeUs_t currentTimeUs) //Servos should be filtered or written only when mixer is using servos or special feaures are enabled #ifdef USE_SIMULATOR - if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { - if (isServoOutputEnabled()) { - writeServos(); - } - - if (motorControlEnable) { - writeMotors(); - } - } + if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { + if (isServoOutputEnabled()) { + writeServos(); + } + + if (motorControlEnable) { + writeMotors(); + } + } #else if (isServoOutputEnabled()) { writeServos(); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 04c910aa63a..96d99ebf3b5 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2855,10 +2855,9 @@ bool isFlightDetected(void) bool isProbablyStillFlying(void) { - bool inFlightSanityCheck = false; - if (STATE(AIRPLANE)) { - inFlightSanityCheck = isGPSHeadingValid(); - } + // Multirotor flight sanity checked after disarm so always true here + bool inFlightSanityCheck = STATE(MULTIROTOR) || (STATE(AIRPLANE) && isGPSHeadingValid()); + return landingDetectorIsActive && !STATE(LANDING_DETECTED) && inFlightSanityCheck; } From 7bb9b69a5f5b89e5d75534248b0931929d0b990f Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 23 Aug 2023 22:16:45 +0100 Subject: [PATCH 03/10] Fix logic --- src/main/fc/fc_core.c | 24 +++++++++++++++++------- src/main/navigation/navigation.c | 7 +++---- 2 files changed, 20 insertions(+), 11 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 187613a9ee4..60c658809ef 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -435,7 +435,10 @@ void disarm(disarmReason_t disarmReason) if (ARMING_FLAG(ARMED)) { lastDisarmReason = disarmReason; lastDisarmTimeUs = micros(); - emergInflightRearmTimeout = US2MS(lastDisarmTimeUs) + (isProbablyStillFlying() ? EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS : 0); + if (disarmReason == DISARM_SWITCH || disarmReason == DISARM_KILLSWITCH) { + emergInflightRearmTimeout = isProbablyStillFlying() ? US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS : 0; + } + DISABLE_ARMING_FLAG(ARMED); #ifdef USE_BLACKBOX @@ -506,17 +509,23 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn) bool emergInflightRearmEnabled(void) { /* Emergency rearm allowed within emergInflightRearmTimeout window. - * On MR emergency rearm only allowed after 1.5s if MR dropping after disarm, i.e. still in flight */ + * On MR emergency rearm only allowed after 1.5s if MR dropping or climbing after disarm, i.e. still in flight */ + timeMs_t currentTimeMs = millis(); + if (!emergInflightRearmTimeout || currentTimeMs > emergInflightRearmTimeout) { + return false; + } + if (STATE(MULTIROTOR)) { - uint16_t mcFlightSanityCheckTime = EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS - 1500; // check MR vertical speed at least 2 sec after disarm - if (getEstimatedActualVelocity(Z) > -100.0f && (emergInflightRearmTimeout - currentTimeMs < mcFlightSanityCheckTime)) { - emergInflightRearmTimeout = currentTimeMs; // MR doesn't appear to be flying so don't allow emergency rearm + uint16_t mcFlightSanityCheckTime = EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS - 1500; // check MR vertical speed at least 1.5 sec after disarm + if (fabsf(getEstimatedActualVelocity(Z)) < 100.0f && (emergInflightRearmTimeout - currentTimeMs < mcFlightSanityCheckTime)) { + return false; // MR doesn't appear to be flying so don't allow emergency rearm } else { mcEmergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise MR } } - return currentTimeMs < emergInflightRearmTimeout; + + return true; } void tryArm(void) @@ -555,6 +564,7 @@ void tryArm(void) } lastDisarmReason = DISARM_NONE; + emergInflightRearmTimeout = 0; ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(WAS_EVER_ARMED); @@ -657,7 +667,7 @@ void processRx(timeUs_t currentTimeUs) processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData); } - // Angle mode forced on briefly after multirotor emergency in flight rearm to help stabilise attitude + // Angle mode forced on briefly after multirotor emergency inflight rearm to help stabilise attitude bool mcEmergRearmAngleEnforce = STATE(MULTIROTOR) && mcEmergRearmStabiliseTimeout > millis(); bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || mcEmergRearmAngleEnforce; bool canUseHorizonMode = true; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 96d99ebf3b5..759ddd42f3f 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2808,10 +2808,9 @@ void updateLandingStatus(timeMs_t currentTimeMs) DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED)); if (!ARMING_FLAG(ARMED)) { - if (!emergInflightRearmEnabled()) { - resetLandingDetector(); - landingDetectorIsActive = false; - } + resetLandingDetector(); + landingDetectorIsActive = false; + if (!IS_RC_MODE_ACTIVE(BOXARM)) { DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); } From 6e58a94cf3a9230096cffb21c6816b92eebcd47a Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 27 Aug 2023 23:28:04 +0100 Subject: [PATCH 04/10] Change logic + add gyro check for MR --- src/main/fc/fc_core.c | 38 ++++++++++++++------------------ src/main/navigation/navigation.c | 20 ++++++++++++----- 2 files changed, 30 insertions(+), 28 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 60c658809ef..a00e7972423 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -119,8 +119,7 @@ uint8_t motorControlEnable = false; static bool isRXDataNew; static disarmReason_t lastDisarmReason = DISARM_NONE; timeUs_t lastDisarmTimeUs = 0; -timeMs_t emergInflightRearmTimeout = 0; -timeMs_t mcEmergRearmStabiliseTimeout = 0; +timeMs_t emergRearmStabiliseTimeout = 0; static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible static timeMs_t prearmActivationTime = 0; @@ -435,10 +434,6 @@ void disarm(disarmReason_t disarmReason) if (ARMING_FLAG(ARMED)) { lastDisarmReason = disarmReason; lastDisarmTimeUs = micros(); - if (disarmReason == DISARM_SWITCH || disarmReason == DISARM_KILLSWITCH) { - emergInflightRearmTimeout = isProbablyStillFlying() ? US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS : 0; - } - DISABLE_ARMING_FLAG(ARMED); #ifdef USE_BLACKBOX @@ -508,24 +503,24 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn) bool emergInflightRearmEnabled(void) { - /* Emergency rearm allowed within emergInflightRearmTimeout window. - * On MR emergency rearm only allowed after 1.5s if MR dropping or climbing after disarm, i.e. still in flight */ - + /* Emergency rearm allowed within 5s timeout period after disarm if craft still flying */ timeMs_t currentTimeMs = millis(); - if (!emergInflightRearmTimeout || currentTimeMs > emergInflightRearmTimeout) { + emergRearmStabiliseTimeout = 0; + + if ((lastDisarmReason != DISARM_SWITCH && lastDisarmReason != DISARM_KILLSWITCH) || + (currentTimeMs > US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS)) { return false; } - if (STATE(MULTIROTOR)) { - uint16_t mcFlightSanityCheckTime = EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS - 1500; // check MR vertical speed at least 1.5 sec after disarm - if (fabsf(getEstimatedActualVelocity(Z)) < 100.0f && (emergInflightRearmTimeout - currentTimeMs < mcFlightSanityCheckTime)) { - return false; // MR doesn't appear to be flying so don't allow emergency rearm - } else { - mcEmergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise MR - } + if (isProbablyStillFlying()) { + emergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise craft + return true; + } else if (STATE(MULTIROTOR) && (currentTimeMs > US2MS(lastDisarmTimeUs) + 1500) && fabsf(getEstimatedActualVelocity(Z)) > 100.0f) { + // allow emergency rearm if MR has vertical speed at least 1.5 sec after disarm indicating still flying + return true; } - return true; + return false; // craft doesn't appear to be flying, don't allow emergency rearm } void tryArm(void) @@ -564,7 +559,6 @@ void tryArm(void) } lastDisarmReason = DISARM_NONE; - emergInflightRearmTimeout = 0; ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(WAS_EVER_ARMED); @@ -667,9 +661,9 @@ void processRx(timeUs_t currentTimeUs) processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData); } - // Angle mode forced on briefly after multirotor emergency inflight rearm to help stabilise attitude - bool mcEmergRearmAngleEnforce = STATE(MULTIROTOR) && mcEmergRearmStabiliseTimeout > millis(); - bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || mcEmergRearmAngleEnforce; + // Angle mode forced on briefly after emergency inflight rearm to help stabilise attitude (currently limited to MR) + bool emergRearmAngleEnforce = STATE(MULTIROTOR) && emergRearmStabiliseTimeout > US2MS(currentTimeUs); + bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || emergRearmAngleEnforce; bool canUseHorizonMode = true; if (sensors(SENSOR_ACC) && (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle)) { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 759ddd42f3f..eba38c68b10 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -61,6 +61,7 @@ #include "sensors/acceleration.h" #include "sensors/boardalignment.h" #include "sensors/battery.h" +#include "sensors/gyro.h" #include "programming/global_variables.h" @@ -2808,13 +2809,16 @@ void updateLandingStatus(timeMs_t currentTimeMs) DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED)); if (!ARMING_FLAG(ARMED)) { - resetLandingDetector(); - landingDetectorIsActive = false; - + if (STATE(LANDING_DETECTED)) { + resetLandingDetector(); + landingDetectorIsActive = false; + } if (!IS_RC_MODE_ACTIVE(BOXARM)) { DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); } return; + } else if (getArmTime() < 0.25f && landingDetectorIsActive) { // force reset landing detector immediately after arming + landingDetectorIsActive = false; } if (!landingDetectorIsActive) { @@ -2854,10 +2858,14 @@ bool isFlightDetected(void) bool isProbablyStillFlying(void) { - // Multirotor flight sanity checked after disarm so always true here - bool inFlightSanityCheck = STATE(MULTIROTOR) || (STATE(AIRPLANE) && isGPSHeadingValid()); + bool inFlightSanityCheck; + if (STATE(MULTIROTOR)) { + inFlightSanityCheck = posControl.actualState.velXY > MC_LAND_CHECK_VEL_XY_MOVING || averageAbsGyroRates() > 4.0f; + } else { + inFlightSanityCheck = isGPSHeadingValid(); + } - return landingDetectorIsActive && !STATE(LANDING_DETECTED) && inFlightSanityCheck; + return landingDetectorIsActive && inFlightSanityCheck; } /*----------------------------------------------------------- From 1338fb4a54fcf891a175c5285ca48bfe991b34dd Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 28 Aug 2023 12:47:11 +0100 Subject: [PATCH 05/10] Update fc_core.h --- src/main/fc/fc_core.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 1d34e31742b..37d0bbda79a 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -43,7 +43,6 @@ void tryArm(void); disarmReason_t getDisarmReason(void); bool emergencyArmingUpdate(bool armingSwitchIsOn); -bool emergInflightRearmEnabled(void); bool areSensorsCalibrating(void); float getFlightTime(void); From 2d2195d841b31ace59b94831595a9e22cd366f25 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 29 Aug 2023 11:41:39 +0100 Subject: [PATCH 06/10] fix landing detector active state logic --- src/main/fc/fc_core.c | 2 ++ src/main/navigation/navigation.c | 13 +++++++------ src/main/navigation/navigation.h | 1 + 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index a00e7972423..92705ab6910 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -558,6 +558,8 @@ void tryArm(void) ENABLE_STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED); } + resetLandingDetectorActiveState(); // reset landing detector after arming to avoid false detection before flight + lastDisarmReason = DISARM_NONE; ENABLE_ARMING_FLAG(ARMED); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index eba38c68b10..63c5b9884a9 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2809,16 +2809,12 @@ void updateLandingStatus(timeMs_t currentTimeMs) DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED)); if (!ARMING_FLAG(ARMED)) { - if (STATE(LANDING_DETECTED)) { - resetLandingDetector(); - landingDetectorIsActive = false; - } + resetLandingDetector(); + if (!IS_RC_MODE_ACTIVE(BOXARM)) { DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); } return; - } else if (getArmTime() < 0.25f && landingDetectorIsActive) { // force reset landing detector immediately after arming - landingDetectorIsActive = false; } if (!landingDetectorIsActive) { @@ -2851,6 +2847,11 @@ void resetLandingDetector(void) posControl.flags.resetLandingDetector = true; } +void resetLandingDetectorActiveState(void) +{ + landingDetectorIsActive = false; +} + bool isFlightDetected(void) { return STATE(AIRPLANE) ? isFixedWingFlying() : isMulticopterFlying(); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 9ffe532e6c5..3fd213a2450 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -611,6 +611,7 @@ float calculateAverageSpeed(void); void updateLandingStatus(timeMs_t currentTimeMs); bool isProbablyStillFlying(void); +void resetLandingDetectorActiveState(void); const navigationPIDControllers_t* getNavigationPIDControllers(void); From fa60cfda81a4d50591bea9d74a85378f75844e81 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 6 Sep 2023 23:25:13 +0100 Subject: [PATCH 07/10] fixes --- src/main/fc/fc_core.c | 10 +++++----- src/main/navigation/navigation.c | 3 +++ 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 92705ab6910..19015989ca6 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -512,11 +512,11 @@ bool emergInflightRearmEnabled(void) return false; } - if (isProbablyStillFlying()) { - emergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise craft - return true; - } else if (STATE(MULTIROTOR) && (currentTimeMs > US2MS(lastDisarmTimeUs) + 1500) && fabsf(getEstimatedActualVelocity(Z)) > 100.0f) { - // allow emergency rearm if MR has vertical speed at least 1.5 sec after disarm indicating still flying + // allow emergency rearm if MR has vertical speed at least 1.5 sec after disarm indicating still flying + bool mcDisarmVertVelCheck = STATE(MULTIROTOR) && (currentTimeMs > US2MS(lastDisarmTimeUs) + 1500) && fabsf(getEstimatedActualVelocity(Z)) > 100.0f; + + if (isProbablyStillFlying() || mcDisarmVertVelCheck) { + emergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise craft return true; } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 63c5b9884a9..5b3f17bb2ab 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2809,6 +2809,9 @@ void updateLandingStatus(timeMs_t currentTimeMs) DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED)); if (!ARMING_FLAG(ARMED)) { + if (STATE(LANDING_DETECTED)) { + landingDetectorIsActive = false; + } resetLandingDetector(); if (!IS_RC_MODE_ACTIVE(BOXARM)) { From 52f5cffc43e73283f599054236ff5b460ea146ed Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 26 Sep 2023 23:33:56 +0100 Subject: [PATCH 08/10] prevent unwanted resets on disarm --- src/main/fc/fc_core.c | 12 +++++- src/main/fc/runtime_config.h | 1 + src/main/io/osd.c | 6 ++- src/main/navigation/navigation.c | 34 ++++++++++------ .../navigation/navigation_pos_estimator.c | 40 ++++++++++++++----- 5 files changed, 67 insertions(+), 26 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 76d1a2626c5..66c275366b4 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -436,6 +436,7 @@ void disarm(disarmReason_t disarmReason) lastDisarmReason = disarmReason; lastDisarmTimeUs = micros(); DISABLE_ARMING_FLAG(ARMED); + DISABLE_STATE(IN_FLIGHT_EMERG_REARM); #ifdef USE_BLACKBOX if (feature(FEATURE_BLACKBOX)) { @@ -522,6 +523,7 @@ bool emergInflightRearmEnabled(void) if (isProbablyStillFlying() || mcDisarmVertVelCheck) { emergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise craft + ENABLE_STATE(IN_FLIGHT_EMERG_REARM); return true; } @@ -574,11 +576,13 @@ void tryArm(void) ENABLE_ARMING_FLAG(WAS_EVER_ARMED); //It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD); - logicConditionReset(); + if (!STATE(IN_FLIGHT_EMERG_REARM)) { + logicConditionReset(); #ifdef USE_PROGRAMMING_FRAMEWORK - programmingPidReset(); + programmingPidReset(); #endif + } headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); @@ -902,6 +906,10 @@ void taskMainPidLoop(timeUs_t currentTimeUs) processDelayedSave(); } + if (armTime > 1 * USECS_PER_SEC) { // reset in flight emerg rearm flag 1 sec after arming once it's served its purpose + DISABLE_STATE(IN_FLIGHT_EMERG_REARM); + } + #if defined(SITL_BUILD) if (lockMainPID()) { #endif diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 16d1b411df8..241e5f7e258 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -139,6 +139,7 @@ typedef enum { FW_HEADING_USE_YAW = (1 << 24), ANTI_WINDUP_DEACTIVATED = (1 << 25), LANDING_DETECTED = (1 << 26), + IN_FLIGHT_EMERG_REARM = (1 << 27), } stateFlags_t; #define DISABLE_STATE(mask) (stateFlags &= ~(mask)) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f5daeaf63c2..b34751096eb 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -4579,9 +4579,13 @@ static void osdRefresh(timeUs_t currentTimeUs) osdResetStats(); osdShowArmed(); uint32_t delay = ARMED_SCREEN_DISPLAY_TIME; + if (STATE(IN_FLIGHT_EMERG_REARM)) { + delay /= 3; + } #if defined(USE_SAFE_HOME) - if (posControl.safehomeState.distance) + else if (posControl.safehomeState.distance) { delay *= 3; + } #endif osdSetNextRefreshIn(delay); } else { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index b6cf9583c7c..0465eceab6c 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2530,11 +2530,15 @@ bool findNearestSafeHome(void) *-----------------------------------------------------------*/ void updateHomePosition(void) { - // Disarmed and have a valid position, constantly update home + // Disarmed and have a valid position, constantly update home before first arm (depending on setting) + // Update immediately after arming thereafter if reset on each arm (required to avoid home reset after emerg in flight rearm) + static bool setHome = false; + navSetWaypointFlags_t homeUpdateFlags = NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING; + if (!ARMING_FLAG(ARMED)) { if (posControl.flags.estPosStatus >= EST_USABLE) { const navigationHomeFlags_t validHomeFlags = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z; - bool setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags; + setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags; switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) { case NAV_RESET_NEVER: break; @@ -2545,24 +2549,16 @@ void updateHomePosition(void) setHome = true; break; } - if (setHome) { -#if defined(USE_SAFE_HOME) - findNearestSafeHome(); -#endif - setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); - // save the current location in case it is replaced by a safehome or HOME_RESET - posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos; - } } } else { static bool isHomeResetAllowed = false; - // If pilot so desires he may reset home position to current position if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) { if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) { - const navSetWaypointFlags_t homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); - setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity()); + homeUpdateFlags = 0; + homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + setHome = true; isHomeResetAllowed = false; } } @@ -2577,6 +2573,18 @@ void updateHomePosition(void) posControl.homeDirection = calculateBearingToDestination(tmpHomePos); updateHomePositionCompatibility(); } + + setHome &= !STATE(IN_FLIGHT_EMERG_REARM); // prevent reset following emerg in flight rearm + } + + if (setHome && (!ARMING_FLAG(WAS_EVER_ARMED) || ARMING_FLAG(ARMED))) { +#if defined(USE_SAFE_HOME) + findNearestSafeHome(); +#endif + setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity()); + // save the current location in case it is replaced by a safehome or HOME_RESET + posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos; + setHome = false; } } diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 5c314bfddda..220ff277c4e 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -108,15 +108,35 @@ static bool updateTimer(navigationTimer_t * tim, timeUs_t interval, timeUs_t cur } } -static bool shouldResetReferenceAltitude(void) +static bool shouldResetReferenceAltitude(uint8_t updateSource) { - switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) { - case NAV_RESET_NEVER: - return false; - case NAV_RESET_ON_FIRST_ARM: - return !ARMING_FLAG(ARMED) && !ARMING_FLAG(WAS_EVER_ARMED); - case NAV_RESET_ON_EACH_ARM: - return !ARMING_FLAG(ARMED); + /* Altitude reference reset constantly before first arm. + * After first arm altitude ref only reset immediately after arming (to avoid reset after emerg in flight rearm) + * Need to check reset status for both altitude sources immediately after rearming before reset complete */ + + static bool resetAltitudeOnArm = false; + if (ARMING_FLAG(ARMED) && resetAltitudeOnArm) { + static uint8_t sourceCheck = 0; + sourceCheck |= updateSource; + bool allAltitudeSources = STATE(GPS_FIX) && sensors(SENSOR_BARO); + + if ((allAltitudeSources && sourceCheck > SENSOR_GPS) || (!allAltitudeSources && sourceCheck)) { + resetAltitudeOnArm = false; + sourceCheck = 0; + } + return !STATE(IN_FLIGHT_EMERG_REARM); + } + + if (!ARMING_FLAG(ARMED)) { + switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) { + case NAV_RESET_NEVER: + return false; + case NAV_RESET_ON_FIRST_ARM: + break; + case NAV_RESET_ON_EACH_ARM: + resetAltitudeOnArm = true; + } + return !ARMING_FLAG(WAS_EVER_ARMED); } return false; @@ -226,7 +246,7 @@ void onNewGPSData(void) if (!posControl.gpsOrigin.valid) { geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_SET); } - else if (shouldResetReferenceAltitude()) { + else if (shouldResetReferenceAltitude(SENSOR_GPS)) { /* If we were never armed - keep altitude at zero */ geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_RESET_ALTITUDE); } @@ -309,7 +329,7 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) float newBaroAlt = baroCalculateAltitude(); /* If we are required - keep altitude at zero */ - if (shouldResetReferenceAltitude()) { + if (shouldResetReferenceAltitude(SENSOR_BARO)) { initialBaroAltitudeOffset = newBaroAlt; } From a181fc7f4eca15e8ff5e29fe54d3dd22dbb12eac Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 28 Sep 2023 13:19:10 +0100 Subject: [PATCH 09/10] fixes - rearm check sequence + altitude reset --- src/main/fc/fc_core.c | 7 +++---- src/main/io/osd.c | 2 +- src/main/navigation/navigation_pos_estimator.c | 3 +++ 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 66c275366b4..1272ad3aa11 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -553,10 +553,10 @@ void tryArm(void) #endif #ifdef USE_PROGRAMMING_FRAMEWORK - if (!isArmingDisabled() || emergencyArmingIsEnabled() || emergInflightRearmEnabled() || + if (emergInflightRearmEnabled() || !isArmingDisabled() || emergencyArmingIsEnabled() || LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) { #else - if (!isArmingDisabled() || emergencyArmingIsEnabled() || emergInflightRearmEnabled()) { + if (emergInflightRearmEnabled() || !isArmingDisabled() || emergencyArmingIsEnabled()) { #endif // If nav_extra_arming_safety was bypassed we always // allow bypassing it even without the sticks set @@ -568,8 +568,6 @@ void tryArm(void) ENABLE_STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED); } - resetLandingDetectorActiveState(); // reset landing detector after arming to avoid false detection before flight - lastDisarmReason = DISARM_NONE; ENABLE_ARMING_FLAG(ARMED); @@ -578,6 +576,7 @@ void tryArm(void) ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD); if (!STATE(IN_FLIGHT_EMERG_REARM)) { + resetLandingDetectorActiveState(); // reset landing detector after arming to avoid false detection before flight logicConditionReset(); #ifdef USE_PROGRAMMING_FRAMEWORK programmingPidReset(); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b34751096eb..3e464f2d764 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1202,7 +1202,7 @@ int32_t osdGetAltitude(void) static inline int32_t osdGetAltitudeMsl(void) { - return getEstimatedActualPosition(Z)+GPS_home.alt; + return getEstimatedActualPosition(Z) + posControl.gpsOrigin.alt; } uint16_t osdGetRemainingGlideTime(void) { diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 220ff277c4e..4422d6fe9ef 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -121,6 +121,9 @@ static bool shouldResetReferenceAltitude(uint8_t updateSource) bool allAltitudeSources = STATE(GPS_FIX) && sensors(SENSOR_BARO); if ((allAltitudeSources && sourceCheck > SENSOR_GPS) || (!allAltitudeSources && sourceCheck)) { + if (positionEstimationConfig()->reset_home_type == NAV_RESET_ON_EACH_ARM) { + posControl.rthState.homePosition.pos.z = 0; + } resetAltitudeOnArm = false; sourceCheck = 0; } From 3c6128e8eb0bd55349eaa21b89e9d9f9f5413298 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 1 Oct 2023 15:30:35 +0100 Subject: [PATCH 10/10] simplify alt ref reset + fix 0 home alt --- src/main/navigation/navigation.c | 4 ++ .../navigation/navigation_pos_estimator.c | 58 +++++++++---------- 2 files changed, 31 insertions(+), 31 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 0465eceab6c..81c9b4b85c8 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2582,6 +2582,10 @@ void updateHomePosition(void) findNearestSafeHome(); #endif setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity()); + + if (ARMING_FLAG(ARMED) && positionEstimationConfig()->reset_altitude_type == NAV_RESET_ON_EACH_ARM) { + posControl.rthState.homePosition.pos.z = 0; // force to 0 if reference altitude also reset every arm + } // save the current location in case it is replaced by a safehome or HOME_RESET posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos; setHome = false; diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 4422d6fe9ef..d7ef73c9382 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -54,6 +54,7 @@ #include "sensors/opflow.h" navigationPosEstimator_t posEstimator; +static float initialBaroAltitudeOffset = 0.0f; PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 5); @@ -108,38 +109,34 @@ static bool updateTimer(navigationTimer_t * tim, timeUs_t interval, timeUs_t cur } } -static bool shouldResetReferenceAltitude(uint8_t updateSource) +static bool shouldResetReferenceAltitude(void) { - /* Altitude reference reset constantly before first arm. - * After first arm altitude ref only reset immediately after arming (to avoid reset after emerg in flight rearm) - * Need to check reset status for both altitude sources immediately after rearming before reset complete */ - - static bool resetAltitudeOnArm = false; - if (ARMING_FLAG(ARMED) && resetAltitudeOnArm) { - static uint8_t sourceCheck = 0; - sourceCheck |= updateSource; - bool allAltitudeSources = STATE(GPS_FIX) && sensors(SENSOR_BARO); - - if ((allAltitudeSources && sourceCheck > SENSOR_GPS) || (!allAltitudeSources && sourceCheck)) { - if (positionEstimationConfig()->reset_home_type == NAV_RESET_ON_EACH_ARM) { - posControl.rthState.homePosition.pos.z = 0; - } - resetAltitudeOnArm = false; - sourceCheck = 0; + /* Reference altitudes reset constantly when disarmed. + * On arming ref altitudes saved as backup in case of emerg in flight rearm + * If emerg in flight rearm active ref altitudes reset to backup values to avoid unwanted altitude reset */ + + static float backupInitialBaroAltitudeOffset = 0.0f; + static int32_t backupGpsOriginAltitude = 0; + static bool emergRearmResetCheck = false; + + if (ARMING_FLAG(ARMED) && emergRearmResetCheck) { + if (STATE(IN_FLIGHT_EMERG_REARM)) { + initialBaroAltitudeOffset = backupInitialBaroAltitudeOffset; + posControl.gpsOrigin.alt = backupGpsOriginAltitude; + } else { + backupInitialBaroAltitudeOffset = initialBaroAltitudeOffset; + backupGpsOriginAltitude = posControl.gpsOrigin.alt; } - return !STATE(IN_FLIGHT_EMERG_REARM); } + emergRearmResetCheck = !ARMING_FLAG(ARMED); - if (!ARMING_FLAG(ARMED)) { - switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) { - case NAV_RESET_NEVER: - return false; - case NAV_RESET_ON_FIRST_ARM: - break; - case NAV_RESET_ON_EACH_ARM: - resetAltitudeOnArm = true; - } - return !ARMING_FLAG(WAS_EVER_ARMED); + switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) { + case NAV_RESET_NEVER: + return false; + case NAV_RESET_ON_FIRST_ARM: + return !ARMING_FLAG(ARMED) && !ARMING_FLAG(WAS_EVER_ARMED); + case NAV_RESET_ON_EACH_ARM: + return !ARMING_FLAG(ARMED); } return false; @@ -249,7 +246,7 @@ void onNewGPSData(void) if (!posControl.gpsOrigin.valid) { geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_SET); } - else if (shouldResetReferenceAltitude(SENSOR_GPS)) { + else if (shouldResetReferenceAltitude()) { /* If we were never armed - keep altitude at zero */ geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_RESET_ALTITUDE); } @@ -328,11 +325,10 @@ void onNewGPSData(void) */ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) { - static float initialBaroAltitudeOffset = 0.0f; float newBaroAlt = baroCalculateAltitude(); /* If we are required - keep altitude at zero */ - if (shouldResetReferenceAltitude(SENSOR_BARO)) { + if (shouldResetReferenceAltitude()) { initialBaroAltitudeOffset = newBaroAlt; }