mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-26 01:35:35 +03:00
Merge pull request #10099 from breadoven/abo_mr_inverted_crash_detection
Multirotor inverted crash detection
This commit is contained in:
breadoven
GitHub
commit
f9e25c625a
6 changed files with 67 additions and 22 deletions
|
|
@ -3742,6 +3742,16 @@ Multicopter hover throttle hint for altitude controller. Should be set to approx
|
|||
|
||||
---
|
||||
|
||||
### nav_mc_inverted_crash_detection
|
||||
|
||||
Setting a value > 0 enables inverted crash detection for multirotors. It will auto disarm in situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control or for some other reason. When enabled this setting defines the additional number of seconds before disarm beyond a minimum fixed time delay of 3s. Requires a barometer to work.
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| 0 | 0 | 15 |
|
||||
|
||||
---
|
||||
|
||||
### nav_mc_manual_climb_rate
|
||||
|
||||
Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]
|
||||
|
|
|
|||
|
|
@ -2470,6 +2470,12 @@ groups:
|
|||
default_value: OFF
|
||||
field: general.flags.landing_bump_detection
|
||||
type: bool
|
||||
- name: nav_mc_inverted_crash_detection
|
||||
description: "Setting a value > 0 enables inverted crash detection for multirotors. It will auto disarm in situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control or for some other reason. When enabled this setting defines the additional number of seconds before disarm beyond a minimum fixed time delay of 3s. Requires a barometer to work."
|
||||
default_value: 0
|
||||
field: mc.inverted_crash_detection
|
||||
min: 0
|
||||
max: 15
|
||||
- name: nav_mc_althold_throttle
|
||||
description: "If set to STICK the FC remembers the throttle stick position when enabling ALTHOLD and treats it as the neutral midpoint for holding altitude. If set to MID_STICK or HOVER the neutral midpoint is set to the mid stick position or the hover throttle position respectively."
|
||||
default_value: "STICK"
|
||||
|
|
|
|||
|
|
@ -175,7 +175,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.land_detect_sensitivity = SETTING_NAV_LAND_DETECT_SENSITIVITY_DEFAULT, // Changes sensitivity of landing detection
|
||||
.auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT, // 2000 ms - time delay to disarm when auto disarm after landing enabled
|
||||
.rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT,
|
||||
.cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps
|
||||
.cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps
|
||||
.rth_fs_landing_delay = SETTING_NAV_RTH_FS_LANDING_DELAY_DEFAULT, // Delay before landing in FS. 0 = immedate landing
|
||||
},
|
||||
|
||||
|
|
@ -193,13 +193,14 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.braking_boost_timeout = SETTING_NAV_MC_BRAKING_BOOST_TIMEOUT_DEFAULT, // Timout boost after 750ms
|
||||
.braking_boost_speed_threshold = SETTING_NAV_MC_BRAKING_BOOST_SPEED_THRESHOLD_DEFAULT, // Boost can happen only above 1.5m/s
|
||||
.braking_boost_disengage_speed = SETTING_NAV_MC_BRAKING_BOOST_DISENGAGE_SPEED_DEFAULT, // Disable boost at 1m/s
|
||||
.braking_bank_angle = SETTING_NAV_MC_BRAKING_BANK_ANGLE_DEFAULT, // Max braking angle
|
||||
.braking_bank_angle = SETTING_NAV_MC_BRAKING_BANK_ANGLE_DEFAULT, // Max braking angle
|
||||
#endif
|
||||
|
||||
.posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT, // posDecelerationTime * 100
|
||||
.posResponseExpo = SETTING_NAV_MC_POS_EXPO_DEFAULT, // posResponseExpo * 100
|
||||
.posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT, // posDecelerationTime * 100
|
||||
.posResponseExpo = SETTING_NAV_MC_POS_EXPO_DEFAULT, // posResponseExpo * 100
|
||||
.slowDownForTurning = SETTING_NAV_MC_WP_SLOWDOWN_DEFAULT,
|
||||
.althold_throttle_type = SETTING_NAV_MC_ALTHOLD_THROTTLE_DEFAULT, // STICK
|
||||
.althold_throttle_type = SETTING_NAV_MC_ALTHOLD_THROTTLE_DEFAULT, // STICK
|
||||
.inverted_crash_detection = SETTING_NAV_MC_INVERTED_CRASH_DETECTION_DEFAULT, // 0 - disarm time delay for inverted crash detection
|
||||
},
|
||||
|
||||
// Fixed wing
|
||||
|
|
|
|||
|
|
@ -344,6 +344,7 @@ typedef struct navConfig_s {
|
|||
uint8_t posResponseExpo; // Position controller expo (taret vel expo for MC)
|
||||
bool slowDownForTurning; // Slow down during WP missions when changing heading on next waypoint
|
||||
uint8_t althold_throttle_type; // throttle zero datum type for alt hold
|
||||
uint8_t inverted_crash_detection; // Enables inverted crash detection, setting defines disarm time delay (0 = disabled)
|
||||
} mc;
|
||||
|
||||
struct {
|
||||
|
|
@ -688,7 +689,7 @@ float getEstimatedAglPosition(void);
|
|||
bool isEstimatedAglTrusted(void);
|
||||
|
||||
void checkManualEmergencyLandingControl(bool forcedActivation);
|
||||
float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue);
|
||||
void updateBaroAltitudeRate(float newBaroAltRate);
|
||||
bool rthAltControlStickOverrideCheck(uint8_t axis);
|
||||
|
||||
int8_t navCheckActiveAngleHoldAxis(void);
|
||||
|
|
|
|||
|
|
@ -36,6 +36,7 @@
|
|||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/gyro.h"
|
||||
|
||||
#include "fc/fc_core.h"
|
||||
#include "fc/config.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/rc_curves.h"
|
||||
|
|
@ -756,15 +757,12 @@ bool isMulticopterFlying(void)
|
|||
/*-----------------------------------------------------------
|
||||
* Multicopter land detector
|
||||
*-----------------------------------------------------------*/
|
||||
#if defined(USE_BARO)
|
||||
float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue)
|
||||
{
|
||||
static float baroAltRate;
|
||||
if (updateValue) {
|
||||
baroAltRate = newBaroAltRate;
|
||||
}
|
||||
#if defined(USE_BARO)
|
||||
static float baroAltRate;
|
||||
|
||||
return baroAltRate;
|
||||
void updateBaroAltitudeRate(float newBaroAltRate)
|
||||
{
|
||||
baroAltRate = newBaroAltRate;
|
||||
}
|
||||
|
||||
static bool isLandingGbumpDetected(timeMs_t currentTimeMs)
|
||||
|
|
@ -775,7 +773,6 @@ 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);
|
||||
|
||||
if (!gSpikeDetectTimeMs && acc.accADCf[Z] > 2.0f && baroAltRate < 0.0f) {
|
||||
gSpikeDetectTimeMs = currentTimeMs;
|
||||
|
|
@ -793,7 +790,28 @@ static bool isLandingGbumpDetected(timeMs_t currentTimeMs)
|
|||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool isMulticopterCrashedInverted(timeMs_t currentTimeMs)
|
||||
{
|
||||
/* Disarms MR if inverted on the ground. Checks vertical velocity is low based on Baro rate below 2 m/s */
|
||||
|
||||
static timeMs_t startTime = 0;
|
||||
|
||||
if ((ABS(attitude.values.roll) > 1000 || ABS(attitude.values.pitch) > 700) && fabsf(baroAltRate) < 200.0f) {
|
||||
if (startTime == 0) {
|
||||
startTime = currentTimeMs;
|
||||
}
|
||||
|
||||
/* Minimum 3s disarm delay + extra user set delay time (min overall delay of 4s) */
|
||||
uint16_t disarmTimeDelay = 3000 + S2MS(navConfig()->mc.inverted_crash_detection);
|
||||
return currentTimeMs - startTime > disarmTimeDelay;
|
||||
}
|
||||
|
||||
startTime = 0;
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
bool isMulticopterLandingDetected(void)
|
||||
{
|
||||
DEBUG_SET(DEBUG_LANDING, 4, 0);
|
||||
|
|
@ -802,13 +820,22 @@ bool isMulticopterLandingDetected(void)
|
|||
const timeMs_t currentTimeMs = millis();
|
||||
|
||||
#if defined(USE_BARO)
|
||||
/* G bump landing detection only used when xy velocity is usable and low or failsafe is active */
|
||||
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)) {
|
||||
/* Inverted crash landing detection - immediate disarm */
|
||||
if (navConfig()->mc.inverted_crash_detection && !FLIGHT_MODE(TURTLE_MODE) && isMulticopterCrashedInverted(currentTimeMs)) {
|
||||
ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
|
||||
disarm(DISARM_LANDING);
|
||||
}
|
||||
|
||||
if (gBumpDetectionUsable && isLandingGbumpDetected(currentTimeMs)) {
|
||||
return true; // Landing flagged immediately if landing bump detected
|
||||
/* G bump landing detection *
|
||||
* Only used when xy velocity is low or failsafe is active */
|
||||
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
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
else {
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue