src/main/drivers/pitotmeter/pitotmeter_msp.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27
  28 #include "platform.h"
  29
  30 #if defined(USE_PITOT_MSP)
  31
  32 #include "build/build_config.h"
  33 #include "build/debug.h"
  34
  35 #include "common/utils.h"
  36 #include "common/time.h"
  37
  38 #include "drivers/time.h"
  39 #include "drivers/pitotmeter/pitotmeter.h"
  40 #include "drivers/pitotmeter/pitotmeter_msp.h"
  41
  42 #include "msp/msp_protocol_v2_sensor_msg.h"
  43
  44 #define MSP_PITOT_TIMEOUT_MS      500     // Less than 2Hz updates is considered a failure
  45
  46 static int32_t mspPitotPressure;
  47 static int32_t mspPitotTemperature;
  48 static timeMs_t mspPitotLastUpdateMs;
  49
  50 static bool mspPitotStart(pitotDev_t *pitot)
  51 {
  52     UNUSED(pitot);
  53     return true;
  54 }
  55
  56 static bool mspPitotRead(pitotDev_t *pitot)
  57 {
  58     UNUSED(pitot);
  59     return true;
  60 }
  61
  62 static void mspPitotCalculate(pitotDev_t *pitot, float *pressure, float *temperature)
  63 {
  64     UNUSED(pitot);
  65
  66     if (pressure) {
  67         *pressure = mspPitotPressure;
  68     }
  69
  70     if (temperature) {
  71         *temperature = (mspPitotTemperature - 27315) / 100.0f;  // Pitot expects temp in Kelvin
  72     }
  73 }
  74
  75 void mspPitotmeterReceiveNewData(uint8_t * bufferPtr)
  76 {
  77     const mspSensorAirspeedDataMessage_t * pkt = (const mspSensorAirspeedDataMessage_t *)bufferPtr;
  78
  79     mspPitotPressure = pkt->diffPressurePa;
  80     mspPitotTemperature = pkt->temp;
  81     mspPitotLastUpdateMs = millis();
  82 }
  83
  84 bool mspPitotmeterDetect(pitotDev_t *pitot)
  85 {
  86     mspPitotPressure = 0;
  87     mspPitotTemperature = 27315;    // 0 deg/c
  88
  89     pitot->delay = 10000;
  90     pitot->calibThreshold = 0.00001f;
  91     pitot->start = mspPitotStart;
  92     pitot->get = mspPitotRead;
  93     pitot->calculate = mspPitotCalculate;
  94
  95     return true;
  96 }
  97
  98 #endif
  99