From cb7e5670070db83d6d480322c11937741eddc951 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 17 Jun 2024 22:46:36 +0100 Subject: [PATCH] CR128 --- src/main/fc/settings.yaml | 2 +- src/main/flight/pid.c | 10 ++-- src/main/io/osd.c | 2 +- src/main/navigation/navigation.c | 5 +- src/main/navigation/navigation.h | 2 +- src/main/navigation/navigation_multicopter.c | 56 ++++++++++--------- .../navigation/navigation_pos_estimator.c | 2 +- 7 files changed, 43 insertions(+), 36 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b82c89ed2c4..5302cdeab89 100644 --- a/src/main/fc/settings.yaml +++ b/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 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 1bb5b8101cf..445575b39fa 100644 --- a/src/main/flight/pid.c +++ b/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) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 556136e8c6d..47be1c1cb17 100644 --- a/src/main/io/osd.c +++ b/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; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 70d4901c92c..2c1c4d49101 100644 --- a/src/main/navigation/navigation.c +++ b/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())); } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 0765b719e3e..1e45ee6d7d5 100644 --- a/src/main/navigation/navigation.h +++ b/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); diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index f695b9740b1..e1b0dabf3d1 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/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,6 +830,7 @@ bool isMulticopterCrashedInverted(void) startTime = 0; return false; } +#endif // CR128 bool isMulticopterLandingDetected(void) { @@ -834,21 +838,23 @@ bool isMulticopterLandingDetected(void) 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())); diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index be008c390d2..57538d8f15f 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/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 {