src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.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
  21 #include <platform.h>
  22 #include <build/debug.h>
  23
  24 #include "common/log.h"
  25 #include "common/utils.h"
  26 #include "common/maths.h"
  27 #include "drivers/bus_i2c.h"
  28 #include "drivers/time.h"
  29 #include "drivers/pitotmeter/pitotmeter.h"
  30
  31 #define DLVR_L10D_ADDR                 0x28     // this var is not used !!!
  32
  33 // //---------------------------------------------------
  34 // // Gas Constant is from Aerodynamics for Engineering Students, Third Edition, E.L.Houghton and N.B.Carruthers
  35 // #define ISA_GAS_CONSTANT 287.26f
  36 // #define ISA_LAPSE_RATE 0.0065f
  37
  38 // // Standard Sea Level values
  39 // // Ref: https://en.wikipedia.org/wiki/Standard_sea_level
  40 // #define SSL_AIR_DENSITY         1.225f // kg/m^3
  41 // #define SSL_AIR_PRESSURE 101325.01576f // Pascal
  42 // #define SSL_AIR_TEMPERATURE    288.15f // K
  43 // //---------------------------------------------------
  44
  45 #define INCH_OF_H2O_TO_PASCAL   248.84f
  46
  47 #define INCH_H2O_TO_PASCAL(press) (INCH_OF_H2O_TO_PASCAL * (press))
  48
  49 #define RANGE_INCH_H2O      10
  50 #define DLVR_OFFSET         8192.0f
  51 #define DLVR_SCALE          16384.0f
  52 // NOTE :: DLVR_OFFSET_CORR can be used for offset correction. Now firmware relies on zero calibration
  53 #define DLVR_OFFSET_CORR    0.0f   //-9.0f
  54
  55
  56 typedef struct __attribute__ ((__packed__)) dlvrCtx_s {
  57     bool     dataValid;
  58     uint32_t dlvr_ut;
  59     uint32_t dlvr_up;
  60 } dlvrCtx_t;
  61
  62 STATIC_ASSERT(sizeof(dlvrCtx_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small);
  63
  64 static bool dlvr_start(pitotDev_t * pitot)
  65 {
  66     UNUSED(pitot);
  67     return true;
  68 }
  69
  70 static bool dlvr_read(pitotDev_t * pitot)
  71 {
  72     uint8_t rxbuf1[4];
  73
  74     dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
  75     ctx->dataValid = false;
  76
  77     if (!busReadBuf(pitot->busDev, 0xFF, rxbuf1, 4)) {
  78         return false;
  79     }
  80
  81     // status = 00 -> ok, new data
  82         // status = 01 -> reserved
  83     // status = 10 -> ok, data stale
  84     // status = 11 -> error
  85     const uint8_t status = ((rxbuf1[0] & 0xC0) >> 6);
  86
  87     if (status) {
  88         // anything other then 00 in the status bits is an error
  89         LOG_DEBUG( PITOT, "DLVR: Bad status read. status = %u", (unsigned int)(status) );
  90         return false;
  91     }
  92
  93     int16_t dP_raw1, dT_raw1;
  94
  95     dP_raw1 = 0x3FFF & ((rxbuf1[0] << 8) + rxbuf1[1]);
  96     dT_raw1 = (0xFFE0 & ((rxbuf1[2] << 8) + rxbuf1[3])) >> 5;
  97
  98     // Data valid, update ut/up values
  99     ctx->dataValid = true;
 100     ctx->dlvr_up = dP_raw1;
 101     ctx->dlvr_ut = dT_raw1;
 102
 103     return true;
 104 }
 105
 106 static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperature)
 107 {
 108     dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
 109
 110
 111     // pressure in inchH2O
 112     float dP_inchH2O = 1.25f *  2.0f * RANGE_INCH_H2O  * (((float)ctx->dlvr_up - (DLVR_OFFSET + DLVR_OFFSET_CORR) ) / DLVR_SCALE);
 113
 114     // temperature in deg C
 115     float T_C = (float)ctx->dlvr_ut * (200.0f / 2047.0f) - 50.0f;
 116
 117     // result must fit inside the max pressure range
 118     if ((dP_inchH2O > RANGE_INCH_H2O) || (dP_inchH2O < -RANGE_INCH_H2O)) {
 119         LOG_DEBUG( PITOT,"DLVR: Out of range. pressure = %f", (double)(dP_inchH2O) );
 120         return;
 121     }
 122
 123     if (pressure) {
 124         *pressure = INCH_H2O_TO_PASCAL( dP_inchH2O);   // Pa
 125     }
 126
 127     if (temperature) {
 128         *temperature = C_TO_KELVIN( T_C); // K
 129     }
 130 }
 131
 132 bool dlvrDetect(pitotDev_t * pitot)
 133 {
 134     uint8_t rxbuf[4];
 135     bool ack = false;
 136
 137     pitot->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_DLVR, 0, OWNER_AIRSPEED);
 138     if (pitot->busDev == NULL) {
 139         return false;
 140     }
 141
 142     // Read twice to fix:
 143     // Sending a start-stop condition without any transitions on the SCL line (no clock pulses in between) creates a
 144     // communication error for the next communication, even if the next start condition is correct and the clock pulse is applied.
 145     // An additional start condition must be sent, which results in restoration of proper communication.
 146     ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 4);
 147     ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 4);
 148     if (!ack) {
 149         return false;
 150     }
 151
 152     // Initialize busDev data
 153     dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
 154     ctx->dataValid = false;
 155     ctx->dlvr_ut = 0;
 156     ctx->dlvr_up = 0;
 157
 158     // Initialize pitotDev object
 159     pitot->delay = 10000;
 160     pitot->calibThreshold = 0.00001f;   // low noise sensor
 161     pitot->start = dlvr_start;
 162     pitot->get = dlvr_read;
 163     pitot->calculate = dlvr_calculate;
 164     return true;
 165 }