2 * This file is part of Cleanflight and Betaflight.
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
28 #ifdef USE_RANGEFINDER
30 #include "build/build_config.h"
31 #include "build/debug.h"
33 #include "common/maths.h"
34 #include "common/time.h"
35 #include "common/utils.h"
37 #include "config/config.h"
38 #include "config/feature.h"
40 #include "drivers/io.h"
41 #include "drivers/rangefinder/rangefinder.h"
42 #include "drivers/rangefinder/rangefinder_hcsr04.h"
43 #include "drivers/rangefinder/rangefinder_lidartf.h"
44 #include "drivers/time.h"
46 #include "fc/runtime_config.h"
49 #include "pg/pg_ids.h"
51 #include "scheduler/scheduler.h"
53 #include "sensors/sensors.h"
54 #include "sensors/rangefinder.h"
55 #include "sensors/battery.h"
57 //#include "uav_interconnect/uav_interconnect.h"
59 // XXX Interface to CF/BF legacy(?) altitude estimation code.
60 // XXX Will be gone once iNav's estimator is ported.
61 int16_t rangefinderMaxRangeCm
;
62 int16_t rangefinderMaxAltWithTiltCm
;
63 int16_t rangefinderCfAltCm
; // Complimentary Filter altitude
65 rangefinder_t rangefinder
;
67 #define RANGEFINDER_HARDWARE_TIMEOUT_MS 500 // Accept 500ms of non-responsive sensor, report HW failure otherwise
69 #define RANGEFINDER_DYNAMIC_THRESHOLD 600 //Used to determine max. usable rangefinder disatance
70 #define RANGEFINDER_DYNAMIC_FACTOR 75
72 PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t
, rangefinderConfig
, PG_RANGEFINDER_CONFIG
, 0);
74 PG_RESET_TEMPLATE(rangefinderConfig_t
, rangefinderConfig
,
75 .rangefinder_hardware
= RANGEFINDER_NONE
,
78 #ifdef USE_RANGEFINDER_HCSR04
79 PG_REGISTER_WITH_RESET_TEMPLATE(sonarConfig_t
, sonarConfig
, PG_SONAR_CONFIG
, 1);
81 PG_RESET_TEMPLATE(sonarConfig_t
, sonarConfig
,
82 .triggerTag
= IO_TAG(RANGEFINDER_HCSR04_TRIGGER_PIN
),
83 .echoTag
= IO_TAG(RANGEFINDER_HCSR04_ECHO_PIN
),
88 * Detect which rangefinder is present
90 static bool rangefinderDetect(rangefinderDev_t
* dev
, uint8_t rangefinderHardwareToUse
)
92 rangefinderType_e rangefinderHardware
= RANGEFINDER_NONE
;
93 requestedSensors
[SENSOR_INDEX_RANGEFINDER
] = rangefinderHardwareToUse
;
95 #if !defined(USE_RANGEFINDER_HCSR04) && !defined(USE_RANGEFINDER_TF)
99 switch (rangefinderHardwareToUse
) {
100 case RANGEFINDER_HCSR04
:
101 #ifdef USE_RANGEFINDER_HCSR04
103 if (hcsr04Detect(dev
, sonarConfig())) { // FIXME: Do actual detection if HC-SR04 is plugged in
104 rangefinderHardware
= RANGEFINDER_HCSR04
;
105 rescheduleTask(TASK_RANGEFINDER
, TASK_PERIOD_MS(RANGEFINDER_HCSR04_TASK_PERIOD_MS
));
111 case RANGEFINDER_TFMINI
:
112 #if defined(USE_RANGEFINDER_TF)
113 if (lidarTFminiDetect(dev
)) {
114 rangefinderHardware
= RANGEFINDER_TFMINI
;
115 rescheduleTask(TASK_RANGEFINDER
, TASK_PERIOD_MS(RANGEFINDER_TF_TASK_PERIOD_MS
));
120 case RANGEFINDER_TF02
:
121 #if defined(USE_RANGEFINDER_TF)
122 if (lidarTF02Detect(dev
)) {
123 rangefinderHardware
= RANGEFINDER_TF02
;
124 rescheduleTask(TASK_RANGEFINDER
, TASK_PERIOD_MS(RANGEFINDER_TF_TASK_PERIOD_MS
));
129 case RANGEFINDER_NONE
:
130 rangefinderHardware
= RANGEFINDER_NONE
;
134 if (rangefinderHardware
== RANGEFINDER_NONE
) {
135 sensorsClear(SENSOR_RANGEFINDER
);
139 detectedSensors
[SENSOR_INDEX_RANGEFINDER
] = rangefinderHardware
;
140 sensorsSet(SENSOR_RANGEFINDER
);
144 void rangefinderResetDynamicThreshold(void)
146 rangefinder
.snrThresholdReached
= false;
147 rangefinder
.dynamicDistanceThreshold
= 0;
150 bool rangefinderInit(void)
152 if (!rangefinderDetect(&rangefinder
.dev
, rangefinderConfig()->rangefinder_hardware
)) {
156 rangefinder
.dev
.init(&rangefinder
.dev
);
157 rangefinder
.rawAltitude
= RANGEFINDER_OUT_OF_RANGE
;
158 rangefinder
.calculatedAltitude
= RANGEFINDER_OUT_OF_RANGE
;
159 rangefinder
.maxTiltCos
= cos_approx(DECIDEGREES_TO_RADIANS(rangefinder
.dev
.detectionConeExtendedDeciDegrees
/ 2.0f
));
160 rangefinder
.lastValidResponseTimeMs
= millis();
163 rangefinderResetDynamicThreshold();
165 // XXX Interface to CF/BF legacy(?) altitude estimation code.
166 // XXX Will be gone once iNav's estimator is ported.
167 rangefinderMaxRangeCm
= rangefinder
.dev
.maxRangeCm
;
168 rangefinderMaxAltWithTiltCm
= rangefinderMaxRangeCm
* rangefinder
.maxTiltCos
;
169 rangefinderCfAltCm
= rangefinder
.dev
.maxRangeCm
/ 2 ; // Complimentary Filter altitude
174 static int32_t applyMedianFilter(int32_t newReading
)
176 #define DISTANCE_SAMPLES_MEDIAN 5
177 static int32_t filterSamples
[DISTANCE_SAMPLES_MEDIAN
];
178 static int filterSampleIndex
= 0;
179 static bool medianFilterReady
= false;
181 if (newReading
> RANGEFINDER_OUT_OF_RANGE
) {// only accept samples that are in range
182 filterSamples
[filterSampleIndex
] = newReading
;
184 if (filterSampleIndex
== DISTANCE_SAMPLES_MEDIAN
) {
185 filterSampleIndex
= 0;
186 medianFilterReady
= true;
189 return medianFilterReady
? quickMedianFilter5(filterSamples
) : newReading
;
192 static int16_t computePseudoSnr(int32_t newReading
)
194 #define SNR_SAMPLES 5
195 static int16_t snrSamples
[SNR_SAMPLES
];
196 static uint8_t snrSampleIndex
= 0;
197 static int32_t previousReading
= RANGEFINDER_OUT_OF_RANGE
;
198 static bool snrReady
= false;
199 int16_t pseudoSnr
= 0;
201 const int delta
= newReading
- previousReading
;
202 snrSamples
[snrSampleIndex
] = constrain(delta
* delta
/ 10, 0, 6400);
204 if (snrSampleIndex
== SNR_SAMPLES
) {
209 previousReading
= newReading
;
213 for (uint8_t i
= 0; i
< SNR_SAMPLES
; i
++) {
214 pseudoSnr
+= snrSamples
[i
];
217 return constrain(pseudoSnr
, 0, 32000);
219 return RANGEFINDER_OUT_OF_RANGE
;
224 * This is called periodically by the scheduler
226 void rangefinderUpdate(void)
228 if (rangefinder
.dev
.update
) {
229 rangefinder
.dev
.update(&rangefinder
.dev
);
233 bool isSurfaceAltitudeValid(void)
237 * Preconditions: raw and calculated altidude > 0
238 * SNR lower than threshold
241 rangefinder
.calculatedAltitude
> 0 &&
242 rangefinder
.rawAltitude
> 0 &&
243 rangefinder
.snr
< RANGEFINDER_DYNAMIC_THRESHOLD
247 * When critical altitude was determined, distance reported by rangefinder
248 * has to be lower than it to assume healthy readout
250 if (rangefinder
.snrThresholdReached
) {
251 return (rangefinder
.rawAltitude
< rangefinder
.dynamicDistanceThreshold
);
263 * Get the last distance measured by the sonar in centimeters. When the ground is too far away, RANGEFINDER_OUT_OF_RANGE is returned.
265 bool rangefinderProcess(float cosTiltAngle
)
267 if (rangefinder
.dev
.read
) {
268 const int32_t distance
= rangefinder
.dev
.read(&rangefinder
.dev
);
270 // If driver reported no new measurement - don't do anything
271 if (distance
== RANGEFINDER_NO_NEW_DATA
) {
276 rangefinder
.lastValidResponseTimeMs
= millis();
277 rangefinder
.rawAltitude
= applyMedianFilter(distance
);
278 } else if (distance
== RANGEFINDER_OUT_OF_RANGE
) {
279 rangefinder
.lastValidResponseTimeMs
= millis();
280 rangefinder
.rawAltitude
= RANGEFINDER_OUT_OF_RANGE
;
283 // Invalid response / hardware failure
284 rangefinder
.rawAltitude
= RANGEFINDER_HARDWARE_FAILURE
;
287 rangefinder
.snr
= computePseudoSnr(distance
);
289 if (rangefinder
.snrThresholdReached
== false && rangefinder
.rawAltitude
> 0) {
291 if (rangefinder
.snr
< RANGEFINDER_DYNAMIC_THRESHOLD
&& rangefinder
.dynamicDistanceThreshold
< rangefinder
.rawAltitude
) {
292 rangefinder
.dynamicDistanceThreshold
= rangefinder
.rawAltitude
* RANGEFINDER_DYNAMIC_FACTOR
/ 100;
295 if (rangefinder
.snr
>= RANGEFINDER_DYNAMIC_THRESHOLD
) {
296 rangefinder
.snrThresholdReached
= true;
301 DEBUG_SET(DEBUG_RANGEFINDER
, 3, rangefinder
.snr
);
303 DEBUG_SET(DEBUG_RANGEFINDER_QUALITY
, 0, rangefinder
.rawAltitude
);
304 DEBUG_SET(DEBUG_RANGEFINDER_QUALITY
, 1, rangefinder
.snrThresholdReached
);
305 DEBUG_SET(DEBUG_RANGEFINDER_QUALITY
, 2, rangefinder
.dynamicDistanceThreshold
);
306 DEBUG_SET(DEBUG_RANGEFINDER_QUALITY
, 3, isSurfaceAltitudeValid());
311 rangefinder
.rawAltitude
= RANGEFINDER_OUT_OF_RANGE
;
315 * Apply tilt correction to the given raw sonar reading in order to compensate for the tilt of the craft when estimating
316 * the altitude. Returns the computed altitude in centimeters.
318 * When the ground is too far away or the tilt is too large, RANGEFINDER_OUT_OF_RANGE is returned.
320 if (cosTiltAngle
< rangefinder
.maxTiltCos
|| rangefinder
.rawAltitude
< 0) {
321 rangefinder
.calculatedAltitude
= RANGEFINDER_OUT_OF_RANGE
;
323 rangefinder
.calculatedAltitude
= rangefinder
.rawAltitude
* cosTiltAngle
;
326 DEBUG_SET(DEBUG_RANGEFINDER
, 1, rangefinder
.rawAltitude
);
327 DEBUG_SET(DEBUG_RANGEFINDER
, 2, rangefinder
.calculatedAltitude
);
333 * Get the latest altitude that was computed, or RANGEFINDER_OUT_OF_RANGE if sonarCalculateAltitude
334 * has never been called.
336 int32_t rangefinderGetLatestAltitude(void)
338 return rangefinder
.calculatedAltitude
;
341 int32_t rangefinderGetLatestRawAltitude(void)
343 return rangefinder
.rawAltitude
;
346 bool rangefinderIsHealthy(void)
348 return (millis() - rangefinder
.lastValidResponseTimeMs
) < RANGEFINDER_HARDWARE_TIMEOUT_MS
;