From 1fcaddde6ac533250dfeb5f32d765e9edf37d2a1 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Tue, 11 Feb 2025 23:58:23 +0100 Subject: [PATCH] gps data in msp,blackbox and telem --- lib/Espfc/src/Blackbox/Blackbox.cpp | 12 ++++ lib/Espfc/src/Connect/Cli.cpp | 78 +++++++++++++++++++++++++ lib/Espfc/src/Connect/Cli.hpp | 1 + lib/Espfc/src/Connect/MspProcessor.cpp | 2 +- lib/Espfc/src/ModelConfig.h | 9 +-- lib/Espfc/src/ModelState.h | 40 ++++++++----- lib/Espfc/src/Rc/Crsf.h | 1 + lib/Espfc/src/Sensor/GpsSensor.cpp | 51 ++++++++++------ lib/Espfc/src/Telemetry/TelemetryCRSF.h | 35 ++++++++--- lib/betaflight/src/platform.c | 4 ++ lib/betaflight/src/platform.h | 33 +++++++++++ 11 files changed, 219 insertions(+), 47 deletions(-) diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index 991b276..07409eb 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/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() diff --git a/lib/Espfc/src/Connect/Cli.cpp b/lib/Espfc/src/Connect/Cli.cpp index f38a82b..86c697a 100644 --- a/lib/Espfc/src/Connect/Cli.cpp +++ b/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); diff --git a/lib/Espfc/src/Connect/Cli.hpp b/lib/Espfc/src/Connect/Cli.hpp index 384cd70..22d05d2 100644 --- a/lib/Espfc/src/Connect/Cli.hpp +++ b/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; diff --git a/lib/Espfc/src/Connect/MspProcessor.cpp b/lib/Espfc/src/Connect/MspProcessor.cpp index bdddc9c..af0bbb2 100644 --- a/lib/Espfc/src/Connect/MspProcessor.cpp +++ b/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::max())); // height [m] diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 4f93e54..e20b02e 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/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 diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index e8963f3..a84bfd3 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -340,9 +340,9 @@ struct GpsSupportState template 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 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]; }; diff --git a/lib/Espfc/src/Rc/Crsf.h b/lib/Espfc/src/Rc/Crsf.h index a3768ba..77eb87d 100644 --- a/lib/Espfc/src/Rc/Crsf.h +++ b/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, diff --git a/lib/Espfc/src/Sensor/GpsSensor.cpp b/lib/Espfc/src/Sensor/GpsSensor.cpp index 6ccc8c6..a2bdccd 100644 --- a/lib/Espfc/src/Sensor/GpsSensor.cpp +++ b/lib/Espfc/src/Sensor/GpsSensor.cpp @@ -1,12 +1,13 @@ #include "Sensor/GpsSensor.hpp" #include #include +#include namespace Espfc::Sensor { static constexpr std::array BAUDS{ - 115200, 9600, 230400, 57600, 38400, 19200, + 9600, 115200, 230400, 57600, 38400, 19200, }; static constexpr std::array 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(); - _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 diff --git a/lib/Espfc/src/Telemetry/TelemetryCRSF.h b/lib/Espfc/src/Telemetry/TelemetryCRSF.h index 728ba98..e332743 100644 --- a/lib/Espfc/src/Telemetry/TelemetryCRSF.h +++ b/lib/Espfc/src/Telemetry/TelemetryCRSF.h @@ -4,6 +4,7 @@ #include "Device/SerialDevice.h" #include "Rc/Crsf.h" #include "Utils/Math.hpp" +#include // https://github.com/crsf-wg/crsf/wiki/CRSF_FRAMETYPE_MSP_REQ - not correct // https://github.com/betaflight/betaflight/blob/2525be9a3369fa666d8ce1485ec5ad344326b085/src/main/telemetry/crsf.c#L664 @@ -18,7 +19,7 @@ enum TelemetryState { CRSF_TELEMETRY_STATE_BAT, CRSF_TELEMETRY_STATE_FM, CRSF_TELEMETRY_STATE_GPS, - CRSF_TELEMETRY_STATE_VARIO, + CRSF_TELEMETRY_STATE_BARO, CRSF_TELEMETRY_STATE_HB, }; @@ -50,15 +51,14 @@ class TelemetryCRSF case CRSF_TELEMETRY_STATE_FM: flightMode(f); send(f, s); - //_current = CRSF_TELEMETRY_STATE_GPS; - _current = CRSF_TELEMETRY_STATE_VARIO; + _current = CRSF_TELEMETRY_STATE_GPS; break; case CRSF_TELEMETRY_STATE_GPS: - //gps(f); - //send(f, s); - _current = CRSF_TELEMETRY_STATE_VARIO; + gps(f); + send(f, s); + _current = CRSF_TELEMETRY_STATE_BARO; break; - case CRSF_TELEMETRY_STATE_VARIO: + case CRSF_TELEMETRY_STATE_BARO: vario(f); send(f, s); _current = CRSF_TELEMETRY_STATE_HB; @@ -144,11 +144,28 @@ class TelemetryCRSF msg.finalize(); } + void gps(Rc::CrsfMessage& msg) const + { + msg.prepare(Rc::CRSF_FRAMETYPE_GPS); + + msg.writeU32(Utils::toBigEndian32(_model.state.gps.location.raw.lat)); // deg * 1e7 + msg.writeU32(Utils::toBigEndian32(_model.state.gps.location.raw.lon)); // deg * 1e7 + msg.writeU16(Utils::toBigEndian16(_model.state.gps.velocity.raw.groundSpeed * 36)); // in km/h * 10 + msg.writeU16(Utils::toBigEndian16((_model.state.gps.velocity.raw.heading + 50) / 100)); // deg * 10 + uint16_t altitude = std::clamp((_model.state.gps.location.raw.height + 500) / 1000, (int32_t)0, (int32_t)5000) + 1000; // m + msg.writeU16(Utils::toBigEndian16(altitude)); + msg.writeU8(_model.state.gps.numSats); + + msg.finalize(); + } + void vario(Rc::CrsfMessage& msg) const { - msg.prepare(Rc::CRSF_FRAMETYPE_VARIO_SENSOR); + // https://github.com/crsf-wg/crsf/wiki/CRSF_FRAMETYPE_BARO_ALTITUDE + msg.prepare(Rc::CRSF_FRAMETYPE_BARO_ALTITUDE); - msg.writeU16(Utils::toBigEndian16(0)); + msg.writeU16(Utils::toBigEndian16(0)); // (cm + 10000) or (m + 0 | 0x8000) + msg.writeU16(Utils::toBigEndian16(0)); // cm/s msg.finalize(); } diff --git a/lib/betaflight/src/platform.c b/lib/betaflight/src/platform.c index 9274a8a..b65de36 100644 --- a/lib/betaflight/src/platform.c +++ b/lib/betaflight/src/platform.c @@ -28,6 +28,7 @@ PG_RESET_TEMPLATE_DEF(currentSensorADCConfig_t, currentSensorADCConfig); PG_RESET_TEMPLATE_DEF(rxConfig_t, rxConfig); PG_RESET_TEMPLATE_DEF(positionConfig_t, positionConfig); PG_RESET_TEMPLATE_DEF(dynNotchConfig_t, dynNotchConfig); +PG_RESET_TEMPLATE_DEF(gpsConfig_t, gpsConfig); PG_RESET_TEMPLATE_ARRAY_DEF(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); PG_RESET_TEMPLATE_ARRAY_DEF(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles); @@ -61,6 +62,9 @@ float motor_disarmed[MAX_SUPPORTED_MOTORS]; uint32_t targetPidLooptime; float rcCommand[4]; +int32_t GPS_home[2]; +gpsSolutionData_t gpsSol; + const char* const lookupTableMixerType[] = { "LEGACY", "LINEAR", "DYNAMIC", "EZLANDING", }; diff --git a/lib/betaflight/src/platform.h b/lib/betaflight/src/platform.h index f5e31b1..1419d65 100644 --- a/lib/betaflight/src/platform.h +++ b/lib/betaflight/src/platform.h @@ -10,6 +10,7 @@ #define USE_ITERM_RELAX #define USE_DSHOT_TELEMETRY #define USE_RPM_FILTER +#define USE_GPS #include #include @@ -994,6 +995,38 @@ int16_t getMotorOutputHigh(); uint16_t getDshotErpm(uint8_t i); /* FAILSAFE END */ +typedef enum { + GPS_LATITUDE, + GPS_LONGITUDE +} gpsCoordinateType_e; + +/* LLH Location in NEU axis system */ +typedef struct gpsLocation_s { + int32_t lat; // latitude * 1e+7 + int32_t lon; // longitude * 1e+7 + int32_t altCm; // altitude in 0.01m +} gpsLocation_t; + +typedef struct gpsSolutionData_s { + gpsLocation_t llh; + uint16_t speed3d; // speed in 0.1m/s + uint16_t groundSpeed; // speed in 0.1m/s + uint16_t groundCourse; // degrees * 10 + uint16_t hdop; // generic HDOP value (*100) + uint8_t numSat; +} gpsSolutionData_t; + +typedef struct gpsConfig_s { + uint8_t provider; + bool gps_set_home_point_once; + bool gps_use_3d_speed; +} gpsConfig_t; + +PG_DECLARE(gpsConfig_t, gpsConfig); + +extern int32_t GPS_home[2]; +extern gpsSolutionData_t gpsSol; + #define PARAM_NAME_GYRO_HARDWARE_LPF "gyro_hardware_lpf" #define PARAM_NAME_GYRO_LPF1_TYPE "gyro_lpf1_type" #define PARAM_NAME_GYRO_LPF1_STATIC_HZ "gyro_lpf1_static_hz"