gps data in msp,blackbox and telem

lib/Espfc/src/Blackbox/Blackbox.cpp

@@ -191,6 +191,10 @@ int Blackbox::begin()
   rpmFilterConfigMutable()->rpm_filter_weights[1] = _model.config.gyro.rpmFilter.weights[1];
   rpmFilterConfigMutable()->rpm_filter_weights[2] = _model.config.gyro.rpmFilter.weights[2];
 
+  gpsConfigMutable()->provider = 1; // ubx
+  gpsConfigMutable()->gps_set_home_point_once = false;
+  gpsConfigMutable()->gps_use_3d_speed = false;
+
   updateModeFlag(&rcModeActivationPresent, BOXARM, _model.state.mode.maskPresent & 1 << MODE_ARMED);
   updateModeFlag(&rcModeActivationPresent, BOXANGLE, _model.state.mode.maskPresent & 1 << MODE_ANGLE);
   updateModeFlag(&rcModeActivationPresent, BOXAIRMODE, _model.state.mode.maskPresent & 1 << MODE_AIRMODE);
@@ -270,6 +274,14 @@ void FAST_CODE_ATTR Blackbox::updateData()
       debug[i] = _model.state.debug[i];
     }
   }
+  GPS_home[0] = _model.state.gps.location.home.lat;
+  GPS_home[1] = _model.state.gps.location.home.lon;
+  gpsSol.llh.lat = _model.state.gps.location.raw.lat;
+  gpsSol.llh.lon = _model.state.gps.location.raw.lon;
+  gpsSol.llh.altCm = (_model.state.gps.location.raw.height + 50) / 100; // 0.1 m
+  gpsSol.groundSpeed = (_model.state.gps.velocity.raw.groundSpeed + 5) / 10; // cm/s
+  gpsSol.groundCourse = (_model.state.gps.velocity.raw.heading + 5000) / 10000; // 0.1 deg
+  gpsSol.numSat = _model.state.gps.numSats;
 }
 
 void FAST_CODE_ATTR Blackbox::updateArmed()

lib/Espfc/src/Connect/Cli.cpp

@@ -1038,6 +1038,10 @@ void Cli::execute(CliCmd& cmd, Stream& s)
       s.println(F("OK"));
     }
   }
+  else if(strcmp_P(cmd.args[0], PSTR("gps")) == 0)
+  {
+    printGpsStatus(s, true);
+  }
   else if(strcmp_P(cmd.args[0], PSTR("preset")) == 0)
   {
     if(!cmd.args[1])
@@ -1430,6 +1434,80 @@ void Cli::print(const Param& param, Stream& s) const
   s.println();
 }
 
+void Cli::printGpsStatus(Stream& s, bool full) const
+{
+  s.println(F("GPS STATUS:"));
+
+  s.print(F("Fix: "));
+  s.print(_model.state.gps.fix);
+  s.print(F(" ("));
+  s.print(_model.state.gps.fixType);
+  s.println(F(")"));
+
+  s.print(F("Lat: "));
+  s.print(_model.state.gps.location.raw.lat);
+  s.print(F(" ("));
+  s.print((float)_model.state.gps.location.raw.lat * 1e-7f, 7);
+  s.print(F(" deg)"));
+  s.println();
+
+  s.print(F("Lon: "));
+  s.print(_model.state.gps.location.raw.lon);
+  s.print(F(" ("));
+  s.print((float)_model.state.gps.location.raw.lon * 1e-7f, 7);
+  s.print(F(" deg)"));
+  s.println();
+
+  s.print(F("Hei: "));
+  s.print(_model.state.gps.location.raw.height);
+  s.print(F(" ("));
+  s.print((float)_model.state.gps.location.raw.height * 0.001f);
+  s.print(F(" m)"));
+  s.println();
+
+  s.print(F("Acc: "));
+  s.print((float)_model.state.gps.accuracy.horizontal * 0.001f);
+  s.print(F(" m, "));
+  s.print((float)_model.state.gps.accuracy.vertical * 0.001f);
+  s.print(F(" m, "));
+  s.print((float)_model.state.gps.accuracy.speed * 0.001f);
+  s.print(F(" m/s, "));
+  s.print((float)_model.state.gps.accuracy.heading * 0.00001f);
+  s.print(F(" deg, pDOP: "));
+  s.print((float)_model.state.gps.accuracy.pDop * 0.01f);
+  s.println();
+
+  s.print(F("Tim: "));
+  s.print(_model.state.gps.dateTime.year);
+  s.print(F("-"));
+  s.print(_model.state.gps.dateTime.month);
+  s.print(F("-"));
+  s.print(_model.state.gps.dateTime.day);
+  s.print(F(" "));
+  s.print(_model.state.gps.dateTime.hour);
+  s.print(F(":"));
+  s.print(_model.state.gps.dateTime.minute);
+  s.print(F(":"));
+  s.print(_model.state.gps.dateTime.second);
+  s.println(F(" UTC"));
+
+  s.print(F("Sat: "));
+  s.print(_model.state.gps.numSats);
+  s.print(F(" ("));
+  s.print(_model.state.gps.numCh);
+  s.println(F(" ch)"));
+  for (size_t i = 0; i < _model.state.gps.numCh; i++)
+  {
+    s.print(_model.state.gps.svinfo[i].gnssId);
+    s.print(' ');
+    s.print(_model.state.gps.svinfo[i].id);
+    s.print(' ');
+    s.print(_model.state.gps.svinfo[i].quality);
+    s.print(' ');
+    s.println(_model.state.gps.svinfo[i].cno);
+  }
+}
+
 void Cli::printVersion(Stream& s) const
 {
   s.print(boardIdentifier);

lib/Espfc/src/Connect/Cli.hpp

@@ -93,6 +93,7 @@ class Cli
 
 private:
   void print(const Param& param, Stream& s) const;
+  void printGpsStatus(Stream& s, bool full) const;
   void printVersion(Stream& s) const;
   void printStats(Stream& s) const;
 

lib/Espfc/src/Connect/MspProcessor.cpp

@@ -1469,7 +1469,7 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD
 
   case MSP_RAW_GPS:
       r.writeU8(_model.state.gps.fixType > 2); // STATE(GPS_FIX));
-      r.writeU8(_model.state.gps.numSats); //gpsSol.numSat
+      r.writeU8(_model.state.gps.numSats); // numSat
       r.writeU32(_model.state.gps.location.raw.lat); // lat
       r.writeU32(_model.state.gps.location.raw.lon); // lon
       r.writeU16(std::clamp((int)_model.state.gps.location.raw.height / 1000, 0, (int)std::numeric_limits<uint16_t>::max())); // height [m]

lib/Espfc/src/ModelConfig.h

@@ -651,12 +651,7 @@ struct ControllerConfig
 
 struct GpsConfig
 {
-  uint8_t provider;
-  uint8_t sbasMode;
-  uint8_t autoConfig;
-  uint8_t autoBaud;
-  uint8_t setHomeOnce;
-  uint8_t useGalileo;
+  uint8_t minSats = 8;
 };
 
 // persistent data
@@ -673,6 +668,7 @@ class ModelConfig
     FusionConfig fusion;
     VBatConfig vbat;
     IBatConfig ibat;
+    GpsConfig gps;
 
     ActuatorCondition conditions[ACTUATOR_CONDITIONS];
     ScalerConfig scaler[SCALER_COUNT];
@@ -695,7 +691,6 @@ class ModelConfig
     DtermConfig dterm;
     ItermConfig iterm;
     ControllerConfig controller;
-
     // hardware
     int8_t pin[PIN_COUNT] = {
 #ifdef ESPFC_INPUT

lib/Espfc/src/ModelState.h

@@ -340,9 +340,9 @@ struct GpsSupportState
 template<typename T>
 struct GpsCoordinate
 {
-  T lat = T{};
-  T lon = T{};
-  T height = T{};
+  T lat = T{}; // deg * 1e7
+  T lon = T{}; // deg * 1e7
+  T height = T{}; // mm (1e3)
 };
 
 struct GpsPosition
@@ -354,11 +354,12 @@ struct GpsPosition
 template<typename T>
 struct GpsSpeed
 {
-  T north = T{};
-  T east = T{};
-  T down = T{};
-  T groundSpeed = T{};
-  T heading = T{};
+  T north = T{}; // mm/s (1e3)
+  T east = T{}; // mm/s (1e3)
+  T down = T{}; // mm/s (1e3)
+  T groundSpeed = T{}; // mm/s (1e3)
+  T heading = T{}; // deg * 1e5
+  T speed3d = T{}; // mm/s (1e3)
 };
 
 struct GpsVelocity
@@ -368,11 +369,11 @@ struct GpsVelocity
 
 struct GpsAccuracy
 {
-  uint32_t horizontal = 0;
-  uint32_t vertical = 0;
-  uint32_t speed = 0;
-  uint32_t heading = 0;
-  uint32_t pDop = 0;
+  uint32_t horizontal = 0; // mm (1e3)
+  uint32_t vertical = 0; // mm (1e3)
+  uint32_t speed = 0; // mm/s (1e3)
+  uint32_t heading = 0; // deg * 1e5
+  uint32_t pDop = 0; // (1e2)
 };
 
 struct GpsSatelite
@@ -383,10 +384,22 @@ struct GpsSatelite
   uint8_t quality = 0;
 };
 
+struct GpsDateTime
+{
+  uint16_t year; // full year
+  uint8_t month; // 1-12
+  uint8_t day; // 1-31
+  uint8_t hour; // 0-23
+  uint8_t minute; // 0-59
+  uint8_t second; // 0-59
+  uint16_t msec; // 0-999
+};
+
 static constexpr size_t SAT_MAX = 32u;
 
 struct GpsState
 {
+  bool fix = 0;
   uint8_t fixType = 0;
   uint8_t numSats = 0;
   uint8_t numCh = 0;
@@ -395,6 +408,7 @@ struct GpsState
   GpsPosition location;
   GpsVelocity velocity;
   GpsAccuracy accuracy;
+  GpsDateTime dateTime;
   GpsSatelite svinfo[SAT_MAX];
 };
 

lib/Espfc/src/Rc/Crsf.h

@@ -33,6 +33,7 @@ enum {
     CRSF_FRAMETYPE_GPS = 0x02,
     CRSF_FRAMETYPE_VARIO_SENSOR = 0x07,
     CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
+    CRSF_FRAMETYPE_BARO_ALTITUDE = 0x09,
     CRSF_FRAMETYPE_HEARTBEAT = 0x0B,
     CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
     CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,

lib/Espfc/src/Sensor/GpsSensor.cpp

@@ -1,12 +1,13 @@
 #include "Sensor/GpsSensor.hpp"
 #include <GpsProtocol.hpp>
 #include <Arduino.h>
+#include <cmath>
 
 namespace Espfc::Sensor
 {
 
 static constexpr std::array<int, 6> BAUDS{
-  115200, 9600, 230400, 57600, 38400, 19200,
+  9600, 115200, 230400, 57600, 38400, 19200,
 };
 
 static constexpr std::array<uint16_t, 6> NMEA_MSG_OFF{
@@ -256,7 +257,7 @@ void GpsSensor::handle()
 void GpsSensor::setBaud(int baud)
 {
   _port->updateBadRate(baud);
-  _model.logger.info().log(F("GPS BAUD")).logln(BAUDS[_counter]);
+  _model.logger.info().log(F("GPS BAUD")).logln(baud);
 }
 
 void GpsSensor::setState(State state, State ackState, State timeoutState)
@@ -280,26 +281,42 @@ void GpsSensor::handleError() const
 void GpsSensor::handleNavPvt() const
 {
   const auto &m = *_ubxMsg.getAs<Gps::UbxNavPvt92>();
-  _model.state.gps.location.raw.lat = m.lat;
-  _model.state.gps.location.raw.lon = m.lon;
-  _model.state.gps.location.raw.height = m.height;
-
-  _model.state.gps.velocity.raw.north = m.velN;
-  _model.state.gps.velocity.raw.east = m.velE;
-  _model.state.gps.velocity.raw.down = m.velD;
-  _model.state.gps.velocity.raw.groundSpeed = m.gSpeed;
-  _model.state.gps.velocity.raw.heading = m.headMot;
 
+  _model.state.gps.fix = m.fixType == 3 && m.flags.gnssFixOk;
   _model.state.gps.fixType = m.fixType;
   _model.state.gps.numSats = m.numSV;
+
   _model.state.gps.accuracy.pDop = m.pDOP;
+  _model.state.gps.accuracy.horizontal = m.hAcc; // mm
+  _model.state.gps.accuracy.vertical = m.vAcc; // mm
+  _model.state.gps.accuracy.speed = m.sAcc; // mm/s
+  _model.state.gps.accuracy.heading = (m.headAcc + 5000) / 10000; // deg * 0.1
 
-  //_hal->printf("\n");
-  //_hal->printf("Dat: %d-%d-%d %d:%d:%d %d\n", m.year, m.month, m.day, m.hour, m.min, m.sec, m.iTow);
-  //_hal->printf("Pos: %.7f %.7f %.7f %d %d %d\n", 1e-7f * m.lat, 1e-7f * m.lon, 1e-3f * m.height, m.lat, m.lon, m.height);
-  //_hal->printf("Vel: %d %d %d %d %d\n", m.velN, m.velE, m.velD, m.gSpeed, m.headMot);
-  //_hal->printf("Fix: %d %d\n", m.numSV, m.fixType);
-  //_hal->printf("Acu: %d %d %d %d %d\n", m.hAcc, m.vAcc, m.sAcc, m.headAcc, m.pDOP);
+  _model.state.gps.location.raw.lat = m.lat;
+  _model.state.gps.location.raw.lon = m.lon;
+  _model.state.gps.location.raw.height = m.hSML; // mm
+
+  _model.state.gps.velocity.raw.groundSpeed = m.gSpeed; // mm/s
+  _model.state.gps.velocity.raw.heading = m.headMot; // deg * 1e5
+
+  _model.state.gps.velocity.raw.north = m.velN; // mm/s
+  _model.state.gps.velocity.raw.east  = m.velE; // mm/s
+  _model.state.gps.velocity.raw.down  = m.velD; // mm/s
+  _model.state.gps.velocity.raw.speed3d = lrintf(std::sqrt(
+    _model.state.gps.velocity.raw.groundSpeed * _model.state.gps.velocity.raw.groundSpeed +
+    _model.state.gps.velocity.raw.down * _model.state.gps.velocity.raw.down
+  ));
+
+  if(m.valid.validDate && m.valid.validTime)
+  {
+    _model.state.gps.dateTime.year = m.year;
+    _model.state.gps.dateTime.month = m.month;
+    _model.state.gps.dateTime.day = m.day;
+    _model.state.gps.dateTime.hour = m.hour;
+    _model.state.gps.dateTime.minute = m.min;
+    _model.state.gps.dateTime.second = m.sec;
+    _model.state.gps.dateTime.msec = m.nano >= 0 ? m.nano / 1000 : 0;
+  }
 }
 
 void GpsSensor::handleNavSat() const