mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 00:05:28 +03:00
Initial build
This commit is contained in:
breadoven
parent
9df51dbbbb
commit
864c3366f8
13 changed files with 215 additions and 83 deletions
|
|
@ -294,6 +294,7 @@
|
|||
| nav_emerg_landing_speed | 500 | 100 | 2000 | Rate of descent UAV will try to maintain when doing emergency descent sequence [cm/s] |
|
||||
| nav_extra_arming_safety | ON | | | If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured. ALLOW_BYPASS allows the user to momentarily disable this check by holding yaw high (left stick held at the bottom right in mode 2) when switch arming is used |
|
||||
| nav_fw_allow_manual_thr_increase | OFF | | | Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing |
|
||||
| nav_fw_auto_disarm_delay | 2000 | 100 | 10000 | Delay before plane disarms when `nav_disarm_on_landing` is set (ms) |
|
||||
| nav_fw_bank_angle | 35 | 5 | 80 | Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll |
|
||||
| nav_fw_climb_angle | 20 | 5 | 80 | Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit |
|
||||
| nav_fw_control_smoothness | 0 | 0 | 9 | How smoothly the autopilot controls the airplane to correct the navigation error |
|
||||
|
|
@ -341,7 +342,7 @@
|
|||
| nav_manual_speed | 500 | 10 | 2000 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] |
|
||||
| nav_max_altitude | 0 | 0 | 65000 | Max allowed altitude (above Home Point) that applies to all NAV modes (including Altitude Hold). 0 means limit is disabled |
|
||||
| nav_max_terrain_follow_alt | 100 | | 1000 | Max allowed above the ground altitude for terrain following mode |
|
||||
| nav_mc_auto_disarm_delay | 2000 | 100 | 10000 | Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s) |
|
||||
| nav_mc_auto_disarm_delay | 2000 | 100 | 10000 | Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (ms) |
|
||||
| nav_mc_bank_angle | 30 | 15 | 45 | Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude |
|
||||
| nav_mc_braking_bank_angle | 40 | 15 | 60 | max angle that MR is allowed to bank in BOOST mode |
|
||||
| nav_mc_braking_boost_disengage_speed | 100 | 0 | 1000 | BOOST will be disabled when speed goes below this value |
|
||||
|
|
|
|||
|
|
@ -930,6 +930,11 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
|
|||
writeMotors();
|
||||
}
|
||||
|
||||
// Check if landed, FW and MR
|
||||
if (STATE(ALTITUDE_CONTROL)) {
|
||||
updateLandingStatus();
|
||||
}
|
||||
|
||||
#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.
|
||||
|
|
@ -134,6 +136,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))
|
||||
|
|
|
|||
|
|
@ -2490,7 +2490,7 @@ groups:
|
|||
min: 1000
|
||||
max: 2000
|
||||
- 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
|
||||
|
|
@ -2588,6 +2588,12 @@ groups:
|
|||
field: fw.max_throttle
|
||||
min: 1000
|
||||
max: 2000
|
||||
- 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
|
||||
|
|
|
|||
|
|
@ -747,6 +747,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:
|
||||
|
|
@ -3284,7 +3286,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;
|
||||
|
|
@ -3873,6 +3875,9 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
|
|||
if (FLIGHT_MODE(HEADFREE_MODE)) {
|
||||
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE);
|
||||
}
|
||||
if (STATE(LANDING_DETECTED)) {
|
||||
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
|
||||
}
|
||||
}
|
||||
// Pick one of the available messages. Each message lasts
|
||||
// a second.
|
||||
|
|
|
|||
|
|
@ -507,6 +507,8 @@ static const 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:
|
||||
|
|
|
|||
|
|
@ -57,7 +57,6 @@
|
|||
#include "sensors/acceleration.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
|
||||
|
||||
// Multirotors:
|
||||
#define MR_RTH_CLIMB_OVERSHOOT_CM 100 // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt)
|
||||
#define MR_RTH_CLIMB_MARGIN_MIN_CM 100 // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend)
|
||||
|
|
@ -197,6 +196,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT,
|
||||
.useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
|
||||
.yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT,
|
||||
.auto_disarm_delay = SETTING_NAV_FW_AUTO_DISARM_DELAY_DEFAULT, // ms - time delay to disarm when auto disarm after landing enabled
|
||||
}
|
||||
);
|
||||
|
||||
|
|
@ -1314,7 +1314,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
|
|||
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) {
|
||||
// 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
|
||||
}
|
||||
|
|
@ -1371,7 +1371,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
if (!ARMING_FLAG(ARMED)) {
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
else if (isLandingDetected()) {
|
||||
else if (STATE(LANDING_DETECTED)) {
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
else {
|
||||
|
|
@ -1410,7 +1410,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);
|
||||
}
|
||||
|
||||
|
|
@ -1688,20 +1688,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)
|
||||
|
|
@ -2538,28 +2541,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 * (navConfig()->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();
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -275,6 +275,7 @@ typedef struct navConfig_s {
|
|||
bool allow_manual_thr_increase;
|
||||
bool useFwNavYawControl;
|
||||
uint8_t yawControlDeadband;
|
||||
uint16_t auto_disarm_delay; // fixed wing disarm delay for landing detector
|
||||
} fw;
|
||||
} navConfig_t;
|
||||
|
||||
|
|
@ -539,6 +540,8 @@ const char * fixedWingLaunchStateMessage(void);
|
|||
|
||||
float calculateAverageSpeed(void);
|
||||
|
||||
void updateLandingStatus(void);
|
||||
|
||||
const navigationPIDControllers_t* getNavigationPIDControllers(void);
|
||||
|
||||
int32_t navigationGetHeadingError(void);
|
||||
|
|
|
|||
|
|
@ -35,6 +35,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"
|
||||
|
|
@ -573,21 +575,64 @@ bool isFixedWingAutoThrottleManuallyIncreased()
|
|||
return isAutoThrottleManuallyIncreased;
|
||||
}
|
||||
|
||||
bool isFixedWingFlying(void)
|
||||
{
|
||||
float airspeed = 0;
|
||||
#ifdef USE_PITOT
|
||||
airspeed = pitot.airSpeed;
|
||||
#endif
|
||||
bool throttleCondition = rcCommand[THROTTLE] > navConfig()->fw.cruise_throttle;
|
||||
bool velCondition = posControl.actualState.velXY > 250 || airspeed > 250;
|
||||
|
||||
return isImuHeadingValid() && throttleCondition && velCondition;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* 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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -36,6 +36,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"
|
||||
|
|
@ -672,51 +673,74 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs)
|
|||
}
|
||||
}
|
||||
|
||||
bool isMulticopterFlying(void)
|
||||
{
|
||||
bool throttleCondition = rcCommand[THROTTLE] > navConfig()->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) > 25.0f;
|
||||
// check vertical and horizontal velocities are low (cm/s)
|
||||
bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < 25.0f && posControl.actualState.velXY < 100.0f;
|
||||
// check gyro rates are low (degs/s)
|
||||
bool gyroCondition = averageAbsGyroRates() < 2.0f;
|
||||
|
||||
// check if we are moving horizontally
|
||||
bool horizontalMovement = posControl.actualState.velXY > 100.0f;
|
||||
bool possibleLandingDetected = false;
|
||||
const timeUs_t currentTimeUs = micros();
|
||||
|
||||
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)
|
||||
// Wait for 1 second so throttle has stabilized.
|
||||
|
||||
static int32_t landingThrSum;
|
||||
static int32_t landingThrSamples;
|
||||
bool isAtMinimalThrust = false;
|
||||
if (currentTimeUs - landingDetectorStartedAt > 1000 * 1000) {
|
||||
|
||||
if (landingDetectorStartedAt == 0) {
|
||||
landingThrSum = landingThrSamples = 0;
|
||||
landingDetectorStartedAt = currentTimeUs;
|
||||
}
|
||||
if (!landingThrSamples) {
|
||||
if (currentTimeUs - landingDetectorStartedAt < 1000000) { // Wait for 1 second so throttle has stabilized.
|
||||
return false;
|
||||
} else {
|
||||
landingDetectorStartedAt = currentTimeUs;
|
||||
}
|
||||
}
|
||||
landingThrSamples += 1;
|
||||
landingThrSum += rcCommandAdjustedThrottle;
|
||||
isAtMinimalThrust = rcCommandAdjustedThrottle < (landingThrSum / landingThrSamples - 40);
|
||||
}
|
||||
|
||||
bool possibleLandingDetected = isAtMinimalThrust && !verticalMovement && !horizontalMovement;
|
||||
possibleLandingDetected = isAtMinimalThrust && velCondition;
|
||||
} else { // non autonomous and emergency landing
|
||||
if (landingDetectorStartedAt != 0) {
|
||||
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)) {
|
||||
|
|
@ -726,13 +750,13 @@ bool isMulticopterLandingDetected(void)
|
|||
possibleLandingDetected = possibleLandingDetected && (posControl.actualState.agl.pos.z <= (posControl.actualState.surfaceMin + 5.0f));
|
||||
}
|
||||
|
||||
if (!possibleLandingDetected) {
|
||||
landingTimer = currentTimeUs;
|
||||
if (possibleLandingDetected) {
|
||||
timeUs_t safetyTimeDelay = 1000 * (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) > (navConfig()->mc.auto_disarm_delay * 1000)) ? true : false;
|
||||
}
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -92,6 +92,9 @@ typedef struct navigationFlags_s {
|
|||
bool isTerrainFollowEnabled; // Does iNav use rangefinder for terrain following (adjusting baro altitude target according to rangefinders readings)
|
||||
|
||||
bool forcedRTHActivated;
|
||||
|
||||
/* Landing detector */
|
||||
bool resetLandingDetector;
|
||||
} navigationFlags_t;
|
||||
|
||||
typedef struct {
|
||||
|
|
@ -389,8 +392,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);
|
||||
|
||||
|
|
@ -428,11 +435,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);
|
||||
|
||||
|
|
@ -451,6 +454,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 */
|
||||
|
|
|
|||
|
|
@ -101,3 +101,4 @@ bool gyroReadTemperature(void);
|
|||
int16_t gyroGetTemperature(void);
|
||||
int16_t gyroRateDps(int axis);
|
||||
void gyroUpdateDynamicLpf(float cutoffFreq);
|
||||
float averageAbsGyroRates(void);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue