src/main/drivers/pitotmeter/pitotmeter_ms4525.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/utils.h"
  25 #include "common/maths.h"
  26 #include "drivers/bus_i2c.h"
  27 #include "drivers/time.h"
  28 #include "drivers/pitotmeter/pitotmeter.h"
  29
  30 // MS4525, Standard address 0x28
  31 #define MS4525_ADDR                 0x28
  32
  33 typedef struct __attribute__ ((__packed__)) ms4525Ctx_s {
  34     bool     dataValid;
  35     uint32_t ms4525_ut;
  36     uint32_t ms4525_up;
  37 } ms4525Ctx_t;
  38
  39 STATIC_ASSERT(sizeof(ms4525Ctx_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small);
  40
  41 static bool ms4525_start(pitotDev_t * pitot)
  42 {
  43     uint8_t rxbuf[1];
  44     bool ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 1);
  45     return ack;
  46 }
  47
  48 static bool ms4525_read(pitotDev_t * pitot)
  49 {
  50     uint8_t rxbuf1[4];
  51     uint8_t rxbuf2[4];
  52
  53     ms4525Ctx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
  54     ctx->dataValid = false;
  55
  56     if (!busReadBuf(pitot->busDev, 0xFF, rxbuf1, 4)) {
  57         return false;
  58     }
  59
  60     if (!busReadBuf(pitot->busDev, 0xFF, rxbuf2, 4)) {
  61         return false;
  62     }
  63
  64     const uint8_t status = ((rxbuf1[0] & 0xC0) >> 6);
  65     if (status == 2 || status == 3) {
  66         return false;
  67     }
  68
  69     int16_t dP_raw1, dT_raw1;
  70     int16_t dP_raw2, dT_raw2;
  71
  72     dP_raw1 = 0x3FFF & ((rxbuf1[0] << 8) + rxbuf1[1]);
  73     dT_raw1 = (0xFFE0 & ((rxbuf1[2] << 8) + rxbuf1[3])) >> 5;
  74     dP_raw2 = 0x3FFF & ((rxbuf2[0] << 8) + rxbuf2[1]);
  75     dT_raw2 = (0xFFE0 & ((rxbuf2[2] << 8) + rxbuf2[3])) >> 5;
  76
  77     // reject any values that are the absolute minimum or maximums these
  78     // can happen due to gnd lifts or communication errors on the bus
  79     if (dP_raw1 == 0x3FFF || dP_raw1 == 0 || dT_raw1 == 0x7FF || dT_raw1 == 0 ||
  80         dP_raw2 == 0x3FFF || dP_raw2 == 0 || dT_raw2 == 0x7FF || dT_raw2 == 0) {
  81         return false;
  82     }
  83
  84     // reject any double reads where the value has shifted in the upper more than 0xFF
  85     if (ABS(dP_raw1 - dP_raw2) > 0xFF || ABS(dT_raw1 - dT_raw2) > 0xFF) {
  86         return false;
  87     }
  88
  89     // Data valid, update ut/up values
  90     ctx->dataValid = true;
  91     ctx->ms4525_up = (dP_raw1 + dP_raw2) / 2;
  92     ctx->ms4525_ut = (dT_raw1 + dT_raw2) / 2;
  93
  94     return true;
  95 }
  96
  97 static void ms4525_calculate(pitotDev_t * pitot, float *pressure, float *temperature)
  98 {
  99     ms4525Ctx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
 100
 101     const float P_max = 1.0f;
 102     const float P_min = -P_max;
 103     const float PSI_to_Pa = 6894.757f;
 104
 105     //float dP = ctx->ms4525_up * 10.0f * 0.1052120688f;
 106     const float dP_psi = -((ctx->ms4525_up - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min);
 107     float dP = dP_psi * PSI_to_Pa;
 108     float T  = C_TO_KELVIN((float)(200.0f * (int32_t)ctx->ms4525_ut) / 2047.0f - 50.0f);
 109
 110     if (pressure) {
 111         *pressure = dP;    // Pa
 112     }
 113
 114     if (temperature) {
 115         *temperature = T; // K
 116     }
 117 }
 118
 119 bool ms4525Detect(pitotDev_t * pitot)
 120 {
 121     uint8_t rxbuf[4];
 122     bool ack = false;
 123
 124     pitot->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_MS4525, 0, OWNER_AIRSPEED);
 125     if (pitot->busDev == NULL) {
 126         return false;
 127     }
 128
 129     // Read twice to fix:
 130     // Sending a start-stop condition without any transitions on the SCL line (no clock pulses in between) creates a
 131     // communication error for the next communication, even if the next start condition is correct and the clock pulse is applied.
 132     // An additional start condition must be sent, which results in restoration of proper communication.
 133     ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 4);
 134     ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 4);
 135     if (!ack) {
 136         return false;
 137     }
 138
 139     // Initialize busDev data
 140     ms4525Ctx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
 141     ctx->dataValid = false;
 142     ctx->ms4525_ut = 0;
 143     ctx->ms4525_up = 0;
 144
 145     // Initialize pitotDev object
 146     pitot->delay = 10000;
 147     pitot->calibThreshold = 0.00005f;   // noisy sensor
 148     pitot->start = ms4525_start;
 149     pitot->get = ms4525_read;
 150     pitot->calculate = ms4525_calculate;
 151     return true;
 152 }