src/main/sensors/rangefinder.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21 #include <math.h>
  22
  23 #include <platform.h>
  24
  25 #ifdef USE_RANGEFINDER
  26
  27 #include "build/build_config.h"
  28 #include "build/debug.h"
  29
  30 #include "common/maths.h"
  31 #include "common/utils.h"
  32 #include "common/time.h"
  33
  34 #include "config/feature.h"
  35 #include "pg/pg.h"
  36 #include "pg/pg_ids.h"
  37
  38 #include "drivers/io.h"
  39 #include "drivers/time.h"
  40 #include "drivers/rangefinder/rangefinder.h"
  41 #include "drivers/rangefinder/rangefinder_hcsr04.h"
  42 #include "drivers/rangefinder/rangefinder_lidartf.h"
  43
  44 #include "fc/config.h"
  45 #include "fc/runtime_config.h"
  46 #include "fc/fc_tasks.h"
  47
  48 #include "sensors/sensors.h"
  49 #include "sensors/rangefinder.h"
  50 #include "sensors/battery.h"
  51
  52 #include "scheduler/scheduler.h"
  53
  54 //#include "uav_interconnect/uav_interconnect.h"
  55
  56 // XXX Interface to CF/BF legacy(?) altitude estimation code.
  57 // XXX Will be gone once iNav's estimator is ported.
  58 int16_t rangefinderMaxRangeCm;
  59 int16_t rangefinderMaxAltWithTiltCm;
  60 int16_t rangefinderCfAltCm; // Complimentary Filter altitude
  61
  62 rangefinder_t rangefinder;
  63
  64 #define RANGEFINDER_HARDWARE_TIMEOUT_MS         500     // Accept 500ms of non-responsive sensor, report HW failure otherwise
  65
  66 #define RANGEFINDER_DYNAMIC_THRESHOLD           600     //Used to determine max. usable rangefinder disatance
  67 #define RANGEFINDER_DYNAMIC_FACTOR              75
  68
  69 PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, PG_RANGEFINDER_CONFIG, 0);
  70
  71 PG_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig,
  72     .rangefinder_hardware = RANGEFINDER_NONE,
  73 );
  74
  75 #ifdef USE_RANGEFINDER_HCSR04
  76 PG_REGISTER_WITH_RESET_TEMPLATE(sonarConfig_t, sonarConfig, PG_SONAR_CONFIG, 1);
  77
  78 #ifndef RANGEFINDER_HCSR04_TRIGGER_PIN
  79 #define RANGEFINDER_HCSR04_TRIGGER_PIN NONE
  80 #endif
  81 #ifndef RANGEFINDER_HCSR04_ECHO_PIN
  82 #define RANGEFINDER_HCSR04_ECHO_PIN NONE
  83 #endif
  84
  85 PG_RESET_TEMPLATE(sonarConfig_t, sonarConfig,
  86     .triggerTag = IO_TAG(RANGEFINDER_HCSR04_TRIGGER_PIN),
  87     .echoTag = IO_TAG(RANGEFINDER_HCSR04_ECHO_PIN),
  88 );
  89 #endif
  90
  91 /*
  92  * Detect which rangefinder is present
  93  */
  94 static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwareToUse)
  95 {
  96     rangefinderType_e rangefinderHardware = RANGEFINDER_NONE;
  97     requestedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderHardwareToUse;
  98
  99     switch (rangefinderHardwareToUse) {
 100         case RANGEFINDER_HCSR04:
 101 #ifdef USE_RANGEFINDER_HCSR04
 102             {
 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));
 106                 }
 107             }
 108 #endif
 109             break;
 110
 111         case RANGEFINDER_SRF10:
 112 #ifdef USE_RANGEFINDER_SRF10
 113             if (srf10Detect(dev)) {
 114                 rangefinderHardware = RANGEFINDER_SRF10;
 115                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_SRF10_TASK_PERIOD_MS));
 116             }
 117 #endif
 118             break;
 119
 120             case RANGEFINDER_HCSR04I2C:
 121 #ifdef USE_RANGEFINDER_HCSR04_I2C
 122             if (hcsr04i2c0Detect(dev)) {
 123                 rangefinderHardware = RANGEFINDER_HCSR04I2C;
 124                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_HCSR04_i2C_TASK_PERIOD_MS));
 125             }
 126 #endif
 127             break;
 128
 129             case RANGEFINDER_VL53L0X:
 130 #if defined(USE_RANGEFINDER_VL53L0X)
 131             if (vl53l0xDetect(dev)) {
 132                 rangefinderHardware = RANGEFINDER_VL53L0X;
 133                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VL53L0X_TASK_PERIOD_MS));
 134             }
 135 #endif
 136             break;
 137
 138         case RANGEFINDER_UIB:
 139 #if defined(USE_RANGEFINDER_UIB)
 140             if (uibRangefinderDetect(dev)) {
 141                 rangefinderHardware = RANGEFINDER_UIB;
 142                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_UIB_TASK_PERIOD_MS));
 143             }
 144 #endif
 145             break;
 146
 147         case RANGEFINDER_TFMINI:
 148 #if defined(USE_RANGEFINDER_TF)
 149             if (lidarTFminiDetect(dev)) {
 150                 rangefinderHardware = RANGEFINDER_TFMINI;
 151                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_TF_TASK_PERIOD_MS));
 152             }
 153 #endif
 154             break;
 155
 156         case RANGEFINDER_TF02:
 157 #if defined(USE_RANGEFINDER_TF)
 158             if (lidarTF02Detect(dev)) {
 159                 rangefinderHardware = RANGEFINDER_TF02;
 160                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_TF_TASK_PERIOD_MS));
 161             }
 162 #endif
 163             break;
 164
 165         case RANGEFINDER_NONE:
 166             rangefinderHardware = RANGEFINDER_NONE;
 167             break;
 168     }
 169
 170     if (rangefinderHardware == RANGEFINDER_NONE) {
 171         sensorsClear(SENSOR_RANGEFINDER);
 172         return false;
 173     }
 174
 175     detectedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderHardware;
 176     sensorsSet(SENSOR_RANGEFINDER);
 177     return true;
 178 }
 179
 180 void rangefinderResetDynamicThreshold(void)
 181 {
 182     rangefinder.snrThresholdReached = false;
 183     rangefinder.dynamicDistanceThreshold = 0;
 184 }
 185
 186 bool rangefinderInit(void)
 187 {
 188     if (!rangefinderDetect(&rangefinder.dev, rangefinderConfig()->rangefinder_hardware)) {
 189         return false;
 190     }
 191
 192     rangefinder.dev.init(&rangefinder.dev);
 193     rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
 194     rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
 195     rangefinder.maxTiltCos = cos_approx(DECIDEGREES_TO_RADIANS(rangefinder.dev.detectionConeExtendedDeciDegrees / 2.0f));
 196     rangefinder.lastValidResponseTimeMs = millis();
 197     rangefinder.snr = 0;
 198
 199     rangefinderResetDynamicThreshold();
 200
 201     // XXX Interface to CF/BF legacy(?) altitude estimation code.
 202     // XXX Will be gone once iNav's estimator is ported.
 203     rangefinderMaxRangeCm = rangefinder.dev.maxRangeCm;
 204     rangefinderMaxAltWithTiltCm = rangefinderMaxRangeCm * rangefinder.maxTiltCos;
 205     rangefinderCfAltCm = rangefinder.dev.maxRangeCm / 2 ; // Complimentary Filter altitude
 206
 207     return true;
 208 }
 209
 210 static int32_t applyMedianFilter(int32_t newReading)
 211 {
 212     #define DISTANCE_SAMPLES_MEDIAN 5
 213     static int32_t filterSamples[DISTANCE_SAMPLES_MEDIAN];
 214     static int filterSampleIndex = 0;
 215     static bool medianFilterReady = false;
 216
 217     if (newReading > RANGEFINDER_OUT_OF_RANGE) {// only accept samples that are in range
 218         filterSamples[filterSampleIndex] = newReading;
 219         ++filterSampleIndex;
 220         if (filterSampleIndex == DISTANCE_SAMPLES_MEDIAN) {
 221             filterSampleIndex = 0;
 222             medianFilterReady = true;
 223         }
 224     }
 225     return medianFilterReady ? quickMedianFilter5(filterSamples) : newReading;
 226 }
 227
 228 static int16_t computePseudoSnr(int32_t newReading) {
 229     #define SNR_SAMPLES 5
 230     static int16_t snrSamples[SNR_SAMPLES];
 231     static uint8_t snrSampleIndex = 0;
 232     static int32_t previousReading = RANGEFINDER_OUT_OF_RANGE;
 233     static bool snrReady = false;
 234     int16_t pseudoSnr = 0;
 235
 236     const int delta = newReading - previousReading;
 237     snrSamples[snrSampleIndex] = constrain(delta * delta / 10, 0, 6400);
 238     ++snrSampleIndex;
 239     if (snrSampleIndex == SNR_SAMPLES) {
 240         snrSampleIndex = 0;
 241         snrReady = true;
 242     }
 243
 244     previousReading = newReading;
 245
 246     if (snrReady) {
 247
 248         for (uint8_t i = 0; i < SNR_SAMPLES; i++) {
 249             pseudoSnr += snrSamples[i];
 250         }
 251
 252         return constrain(pseudoSnr, 0, 32000);
 253     } else {
 254         return RANGEFINDER_OUT_OF_RANGE;
 255     }
 256 }
 257
 258 /*
 259  * This is called periodically by the scheduler
 260  */
 261 // XXX Returns timeDelta_t for iNav for pseudo-RT scheduling.
 262 void rangefinderUpdate(timeUs_t currentTimeUs)
 263 {
 264     UNUSED(currentTimeUs);
 265
 266     if (rangefinder.dev.update) {
 267         rangefinder.dev.update(&rangefinder.dev);
 268     }
 269
 270     // return rangefinder.dev.delayMs * 1000;  // to microseconds XXX iNav only
 271 }
 272
 273 bool isSurfaceAltitudeValid() {
 274
 275     /*
 276      * Preconditions: raw and calculated altidude > 0
 277      * SNR lower than threshold
 278      */
 279     if (
 280         rangefinder.calculatedAltitude > 0 &&
 281         rangefinder.rawAltitude > 0 &&
 282         rangefinder.snr < RANGEFINDER_DYNAMIC_THRESHOLD
 283     ) {
 284
 285         /*
 286          * When critical altitude was determined, distance reported by rangefinder
 287          * has to be lower than it to assume healthy readout
 288          */
 289         if (rangefinder.snrThresholdReached) {
 290             return (rangefinder.rawAltitude < rangefinder.dynamicDistanceThreshold);
 291         } else {
 292             return true;
 293         }
 294
 295     } else {
 296         return false;
 297     }
 298
 299 }
 300
 301 /**
 302  * Get the last distance measured by the sonar in centimeters. When the ground is too far away, RANGEFINDER_OUT_OF_RANGE is returned.
 303  */
 304 bool rangefinderProcess(float cosTiltAngle)
 305 {
 306     if (rangefinder.dev.read) {
 307         const int32_t distance = rangefinder.dev.read(&rangefinder.dev);
 308
 309         // If driver reported no new measurement - don't do anything
 310         if (distance == RANGEFINDER_NO_NEW_DATA) {
 311             return false;
 312         }
 313
 314         if (distance >= 0) {
 315             rangefinder.lastValidResponseTimeMs = millis();
 316             rangefinder.rawAltitude = applyMedianFilter(distance);
 317         }
 318         else if (distance == RANGEFINDER_OUT_OF_RANGE) {
 319             rangefinder.lastValidResponseTimeMs = millis();
 320             rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
 321         }
 322         else {
 323             // Invalid response / hardware failure
 324             rangefinder.rawAltitude = RANGEFINDER_HARDWARE_FAILURE;
 325         }
 326
 327         rangefinder.snr = computePseudoSnr(distance);
 328
 329         if (rangefinder.snrThresholdReached == false && rangefinder.rawAltitude > 0) {
 330
 331             if (rangefinder.snr < RANGEFINDER_DYNAMIC_THRESHOLD && rangefinder.dynamicDistanceThreshold < rangefinder.rawAltitude) {
 332                 rangefinder.dynamicDistanceThreshold = rangefinder.rawAltitude * RANGEFINDER_DYNAMIC_FACTOR / 100;
 333             }
 334
 335             if (rangefinder.snr >= RANGEFINDER_DYNAMIC_THRESHOLD) {
 336                 rangefinder.snrThresholdReached = true;
 337             }
 338
 339         }
 340
 341         DEBUG_SET(DEBUG_RANGEFINDER, 3, rangefinder.snr);
 342
 343         DEBUG_SET(DEBUG_RANGEFINDER_QUALITY, 0, rangefinder.rawAltitude);
 344         DEBUG_SET(DEBUG_RANGEFINDER_QUALITY, 1, rangefinder.snrThresholdReached);
 345         DEBUG_SET(DEBUG_RANGEFINDER_QUALITY, 2, rangefinder.dynamicDistanceThreshold);
 346         DEBUG_SET(DEBUG_RANGEFINDER_QUALITY, 3, isSurfaceAltitudeValid());
 347
 348     }
 349     else {
 350         // Bad configuration
 351         rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
 352     }
 353
 354     /**
 355     * Apply tilt correction to the given raw sonar reading in order to compensate for the tilt of the craft when estimating
 356     * the altitude. Returns the computed altitude in centimeters.
 357     *
 358     * When the ground is too far away or the tilt is too large, RANGEFINDER_OUT_OF_RANGE is returned.
 359     */
 360     if (cosTiltAngle < rangefinder.maxTiltCos || rangefinder.rawAltitude < 0) {
 361         rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
 362     } else {
 363         rangefinder.calculatedAltitude = rangefinder.rawAltitude * cosTiltAngle;
 364     }
 365
 366     DEBUG_SET(DEBUG_RANGEFINDER, 1, rangefinder.rawAltitude);
 367     DEBUG_SET(DEBUG_RANGEFINDER, 2, rangefinder.calculatedAltitude);
 368
 369     return true;
 370 }
 371
 372 /**
 373  * Get the latest altitude that was computed, or RANGEFINDER_OUT_OF_RANGE if sonarCalculateAltitude
 374  * has never been called.
 375  */
 376 int32_t rangefinderGetLatestAltitude(void)
 377 {
 378     return rangefinder.calculatedAltitude;
 379 }
 380
 381 int32_t rangefinderGetLatestRawAltitude(void) {
 382     return rangefinder.rawAltitude;
 383 }
 384
 385 bool rangefinderIsHealthy(void)
 386 {
 387     return (millis() - rangefinder.lastValidResponseTimeMs) < RANGEFINDER_HARDWARE_TIMEOUT_MS;
 388 }
 389 #endif
 390