CR128

src/main/fc/settings.yaml

@@ -2484,7 +2484,7 @@ groups:
         type: bool
         # CR128
       - name: nav_mc_inverted_crash_detection
-        description: "Setting a value > 0 enables inverted crash detection for multirotors. It is intended for situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control. When enabled the setting defines the additional number of seconds before disarm beyond a minimum fixed time delay such that the minimum possible time delay before disarm is 3s."
+        description: "Setting a value > 0 enables inverted crash detection for multirotors. It is intended for situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control. When enabled the setting defines the additional number of seconds before disarm beyond a minimum fixed time delay such that the minimum possible time delay before disarm is 3s. Only works if a barometer is available."
         default_value: 0
         field: mc.inverted_crash_detection
         min: 0

src/main/flight/pid.c

@@ -781,11 +781,11 @@ static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, cons
     pidState->attenuation.aP = dampingFactor;
     pidState->attenuation.aD = dampingFactor;
 
-    if (pidState->axis == FD_ROLL) {
-        DEBUG_SET(DEBUG_ALWAYS, 0, pidState->attenuation.aP * 1000);
-        DEBUG_SET(DEBUG_ALWAYS, 1, pidState->attenuation.aI * 1000);
-        DEBUG_SET(DEBUG_ALWAYS, 2, pidState->attenuation.aD * 1000);
-    }
+    // if (pidState->axis == FD_ROLL) {
+        // DEBUG_SET(DEBUG_ALWAYS, 0, pidState->attenuation.aP * 1000);
+        // DEBUG_SET(DEBUG_ALWAYS, 1, pidState->attenuation.aI * 1000);
+        // DEBUG_SET(DEBUG_ALWAYS, 2, pidState->attenuation.aD * 1000);
+    // }
 }
 
 static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, float dT, float dT_inv)

src/main/io/osd.c

@@ -5894,7 +5894,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
         const char *message = NULL;
         char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH + 1)]; //warning: shared buffer. Make sure it is used by single message in code below!
         // We might have up to 6 messages to show.
-        const char *messages[6];
+        const char *messages[7];
         unsigned messageCount = 0;
         const char *failsafeInfoMessage = NULL;
         const char *invertedInfoMessage = NULL;

src/main/navigation/navigation.c

@@ -3387,9 +3387,10 @@ void updateLandingStatus(timeMs_t currentTimeMs)
             disarm(DISARM_LANDING);
         } else if (!navigationInAutomaticThrottleMode()) {
             // CR123
-            if (STATE(AIRPLANE) && isFlightDetected()) { // cancel landing detection flag if plane redetected in flight
+            if (STATE(AIRPLANE) && isFlightDetected()) {
+                // cancel landing detection flag if plane redetected in flight
                 resetLandingDetector();
-            } else {
+            } else if (STATE(MULTIROTOR)) {
                 // for multirotor - reactivate landing detector without disarm when throttle raised toward hover throttle
                 landingDetectorIsActive = rxGetChannelValue(THROTTLE) < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));
             }

src/main/navigation/navigation.h

@@ -696,7 +696,7 @@ float getEstimatedAglPosition(void);
 bool isEstimatedAglTrusted(void);
 
 void checkManualEmergencyLandingControl(bool forcedActivation);
-float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue);
+void updateBaroAltitudeRate(float newBaroAltRate);   // CR128
 bool rthAltControlStickOverrideCheck(uint8_t axis);
 
 int8_t navCheckActiveAngleHoldAxis(void);

src/main/navigation/navigation_multicopter.c

@@ -101,8 +101,7 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
         // Small - desired acceleration is less than 0.1G. We should be safe setting velocity target directly - any platform should be able to satisfy this
         posControl.desiredState.vel.z = targetVel;
     }
-DEBUG_SET(DEBUG_ALWAYS, 0, posControl.desiredState.vel.z);
-DEBUG_SET(DEBUG_ALWAYS, 1, posControl.desiredState.pos.z);
+
     navDesiredVelocity[Z] = constrain(lrintf(posControl.desiredState.vel.z), -32678, 32767);
 }
 
@@ -768,16 +767,21 @@ bool isMulticopterFlying(void)
  * Multicopter landing detector
  *-----------------------------------------------------------*/
 #if defined(USE_BARO)
-float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue)
+// CR128
+static float baroAltRate;
+
+void updateBaroAltitudeRate(float newBaroAltRate)
 {
-    static float baroAltRate;
-    if (updateValue) {
+    // static float baroAltRate;
+    // if (updateValue) {
         baroAltRate = newBaroAltRate;
-    }
+        DEBUG_SET(DEBUG_ALWAYS, 0, baroAltRate);
+        DEBUG_SET(DEBUG_ALWAYS, 1, millis());
+    // }
 
-    return baroAltRate;
+    // return baroAltRate;
 }
-
+// CR128
 static bool isLandingGbumpDetected(timeMs_t currentTimeMs)
 {
     /* Detection based on G bump at touchdown, falling Baro altitude and throttle below hover.
@@ -786,7 +790,7 @@ static bool isLandingGbumpDetected(timeMs_t currentTimeMs)
      * Throttle trigger: must be below hover throttle with lower threshold for manual throttle control */
 
     static timeMs_t gSpikeDetectTimeMs = 0;
-    float baroAltRate = updateBaroAltitudeRate(0, false);
+    // float baroAltRate = updateBaroAltitudeRate(0, false);  // CR128
 
     if (!gSpikeDetectTimeMs && acc.accADCf[Z] > 2.0f && baroAltRate < 0.0f) {
         gSpikeDetectTimeMs = currentTimeMs;
@@ -804,20 +808,19 @@ static bool isLandingGbumpDetected(timeMs_t currentTimeMs)
 
     return false;
 }
-#endif
 // CR128
 bool isMulticopterCrashedInverted(void)
 {
     static timeMs_t startTime = 0;
 
-    if (ABS(attitude.values.roll) > 1000 || ABS(attitude.values.pitch) > 700) {
-        static uint32_t initialAltitude;
+    if ((ABS(attitude.values.roll) > 1000 || ABS(attitude.values.pitch) > 700) && fabsf(baroAltRate) < 200) {
+        // static uint32_t initialBaroAltitude;
 
         if (startTime == 0) {
             startTime = millis();
-            initialAltitude = navGetCurrentActualPositionAndVelocity()->pos.z;
+            // initialBaroAltitude = baroAlt;
             return false;
-        } else if (ABS(initialAltitude - navGetCurrentActualPositionAndVelocity()->pos.z) < 200) {
+        } else {
             uint16_t disarmTimeDelay = 3000 + S2MS(navConfig()->mc.inverted_crash_detection);
 
             return millis() - startTime > disarmTimeDelay;
@@ -827,28 +830,31 @@ bool isMulticopterCrashedInverted(void)
     startTime = 0;
     return false;
 }
+#endif
 // CR128
 bool isMulticopterLandingDetected(void)
 {
     DEBUG_SET(DEBUG_LANDING, 4, 0);
     DEBUG_SET(DEBUG_LANDING, 3, averageAbsGyroRates() * 100);
 
     const timeMs_t currentTimeMs = millis();
-// CR128
-    if (navConfig()->mc.inverted_crash_detection && isMulticopterCrashedInverted()) {
-        ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
-        disarm(DISARM_LANDING);
-    }
 // CR128
 #if defined(USE_BARO)
-    // CR129
-    bool gBumpDetectionUsable = navConfig()->general.flags.landing_bump_detection && sensors(SENSOR_BARO) &&
-                                ((posControl.flags.estPosStatus >= EST_USABLE && posControl.actualState.velXY < MC_LAND_CHECK_VEL_XY_MOVING) ||
-                                FLIGHT_MODE(FAILSAFE_MODE));
+    if (sensors(SENSOR_BARO)) {
+        if (navConfig()->mc.inverted_crash_detection && isMulticopterCrashedInverted()) {
+            ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
+            disarm(DISARM_LANDING);
+        }
+        // CR129
+        bool gBumpDetectionUsable = navConfig()->general.flags.landing_bump_detection &&
+                                    ((posControl.flags.estPosStatus >= EST_USABLE && posControl.actualState.velXY < MC_LAND_CHECK_VEL_XY_MOVING) ||
+                                    FLIGHT_MODE(FAILSAFE_MODE));
 
-    if (gBumpDetectionUsable && isLandingGbumpDetected(currentTimeMs)) {
-        return true;    // Landing flagged immediately if landing bump detected
+        if (gBumpDetectionUsable && isLandingGbumpDetected(currentTimeMs)) {
+            return true;    // Landing flagged immediately if landing bump detected
+        }
     }
+    // CR128
     // CR129
 #endif
     bool throttleIsBelowMidHover = rcCommand[THROTTLE] < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));

src/main/navigation/navigation_pos_estimator.c

@@ -299,7 +299,7 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs)
             static float baroAltPrevious = 0;
             posEstimator.baro.baroAltRate = (posEstimator.baro.alt - baroAltPrevious) / US2S(baroDtUs);
             baroAltPrevious = posEstimator.baro.alt;
-            updateBaroAltitudeRate(posEstimator.baro.baroAltRate, true);
+            updateBaroAltitudeRate(posEstimator.baro.baroAltRate);  // CR128
         }
     }
     else {