src/main/sensors/rangefinder.c

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