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/time.h"
  35 #include "common/utils.h"
  36
  37 #include "config/config.h"
  38 #include "config/feature.h"
  39
  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"
  45
  46 #include "fc/runtime_config.h"
  47
  48 #include "pg/pg.h"
  49 #include "pg/pg_ids.h"
  50
  51 #include "scheduler/scheduler.h"
  52
  53 #include "sensors/sensors.h"
  54 #include "sensors/rangefinder.h"
  55 #include "sensors/battery.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 #if !defined(USE_RANGEFINDER_HCSR04) && !defined(USE_RANGEFINDER_TF)
  96     UNUSED(dev);
  97 #endif
  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_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));
 116             }
 117 #endif
 118             break;
 119
 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));
 125             }
 126 #endif
 127             break;
 128
 129         case RANGEFINDER_NONE:
 130             rangefinderHardware = RANGEFINDER_NONE;
 131             break;
 132     }
 133
 134     if (rangefinderHardware == RANGEFINDER_NONE) {
 135         sensorsClear(SENSOR_RANGEFINDER);
 136         return false;
 137     }
 138
 139     detectedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderHardware;
 140     sensorsSet(SENSOR_RANGEFINDER);
 141     return true;
 142 }
 143
 144 void rangefinderResetDynamicThreshold(void)
 145 {
 146     rangefinder.snrThresholdReached = false;
 147     rangefinder.dynamicDistanceThreshold = 0;
 148 }
 149
 150 bool rangefinderInit(void)
 151 {
 152     if (!rangefinderDetect(&rangefinder.dev, rangefinderConfig()->rangefinder_hardware)) {
 153         return false;
 154     }
 155
 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();
 161     rangefinder.snr = 0;
 162
 163     rangefinderResetDynamicThreshold();
 164
 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
 170
 171     return true;
 172 }
 173
 174 static int32_t applyMedianFilter(int32_t newReading)
 175 {
 176     #define DISTANCE_SAMPLES_MEDIAN 5
 177     static int32_t filterSamples[DISTANCE_SAMPLES_MEDIAN];
 178     static int filterSampleIndex = 0;
 179     static bool medianFilterReady = false;
 180
 181     if (newReading > RANGEFINDER_OUT_OF_RANGE) {// only accept samples that are in range
 182         filterSamples[filterSampleIndex] = newReading;
 183         ++filterSampleIndex;
 184         if (filterSampleIndex == DISTANCE_SAMPLES_MEDIAN) {
 185             filterSampleIndex = 0;
 186             medianFilterReady = true;
 187         }
 188     }
 189     return medianFilterReady ? quickMedianFilter5(filterSamples) : newReading;
 190 }
 191
 192 static int16_t computePseudoSnr(int32_t newReading)
 193 {
 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;
 200
 201     const int delta = newReading - previousReading;
 202     snrSamples[snrSampleIndex] = constrain(delta * delta / 10, 0, 6400);
 203     ++snrSampleIndex;
 204     if (snrSampleIndex == SNR_SAMPLES) {
 205         snrSampleIndex = 0;
 206         snrReady = true;
 207     }
 208
 209     previousReading = newReading;
 210
 211     if (snrReady) {
 212
 213         for (uint8_t i = 0; i < SNR_SAMPLES; i++) {
 214             pseudoSnr += snrSamples[i];
 215         }
 216
 217         return constrain(pseudoSnr, 0, 32000);
 218     } else {
 219         return RANGEFINDER_OUT_OF_RANGE;
 220     }
 221 }
 222
 223 /*
 224  * This is called periodically by the scheduler
 225  */
 226 void rangefinderUpdate(void)
 227 {
 228     if (rangefinder.dev.update) {
 229         rangefinder.dev.update(&rangefinder.dev);
 230     }
 231 }
 232
 233 bool isSurfaceAltitudeValid()
 234 {
 235
 236     /*
 237      * Preconditions: raw and calculated altidude > 0
 238      * SNR lower than threshold
 239      */
 240     if (
 241         rangefinder.calculatedAltitude > 0 &&
 242         rangefinder.rawAltitude > 0 &&
 243         rangefinder.snr < RANGEFINDER_DYNAMIC_THRESHOLD
 244     ) {
 245
 246         /*
 247          * When critical altitude was determined, distance reported by rangefinder
 248          * has to be lower than it to assume healthy readout
 249          */
 250         if (rangefinder.snrThresholdReached) {
 251             return (rangefinder.rawAltitude < rangefinder.dynamicDistanceThreshold);
 252         } else {
 253             return true;
 254         }
 255
 256     } else {
 257         return false;
 258     }
 259
 260 }
 261
 262 /**
 263  * Get the last distance measured by the sonar in centimeters. When the ground is too far away, RANGEFINDER_OUT_OF_RANGE is returned.
 264  */
 265 bool rangefinderProcess(float cosTiltAngle)
 266 {
 267     if (rangefinder.dev.read) {
 268         const int32_t distance = rangefinder.dev.read(&rangefinder.dev);
 269
 270         // If driver reported no new measurement - don't do anything
 271         if (distance == RANGEFINDER_NO_NEW_DATA) {
 272             return false;
 273         }
 274
 275         if (distance >= 0) {
 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;
 281         }
 282         else {
 283             // Invalid response / hardware failure
 284             rangefinder.rawAltitude = RANGEFINDER_HARDWARE_FAILURE;
 285         }
 286
 287         rangefinder.snr = computePseudoSnr(distance);
 288
 289         if (rangefinder.snrThresholdReached == false && rangefinder.rawAltitude > 0) {
 290
 291             if (rangefinder.snr < RANGEFINDER_DYNAMIC_THRESHOLD && rangefinder.dynamicDistanceThreshold < rangefinder.rawAltitude) {
 292                 rangefinder.dynamicDistanceThreshold = rangefinder.rawAltitude * RANGEFINDER_DYNAMIC_FACTOR / 100;
 293             }
 294
 295             if (rangefinder.snr >= RANGEFINDER_DYNAMIC_THRESHOLD) {
 296                 rangefinder.snrThresholdReached = true;
 297             }
 298
 299         }
 300
 301         DEBUG_SET(DEBUG_RANGEFINDER, 3, rangefinder.snr);
 302
 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());
 307
 308     }
 309     else {
 310         // Bad configuration
 311         rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
 312     }
 313
 314     /**
 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.
 317     *
 318     * When the ground is too far away or the tilt is too large, RANGEFINDER_OUT_OF_RANGE is returned.
 319     */
 320     if (cosTiltAngle < rangefinder.maxTiltCos || rangefinder.rawAltitude < 0) {
 321         rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
 322     } else {
 323         rangefinder.calculatedAltitude = rangefinder.rawAltitude * cosTiltAngle;
 324     }
 325
 326     DEBUG_SET(DEBUG_RANGEFINDER, 1, rangefinder.rawAltitude);
 327     DEBUG_SET(DEBUG_RANGEFINDER, 2, rangefinder.calculatedAltitude);
 328
 329     return true;
 330 }
 331
 332 /**
 333  * Get the latest altitude that was computed, or RANGEFINDER_OUT_OF_RANGE if sonarCalculateAltitude
 334  * has never been called.
 335  */
 336 int32_t rangefinderGetLatestAltitude(void)
 337 {
 338     return rangefinder.calculatedAltitude;
 339 }
 340
 341 int32_t rangefinderGetLatestRawAltitude(void)
 342 {
 343     return rangefinder.rawAltitude;
 344 }
 345
 346 bool rangefinderIsHealthy(void)
 347 {
 348     return (millis() - rangefinder.lastValidResponseTimeMs) < RANGEFINDER_HARDWARE_TIMEOUT_MS;
 349 }
 350 #endif
 351