Merge branch 'emuflight-1.0.0-master' into 20220329_cherry_pick_Smith_Predictor

Author: nerdCopter

src/main/cli/cli.c

@@ -6162,8 +6162,8 @@ static void cliDshotTelemetryInfo(const char *cmdName, char *cmdline)
     if (useDshotTelemetry) {
         cliPrintLinef("Dshot reads: %u", dshotTelemetryState.readCount);
         cliPrintLinef("Dshot invalid pkts: %u", dshotTelemetryState.invalidPacketCount);
-        uint32_t directionChangeCycles = dshotDMAHandlerCycleCounters.changeDirectionCompletedAt - dshotDMAHandlerCycleCounters.irqAt;
-        uint32_t directionChangeDurationUs = clockCyclesToMicros(directionChangeCycles);
+        int32_t directionChangeCycles = cmp32(dshotDMAHandlerCycleCounters.changeDirectionCompletedAt, dshotDMAHandlerCycleCounters.irqAt);
+        int32_t directionChangeDurationUs = clockCyclesToMicros(directionChangeCycles);
         cliPrintLinef("Dshot directionChange cycles: %u, micros: %u", directionChangeCycles, directionChangeDurationUs);
         cliPrintLinefeed();
 

src/main/drivers/dshot_bitbang.c

@@ -751,7 +751,7 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
 #if defined(STM32F4) || defined(STM32F3)
         bbMotors[motorIndex].iocfg = IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_50MHz, GPIO_OType_PP, bbPuPdMode);
 #elif defined(STM32F7) || defined(STM32G4) || defined(STM32H7)
-        bbMotors[motorIndex].iocfg = IO_CONFIG(GPIO_MODE_OUTPUT_PP, GPIO_SPEED_FREQ_VERY_HIGH, bbPuPdMode);
+        bbMotors[motorIndex].iocfg = IO_CONFIG(GPIO_MODE_OUTPUT_PP, GPIO_SPEED_FREQ_LOW, bbPuPdMode);
 #endif
 
         IOInit(io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));

src/main/drivers/system.c

@@ -153,7 +153,7 @@ uint32_t getCycleCounter(void)
     return DWT->CYCCNT;
 }
 
-uint32_t clockCyclesToMicros(uint32_t clockCycles)
+int32_t clockCyclesToMicros(int32_t clockCycles)
 {
     return clockCycles / usTicks;
 }

src/main/drivers/system.h

@@ -65,7 +65,7 @@ void systemReset(void);
 void systemResetToBootloader(bootloaderRequestType_e requestType);
 bool isMPUSoftReset(void);
 void cycleCounterInit(void);
-uint32_t clockCyclesToMicros(uint32_t clockCycles);
+int32_t clockCyclesToMicros(int32_t clockCycles);
 int32_t clockCyclesTo10thMicros(int32_t clockCycles);
 uint32_t clockMicrosToCycles(uint32_t micros);
 uint32_t getCycleCounter(void);

src/main/fc/core.c

@@ -744,12 +744,6 @@ bool isAirmodeActivated()
  */
 bool processRx(timeUs_t currentTimeUs)
 {
-    timeDelta_t frameAgeUs;
-    timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs);
-
-    DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(frameDeltaUs / 10, INT16_MAX));
-    DEBUG_SET(DEBUG_RX_TIMING, 1, MIN(frameAgeUs / 10, INT16_MAX));
-
     if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) {
         return false;
     }

src/main/fc/rc.c

@@ -298,13 +298,18 @@ void updateRcRefreshRate(timeUs_t currentTimeUs)
     static timeUs_t lastRxTimeUs;
 
     timeDelta_t frameAgeUs;
-    timeDelta_t refreshRateUs = rxGetFrameDelta(&frameAgeUs);
-    if (!refreshRateUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) {
-        refreshRateUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol
+    timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs);
+
+    if (!frameDeltaUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) {
+        frameDeltaUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol
     }
+
+    DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(frameDeltaUs / 10, INT16_MAX));
+    DEBUG_SET(DEBUG_RX_TIMING, 1, MIN(frameAgeUs / 10, INT16_MAX));
+
     lastRxTimeUs = currentTimeUs;
-    isRxRateValid = (refreshRateUs >= RC_RX_RATE_MIN_US && refreshRateUs <= RC_RX_RATE_MAX_US);
-    currentRxRefreshRate = constrain(refreshRateUs, RC_RX_RATE_MIN_US, RC_RX_RATE_MAX_US);
+    isRxRateValid = (frameDeltaUs >= RC_RX_RATE_MIN_US && frameDeltaUs <= RC_RX_RATE_MAX_US);
+    currentRxRefreshRate = constrain(frameDeltaUs, RC_RX_RATE_MIN_US, RC_RX_RATE_MAX_US);
 }
 
 uint16_t getCurrentRxRefreshRate(void)

src/main/flight/failsafe.c

@@ -65,11 +65,11 @@ PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CO
 PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
     .failsafe_throttle = 1000,                       // default throttle off.
     .failsafe_throttle_low_delay = 100,              // default throttle low delay for "just disarm" on failsafe condition
-    .failsafe_delay = 4,                             // 0,4sec
-    .failsafe_off_delay = 10,                        // 1sec
-    .failsafe_switch_mode = 0,                       // default failsafe switch action is identical to rc link loss
+    .failsafe_delay = 10,                                // 1 sec, can regain control on signal recovery, at idle in drop mode
+    .failsafe_off_delay = 10,                            // 1 sec in landing phase, if enabled
+    .failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE1, // default failsafe switch action is identical to rc link loss
     .failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT,// default full failsafe procedure is 0: auto-landing
-    .failsafe_recovery_delay = 20,                   // 2 sec of valid rx data (plus 200ms) needed to allow recovering from failsafe procedure
+    .failsafe_recovery_delay = 10,                       // 1 sec of valid rx data needed to allow recovering from failsafe procedure
     .failsafe_stick_threshold = 30                   // 30 percent of stick deflection to exit GPS Rescue procedure
 );
 
@@ -86,8 +86,12 @@ const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = {
  */
 void failsafeReset(void)
 {
-    failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND;
-    failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY + failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND;
+    failsafeState.rxDataFailurePeriod = failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND; // time to start stage2
+    failsafeState.rxDataRecoveryPeriod = failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND;
+    if (failsafeState.rxDataRecoveryPeriod < PERIOD_RXDATA_RECOVERY){
+        // PERIOD_RXDATA_RECOVERY (200ms) is the minimum allowed RxData recovery time
+        failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY;
+    }
     failsafeState.validRxDataReceivedAt = 0;
     failsafeState.validRxDataFailedAt = 0;
     failsafeState.throttleLowPeriod = 0;
@@ -121,6 +125,13 @@ bool failsafeIsActive(void)
     return failsafeState.active;
 }
 
+#ifdef USE_GPS_RESCUE
+bool failsafePhaseIsGpsRescue(void)
+{
+    return failsafeState.phase == FAILSAFE_GPS_RESCUE;
+}
+#endif
+
 void failsafeStartMonitoring(void)
 {
     failsafeState.monitoring = true;
@@ -138,26 +149,12 @@ static void failsafeActivate(void)
     failsafeState.phase = FAILSAFE_LANDING;
 
     ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
+
     failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
 
     failsafeState.events++;
 }
 
-static void failsafeApplyControlInput(void)
-{
-#ifdef USE_GPS_RESCUE
-    if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE) {
-        ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
-        return;
-    }
-#endif
-
-    for (int i = 0; i < 3; i++) {
-        rcData[i] = rxConfig()->midrc;
-    }
-    rcData[THROTTLE] = failsafeConfig()->failsafe_throttle;
-}
-
 bool failsafeIsReceivingRxData(void)
 {
     return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
@@ -177,42 +174,61 @@ void failsafeOnRxResume(void)
 void failsafeOnValidDataReceived(void)
 {
     failsafeState.validRxDataReceivedAt = millis();
-    if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > failsafeState.rxDataRecoveryPeriod) {
+    if ((cmp32(failsafeState.validRxDataReceivedAt, failsafeState.validRxDataFailedAt) > (int32_t)failsafeState.rxDataRecoveryPeriod) ||
+        (cmp32(failsafeState.validRxDataFailedAt, failsafeState.validRxDataReceivedAt) > 0)) {
+        // rxDataRecoveryPeriod defaults to 1.0s with minimum of PERIOD_RXDATA_RECOVERY (200ms)
         failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;
+        // Allow arming now we have an RX link
         unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
     }
 }
 
 void failsafeOnValidDataFailed(void)
 {
-    setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); // To prevent arming with no RX link
     failsafeState.validRxDataFailedAt = millis();
-    if ((failsafeState.validRxDataFailedAt - failsafeState.validRxDataReceivedAt) > failsafeState.rxDataFailurePeriod) {
+    if ((cmp32(failsafeState.validRxDataFailedAt, failsafeState.validRxDataReceivedAt) > (int32_t)failsafeState.rxDataFailurePeriod)) {
+        // rxDataFailurePeriod is stage 1 guard time
         failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
+        // Prevent arming with no RX link
+        setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
     }
 }
 
 void failsafeCheckDataFailurePeriod(void)
+// forces link down directly from scheduler?
 {
     if (cmp32(millis(), failsafeState.validRxDataReceivedAt) > (int32_t)failsafeState.rxDataFailurePeriod) {
-        setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); // To prevent arming with no RX link
         failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
+        // Prevent arming with no RX link
+        setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
     }
 }
 
+uint32_t failsafeFailurePeriodMs(void)
+{
+    return failsafeState.rxDataFailurePeriod;
+}
+
 FAST_CODE_NOINLINE void failsafeUpdateState(void)
+// triggered directly, and ONLY, by the cheduler, at 10ms = PERIOD_RXDATA_FAILURE - intervals
 {
     if (!failsafeIsMonitoring()) {
         return;
     }
 
     bool receivingRxData = failsafeIsReceivingRxData();
+    // should be true when FAILSAFE_RXLINK_UP
+    // FAILSAFE_RXLINK_UP is set in failsafeOnValidDataReceived
+    // failsafeOnValidDataReceived runs from detectAndApplySignalLossBehaviour
+
     bool armed = ARMING_FLAG(ARMED);
     bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
     beeperMode_e beeperMode = BEEPER_SILENCE;
 
-    if (failsafeSwitchIsOn && failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2) {
-        receivingRxData = false; // force Stage2
+    if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2)) {
+        // Aux switch set to failsafe stage2 emulates loss of signal without waiting
+        failsafeOnValidDataFailed();
+        receivingRxData = false;
     }
 
     // Beep RX lost only if we are not seeing data and we have been armed earlier
@@ -232,23 +248,27 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                     if (THROTTLE_HIGH == calculateThrottleStatus()) {
                         failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
                     }
-                    // Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming)
-                    if (failsafeSwitchIsOn && failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL) {
-                        // KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON
+                    if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL)) {
+                        // Failsafe switch is configured as KILL switch and is switched ON
                         failsafeActivate();
-                        failsafeState.phase = FAILSAFE_LANDED;      // skip auto-landing procedure
-                        failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS;    // require 1 seconds of valid rxData
+                        // Skip auto-landing procedure
+                        failsafeState.phase = FAILSAFE_LANDED;
+                        // Require 3 seconds of valid rxData
+                        failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
                         reprocessState = true;
                     } else if (!receivingRxData) {
                         if (millis() > failsafeState.throttleLowPeriod
 #ifdef USE_GPS_RESCUE
                             && failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_GPS_RESCUE
 #endif
                             ) {
-                            // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds
+                            // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds before failsafe
+                            // protects against false arming when the Tx is powered up after the quad
                             failsafeActivate();
-                            failsafeState.phase = FAILSAFE_LANDED;      // skip auto-landing procedure
-                            failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData
+                            // skip auto-landing procedure
+                            failsafeState.phase = FAILSAFE_LANDED;
+                            // re-arm at rxDataRecoveryPeriod - default is 1.0 seconds
+                            failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
                         } else {
                             failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
                         }
@@ -279,7 +299,10 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                         case FAILSAFE_PROCEDURE_DROP_IT:
                             // Drop the craft
                             failsafeActivate();
-                            failsafeState.phase = FAILSAFE_LANDED;      // skip auto-landing procedure
+                            // re-arm at rxDataRecoveryPeriod - default is 1.0 seconds
+                            failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
+                            // skip auto-landing procedure
+                            failsafeState.phase = FAILSAFE_LANDED;
                             break;
 #ifdef USE_GPS_RESCUE
                         case FAILSAFE_PROCEDURE_GPS_RESCUE:
@@ -296,37 +319,42 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                 if (receivingRxData) {
                     failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
                     reprocessState = true;
-                }
-                if (armed) {
-                    failsafeApplyControlInput();
-                    beeperMode = BEEPER_RX_LOST_LANDING;
-                }
-                if (failsafeShouldHaveCausedLandingByNow() || crashRecoveryModeActive() || !armed) {
-                    failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData
-                    failsafeState.phase = FAILSAFE_LANDED;
-                    reprocessState = true;
+                } else {
+                    if (armed) {
+                        beeperMode = BEEPER_RX_LOST_LANDING;
+                    }
+                    if (failsafeShouldHaveCausedLandingByNow() || crashRecoveryModeActive() || !armed) {
+                        // require 3 seconds of valid rxData
+                        failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
+                        failsafeState.phase = FAILSAFE_LANDED;
+                        reprocessState = true;
+                    }
                 }
                 break;
 #ifdef USE_GPS_RESCUE
             case FAILSAFE_GPS_RESCUE:
                 if (receivingRxData) {
                     if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) {
+                        //  hence we must allow stick inputs during FAILSAFE_GPS_RESCUE see PR #7936 for rationale
                         failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
                         reprocessState = true;
                     }
-                }
-                if (armed) {
-                    failsafeApplyControlInput();
-                    beeperMode = BEEPER_RX_LOST_LANDING;
                 } else {
-                    failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData
-                    failsafeState.phase = FAILSAFE_LANDED;
-                    reprocessState = true;
+                    if (armed) {
+                        ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
+                        beeperMode = BEEPER_RX_LOST_LANDING;
+                    } else {
+                        // require 3 seconds of valid rxData
+                        failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
+                        failsafeState.phase = FAILSAFE_LANDED;
+                        reprocessState = true;
+                    }
                 }
                 break;
 #endif
             case FAILSAFE_LANDED:
-                setArmingDisabled(ARMING_DISABLED_FAILSAFE); // To prevent accidently rearming by an intermittent rx link
+                // Prevent accidently rearming by an intermittent rx link
+                setArmingDisabled(ARMING_DISABLED_FAILSAFE);
                 disarm(DISARM_REASON_FAILSAFE);
                 failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData
                 failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING;
@@ -337,12 +365,9 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                 // Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time.
                 if (receivingRxData) {
                     if (millis() > failsafeState.receivingRxDataPeriod) {
-                        // rx link is good now, when arming via ARM switch, it must be OFF first
-                        if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) {
-                            unsetArmingDisabled(ARMING_DISABLED_FAILSAFE);
-                            failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
-                            reprocessState = true;
-                        }
+                        // rx link is good now
+                        failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
+                        reprocessState = true;
                     }
                 } else {
                     failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset;
@@ -357,6 +382,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
                 failsafeState.phase = FAILSAFE_IDLE;
                 failsafeState.active = false;
                 DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
+                unsetArmingDisabled(ARMING_DISABLED_FAILSAFE);
                 reprocessState = true;
                 break;