src/main/sensors/pitotmeter.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 <math.h>
  21
  22 #include "platform.h"
  23
  24 #include "common/log.h"
  25 #include "common/maths.h"
  26 #include "common/time.h"
  27 #include "common/utils.h"
  28
  29 #include "config/parameter_group.h"
  30 #include "config/parameter_group_ids.h"
  31
  32 #include "drivers/pitotmeter/pitotmeter.h"
  33 #include "drivers/pitotmeter/pitotmeter_ms4525.h"
  34 #include "drivers/pitotmeter/pitotmeter_dlvr_l10d.h"
  35 #include "drivers/pitotmeter/pitotmeter_adc.h"
  36 #include "drivers/pitotmeter/pitotmeter_msp.h"
  37 #include "drivers/pitotmeter/pitotmeter_virtual.h"
  38 #include "drivers/pitotmeter/pitotmeter_fake.h"
  39 #include "drivers/time.h"
  40
  41 #include "fc/config.h"
  42 #include "fc/runtime_config.h"
  43 #include "fc/settings.h"
  44
  45 #include "scheduler/protothreads.h"
  46
  47 #include "sensors/pitotmeter.h"
  48 #include "sensors/barometer.h"
  49 #include "sensors/sensors.h"
  50
  51
  52 //#include "build/debug.h"
  53
  54
  55 #ifdef USE_PITOT
  56
  57 extern baro_t baro;
  58
  59 pitot_t pitot = {.lastMeasurementUs = 0, .lastSeenHealthyMs = 0};
  60
  61 PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTMETER_CONFIG, 2);
  62
  63 #define PITOT_HARDWARE_TIMEOUT_MS   500     // Accept 500ms of non-responsive sensor, report HW failure otherwise
  64
  65 #ifdef USE_PITOT
  66 #define PITOT_HARDWARE_DEFAULT    PITOT_AUTODETECT
  67 #else
  68 #define PITOT_HARDWARE_DEFAULT    PITOT_NONE
  69 #endif
  70
  71 PG_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig,
  72     .pitot_hardware = SETTING_PITOT_HARDWARE_DEFAULT,
  73     .pitot_lpf_milli_hz = SETTING_PITOT_LPF_MILLI_HZ_DEFAULT,
  74     .pitot_scale = SETTING_PITOT_SCALE_DEFAULT
  75 );
  76
  77 bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse)
  78 {
  79     pitotSensor_e pitotHardware = PITOT_NONE;
  80     requestedSensors[SENSOR_INDEX_PITOT] = pitotHardwareToUse;
  81
  82     switch (pitotHardwareToUse) {
  83         case PITOT_AUTODETECT:
  84         case PITOT_MS4525:
  85 #ifdef USE_PITOT_MS4525
  86             if (ms4525Detect(dev)) {
  87                 pitotHardware = PITOT_MS4525;
  88                 break;
  89             }
  90 #endif
  91             /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
  92             if (pitotHardwareToUse != PITOT_AUTODETECT) {
  93                 break;
  94             }
  95             FALLTHROUGH;
  96
  97         case PITOT_DLVR:
  98
  99                         // Skip autodetection for DLVR (it is indistinguishable from MS4525) and allow only manual config
 100             if (pitotHardwareToUse != PITOT_AUTODETECT && dlvrDetect(dev)) {
 101                 pitotHardware = PITOT_DLVR;
 102                 break;
 103             }
 104             FALLTHROUGH;
 105
 106         case PITOT_ADC:
 107 #if defined(USE_ADC) && defined(USE_PITOT_ADC)
 108             if (adcPitotDetect(dev)) {
 109                 pitotHardware = PITOT_ADC;
 110                 break;
 111             }
 112 #endif
 113             /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 114             if (pitotHardwareToUse != PITOT_AUTODETECT) {
 115                 break;
 116             }
 117             FALLTHROUGH;
 118
 119         case PITOT_VIRTUAL:
 120 #if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL)
 121             if ((pitotHardwareToUse != PITOT_AUTODETECT) && virtualPitotDetect(dev)) {
 122                 pitotHardware = PITOT_VIRTUAL;
 123                 break;
 124             }
 125 #endif
 126             /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 127             if (pitotHardwareToUse != PITOT_AUTODETECT) {
 128                 break;
 129             }
 130             FALLTHROUGH;
 131
 132         case PITOT_MSP:
 133 #ifdef USE_PITOT_MSP
 134             // Skip autodetection for MSP baro, only allow manual config
 135             if (pitotHardwareToUse != PITOT_AUTODETECT && mspPitotmeterDetect(dev)) {
 136                 pitotHardware = PITOT_MSP;
 137                 break;
 138             }
 139 #endif
 140             /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 141             if (pitotHardwareToUse != PITOT_AUTODETECT) {
 142                 break;
 143             }
 144             FALLTHROUGH;
 145
 146         case PITOT_FAKE:
 147 #ifdef USE_PITOT_FAKE
 148             if (fakePitotDetect(dev)) {
 149                 pitotHardware = PITOT_FAKE;
 150                 break;
 151             }
 152 #endif
 153             /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 154             if (pitotHardwareToUse != PITOT_AUTODETECT) {
 155                 break;
 156             }
 157             FALLTHROUGH;
 158
 159         case PITOT_NONE:
 160             pitotHardware = PITOT_NONE;
 161             break;
 162     }
 163
 164     if (pitotHardware == PITOT_NONE) {
 165         sensorsClear(SENSOR_PITOT);
 166         return false;
 167     }
 168
 169     detectedSensors[SENSOR_INDEX_PITOT] = pitotHardware;
 170     sensorsSet(SENSOR_PITOT);
 171     return true;
 172 }
 173
 174 bool pitotInit(void)
 175 {
 176     if (!pitotDetect(&pitot.dev, pitotmeterConfig()->pitot_hardware)) {
 177         return false;
 178     }
 179     return true;
 180 }
 181
 182 bool pitotIsCalibrationComplete(void)
 183 {
 184     return zeroCalibrationIsCompleteS(&pitot.zeroCalibration) && zeroCalibrationIsSuccessfulS(&pitot.zeroCalibration);
 185 }
 186
 187 void pitotStartCalibration(void)
 188 {
 189     zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, SSL_AIR_PRESSURE * pitot.dev.calibThreshold, false);
 190 }
 191
 192 static void performPitotCalibrationCycle(void)
 193 {
 194     zeroCalibrationAddValueS(&pitot.zeroCalibration, pitot.pressure);
 195
 196     if (zeroCalibrationIsCompleteS(&pitot.zeroCalibration)) {
 197         zeroCalibrationGetZeroS(&pitot.zeroCalibration, &pitot.pressureZero);
 198         LOG_DEBUG(PITOT, "Pitot calibration complete (%d)", (int)lrintf(pitot.pressureZero));
 199     }
 200 }
 201
 202 STATIC_PROTOTHREAD(pitotThread)
 203 {
 204     ptBegin(pitotThread);
 205
 206     static float pitotPressureTmp;
 207     static float pitotTemperatureTmp;
 208     timeUs_t currentTimeUs;
 209
 210     // Init filter
 211     pitot.lastMeasurementUs = micros();
 212
 213     pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f);
 214
 215     while(1) {
 216 #ifdef USE_SIMULATOR
 217         while (SIMULATOR_HAS_OPTION(HITL_AIRSPEED) && SIMULATOR_HAS_OPTION(HITL_PITOT_FAILURE))
 218         {
 219             ptDelayUs(10000);
 220         }
 221 #endif
 222
 223         if ( pitot.lastSeenHealthyMs == 0 ) {
 224             if (pitot.dev.start(&pitot.dev)) {
 225                 pitot.lastSeenHealthyMs = millis();
 226             }
 227         }
 228
 229         if ( (millis() - pitot.lastSeenHealthyMs) >= US2MS(pitot.dev.delay)) {
 230             if (pitot.dev.get(&pitot.dev))          // read current data
 231                 pitot.lastSeenHealthyMs = millis();
 232
 233             if (pitot.dev.start(&pitot.dev))        // init for next read
 234                 pitot.lastSeenHealthyMs = millis();
 235         }
 236
 237
 238         pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, &pitotTemperatureTmp);
 239
 240 #ifdef USE_SIMULATOR
 241         if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
 242             pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;
 243         }
 244 #endif
 245 #if defined(USE_PITOT_FAKE)
 246         if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) {
 247             pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;
 248         }
 249 #endif
 250         ptYield();
 251
 252         // Calculate IAS
 253         if (pitotIsCalibrationComplete()) {
 254             // NOTE ::
 255             // https://en.wikipedia.org/wiki/Indicated_airspeed
 256             // Indicated airspeed (IAS) is the airspeed read directly from the airspeed indicator on an aircraft, driven by the pitot-static system.
 257             // The IAS is an important value for the pilot because it is the indicated speeds which are specified in the aircraft flight manual for
 258             // such important performance values as the stall speed. A typical aircraft will always stall at the same indicated airspeed (for the current configuration)
 259             // regardless of density, altitude or true airspeed.
 260             //
 261             // Therefore we shouldn't care about CAS/TAS and only calculate IAS since it's more indicative to the pilot and more useful in calculations
 262             // It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale
 263
 264             // NOTE ::filter pressure - apply filter when NOT calibrating for zero !!!
 265             currentTimeUs = micros();
 266             pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
 267             pitot.lastMeasurementUs = currentTimeUs;
 268
 269             pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;  // cm/s
 270             pitot.temperature = pitotTemperatureTmp;   // Kelvin
 271
 272         } else {
 273             pitot.pressure = pitotPressureTmp;
 274             performPitotCalibrationCycle();
 275             pitot.airSpeed = 0.0f;
 276         }
 277
 278 #if defined(USE_PITOT_FAKE)
 279         if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) {
 280             pitot.airSpeed = fakePitotGetAirspeed();
 281         }
 282 #endif
 283 #ifdef USE_SIMULATOR
 284         if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
 285             pitot.airSpeed = simulatorData.airSpeed;
 286         }
 287 #endif
 288     }
 289
 290     ptEnd(0);
 291 }
 292
 293 void pitotUpdate(void)
 294 {
 295     pitotThread();
 296 }
 297
 298 float getAirspeedEstimate(void)
 299 {
 300     return pitot.airSpeed;
 301 }
 302
 303 bool pitotIsHealthy(void)
 304 {
 305     return (millis() - pitot.lastSeenHealthyMs) < PITOT_HARDWARE_TIMEOUT_MS;
 306 }
 307
 308 #endif /* PITOT */