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

rangefinder debug mode

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2017-07-07 13:35:47 +02:00
parent a1f34965de
commit 0fa7a2a773
3 changed files with 11 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -50,5 +50,6 @@ typedef enum {
DEBUG_NOTCH,
DEBUG_NAV_LANDING_DETECTOR,
DEBUG_FW_CLIMB_RATE_TO_ALTITUDE,
DEBUG_RANGEFINDER,
DEBUG_COUNT
} debugType_e;

3
src/main/fc/cli.c
View file

@ -327,7 +327,8 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
"GYRO",
"NOTCH",
"NAV_LANDING",
"FW_ALTITUDE"
"FW_ALTITUDE",
"RANGEFINDER"
};
#ifdef TELEMETRY_LTM

8
src/main/sensors/rangefinder.c
View file

@ -48,6 +48,8 @@
#include "scheduler/scheduler.h"
#include "build/debug.h"
rangefinder_t rangefinder;
#define RANGEFINDER_HARDWARE_TIMEOUT_MS 500 // Accept 500ms of non-responsive sensor, report HW failure otherwise
@ -182,6 +184,9 @@ int32_t rangefinderRead(void)
{
if (rangefinder.dev.read) {
const int32_t distance = rangefinder.dev.read();
DEBUG_SET(DEBUG_RANGEFINDER, 0, distance);
if (distance >= 0) {
rangefinder.lastValidResponseTimeMs = millis();
rangefinder.rawAltitude = applyMedianFilter(distance);
@ -200,6 +205,8 @@ int32_t rangefinderRead(void)
rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
}
DEBUG_SET(DEBUG_RANGEFINDER, 1, rangefinder.rawAltitude);
return rangefinder.rawAltitude;
}
@ -217,6 +224,7 @@ int32_t rangefinderCalculateAltitude(int32_t rangefinderDistance, float cosTiltA
} else {
rangefinder.calculatedAltitude = rangefinderDistance * cosTiltAngle;
}
DEBUG_SET(DEBUG_RANGEFINDER, 2, rangefinder.calculatedAltitude);
return rangefinder.calculatedAltitude;
}