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_adc.h"
  35 #include "drivers/pitotmeter/pitotmeter_msp.h"
  36 #include "drivers/pitotmeter/pitotmeter_virtual.h"
  37 #include "drivers/time.h"
  38
  39 #include "fc/config.h"
  40 #include "fc/runtime_config.h"
  41 #include "fc/settings.h"
  42
  43 #include "scheduler/protothreads.h"
  44
  45 #include "sensors/pitotmeter.h"
  46 #include "sensors/sensors.h"
  47
  48 #ifdef USE_PITOT
  49
  50 pitot_t pitot;
  51
  52 PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTMETER_CONFIG, 2);
  53
  54 #define PITOT_HARDWARE_TIMEOUT_MS   500     // Accept 500ms of non-responsive sensor, report HW failure otherwise
  55
  56 #ifdef USE_PITOT
  57 #define PITOT_HARDWARE_DEFAULT    PITOT_AUTODETECT
  58 #else
  59 #define PITOT_HARDWARE_DEFAULT    PITOT_NONE
  60 #endif
  61 PG_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig,
  62     .pitot_hardware = SETTING_PITOT_HARDWARE_DEFAULT,
  63     .pitot_lpf_milli_hz = SETTING_PITOT_LPF_MILLI_HZ_DEFAULT,
  64     .pitot_scale = SETTING_PITOT_SCALE_DEFAULT
  65 );
  66
  67 bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse)
  68 {
  69     pitotSensor_e pitotHardware = PITOT_NONE;
  70     requestedSensors[SENSOR_INDEX_PITOT] = pitotHardwareToUse;
  71
  72     switch (pitotHardwareToUse) {
  73         case PITOT_AUTODETECT:
  74         case PITOT_MS4525:
  75 #ifdef USE_PITOT_MS4525
  76             if (ms4525Detect(dev)) {
  77                 pitotHardware = PITOT_MS4525;
  78                 break;
  79             }
  80 #endif
  81             /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
  82             if (pitotHardwareToUse != PITOT_AUTODETECT) {
  83                 break;
  84             }
  85             FALLTHROUGH;
  86
  87         case PITOT_ADC:
  88 #if defined(USE_ADC) && defined(USE_PITOT_ADC)
  89             if (adcPitotDetect(dev)) {
  90                 pitotHardware = PITOT_ADC;
  91                 break;
  92             }
  93 #endif
  94             /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
  95             if (pitotHardwareToUse != PITOT_AUTODETECT) {
  96                 break;
  97             }
  98             FALLTHROUGH;
  99
 100         case PITOT_VIRTUAL:
 101 #if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL)
 102             if ((pitotHardwareToUse != PITOT_AUTODETECT) && virtualPitotDetect(dev)) {
 103                 pitotHardware = PITOT_VIRTUAL;
 104                 break;
 105             }
 106 #endif
 107             /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 108             if (pitotHardwareToUse != PITOT_AUTODETECT) {
 109                 break;
 110             }
 111             FALLTHROUGH;
 112
 113         case PITOT_MSP:
 114 #ifdef USE_PITOT_MSP
 115             // Skip autodetection for MSP baro, only allow manual config
 116             if (pitotHardwareToUse != PITOT_AUTODETECT && mspPitotmeterDetect(dev)) {
 117                 pitotHardware = PITOT_MSP;
 118                 break;
 119             }
 120 #endif
 121             /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 122             if (pitotHardwareToUse != PITOT_AUTODETECT) {
 123                 break;
 124             }
 125             FALLTHROUGH;
 126
 127         case PITOT_FAKE:
 128 #ifdef USE_PITOT_FAKE
 129             if (fakePitotDetect(dev)) {
 130                 pitotHardware = PITOT_FAKE;
 131                 break;
 132             }
 133 #endif
 134             /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
 135             if (pitotHardwareToUse != PITOT_AUTODETECT) {
 136                 break;
 137             }
 138             FALLTHROUGH;
 139
 140         case PITOT_NONE:
 141             pitotHardware = PITOT_NONE;
 142             break;
 143     }
 144
 145     if (pitotHardware == PITOT_NONE) {
 146         sensorsClear(SENSOR_PITOT);
 147         return false;
 148     }
 149
 150     detectedSensors[SENSOR_INDEX_PITOT] = pitotHardware;
 151     sensorsSet(SENSOR_PITOT);
 152     return true;
 153 }
 154
 155 bool pitotInit(void)
 156 {
 157     if (!pitotDetect(&pitot.dev, pitotmeterConfig()->pitot_hardware)) {
 158         return false;
 159     }
 160     return true;
 161 }
 162
 163 bool pitotIsCalibrationComplete(void)
 164 {
 165     return zeroCalibrationIsCompleteS(&pitot.zeroCalibration) && zeroCalibrationIsSuccessfulS(&pitot.zeroCalibration);
 166 }
 167
 168 void pitotStartCalibration(void)
 169 {
 170     zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, P0 * 0.00001f, false);
 171 }
 172
 173 static void performPitotCalibrationCycle(void)
 174 {
 175     zeroCalibrationAddValueS(&pitot.zeroCalibration, pitot.pressure);
 176
 177     if (zeroCalibrationIsCompleteS(&pitot.zeroCalibration)) {
 178         zeroCalibrationGetZeroS(&pitot.zeroCalibration, &pitot.pressureZero);
 179         LOG_D(PITOT, "Pitot calibration complete (%d)", (int)lrintf(pitot.pressureZero));
 180     }
 181 }
 182
 183 STATIC_PROTOTHREAD(pitotThread)
 184 {
 185     ptBegin(pitotThread);
 186
 187     static float pitotPressureTmp;
 188     timeUs_t currentTimeUs;
 189
 190     // Init filter
 191     pitot.lastMeasurementUs = micros();
 192     pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0);
 193
 194     while(1) {
 195         // Start measurement
 196         if (pitot.dev.start(&pitot.dev)) {
 197             pitot.lastSeenHealthyMs = millis();
 198         }
 199
 200         ptDelayUs(pitot.dev.delay);
 201
 202         // Read and calculate data
 203         if (pitot.dev.get(&pitot.dev)) {
 204             pitot.lastSeenHealthyMs = millis();
 205         }
 206
 207         pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, NULL);
 208         ptYield();
 209
 210         // Filter pressure
 211         currentTimeUs = micros();
 212         pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, (currentTimeUs - pitot.lastMeasurementUs) * 1e-6f);
 213         pitot.lastMeasurementUs = currentTimeUs;
 214         ptDelayUs(pitot.dev.delay);
 215
 216         // Calculate IAS
 217         if (pitotIsCalibrationComplete()) {
 218             // https://en.wikipedia.org/wiki/Indicated_airspeed
 219             // Indicated airspeed (IAS) is the airspeed read directly from the airspeed indicator on an aircraft, driven by the pitot-static system.
 220             // The IAS is an important value for the pilot because it is the indicated speeds which are specified in the aircraft flight manual for
 221             // such important performance values as the stall speed. A typical aircraft will always stall at the same indicated airspeed (for the current configuration)
 222             // regardless of density, altitude or true airspeed.
 223             //
 224             // 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
 225             // It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale
 226             pitot.airSpeed = pitotmeterConfig()->pitot_scale * sqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / AIR_DENSITY_SEA_LEVEL_15C) * 100;
 227         } else {
 228             performPitotCalibrationCycle();
 229             pitot.airSpeed = 0;
 230         }
 231     }
 232
 233     ptEnd(0);
 234 }
 235
 236 void pitotUpdate(void)
 237 {
 238     pitotThread();
 239 }
 240
 241 int32_t pitotCalculateAirSpeed(void)
 242 {
 243     return pitot.airSpeed;
 244 }
 245
 246 bool pitotIsHealthy(void)
 247 {
 248     return (millis() - pitot.lastSeenHealthyMs) < PITOT_HARDWARE_TIMEOUT_MS;
 249 }
 250
 251 #endif /* PITOT */