src/lib/Baro/devBaro.cpp

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