Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 23:05:17 +03:00
Code Issues Wiki Activity

Merge pull request #7270 from breadoven/abo_landing_detection

Browse source 
Landing detection revamp
This commit is contained in:
Paweł Spychalski 2022-03-13 10:12:29 +01:00 • committed by GitHub
commit 75ae6596ad
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
14 changed files with 238 additions and 87 deletions
Download patch file Download diff file Expand all files Collapse all files

64
src/main/navigation/navigation_fixedwing.c
View file

@ -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;
}