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

Add landing debug

Browse source
This commit is contained in:
breadoven 2022-03-15 10:11:28 +00:00
parent 9499e78ef8
commit 288015a853
5 changed files with 30 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/build/debug.h
View file

@ -86,5 +86,6 @@ typedef enum {
DEBUG_AUTOTRIM,
DEBUG_AUTOTUNE,
DEBUG_RATE_DYNAMICS,
DEBUG_LANDING,
DEBUG_COUNT
} debugType_e;

2
src/main/fc/settings.yaml
View file

@ -96,7 +96,7 @@ tables:
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
"ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
"IRLOCK", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "IMU2", "ALTITUDE",
"SMITH_PREDICTOR", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS"]
"SMITH_PREDICTOR", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING"]
- name: async_mode
values: ["NONE", "GYRO", "ALL"]
- name: aux_operator

3
src/main/navigation/navigation.c
View file

@ -2601,6 +2601,9 @@ void updateLandingStatus(void)
static bool landingDetectorIsActive;
DEBUG_SET(DEBUG_LANDING, 0, landingDetectorIsActive);
DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED));
if (!ARMING_FLAG(ARMED)) {
resetLandingDetector();
landingDetectorIsActive = false;

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

@ -612,6 +612,7 @@ bool isFixedWingFlying(void)
*-----------------------------------------------------------*/
bool isFixedWingLandingDetected(void)
{
DEBUG_SET(DEBUG_LANDING, 4, 0);
static bool fixAxisCheck = false;
const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
@ -623,6 +624,7 @@ bool isFixedWingLandingDetected(void)
if (!startCondition || posControl.flags.resetLandingDetector) {
return fixAxisCheck = posControl.flags.resetLandingDetector = false;
}
DEBUG_SET(DEBUG_LANDING, 4, 1);
static timeMs_t fwLandingTimerStartAt;
static int16_t fwLandSetRollDatum;
@ -634,8 +636,12 @@ bool isFixedWingLandingDetected(void)
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;
DEBUG_SET(DEBUG_LANDING, 2, velCondition);
DEBUG_SET(DEBUG_LANDING, 3, gyroCondition);
if (velCondition && gyroCondition){
DEBUG_SET(DEBUG_LANDING, 4, 2);
DEBUG_SET(DEBUG_LANDING, 5, fixAxisCheck);
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;
@ -644,6 +650,8 @@ bool isFixedWingLandingDetected(void)
} else {
bool isRollAxisStatic = ABS(fwLandSetRollDatum - attitude.values.roll) < 5;
bool isPitchAxisStatic = ABS(fwLandSetPitchDatum - attitude.values.pitch) < 5;
DEBUG_SET(DEBUG_LANDING, 6, isRollAxisStatic);
DEBUG_SET(DEBUG_LANDING, 7, isPitchAxisStatic);
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;

9
src/main/navigation/navigation_multicopter.c
View file

@ -726,6 +726,7 @@ bool isMulticopterFlying(void)
*-----------------------------------------------------------*/
bool isMulticopterLandingDetected(void)
{
DEBUG_SET(DEBUG_LANDING, 4, 0);
static timeUs_t landingDetectorStartedAt;
const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
@ -745,6 +746,8 @@ bool isMulticopterLandingDetected(void)
posControl.actualState.velXY < MC_LAND_CHECK_VEL_XY_MOVING;
// check gyro rates are low (degs/s)
bool gyroCondition = averageAbsGyroRates() < 2.0f;
DEBUG_SET(DEBUG_LANDING, 2, velCondition);
DEBUG_SET(DEBUG_LANDING, 3, gyroCondition);
bool possibleLandingDetected = false;
const timeUs_t currentTimeUs = micros();
@ -753,6 +756,7 @@ bool isMulticopterLandingDetected(void)
// 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)
DEBUG_SET(DEBUG_LANDING, 4, 1);
static int32_t landingThrSum;
static int32_t landingThrSamples;
@ -774,7 +778,11 @@ bool isMulticopterLandingDetected(void)
isAtMinimalThrust = rcCommandAdjustedThrottle < (landingThrSum / landingThrSamples - MC_LAND_DESCEND_THROTTLE);
possibleLandingDetected = isAtMinimalThrust && velCondition;
DEBUG_SET(DEBUG_LANDING, 6, rcCommandAdjustedThrottle);
DEBUG_SET(DEBUG_LANDING, 7, landingThrSum / landingThrSamples - MC_LAND_DESCEND_THROTTLE);
} else { // non autonomous and emergency landing
DEBUG_SET(DEBUG_LANDING, 4, 2);
if (landingDetectorStartedAt) {
possibleLandingDetected = velCondition && gyroCondition;
} else {
@ -790,6 +798,7 @@ bool isMulticopterLandingDetected(void)
// surfaceMin is our ground reference. If we are less than 5cm above the ground - we are likely landed
possibleLandingDetected = possibleLandingDetected && (posControl.actualState.agl.pos.z <= (posControl.actualState.surfaceMin + MC_LAND_SAFE_SURFACE));
}
DEBUG_SET(DEBUG_LANDING, 5, possibleLandingDetected);
if (possibleLandingDetected) {
timeUs_t safetyTimeDelay = MS2US(2000 + navConfig()->mc.auto_disarm_delay); // check conditions stable for 2s + optional extra delay