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 #include "build/build_config.h"
  26 #include "build/debug.h"
  27
  28 #include "common/maths.h"
  29 #include "common/utils.h"
  30 #include "common/time.h"
  31
  32 #include "config/feature.h"
  33 #include "config/parameter_group.h"
  34 #include "config/parameter_group_ids.h"
  35
  36 #include "drivers/io.h"
  37 #include "drivers/time.h"
  38 #include "drivers/rangefinder/rangefinder.h"
  39 #include "drivers/rangefinder/rangefinder_srf10.h"
  40 #include "drivers/rangefinder/rangefinder_vl53l0x.h"
  41 #include "drivers/rangefinder/rangefinder_vl53l1x.h"
  42 #include "drivers/rangefinder/rangefinder_virtual.h"
  43 #include "drivers/rangefinder/rangefinder_us42.h"
  44 #include "drivers/rangefinder/rangefinder_teraranger_evo.h"
  45 #include "drivers/rangefinder/rangefinder_tof10120_i2c.h"
  46
  47 #include "fc/config.h"
  48 #include "fc/runtime_config.h"
  49 #include "fc/settings.h"
  50
  51 #include "sensors/sensors.h"
  52 #include "sensors/rangefinder.h"
  53 #include "sensors/battery.h"
  54
  55 #include "io/rangefinder.h"
  56
  57 #include "scheduler/scheduler.h"
  58
  59 rangefinder_t rangefinder;
  60
  61 #define RANGEFINDER_HARDWARE_TIMEOUT_MS         500     // Accept 500ms of non-responsive sensor, report HW failure otherwise
  62
  63 #define RANGEFINDER_DYNAMIC_THRESHOLD           600     //Used to determine max. usable rangefinder disatance
  64 #define RANGEFINDER_DYNAMIC_FACTOR              75
  65
  66 #ifdef USE_RANGEFINDER
  67 PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, PG_RANGEFINDER_CONFIG, 3);
  68
  69 PG_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig,
  70     .rangefinder_hardware = SETTING_RANGEFINDER_HARDWARE_DEFAULT,
  71     .use_median_filtering = SETTING_RANGEFINDER_MEDIAN_FILTER_DEFAULT,
  72 );
  73
  74 /*
  75  * Detect which rangefinder is present
  76  */
  77 static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwareToUse)
  78 {
  79     rangefinderType_e rangefinderHardware = RANGEFINDER_NONE;
  80     requestedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderHardwareToUse;
  81
  82     switch (rangefinderHardwareToUse) {
  83         case RANGEFINDER_SRF10:
  84 #ifdef USE_RANGEFINDER_SRF10
  85             if (srf10Detect(dev)) {
  86                 rangefinderHardware = RANGEFINDER_SRF10;
  87                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_SRF10_TASK_PERIOD_MS));
  88             }
  89 #endif
  90             break;
  91
  92             case RANGEFINDER_TERARANGER_EVO:
  93 #if defined(USE_RANGEFINDER_TERARANGER_EVO_I2C)
  94             if (teraRangerDetect(dev)) {
  95                 rangefinderHardware = RANGEFINDER_TERARANGER_EVO;
  96                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_TERA_EVO_TASK_PERIOD_MS));
  97             }
  98 #endif
  99             break;
 100             case RANGEFINDER_VL53L0X:
 101 #if defined(USE_RANGEFINDER_VL53L0X)
 102             if (vl53l0xDetect(dev)) {
 103                 rangefinderHardware = RANGEFINDER_VL53L0X;
 104                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VL53L0X_TASK_PERIOD_MS));
 105             }
 106 #endif
 107             break;
 108
 109             case RANGEFINDER_VL53L1X:
 110 #if defined(USE_RANGEFINDER_VL53L1X)
 111             if (vl53l1xDetect(dev)) {
 112                 rangefinderHardware = RANGEFINDER_VL53L1X;
 113                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VL53L1X_TASK_PERIOD_MS));
 114             }
 115 #endif
 116             break;
 117
 118         case RANGEFINDER_MSP:
 119 #if defined(USE_RANGEFINDER_MSP)
 120             if (virtualRangefinderDetect(dev, &rangefinderMSPVtable)) {
 121                 rangefinderHardware = RANGEFINDER_MSP;
 122                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
 123             }
 124 #endif
 125             break;
 126
 127         case RANGEFINDER_BENEWAKE:
 128 #if defined(USE_RANGEFINDER_BENEWAKE)
 129             if (virtualRangefinderDetect(dev, &rangefinderBenewakeVtable)) {
 130                 rangefinderHardware = RANGEFINDER_BENEWAKE;
 131                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
 132             }
 133 #endif
 134             break;
 135         case RANGEFINDER_USD1_V0:
 136 #if defined(USE_RANGEFINDER_USD1_V0)
 137             if (virtualRangefinderDetect(dev, &rangefinderUSD1Vtable)) {
 138                 rangefinderHardware = RANGEFINDER_USD1_V0;
 139                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
 140             }
 141 #endif
 142             break;
 143         case RANGEFINDER_NANORADAR:
 144 #if defined(USE_RANGEFINDER_NANORADAR)
 145             if (virtualRangefinderDetect(dev, &rangefinderNanoradarVtable)) {
 146                 rangefinderHardware = RANGEFINDER_NANORADAR;
 147                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
 148             }
 149 #endif
 150             break;
 151
 152             case RANGEFINDER_US42:
 153 #ifdef USE_RANGEFINDER_US42
 154             if (us42Detect(dev)) {
 155                 rangefinderHardware = RANGEFINDER_US42;
 156                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_US42_TASK_PERIOD_MS));
 157             }
 158 #endif
 159             break;
 160
 161             case RANGEFINDER_TOF10102I2C:
 162 #ifdef USE_RANGEFINDER_TOF10120_I2C
 163             if (tof10120Detect(dev)) {
 164                 rangefinderHardware = RANGEFINDER_TOF10102I2C;
 165                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_TOF10120_I2C_TASK_PERIOD_MS));
 166             }
 167 #endif
 168             break;
 169             case RANGEFINDER_FAKE:
 170 #if defined(USE_RANGEFINDER_FAKE)
 171             if(virtualRangefinderDetect(dev, &rangefinderFakeVtable)) {
 172                 rangefinderHardware = RANGEFINDER_FAKE;
 173                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
 174             }
 175 #endif
 176             break;
 177             case RANGEFINDER_NONE:
 178             rangefinderHardware = RANGEFINDER_NONE;
 179             break;
 180     }
 181
 182     if (rangefinderHardware == RANGEFINDER_NONE) {
 183         sensorsClear(SENSOR_RANGEFINDER);
 184         return false;
 185     }
 186
 187     detectedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderHardware;
 188     sensorsSet(SENSOR_RANGEFINDER);
 189     return true;
 190 }
 191
 192 bool rangefinderInit(void)
 193 {
 194     if (!rangefinderDetect(&rangefinder.dev, rangefinderConfig()->rangefinder_hardware)) {
 195         return false;
 196     }
 197
 198     rangefinder.dev.init(&rangefinder.dev);
 199     rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
 200     rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
 201     rangefinder.maxTiltCos = cos_approx(DECIDEGREES_TO_RADIANS(rangefinder.dev.detectionConeExtendedDeciDegrees / 2.0f));
 202     rangefinder.lastValidResponseTimeMs = millis();
 203
 204     return true;
 205 }
 206
 207 static int32_t applyMedianFilter(int32_t newReading)
 208 {
 209     #define DISTANCE_SAMPLES_MEDIAN 5
 210     static int32_t filterSamples[DISTANCE_SAMPLES_MEDIAN];
 211     static int filterSampleIndex = 0;
 212     static bool medianFilterReady = false;
 213
 214     if (newReading > RANGEFINDER_OUT_OF_RANGE) {// only accept samples that are in range
 215         filterSamples[filterSampleIndex] = newReading;
 216         ++filterSampleIndex;
 217         if (filterSampleIndex == DISTANCE_SAMPLES_MEDIAN) {
 218             filterSampleIndex = 0;
 219             medianFilterReady = true;
 220         }
 221     }
 222     return medianFilterReady ? quickMedianFilter5(filterSamples) : newReading;
 223 }
 224
 225 /*
 226  * This is called periodically by the scheduler
 227  */
 228 timeDelta_t rangefinderUpdate(void)
 229 {
 230     if (rangefinder.dev.update) {
 231         rangefinder.dev.update(&rangefinder.dev);
 232     }
 233
 234     return MS2US(rangefinder.dev.delayMs);
 235 }
 236
 237 /**
 238  * Get the last distance measured by the sonar in centimeters. When the ground is too far away, RANGEFINDER_OUT_OF_RANGE is returned.
 239  */
 240 bool rangefinderProcess(float cosTiltAngle)
 241 {
 242     if (rangefinder.dev.read) {
 243         const int32_t distance = rangefinder.dev.read(&rangefinder.dev);
 244
 245         // If driver reported no new measurement - don't do anything
 246         if (distance == RANGEFINDER_NO_NEW_DATA) {
 247             return false;
 248         }
 249
 250         if (distance >= 0) {
 251             rangefinder.lastValidResponseTimeMs = millis();
 252             rangefinder.rawAltitude = distance;
 253
 254             if (rangefinderConfig()->use_median_filtering) {
 255                 rangefinder.rawAltitude = applyMedianFilter(rangefinder.rawAltitude);
 256             }
 257         }
 258         else if (distance == RANGEFINDER_OUT_OF_RANGE) {
 259             rangefinder.lastValidResponseTimeMs = millis();
 260             rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
 261         }
 262         else {
 263             // Invalid response / hardware failure
 264             rangefinder.rawAltitude = RANGEFINDER_HARDWARE_FAILURE;
 265         }
 266     }
 267     else {
 268         // Bad configuration
 269         rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
 270     }
 271
 272     /**
 273     * Apply tilt correction to the given raw sonar reading in order to compensate for the tilt of the craft when estimating
 274     * the altitude. Returns the computed altitude in centimeters.
 275     *
 276     * When the ground is too far away or the tilt is too large, RANGEFINDER_OUT_OF_RANGE is returned.
 277     */
 278     if (cosTiltAngle < rangefinder.maxTiltCos || rangefinder.rawAltitude < 0) {
 279         rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
 280     } else {
 281         rangefinder.calculatedAltitude = rangefinder.rawAltitude * cosTiltAngle;
 282     }
 283
 284     return true;
 285 }
 286
 287 /**
 288  * Get the latest altitude that was computed, or RANGEFINDER_OUT_OF_RANGE if sonarCalculateAltitude
 289  * has never been called.
 290  */
 291 int32_t rangefinderGetLatestAltitude(void)
 292 {
 293     return rangefinder.calculatedAltitude;
 294 }
 295
 296 int32_t rangefinderGetLatestRawAltitude(void) {
 297     return rangefinder.rawAltitude;
 298 }
 299
 300 bool rangefinderIsHealthy(void)
 301 {
 302     return (millis() - rangefinder.lastValidResponseTimeMs) < RANGEFINDER_HARDWARE_TIMEOUT_MS;
 303 }
 304 #endif