src/main/drivers/barometer/barometer_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_BARO_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/barometer/barometer.h"
  40 #include "drivers/barometer/barometer_msp.h"
  41
  42 #include "sensors/sensors.h"
  43 #include "sensors/barometer.h"
  44 #include "fc/runtime_config.h"
  45
  46 #include "msp/msp_protocol_v2_sensor_msg.h"
  47
  48 #define MSP_BARO_TIMEOUT_MS      250     // Less than 4Hz updates is considered a failure
  49
  50 static int32_t mspBaroPressure;
  51 static int32_t mspBaroTemperature;
  52 static timeMs_t mspBaroLastUpdateMs;
  53
  54 static bool mspBaroStarted = false;
  55
  56 static bool mspBaroStartGet(baroDev_t * baro)
  57 {
  58     UNUSED(baro);
  59     return true;
  60 }
  61
  62 static bool mspBaroCalculate(baroDev_t * baro, int32_t *pressure, int32_t *temperature)
  63 {
  64     UNUSED(baro);
  65
  66     if (((millis() - mspBaroLastUpdateMs) > MSP_BARO_TIMEOUT_MS)) {
  67         sensorsClear(SENSOR_BARO);
  68         mspBaroStarted = false;
  69         return false;
  70     }
  71
  72     if (pressure)
  73         *pressure = mspBaroPressure;
  74
  75     if (temperature)
  76         *temperature = mspBaroTemperature;
  77
  78     return true;
  79 }
  80
  81 void mspBaroReceiveNewData(uint8_t * bufferPtr)
  82 {
  83     const mspSensorBaroDataMessage_t * pkt = (const mspSensorBaroDataMessage_t *)bufferPtr;
  84
  85     mspBaroPressure = pkt->pressurePa;
  86     mspBaroTemperature = pkt->temp;
  87     mspBaroLastUpdateMs = millis();
  88
  89     // This should only happen after a reset (!ARMING_FLAG(WAS_EVER_ARMED)) to avoid
  90     // getting calibrations mid-air or on a surface that is above the home position
  91     if (mspBaroStarted == false && !ARMING_FLAG(WAS_EVER_ARMED)){
  92         baroStartCalibration();
  93         mspBaroStarted = true;
  94         sensorsSet(SENSOR_BARO);
  95     }
  96 }
  97
  98 bool mspBaroDetect(baroDev_t *baro)
  99 {
 100     mspBaroPressure = 101325;    // pressure in Pa (0m MSL)
 101     mspBaroTemperature = 2500;   // temperature in 0.01 C = 25 deg
 102     mspBaroLastUpdateMs = 0;
 103
 104     // these are dummy as temperature is measured as part of pressure
 105     baro->ut_delay = 10000;
 106     baro->get_ut = mspBaroStartGet;
 107     baro->start_ut = mspBaroStartGet;
 108
 109     // only _up part is executed, and gets both temperature and pressure
 110     baro->up_delay = 10000;
 111     baro->start_up = mspBaroStartGet;
 112     baro->get_up = mspBaroStartGet;
 113
 114     baro->calculate = mspBaroCalculate;
 115
 116     return true;
 117 }
 118
 119 #endif