mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-21 15:25:29 +03:00
Merge pull request #7270 from breadoven/abo_landing_detection
Landing detection revamp
This commit is contained in:
Paweł Spychalski
GitHub
commit
75ae6596ad
14 changed files with 238 additions and 87 deletions
|
|
@ -2932,6 +2932,16 @@ Enable the possibility to manually increase the throttle in auto throttle contro
|
|||
|
||||
---
|
||||
|
||||
### nav_fw_auto_disarm_delay
|
||||
|
||||
Delay before plane disarms when `nav_disarm_on_landing` is set (ms)
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| 2000 | 100 | 10000 |
|
||||
|
||||
---
|
||||
|
||||
### nav_fw_bank_angle
|
||||
|
||||
Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll
|
||||
|
|
@ -3444,7 +3454,7 @@ Max allowed above the ground altitude for terrain following mode
|
|||
|
||||
### nav_mc_auto_disarm_delay
|
||||
|
||||
Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s)
|
||||
Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (ms)
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
|
|
|
|||
|
|
@ -932,6 +932,13 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
|
|||
writeMotors();
|
||||
}
|
||||
|
||||
#if defined(USE_NAV)
|
||||
// Check if landed, FW and MR
|
||||
if (STATE(ALTITUDE_CONTROL)) {
|
||||
updateLandingStatus();
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_BLACKBOX
|
||||
if (!cliMode && feature(FEATURE_BLACKBOX)) {
|
||||
blackboxUpdate(micros());
|
||||
|
|
|
|||
|
|
@ -30,6 +30,7 @@ typedef enum disarmReason_e {
|
|||
DISARM_KILLSWITCH = 5,
|
||||
DISARM_FAILSAFE = 6,
|
||||
DISARM_NAVIGATION = 7,
|
||||
DISARM_LANDING = 8,
|
||||
DISARM_REASON_COUNT
|
||||
} disarmReason_t;
|
||||
|
||||
|
|
|
|||
|
|
@ -45,6 +45,7 @@ typedef enum {
|
|||
ARMING_DISABLED_PWM_OUTPUT_ERROR = (1 << 27),
|
||||
ARMING_DISABLED_NO_PREARM = (1 << 28),
|
||||
ARMING_DISABLED_DSHOT_BEEPER = (1 << 29),
|
||||
ARMING_DISABLED_LANDING_DETECTED = (1 << 30),
|
||||
|
||||
ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING |
|
||||
ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE |
|
||||
|
|
@ -53,7 +54,8 @@ typedef enum {
|
|||
ARMING_DISABLED_BOXKILLSWITCH | ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI |
|
||||
ARMING_DISABLED_CMS_MENU | ARMING_DISABLED_OSD_MENU | ARMING_DISABLED_ROLLPITCH_NOT_CENTERED |
|
||||
ARMING_DISABLED_SERVO_AUTOTRIM | ARMING_DISABLED_OOM | ARMING_DISABLED_INVALID_SETTING |
|
||||
ARMING_DISABLED_PWM_OUTPUT_ERROR | ARMING_DISABLED_NO_PREARM | ARMING_DISABLED_DSHOT_BEEPER),
|
||||
ARMING_DISABLED_PWM_OUTPUT_ERROR | ARMING_DISABLED_NO_PREARM | ARMING_DISABLED_DSHOT_BEEPER |
|
||||
ARMING_DISABLED_LANDING_DETECTED),
|
||||
} armingFlag_e;
|
||||
|
||||
// Arming blockers that can be overriden by emergency arming.
|
||||
|
|
@ -135,6 +137,7 @@ typedef enum {
|
|||
SET_REVERSIBLE_MOTORS_FORWARD = (1 << 23),
|
||||
FW_HEADING_USE_YAW = (1 << 24),
|
||||
ANTI_WINDUP_DEACTIVATED = (1 << 25),
|
||||
LANDING_DETECTED = (1 << 26),
|
||||
} stateFlags_t;
|
||||
|
||||
#define DISABLE_STATE(mask) (stateFlags &= ~(mask))
|
||||
|
|
|
|||
|
|
@ -2613,7 +2613,7 @@ groups:
|
|||
min: 15
|
||||
max: 45
|
||||
- name: nav_mc_auto_disarm_delay
|
||||
description: "Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s)"
|
||||
description: "Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (ms)"
|
||||
default_value: 2000
|
||||
field: mc.auto_disarm_delay
|
||||
min: 100
|
||||
|
|
@ -2691,6 +2691,12 @@ groups:
|
|||
default_value: ON
|
||||
field: mc.slowDownForTurning
|
||||
type: bool
|
||||
- name: nav_fw_auto_disarm_delay
|
||||
description: "Delay before plane disarms when `nav_disarm_on_landing` is set (ms)"
|
||||
default_value: 2000
|
||||
field: fw.auto_disarm_delay
|
||||
min: 100
|
||||
max: 10000
|
||||
- name: nav_fw_bank_angle
|
||||
description: "Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll"
|
||||
default_value: 35
|
||||
|
|
|
|||
|
|
@ -830,6 +830,8 @@ static const char * osdArmingDisabledReasonMessage(void)
|
|||
case ARMING_DISABLED_DSHOT_BEEPER:
|
||||
return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER);
|
||||
// Cases without message
|
||||
case ARMING_DISABLED_LANDING_DETECTED:
|
||||
FALLTHROUGH;
|
||||
case ARMING_DISABLED_CMS_MENU:
|
||||
FALLTHROUGH;
|
||||
case ARMING_DISABLED_OSD_MENU:
|
||||
|
|
@ -3618,7 +3620,7 @@ static void osdUpdateStats(void)
|
|||
|
||||
static void osdShowStatsPage1(void)
|
||||
{
|
||||
const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS" };
|
||||
const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS", "LANDING"};
|
||||
uint8_t top = 1; /* first fully visible line */
|
||||
const uint8_t statNameX = 1;
|
||||
const uint8_t statValuesX = 20;
|
||||
|
|
@ -4273,6 +4275,9 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
|
|||
if (posControl.flags.wpMissionPlannerActive) {
|
||||
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MISSION_PLANNER);
|
||||
}
|
||||
if (STATE(LANDING_DETECTED)) {
|
||||
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
|
||||
}
|
||||
}
|
||||
}
|
||||
} else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
|
||||
|
|
|
|||
|
|
@ -527,6 +527,8 @@ static char * osdArmingDisabledReasonMessage(void)
|
|||
case ARMING_DISABLED_DSHOT_BEEPER:
|
||||
return OSD_MESSAGE_STR("MOTOR BEEPER ACTIVE");
|
||||
// Cases without message
|
||||
case ARMING_DISABLED_LANDING_DETECTED:
|
||||
FALLTHROUGH;
|
||||
case ARMING_DISABLED_CMS_MENU:
|
||||
FALLTHROUGH;
|
||||
case ARMING_DISABLED_OSD_MENU:
|
||||
|
|
|
|||
|
|
@ -206,6 +206,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
|
||||
.yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT,
|
||||
.soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT,// pitch angle mode deadband when Saoring mode enabled
|
||||
.auto_disarm_delay = SETTING_NAV_FW_AUTO_DISARM_DELAY_DEFAULT, // ms - time delay to disarm when auto disarm after landing enabled
|
||||
}
|
||||
);
|
||||
|
||||
|
|
@ -1320,7 +1321,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
|
|||
// If position ok OR within valid timeout - continue
|
||||
// Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
|
||||
if ((ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
|
||||
resetLandingDetector();
|
||||
resetLandingDetector(); // force reset landing detector just in case
|
||||
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
|
||||
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
|
||||
} else {
|
||||
|
|
@ -1368,7 +1369,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
||||
if (!ARMING_FLAG(ARMED) || isLandingDetected()) {
|
||||
if (!ARMING_FLAG(ARMED) || STATE(LANDING_DETECTED)) {
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
||||
|
|
@ -1404,7 +1405,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigatio
|
|||
UNUSED(previousState);
|
||||
|
||||
//On ROVER and BOAT disarm immediately
|
||||
if (!STATE(ALTITUDE_CONTROL) || navConfig()->general.flags.disarm_on_landing) {
|
||||
if (!STATE(ALTITUDE_CONTROL)) {
|
||||
disarm(DISARM_NAVIGATION);
|
||||
}
|
||||
|
||||
|
|
@ -1713,20 +1714,23 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITI
|
|||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState)
|
||||
{
|
||||
// TODO:
|
||||
UNUSED(previousState);
|
||||
|
||||
if (STATE(LANDING_DETECTED)) {
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState)
|
||||
{
|
||||
// TODO:
|
||||
UNUSED(previousState);
|
||||
|
||||
// Prevent I-terms growing when already landed
|
||||
pidResetErrorAccumulators();
|
||||
rcCommand[THROTTLE] = getThrottleIdleValue();
|
||||
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
|
||||
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navigationFSMState_t previousState)
|
||||
|
|
@ -2589,28 +2593,56 @@ void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distanc
|
|||
/*-----------------------------------------------------------
|
||||
* NAV land detector
|
||||
*-----------------------------------------------------------*/
|
||||
void resetLandingDetector(void)
|
||||
void updateLandingStatus(void)
|
||||
{
|
||||
if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
|
||||
resetFixedWingLandingDetector();
|
||||
if (STATE(AIRPLANE) && !navConfig()->general.flags.disarm_on_landing) {
|
||||
return; // no point using this with a fixed wing if not set to disarm
|
||||
}
|
||||
else {
|
||||
resetMulticopterLandingDetector();
|
||||
|
||||
static bool landingDetectorIsActive;
|
||||
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
resetLandingDetector();
|
||||
landingDetectorIsActive = false;
|
||||
if (!IS_RC_MODE_ACTIVE(BOXARM)) {
|
||||
DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (!landingDetectorIsActive) {
|
||||
if (isFlightDetected()) {
|
||||
landingDetectorIsActive = true;
|
||||
resetLandingDetector();
|
||||
}
|
||||
} else if (STATE(LANDING_DETECTED)) {
|
||||
pidResetErrorAccumulators();
|
||||
if (navConfig()->general.flags.disarm_on_landing) {
|
||||
ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
|
||||
disarm(DISARM_LANDING);
|
||||
} else if (!navigationIsFlyingAutonomousMode()) {
|
||||
// for multirotor only - reactivate landing detector without disarm when throttle raised toward hover throttle
|
||||
landingDetectorIsActive = rxGetChannelValue(THROTTLE) < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));
|
||||
}
|
||||
} else if (isLandingDetected()) {
|
||||
ENABLE_STATE(LANDING_DETECTED);
|
||||
}
|
||||
}
|
||||
|
||||
bool isLandingDetected(void)
|
||||
{
|
||||
bool landingDetected;
|
||||
return STATE(AIRPLANE) ? isFixedWingLandingDetected() : isMulticopterLandingDetected();
|
||||
}
|
||||
|
||||
if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
|
||||
landingDetected = isFixedWingLandingDetected();
|
||||
}
|
||||
else {
|
||||
landingDetected = isMulticopterLandingDetected();
|
||||
}
|
||||
void resetLandingDetector(void)
|
||||
{
|
||||
DISABLE_STATE(LANDING_DETECTED);
|
||||
posControl.flags.resetLandingDetector = true;
|
||||
}
|
||||
|
||||
return landingDetected;
|
||||
bool isFlightDetected(void)
|
||||
{
|
||||
return STATE(AIRPLANE) ? isFixedWingFlying() : isMulticopterFlying();
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -278,7 +278,7 @@ typedef struct navConfig_s {
|
|||
uint8_t max_dive_angle; // Fixed wing max banking angle (deg)
|
||||
uint16_t cruise_speed; // Speed at cruise throttle (cm/s), used for time/distance left before RTH
|
||||
uint8_t control_smoothness; // The amount of smoothing to apply to controls for navigation
|
||||
uint16_t pitch_to_throttle_smooth; // How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh.
|
||||
uint16_t pitch_to_throttle_smooth; // How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh.
|
||||
uint8_t pitch_to_throttle_thresh; // Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]
|
||||
uint16_t loiter_radius; // Loiter radius when executing PH on a fixed wing
|
||||
int8_t land_dive_angle;
|
||||
|
|
@ -289,7 +289,7 @@ typedef struct navConfig_s {
|
|||
uint16_t launch_idle_motor_timer; // Time to wait before motor starts at_idle throttle (ms)
|
||||
uint16_t launch_motor_spinup_time; // Time to speed-up motors from idle to launch_throttle (ESC desync prevention)
|
||||
uint16_t launch_end_time; // Time to make the transition from launch angle to leveled and throttle transition from launch throttle to the stick position
|
||||
uint16_t launch_min_time; // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early
|
||||
uint16_t launch_min_time; // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early
|
||||
uint16_t launch_timeout; // Launch timeout to disable launch mode and swith to normal flight (ms)
|
||||
uint16_t launch_max_altitude; // cm, altitude where to consider launch ended
|
||||
uint8_t launch_climb_angle; // Target climb angle for launch (deg)
|
||||
|
|
@ -299,6 +299,7 @@ typedef struct navConfig_s {
|
|||
bool useFwNavYawControl;
|
||||
uint8_t yawControlDeadband;
|
||||
uint8_t soaring_pitch_deadband; // soaring mode pitch angle deadband (deg)
|
||||
uint16_t auto_disarm_delay; // fixed wing disarm delay for landing detector
|
||||
} fw;
|
||||
} navConfig_t;
|
||||
|
||||
|
|
@ -574,6 +575,8 @@ const char * fixedWingLaunchStateMessage(void);
|
|||
|
||||
float calculateAverageSpeed(void);
|
||||
|
||||
void updateLandingStatus(void);
|
||||
|
||||
const navigationPIDControllers_t* getNavigationPIDControllers(void);
|
||||
|
||||
int32_t navigationGetHeadingError(void);
|
||||
|
|
|
|||
|
|
@ -33,6 +33,8 @@
|
|||
#include "sensors/sensors.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/pitotmeter.h"
|
||||
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
|
|
@ -592,21 +594,65 @@ bool isFixedWingAutoThrottleManuallyIncreased()
|
|||
return isAutoThrottleManuallyIncreased;
|
||||
}
|
||||
|
||||
bool isFixedWingFlying(void)
|
||||
{
|
||||
float airspeed = 0;
|
||||
#ifdef USE_PITOT
|
||||
airspeed = pitot.airSpeed;
|
||||
#endif
|
||||
bool throttleCondition = rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle;
|
||||
bool velCondition = posControl.actualState.velXY > 250 || airspeed > 250;
|
||||
bool launchCondition = isNavLaunchEnabled() && fixedWingLaunchStatus() == FW_LAUNCH_FLYING;
|
||||
|
||||
return (isImuHeadingValid() && throttleCondition && velCondition) || launchCondition;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* FixedWing land detector
|
||||
*-----------------------------------------------------------*/
|
||||
static timeUs_t landingTimerUs;
|
||||
|
||||
void resetFixedWingLandingDetector(void)
|
||||
{
|
||||
landingTimerUs = micros();
|
||||
}
|
||||
|
||||
bool isFixedWingLandingDetected(void)
|
||||
{
|
||||
timeUs_t currentTimeUs = micros();
|
||||
static bool fixAxisCheck = false;
|
||||
const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
|
||||
|
||||
landingTimerUs = currentTimeUs;
|
||||
// Basic condition to start looking for landing
|
||||
bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
|
||||
|| FLIGHT_MODE(FAILSAFE_MODE)
|
||||
|| (!navigationIsControllingThrottle() && throttleIsLow);
|
||||
|
||||
if (!startCondition || posControl.flags.resetLandingDetector) {
|
||||
return fixAxisCheck = posControl.flags.resetLandingDetector = false;
|
||||
}
|
||||
|
||||
static timeMs_t fwLandingTimerStartAt;
|
||||
static int16_t fwLandSetRollDatum;
|
||||
static int16_t fwLandSetPitchDatum;
|
||||
|
||||
timeMs_t currentTimeMs = millis();
|
||||
|
||||
// Check horizontal and vertical volocities are low (cm/s)
|
||||
bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < 50.0f && posControl.actualState.velXY < 100.0f;
|
||||
// Check angular rates are low (degs/s)
|
||||
bool gyroCondition = averageAbsGyroRates() < 2.0f;
|
||||
|
||||
if (velCondition && gyroCondition){
|
||||
if (!fixAxisCheck) { // capture roll and pitch angles to be used as datums to check for absolute change
|
||||
fwLandSetRollDatum = attitude.values.roll; //0.1 deg increments
|
||||
fwLandSetPitchDatum = attitude.values.pitch;
|
||||
fixAxisCheck = true;
|
||||
fwLandingTimerStartAt = currentTimeMs;
|
||||
} else {
|
||||
bool isRollAxisStatic = ABS(fwLandSetRollDatum - attitude.values.roll) < 5;
|
||||
bool isPitchAxisStatic = ABS(fwLandSetPitchDatum - attitude.values.pitch) < 5;
|
||||
if (isRollAxisStatic && isPitchAxisStatic) {
|
||||
// Probably landed, low horizontal and vertical velocities and no axis rotation in Roll and Pitch
|
||||
timeMs_t safetyTimeDelay = 2000 + navConfig()->fw.auto_disarm_delay;
|
||||
return currentTimeMs - fwLandingTimerStartAt > safetyTimeDelay; // check conditions stable for 2s + optional extra delay
|
||||
} else {
|
||||
fixAxisCheck = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
|
|||
97
src/main/navigation/navigation_multicopter.c
Executable file → Normal file
97
src/main/navigation/navigation_multicopter.c
Executable file → Normal file
|
|
@ -34,6 +34,7 @@
|
|||
#include "sensors/sensors.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/gyro.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/rc_controls.h"
|
||||
|
|
@ -712,51 +713,75 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs)
|
|||
}
|
||||
}
|
||||
|
||||
bool isMulticopterFlying(void)
|
||||
{
|
||||
bool throttleCondition = rcCommand[THROTTLE] > currentBatteryProfile->nav.mc.hover_throttle;
|
||||
bool gyroCondition = averageAbsGyroRates() > 7.0f;
|
||||
|
||||
return throttleCondition && gyroCondition;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Multicopter land detector
|
||||
*-----------------------------------------------------------*/
|
||||
static timeUs_t landingTimer;
|
||||
static timeUs_t landingDetectorStartedAt;
|
||||
static int32_t landingThrSum;
|
||||
static int32_t landingThrSamples;
|
||||
|
||||
void resetMulticopterLandingDetector(void)
|
||||
{
|
||||
// FIXME: This function is called some time before isMulticopterLandingDetected is first called
|
||||
landingTimer = micros();
|
||||
landingDetectorStartedAt = 0; // ugly hack for now
|
||||
|
||||
landingThrSum = 0;
|
||||
landingThrSamples = 0;
|
||||
}
|
||||
|
||||
bool isMulticopterLandingDetected(void)
|
||||
{
|
||||
const timeUs_t currentTimeUs = micros();
|
||||
static timeUs_t landingDetectorStartedAt;
|
||||
const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
|
||||
|
||||
// FIXME: Remove delay between resetMulticopterLandingDetector and first run of this function so this code isn't needed.
|
||||
if (landingDetectorStartedAt == 0) {
|
||||
landingDetectorStartedAt = currentTimeUs;
|
||||
/* Basic condition to start looking for landing
|
||||
* Prevent landing detection if WP mission allowed during Failsafe (except landing states) */
|
||||
bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
|
||||
|| (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE))
|
||||
|| (!navigationIsFlyingAutonomousMode() && throttleIsLow);
|
||||
|
||||
if (!startCondition || posControl.flags.resetLandingDetector) {
|
||||
landingDetectorStartedAt = 0;
|
||||
return posControl.flags.resetLandingDetector = false;
|
||||
}
|
||||
|
||||
// Average climb rate should be low enough
|
||||
bool verticalMovement = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) > MC_LAND_CHECK_VEL_Z_MOVING;
|
||||
// check vertical and horizontal velocities are low (cm/s)
|
||||
bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < MC_LAND_CHECK_VEL_Z_MOVING &&
|
||||
posControl.actualState.velXY < MC_LAND_CHECK_VEL_XY_MOVING;
|
||||
// check gyro rates are low (degs/s)
|
||||
bool gyroCondition = averageAbsGyroRates() < 2.0f;
|
||||
|
||||
// check if we are moving horizontally
|
||||
bool horizontalMovement = posControl.actualState.velXY > MC_LAND_CHECK_VEL_XY_MOVING;
|
||||
bool possibleLandingDetected = false;
|
||||
const timeUs_t currentTimeUs = micros();
|
||||
|
||||
// We have likely landed if throttle is 40 units below average descend throttle
|
||||
// We use rcCommandAdjustedThrottle to keep track of NAV corrected throttle (isLandingDetected is executed
|
||||
// from processRx() and rcCommand at that moment holds rc input, not adjusted values from NAV core)
|
||||
// Wait for 1 second so throttle has stabilized.
|
||||
bool isAtMinimalThrust = false;
|
||||
if (currentTimeUs - landingDetectorStartedAt > HZ2US(MC_LAND_THR_SUM_RATE)) {
|
||||
if (navGetCurrentStateFlags() & NAV_CTL_LAND) {
|
||||
// We have likely landed if throttle is 40 units below average descend throttle
|
||||
// We use rcCommandAdjustedThrottle to keep track of NAV corrected throttle (isLandingDetected is executed
|
||||
// from processRx() and rcCommand at that moment holds rc input, not adjusted values from NAV core)
|
||||
|
||||
static int32_t landingThrSum;
|
||||
static int32_t landingThrSamples;
|
||||
bool isAtMinimalThrust = false;
|
||||
|
||||
if (!landingDetectorStartedAt) {
|
||||
landingThrSum = landingThrSamples = 0;
|
||||
landingDetectorStartedAt = currentTimeUs;
|
||||
}
|
||||
if (!landingThrSamples) {
|
||||
if (currentTimeUs - landingDetectorStartedAt < (USECS_PER_SEC * MC_LAND_THR_STABILISE_DELAY)) { // Wait for 1 second so throttle has stabilized.
|
||||
return false;
|
||||
} else {
|
||||
landingDetectorStartedAt = currentTimeUs;
|
||||
}
|
||||
}
|
||||
landingThrSamples += 1;
|
||||
landingThrSum += rcCommandAdjustedThrottle;
|
||||
isAtMinimalThrust = rcCommandAdjustedThrottle < (landingThrSum / landingThrSamples - MC_LAND_DESCEND_THROTTLE);
|
||||
}
|
||||
|
||||
bool possibleLandingDetected = isAtMinimalThrust && !verticalMovement && !horizontalMovement;
|
||||
possibleLandingDetected = isAtMinimalThrust && velCondition;
|
||||
} else { // non autonomous and emergency landing
|
||||
if (landingDetectorStartedAt) {
|
||||
possibleLandingDetected = velCondition && gyroCondition;
|
||||
} else {
|
||||
landingDetectorStartedAt = currentTimeUs;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// If we have surface sensor (for example sonar) - use it to detect touchdown
|
||||
if ((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z >= 0)) {
|
||||
|
|
@ -766,13 +791,13 @@ bool isMulticopterLandingDetected(void)
|
|||
possibleLandingDetected = possibleLandingDetected && (posControl.actualState.agl.pos.z <= (posControl.actualState.surfaceMin + MC_LAND_SAFE_SURFACE));
|
||||
}
|
||||
|
||||
if (!possibleLandingDetected) {
|
||||
landingTimer = currentTimeUs;
|
||||
if (possibleLandingDetected) {
|
||||
timeUs_t safetyTimeDelay = MS2US(2000 + navConfig()->mc.auto_disarm_delay); // check conditions stable for 2s + optional extra delay
|
||||
return (currentTimeUs - landingDetectorStartedAt > safetyTimeDelay);
|
||||
} else {
|
||||
landingDetectorStartedAt = currentTimeUs;
|
||||
return false;
|
||||
}
|
||||
else {
|
||||
return ((currentTimeUs - landingTimer) > MS2US(navConfig()->mc.auto_disarm_delay)) ? true : false;
|
||||
}
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@
|
|||
|
||||
#define MC_LAND_CHECK_VEL_XY_MOVING 100.0f // cm/s
|
||||
#define MC_LAND_CHECK_VEL_Z_MOVING 25.0f // cm/s
|
||||
#define MC_LAND_THR_SUM_RATE 1 // hz
|
||||
#define MC_LAND_THR_STABILISE_DELAY 1 // seconds
|
||||
#define MC_LAND_DESCEND_THROTTLE 40 // uS
|
||||
#define MC_LAND_SAFE_SURFACE 5.0f // cm
|
||||
|
||||
|
|
@ -101,6 +101,9 @@ typedef struct navigationFlags_s {
|
|||
bool forcedEmergLandingActivated;
|
||||
|
||||
bool wpMissionPlannerActive; // Activation status of WP mission planner
|
||||
|
||||
/* Landing detector */
|
||||
bool resetLandingDetector;
|
||||
} navigationFlags_t;
|
||||
|
||||
typedef struct {
|
||||
|
|
@ -412,8 +415,12 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void);
|
|||
bool isThrustFacingDownwards(void);
|
||||
uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos);
|
||||
int32_t calculateBearingToDestination(const fpVector3_t * destinationPos);
|
||||
void resetLandingDetector(void);
|
||||
|
||||
bool isLandingDetected(void);
|
||||
void resetLandingDetector(void);
|
||||
bool isFlightDetected(void);
|
||||
bool isFixedWingFlying(void);
|
||||
bool isMulticopterFlying(void);
|
||||
|
||||
navigationFSMStateFlags_t navGetCurrentStateFlags(void);
|
||||
|
||||
|
|
@ -451,11 +458,7 @@ bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcR
|
|||
|
||||
void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
|
||||
|
||||
void resetFixedWingLandingDetector(void);
|
||||
void resetMulticopterLandingDetector(void);
|
||||
|
||||
bool isMulticopterLandingDetected(void);
|
||||
bool isFixedWingLandingDetected(void);
|
||||
|
||||
void calculateMulticopterInitialHoldPosition(fpVector3_t * pos);
|
||||
|
||||
|
|
@ -474,6 +477,8 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs);
|
|||
float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingError, bool errorIsDecreasing);
|
||||
void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
|
||||
|
||||
bool isFixedWingLandingDetected(void);
|
||||
|
||||
void calculateFixedWingInitialHoldPosition(fpVector3_t * pos);
|
||||
|
||||
/* Fixed-wing launch controller */
|
||||
|
|
|
|||
|
|
@ -239,7 +239,7 @@ static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t
|
|||
{
|
||||
*applyFn = nullFilterApply;
|
||||
if (cutoff > 0) {
|
||||
switch (type)
|
||||
switch (type)
|
||||
{
|
||||
case FILTER_PT1:
|
||||
*applyFn = (filterApplyFnPtr)pt1FilterApply;
|
||||
|
|
@ -261,7 +261,7 @@ static void gyroInitFilters(void)
|
|||
{
|
||||
//First gyro LPF running at full gyro frequency 8kHz
|
||||
initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_anti_aliasing_lpf_type, gyroConfig()->gyro_anti_aliasing_lpf_hz, getGyroLooptime());
|
||||
|
||||
|
||||
//Second gyro LPF runnig and PID frequency - this filter is dynamic when gyro_use_dyn_lpf = ON
|
||||
initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_type, gyroConfig()->gyro_main_lpf_hz, getLooptime());
|
||||
|
||||
|
|
@ -317,7 +317,7 @@ bool gyroInit(void)
|
|||
// Dynamic notch running at PID frequency
|
||||
dynamicGyroNotchFiltersInit(&dynamicGyroNotchState);
|
||||
gyroDataAnalyseStateInit(
|
||||
&gyroAnalyseState,
|
||||
&gyroAnalyseState,
|
||||
gyroConfig()->dynamicGyroNotchMinHz,
|
||||
getLooptime()
|
||||
);
|
||||
|
|
@ -331,7 +331,7 @@ void gyroStartCalibration(void)
|
|||
return;
|
||||
}
|
||||
|
||||
#ifndef USE_IMU_FAKE // fixes Test Unit compilation error
|
||||
#ifndef USE_IMU_FAKE // fixes Test Unit compilation error
|
||||
if (!gyroConfig()->init_gyro_cal_enabled) {
|
||||
return;
|
||||
}
|
||||
|
|
@ -345,7 +345,7 @@ bool gyroIsCalibrationComplete(void)
|
|||
if (!gyro.initialized) {
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
#ifndef USE_IMU_FAKE // fixes Test Unit compilation error
|
||||
if (!gyroConfig()->init_gyro_cal_enabled) {
|
||||
return true;
|
||||
|
|
@ -488,7 +488,7 @@ void FAST_CODE NOINLINE gyroFilter()
|
|||
|
||||
if (gyroAnalyseState.filterUpdateExecute) {
|
||||
dynamicGyroNotchFiltersUpdate(
|
||||
&dynamicGyroNotchState,
|
||||
&dynamicGyroNotchState,
|
||||
gyroAnalyseState.filterUpdateAxis,
|
||||
gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis]
|
||||
);
|
||||
|
|
@ -518,7 +518,7 @@ void FAST_CODE NOINLINE gyroUpdate()
|
|||
* First gyro LPF is the only filter applied with the full gyro sampling speed
|
||||
*/
|
||||
gyroADCf = gyroLpfApplyFn((filter_t *) &gyroLpfState[axis], gyroADCf);
|
||||
|
||||
|
||||
gyro.gyroADCf[axis] = gyroADCf;
|
||||
}
|
||||
}
|
||||
|
|
@ -565,4 +565,9 @@ void gyroUpdateDynamicLpf(float cutoffFreq) {
|
|||
biquadFilterUpdate(&gyroLpf2State[axis].biquad, cutoffFreq, getLooptime(), BIQUAD_Q, FILTER_LPF);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float averageAbsGyroRates(void)
|
||||
{
|
||||
return (fabsf(gyro.gyroADCf[ROLL]) + fabsf(gyro.gyroADCf[PITCH]) + fabsf(gyro.gyroADCf[YAW])) / 3.0f;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -93,4 +93,5 @@ bool gyroIsCalibrationComplete(void);
|
|||
bool gyroReadTemperature(void);
|
||||
int16_t gyroGetTemperature(void);
|
||||
int16_t gyroRateDps(int axis);
|
||||
void gyroUpdateDynamicLpf(float cutoffFreq);
|
||||
void gyroUpdateDynamicLpf(float cutoffFreq);
|
||||
float averageAbsGyroRates(void);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue