src/lib/Baro/devBaro.cpp

   1
   2 #include "devBaro.h"
   3
   4 #if defined(HAS_BARO)
   5
   6 #include "CRSF.h"
   7 #include "logging.h"
   8 #include "telemetry.h"
   9 #include "baro_spl06.h"
  10 #include "baro_bmp280.h"
  11 //#include "baro_bmp085.h"
  12
  13 #define BARO_STARTUP_INTERVAL       100
  14
  15 /* Shameful externs */
  16 extern Telemetry telemetry;
  17
  18 /* Local statics */
  19 static BaroBase *baro;
  20 static eBaroReadState BaroReadState;
  21
  22 extern bool i2c_enabled;
  23
  24 static bool Baro_Detect()
  25 {
  26     // I2C Baros
  27     if (i2c_enabled)
  28     {
  29         if (SPL06::detect())
  30         {
  31             DBGLN("Detected baro: SPL06");
  32             baro = new SPL06();
  33             return true;
  34         }
  35         if (BMP280::detect())
  36         {
  37             DBGLN("Detected baro: BMP280");
  38             baro = new BMP280();
  39             return true;
  40         }
  41         // Untested
  42         // if (BMP085::detect())
  43         // {
  44         //     DBGLN("Detected baro: BMP085");
  45         //     baro = new BMP085();
  46         //     return true;
  47         // }
  48         // DBGLN("No baro detected");
  49     } // I2C
  50     return false;
  51 }
  52
  53 static int Baro_Init()
  54 {
  55     baro->initialize();
  56     if (baro->isInitialized())
  57     {
  58         // Slow down Vbat updates to save bandwidth
  59         Vbat_enableSlowUpdate(true);
  60         BaroReadState = brsReadTemp;
  61         return DURATION_IMMEDIATELY;
  62     }
  63
  64     // Did not init, try again later
  65     return BARO_STARTUP_INTERVAL;
  66 }
  67
  68 static void Baro_PublishPressure(uint32_t pressuredPa)
  69 {
  70     static int32_t last_altitude_cm;
  71     static int16_t verticalspd_smoothed;
  72     int32_t altitude_cm = baro->pressureToAltitude(pressuredPa);
  73     int32_t altitude_diff_cm = altitude_cm - last_altitude_cm;
  74     last_altitude_cm = altitude_cm;
  75
  76     static uint32_t last_publish_ms;
  77     uint32_t now = millis();
  78     uint32_t dT_ms = now - last_publish_ms;
  79     last_publish_ms = now;
  80
  81     //DBGLN("%udPa %dcm", pressuredPa, altitude_cm);
  82
  83     if (baro->getAltitudeHome() == BaroBase::ALTITUDE_INVALID)
  84     {
  85         baro->setAltitudeHome(altitude_cm);
  86         // skip sending the first reading because the VSpd will be bonkers
  87         return;
  88     }
  89
  90     CRSF_MK_FRAME_T(crsf_sensor_baro_vario_t) crsfBaro = {0};
  91
  92     // Item: Alt
  93     int32_t relative_altitude_dm = (altitude_cm - baro->getAltitudeHome()) / 10;
  94     if (relative_altitude_dm > (0x7FFF - 10000))
  95     {
  96         // If the altitude would be 0x8000 or higher, send it in meters with the high bit set
  97         crsfBaro.p.altitude = 0x8000 | (uint16_t)(relative_altitude_dm / 10);
  98     }
  99     else
 100     {
 101         // Else it is in decimeters + 10000 (-1000m max negative alt)
 102         crsfBaro.p.altitude = relative_altitude_dm + 10000;
 103     }
 104     crsfBaro.p.altitude = htobe16(crsfBaro.p.altitude);
 105
 106     // Item: VSpd
 107     int16_t vspd = altitude_diff_cm * 1000 / (int32_t)dT_ms;
 108     verticalspd_smoothed = (verticalspd_smoothed * 3 + vspd) / 4; // Simple smoothing
 109     crsfBaro.p.verticalspd = htobe16(verticalspd_smoothed);
 110     //DBGLN("diff=%d smooth=%d dT=%u", altitude_diff_cm, verticalspd_smoothed, dT_ms);
 111
 112     // if no external vario is connected output internal Vspd on CRSF_FRAMETYPE_BARO_ALTITUDE packet
 113     if (!telemetry.GetCrsfBaroSensorDetected())
 114     {
 115         CRSF::SetHeaderAndCrc((uint8_t *)&crsfBaro, CRSF_FRAMETYPE_BARO_ALTITUDE, CRSF_FRAME_SIZE(sizeof(crsf_sensor_baro_vario_t)), CRSF_ADDRESS_CRSF_TRANSMITTER);
 116         telemetry.AppendTelemetryPackage((uint8_t *)&crsfBaro);
 117     }
 118 }
 119
 120 static int start()
 121 {
 122     if (Baro_Detect())
 123     {
 124         BaroReadState = brsUninitialized;
 125         return BARO_STARTUP_INTERVAL;
 126     }
 127
 128     BaroReadState = brsNoBaro;
 129     return DURATION_NEVER;
 130 }
 131
 132 static int timeout()
 133 {
 134     if (connectionState >= MODE_STATES)
 135         return DURATION_NEVER;
 136
 137     switch (BaroReadState)
 138     {
 139         default: // fallthrough
 140         case brsNoBaro:
 141             return DURATION_NEVER;
 142
 143         case brsUninitialized:
 144             return Baro_Init();
 145
 146         case brsWaitingTemp:
 147             {
 148                 int32_t temp = baro->getTemperature();
 149                 if (temp == BaroBase::TEMPERATURE_INVALID)
 150                     return DURATION_IMMEDIATELY;
 151             }
 152             // fallthrough
 153
 154         case brsReadPres:
 155             {
 156                 uint8_t pressDuration = baro->getPressureDuration();
 157                 BaroReadState = brsWaitingPress;
 158                 if (pressDuration != 0)
 159                 {
 160                     baro->startPressure();
 161                     return pressDuration;
 162                 }
 163             }
 164             // fallthrough
 165
 166         case brsWaitingPress:
 167             {
 168                 uint32_t press = baro->getPressure();
 169                 if (press == BaroBase::PRESSURE_INVALID)
 170                     return DURATION_IMMEDIATELY;
 171                 Baro_PublishPressure(press);
 172             }
 173             // fallthrough
 174
 175         case brsReadTemp:
 176             {
 177                 uint8_t tempDuration = baro->getTemperatureDuration();
 178                 if (tempDuration == 0)
 179                 {
 180                     BaroReadState = brsReadPres;
 181                     return DURATION_IMMEDIATELY;
 182                 }
 183                 BaroReadState = brsWaitingTemp;
 184                 baro->startTemperature();
 185                 return tempDuration;
 186             }
 187     }
 188 }
 189
 190 device_t Baro_device = {
 191     .initialize = nullptr,
 192     .start = start,
 193     .event = nullptr,
 194     .timeout = timeout,
 195 };
 196
 197 #endif