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_NOTCH,
DEBUG_NAV_LANDING_DETECTOR, DEBUG_NAV_LANDING_DETECTOR,
DEBUG_FW_CLIMB_RATE_TO_ALTITUDE, DEBUG_FW_CLIMB_RATE_TO_ALTITUDE,
DEBUG_RANGEFINDER,
DEBUG_COUNT DEBUG_COUNT
} debugType_e; } debugType_e;

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

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

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

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