diff --git a/docs/connections.md b/docs/connections.md index cb0793e4..7aa7ebe8 100644 --- a/docs/connections.md +++ b/docs/connections.md @@ -59,11 +59,11 @@ https://github.com/esp8266/esp8266-wiki/wiki/Boot-Process#esp-boot-modes ``` Pin | Name | Function | ESPFC external device ----+------+--------------------------------------------+---------------------------------------------- - 0 | | PU,ADC2_CH1,CLK_OUT1 | > BUZZER + 0 | | PU,ADC2_CH1,CLK_OUT1 | > BUTTON 1 | TXD | U0TXD,CLK_OUT3 | TX0, PROG, MSP - 2 | | ADC2_CH2,HSPIWP,HS2_DATA0,SD_DATA0 | > M5 + 2 | | ADC2_CH2,HSPIWP,HS2_DATA0,SD_DATA0 | > LED 3 | RXD | U0RXD,CLK_OUT2 | RX0, PROG, MSP - 4 | | ADC2_CH0,HSPIHD,HS2_DATA1,SD_DATA1 | $ M2 + 4 | | ADC2_CH0,HSPIHD,HS2_DATA1,SD_DATA1 | M2 5 | | VSPICS0,HS1_DATA6 | > SPI_CS0_GYRO, SPI0_SS 12 | TD1 | PD,ADC2_CH5,MTDI,HSPIQ,HS2_DATA2,SD_DATA2 | > M3, SPI1_MISO 13 | TCK | ADC2_CH4,MTCK,HSPID,HS2_DATA3,SD_DATA3 | SPI_CS1_BARO, SPI1_MOSI diff --git a/lib/EscDriver/src/EscDriverBase.cpp b/lib/EscDriver/src/EscDriverBase.cpp index c3cf4aea..66fde71a 100644 --- a/lib/EscDriver/src/EscDriverBase.cpp +++ b/lib/EscDriver/src/EscDriverBase.cpp @@ -1,6 +1,22 @@ #include "EscDriverBase.hpp" #include +const char * const * EscDriverBase::getProtocolNames() +{ + static const char * const protocols[] = { + PSTR("PWM"), PSTR("ONESHOT125"), PSTR("ONESHOT42"), PSTR("MULTISHOT"), PSTR("BRUSHED"), + PSTR("DSHOT150"), PSTR("DSHOT300"), PSTR("DSHOT600"), PSTR("PROSHOT1000"), PSTR("DISABLED"), + nullptr + }; + return protocols; +} + +const char * const EscDriverBase::getProtocolName(EscProtocol protocol) +{ + if(protocol >= ESC_PROTOCOL_COUNT) return PSTR("?"); + return getProtocolNames()[protocol]; +} + uint16_t IRAM_ATTR EscDriverBase::dshotConvert(uint16_t pulse) { return pulse > 1000 ? PWM_TO_DSHOT(pulse) : 0; diff --git a/lib/EscDriver/src/EscDriverBase.hpp b/lib/EscDriver/src/EscDriverBase.hpp index c8c590a6..26407724 100644 --- a/lib/EscDriver/src/EscDriverBase.hpp +++ b/lib/EscDriver/src/EscDriverBase.hpp @@ -55,6 +55,9 @@ class EscDriverBase static uint32_t convertToErpm(uint32_t value); static uint32_t convertToValue(uint32_t value); + static const char * const * getProtocolNames(); + static const char * const getProtocolName(EscProtocol protocol); + #if defined(UNIT_TEST) int begin(const EscConfig& conf) { return 1; } void end() {} diff --git a/lib/EscDriver/src/EscDriverEsp32.cpp b/lib/EscDriver/src/EscDriverEsp32.cpp index 065af8ab..e9902696 100644 --- a/lib/EscDriver/src/EscDriverEsp32.cpp +++ b/lib/EscDriver/src/EscDriverEsp32.cpp @@ -2,7 +2,7 @@ #include #include "EscDriverEsp32.h" -#include "Debug_Espfc.h" +//#include "Debug_Espfc.h" #include "soc/rmt_periph.h" #include "hal/rmt_ll.h" #include "hal/gpio_ll.h" diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index 991b2761..07409eb2 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/Blackbox/BlackboxBridge.cpp b/lib/Espfc/src/Blackbox/BlackboxBridge.cpp index 15fff807..887579f8 100644 --- a/lib/Espfc/src/Blackbox/BlackboxBridge.cpp +++ b/lib/Espfc/src/Blackbox/BlackboxBridge.cpp @@ -45,12 +45,11 @@ failsafePhase_e failsafePhase() return (failsafePhase_e)(*_model_ptr).state.failsafe.phase; } -static uint32_t activeFeaturesLatch = 0; static uint32_t enabledSensors = 0; bool featureIsEnabled(uint32_t mask) { - return activeFeaturesLatch & mask; + return featureConfigMutable()->enabledFeatures & mask; } void sensorsSet(uint32_t mask) diff --git a/lib/Espfc/src/Blackbox/BlackboxSerialBuffer.h b/lib/Espfc/src/Blackbox/BlackboxSerialBuffer.h index 6df25cf0..8f8b5106 100644 --- a/lib/Espfc/src/Blackbox/BlackboxSerialBuffer.h +++ b/lib/Espfc/src/Blackbox/BlackboxSerialBuffer.h @@ -22,45 +22,47 @@ class BlackboxSerialBuffer: public Device::SerialDevice _data = nullptr; } - virtual void wrap(Espfc::Device::SerialDevice * s) + void updateBadRate(int baud) override { }; + + void wrap(Espfc::Device::SerialDevice * s) { _dev = s; _data = new uint8_t[SIZE]; } - virtual void begin(const Espfc::SerialDeviceConfig& conf) + void begin(const Espfc::SerialDeviceConfig& conf) override { //_dev->begin(conf); } - virtual size_t write(uint8_t c) + size_t write(uint8_t c) override { _data[_idx++] = c; if(_idx >= SIZE) flush(); return 1; } - virtual void flush() + void flush() override { if(_dev) _dev->write(_data, _idx); _idx = 0; } - virtual int availableForWrite() + int availableForWrite() override { //return _dev->availableForWrite(); return SIZE - _idx; } - virtual bool isTxFifoEmpty() + bool isTxFifoEmpty() override { //return _dev->isTxFifoEmpty(); return _idx == 0; } - virtual int available() { return _dev->available(); } - virtual int read() { return _dev->read(); } - virtual size_t readMany(uint8_t * c, size_t l) { + int available() override { return _dev->available(); } + int read() override { return _dev->read(); } + size_t readMany(uint8_t * c, size_t l) override { #if defined(ARCH_RP2040) size_t count = std::min(l, (size_t)available()); for(size_t i = 0; i < count; i++) @@ -72,9 +74,9 @@ class BlackboxSerialBuffer: public Device::SerialDevice return _dev->readMany(c, l); #endif } - virtual int peek() { return _dev->peek(); } + int peek() override { return _dev->peek(); } - virtual size_t write(const uint8_t * c, size_t l) + size_t write(const uint8_t * c, size_t l) override { for(size_t i = 0; i < l; i++) { @@ -82,9 +84,8 @@ class BlackboxSerialBuffer: public Device::SerialDevice } return l; } - virtual bool isSoft() const { return false; }; - virtual operator bool() const { return (bool)(*_dev); } - + bool isSoft() const override { return false; }; + operator bool() const override { return (bool)(*_dev); } Device::SerialDevice * _dev; size_t _idx; diff --git a/lib/Espfc/src/Connect/Buzzer.cpp b/lib/Espfc/src/Connect/Buzzer.cpp index 2ce8c669..6aa7f2fa 100644 --- a/lib/Espfc/src/Connect/Buzzer.cpp +++ b/lib/Espfc/src/Connect/Buzzer.cpp @@ -108,7 +108,7 @@ const uint8_t** Buzzer::schemes() beeperBatteryCritical, //BUZZER_BAT_LOW, // Warning beeps when battery is getting low (repeats) beeperBatteryLow, - //BUZZER_GPS_STATUS, // FIXME **** Disable beeper when connected to USB **** + //BUZZER_GPS_STATUS, // Num beeps = num Gps Sats, FIXME **** Disable beeper when connected to USB **** beeperSilence, //BUZZER_RX_SET, // Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled beeperRxLost, @@ -117,7 +117,7 @@ const uint8_t** Buzzer::schemes() //BUZZER_ACC_CALIBRATION_FAIL, // ACC inflight calibration failed beeperSilence, //BUZZER_READY_BEEP, // Ring a tone when GPS is locked and ready - beeperSilence, + beeperGyroCalibrated, //BUZZER_MULTI_BEEPS, // Internal value used by 'beeperConfirmationBeeps()'. beeperSilence, //BUZZER_DISARM_REPEAT, // Beeps sounded while stick held in disarm position diff --git a/lib/Espfc/src/Connect/Cli.cpp b/lib/Espfc/src/Connect/Cli.cpp index d7a2cd07..38493d89 100644 --- a/lib/Espfc/src/Connect/Cli.cpp +++ b/lib/Espfc/src/Connect/Cli.cpp @@ -326,6 +326,8 @@ const Cli::Param * Cli::initialize(ModelConfig& c) const char ** magDevChoices = Device::MagDevice::getNames(); const char ** fusionModeChoices = FusionConfig::getModeNames(); + static const char * const * protocolChoices = EscDriver::getProtocolNames(); + static const char* gyroDlpfChoices[] = { PSTR("256Hz"), PSTR("188Hz"), PSTR("98Hz"), PSTR("42Hz"), PSTR("20Hz"), PSTR("10Hz"), PSTR("5Hz"), PSTR("EXPERIMENTAL"), NULL }; static const char* debugModeChoices[] = { PSTR("NONE"), PSTR("CYCLETIME"), PSTR("BATTERY"), PSTR("GYRO_FILTERED"), PSTR("ACCELEROMETER"), PSTR("PIDLOOP"), PSTR("GYRO_SCALED"), PSTR("RC_INTERPOLATION"), PSTR("ANGLERATE"), PSTR("ESC_SENSOR"), PSTR("SCHEDULER"), PSTR("STACK"), PSTR("ESC_SENSOR_RPM"), PSTR("ESC_SENSOR_TMP"), PSTR("ALTITUDE"), PSTR("FFT"), @@ -345,8 +347,6 @@ const Cli::Param * Cli::initialize(ModelConfig& c) PSTR("DUALCOPTER"), PSTR("SINGLECOPTER"), PSTR("ATAIL4"), PSTR("CUSTOM"), PSTR("CUSTOMAIRPLANE"), PSTR("CUSTOMTRI"), PSTR("QUADX1234"), NULL }; static const char* interpolChoices[] = { PSTR("NONE"), PSTR("DEFAULT"), PSTR("AUTO"), PSTR("MANUAL"), NULL }; - static const char* protocolChoices[] = { PSTR("PWM"), PSTR("ONESHOT125"), PSTR("ONESHOT42"), PSTR("MULTISHOT"), PSTR("BRUSHED"), - PSTR("DSHOT150"), PSTR("DSHOT300"), PSTR("DSHOT600"), PSTR("PROSHOT1000"), PSTR("DISABLED"), NULL }; static const char* inputRateTypeChoices[] = { PSTR("BETAFLIGHT"), PSTR("RACEFLIGHT"), PSTR("KISS"), PSTR("ACTUAL"), PSTR("QUICK"), NULL }; static const char* throtleLimitTypeChoices[] = { PSTR("NONE"), PSTR("SCALE"), PSTR("CLIP"), NULL }; static const char* inputFilterChoices[] = { PSTR("INTERPOLATION"), PSTR("FILTER"), NULL }; @@ -360,6 +360,7 @@ const Cli::Param * Cli::initialize(ModelConfig& c) size_t i = 0; static const Param params[] = { + Param(PSTR("feature_gps"), &c.featureMask, 7), Param(PSTR("feature_dyn_notch"), &c.featureMask, 29), Param(PSTR("feature_motor_stop"), &c.featureMask, 4), Param(PSTR("feature_rx_ppm"), &c.featureMask, 0), @@ -428,6 +429,9 @@ const Cli::Param * Cli::initialize(ModelConfig& c) Param(PSTR("baro_lpf_type"), &c.baro.filter.type, filterTypeChoices), Param(PSTR("baro_lpf_freq"), &c.baro.filter.freq), + Param(PSTR("gps_min_sats"), &c.gps.minSats), + Param(PSTR("gps_set_home_once"), &c.gps.setHomeOnce), + Param(PSTR("board_align_roll"), &c.boardAlignment[0]), Param(PSTR("board_align_pitch"), &c.boardAlignment[1]), Param(PSTR("board_align_yaw"), &c.boardAlignment[2]), @@ -633,7 +637,9 @@ const Cli::Param * Cli::initialize(ModelConfig& c) #if ESPFC_OUTPUT_COUNT > 7 Param(PSTR("pin_output_7"), &c.pin[PIN_OUTPUT_7]), #endif + Param(PSTR("pin_button"), &c.pin[PIN_BUTTON]), Param(PSTR("pin_buzzer"), &c.pin[PIN_BUZZER]), + Param(PSTR("pin_led"), &c.pin[PIN_LED_BLINK]), #if defined(ESPFC_SERIAL_0) && defined(ESPFC_SERIAL_REMAP_PINS) Param(PSTR("pin_serial_0_tx"), &c.pin[PIN_SERIAL_0_TX]), Param(PSTR("pin_serial_0_rx"), &c.pin[PIN_SERIAL_0_RX]), @@ -665,6 +671,7 @@ const Cli::Param * Cli::initialize(ModelConfig& c) Param(PSTR("pin_spi_cs_2"), &c.pin[PIN_SPI_CS2]), #endif Param(PSTR("pin_buzzer_invert"), &c.buzzer.inverted), + Param(PSTR("pin_led_invert"), &c.led.invert), #ifdef ESPFC_I2C_0 Param(PSTR("i2c_speed"), &c.i2cSpeed), @@ -850,16 +857,16 @@ void Cli::execute(CliCmd& cmd, Stream& s) if(strcmp_P(cmd.args[0], PSTR("help")) == 0) { - static const char * helps[] = { + static const char * const helps[] = { PSTR("available commands:"), PSTR(" help"), PSTR(" dump"), PSTR(" get param"), PSTR(" set param value ..."), PSTR(" cal [gyro]"), PSTR(" defaults"), PSTR(" save"), PSTR(" reboot"), PSTR(" scaler"), PSTR(" mixer"), - PSTR(" stats"), PSTR(" status"), PSTR(" devinfo"), PSTR(" version"), PSTR(" logs"), + PSTR(" stats"), PSTR(" status"), PSTR(" devinfo"), PSTR(" version"), PSTR(" logs"), PSTR(" gps"), //PSTR(" load"), PSTR(" eeprom"), //PSTR(" fsinfo"), PSTR(" fsformat"), PSTR(" log"), - NULL + nullptr }; - for(const char ** ptr = helps; *ptr; ptr++) + for(const char * const * ptr = helps; *ptr; ptr++) { s.println(FPSTR(*ptr)); } @@ -911,15 +918,16 @@ void Cli::execute(CliCmd& cmd, Stream& s) printVersion(s); s.println(); - s.print(F("config size: ")); - s.println(sizeof(ModelConfig)); - - s.print(F(" free heap: ")); - s.println(targetFreeHeap()); - - s.print(F(" cpu freq: ")); + s.print(F("cpu freq: ")); s.print(targetCpuFreq()); s.println(F(" MHz")); + + s.print(F(" memory: ")); + s.print(sizeof(ModelConfig)); + s.print(F(", ")); + s.print(sizeof(ModelState)); + s.print(F(", ")); + s.println(targetFreeHeap()); } else if(strcmp_P(cmd.args[0], PSTR("get")) == 0) { @@ -1037,6 +1045,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]) @@ -1192,7 +1204,7 @@ void Cli::execute(CliCmd& cmd, Stream& s) } else { - s.print(F("NO_GYRO")); + s.print(F("NO GYRO")); } if(baro) @@ -1202,10 +1214,6 @@ void Cli::execute(CliCmd& cmd, Stream& s) s.print('/'); s.print(FPSTR(Device::BusDevice::getName(baro->getBus()->getType()))); } - else - { - s.print(F(", NO_BARO")); - } if(mag) { @@ -1214,9 +1222,10 @@ void Cli::execute(CliCmd& cmd, Stream& s) s.print('/'); s.print(FPSTR(Device::BusDevice::getName(mag->getBus()->getType()))); } - else + + if(_model.state.gps.present) { - s.print(F(", NO_MAG")); + s.print(F(", GPS")); } s.println(); @@ -1238,7 +1247,7 @@ void Cli::execute(CliCmd& cmd, Stream& s) }; const size_t armingDisableNamesLength = sizeof(armingDisableNames) / sizeof(armingDisableNames[0]); - s.print(F("arming flags:")); + s.print(F(" arm flags:")); for(size_t i = 0; i < armingDisableNamesLength; i++) { if(_model.state.mode.armingDisabledFlags & (1 << i)) { @@ -1429,6 +1438,116 @@ void Cli::print(const Param& param, Stream& s) const s.println(); } +static constexpr const char * const gnssNames[] = {" GPS", "SBAS", "GALI", "BEID", "IMES", "QZSS", "GLON"}; +static constexpr const char * const qualityNames[] = {"no_signal", "searching", "acquired", "unusable", "locked", "fully_locked", "fully_locked", "fully_locked"}; +static constexpr const char * const usedNames[] = {" No", "Yes"}; + +static const char * const getGnssName(size_t num) +{ + constexpr size_t gnssNamesMax = sizeof(gnssNames) / sizeof(gnssNames[0]); + if(num < gnssNamesMax) return gnssNames[num]; + return "?"; +} + +static const char * const getQualityName(size_t num) +{ + constexpr size_t qualityNamesMax = sizeof(qualityNames) / sizeof(qualityNames[0]); + if(num < qualityNamesMax) return qualityNames[num]; + return "?"; +} + +static const char * const getUsedName(size_t num) +{ + constexpr size_t usedNamesMax = sizeof(usedNames) / sizeof(usedNames[0]); + if(num < usedNamesMax) return usedNames[num]; + return "?"; +} + +void Cli::printGpsStatus(Stream& s, bool full) const +{ +#ifndef UNIT_TEST + 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(_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(_model.state.gps.location.raw.lon * 1e-7f, 7); + s.print(F(" deg)")); + s.println(); + + s.print(F("Height: ")); + s.print(_model.state.gps.location.raw.height); + s.print(F(" (")); + s.print(_model.state.gps.location.raw.height * 0.001f); + s.print(F(" m)")); + s.println(); + + s.print(F(" Speed: ")); + s.print(_model.state.gps.velocity.raw.groundSpeed); + s.print(F(" (")); + s.print(_model.state.gps.velocity.raw.groundSpeed * 0.001f); + s.print(F(" m/s, ")); + s.print(_model.state.gps.velocity.raw.groundSpeed * 0.0036f); + s.print(F(" km/h)")); + s.println(); + + s.print(F(" Head: ")); + s.print(_model.state.gps.velocity.raw.heading); + s.print(F(" (")); + s.print(_model.state.gps.velocity.raw.heading * 0.00001f); + s.print(F(" deg)")); + s.println(); + + s.print(F(" Accu: ")); + s.print(_model.state.gps.accuracy.horizontal * 0.001f); + s.print(F(" m, ")); + s.print(_model.state.gps.accuracy.vertical * 0.001f); + s.print(F(" m, ")); + s.print(_model.state.gps.accuracy.speed * 0.001f); + s.print(F(" m/s, ")); + s.print(_model.state.gps.accuracy.heading * 0.00001f); + s.print(F(" deg, pDOP: ")); + s.print(_model.state.gps.accuracy.pDop * 0.01f); + s.println(); + + const GpsDateTime& gdt = _model.state.gps.dateTime; + s.printf(" Time: %04d-%02d-%02d %02d:%02d:%02d.%03d UTC", gdt.year, gdt.month, gdt.day, gdt.hour, gdt.minute, gdt.second, gdt.msec); + s.println(); + + s.print(F(" Rate: ")); + s.print(1000000.0f / _model.state.gps.interval, 1); + s.println(F(" Hz")); + + s.print(F(" Sats: ")); + s.print(_model.state.gps.numSats); + s.print(F(" (")); + s.print(_model.state.gps.numCh); + s.println(F(" ch)")); + + s.printf("GNSS ID Sig Used Quality"); + s.println(); + for (size_t i = 0; i < _model.state.gps.numCh; i++) + { + const GpsSatelite& sv = _model.state.gps.svinfo[i]; + s.printf("%s %3d %3d %s %s", getGnssName(sv.gnssId), sv.id, sv.cno, getUsedName(sv.quality.svUsed), getQualityName(sv.quality.qualityInd)); + s.println(); + } +#endif +} + 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 384cd70e..573b0c32 100644 --- a/lib/Espfc/src/Connect/Cli.hpp +++ b/lib/Espfc/src/Connect/Cli.hpp @@ -33,7 +33,7 @@ class Cli Param(): Param(NULL, PARAM_NONE, NULL, NULL) {} Param(const Param& p): Param(p.name, p.type, p.addr, p.choices) {} - Param(const char * n, ParamType t, char * a, const char ** c, size_t l = 16): name(n), type(t), addr(a), choices(c), maxLen(l) {} + Param(const char * n, ParamType t, char * a, const char * const * c, size_t l = 16): name(n), type(t), addr(a), choices(c), maxLen(l) {} Param(const char * n, bool * a): Param(n, PARAM_BOOL, reinterpret_cast(a), NULL) {} Param(const char * n, int8_t * a): Param(n, PARAM_BYTE, reinterpret_cast(a), NULL) {} @@ -41,7 +41,7 @@ class Cli Param(const char * n, int16_t * a): Param(n, PARAM_SHORT, reinterpret_cast(a), NULL) {} Param(const char * n, int32_t * a): Param(n, PARAM_INT, reinterpret_cast(a), NULL) {} - Param(const char * n, int8_t * a, const char ** c): Param(n, PARAM_BYTE, reinterpret_cast(a), c) {} + Param(const char * n, int8_t * a, const char * const * c): Param(n, PARAM_BYTE, reinterpret_cast(a), c) {} Param(const char * n, int32_t * a, uint8_t b): Param(n, PARAM_BITMASK, reinterpret_cast(a), NULL, b) {} Param(const char * n, InputChannelConfig * a): Param(n, PARAM_INPUT_CHANNEL, reinterpret_cast(a), NULL) {} @@ -81,7 +81,7 @@ class Cli const char * name; ParamType type; char * addr; - const char ** choices; + const char * const * choices; size_t maxLen; }; @@ -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 3940f7bc..f8bf9124 100644 --- a/lib/Espfc/src/Connect/MspProcessor.cpp +++ b/lib/Espfc/src/Connect/MspProcessor.cpp @@ -1,6 +1,8 @@ #include "Connect/MspProcessor.hpp" #include "Hardware.h" #include +#include +#include #if defined(ESPFC_MULTI_CORE) && defined(ESPFC_FREE_RTOS) #include #endif @@ -237,7 +239,7 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD r.writeU16(_model.state.stats.loopTime()); r.writeU16(_model.state.i2cErrorCount); // i2c error count // acc, baro, mag, gps, sonar, gyro - r.writeU16(_model.accelActive() | _model.baroActive() << 1 | _model.magActive() << 2 | 0 << 3 | 0 << 4 | _model.gyroActive() << 5); + r.writeU16(_model.accelActive() | _model.baroActive() << 1 | _model.magActive() << 2 | _model.gpsActive() << 3 | 0 << 4 | _model.gyroActive() << 5); r.writeU32(_model.state.mode.mask); // flight mode flags r.writeU8(0); // pid profile r.writeU16(lrintf(_model.state.stats.getCpuLoad())); @@ -1040,16 +1042,6 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD _model.reload(); break; - case MSP_GPS_CONFIG: - r.writeU8(0); // provider - r.writeU8(0); // sbasMode - r.writeU8(0); // autoConfig - r.writeU8(0); // autoBaud - // 1.43+ - r.writeU8(0); // gps_set_home_point_once - r.writeU8(0); // gps_ublox_use_galileo - break; - //case MSP_COMPASS_CONFIG: // r.writeU16(0); // mag_declination * 10 // break; @@ -1328,6 +1320,10 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD break; case MSP_SET_MOTOR: + if(_model.isFeatureActive(FEATURE_GPS) && _model.config.blackbox.mode > 0) + { + _model.setGpsHome(true); + } for(size_t i = 0; i < OUTPUT_CHANNELS; i++) { _model.state.output.disarmed[i] = m.readU16(); @@ -1453,6 +1449,56 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD } break; + case MSP_SET_GPS_CONFIG: + m.readU8(); // provider + m.readU8(); // sbas mode + m.readU8(); // auto config + m.readU8(); // auto baud + if (m.remain() >= 2) { + // Added in API version 1.43 + _model.config.gps.setHomeOnce = m.readU8(); // gps_set_home_point_once + m.readU8(); // gps_ublox_use_galileo + } + break; + + case MSP_GPS_CONFIG: + r.writeU8(1); // provider + r.writeU8(0); // sbasMode, 0: auto + r.writeU8(1); // autoConfig, 0: off, 1: on + r.writeU8(1); // autoBaud, 0: off, 1: on + // Added in API version 1.43 + r.writeU8(_model.config.gps.setHomeOnce); // gps_set_home_point_once + r.writeU8(1); // gps_ublox_use_galileo + break; + + case MSP_RAW_GPS: + r.writeU8(_model.state.gps.fixType > 2); // STATE(GPS_FIX)); + 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] + r.writeU16(_model.state.gps.velocity.raw.groundSpeed / 10); // cm/s + r.writeU16(_model.state.gps.velocity.raw.heading / 10000); // deg * 10 + // Added in API version 1.44 + r.writeU16(_model.state.gps.accuracy.pDop); // pDOP + break; + + case MSP_COMP_GPS: + r.writeU16(0); // GPS_distanceToHome + r.writeU16(0); // GPS_directionToHome / 10 // resolution increased in Betaflight 4.4 by factor of 10, this maintains backwards compatibility for DJI OSD + r.writeU8(0); // GPS_update & 1 // direct or msp + break; + + case MSP_GPSSVINFO: + r.writeU8(_model.state.gps.numCh); // GPS_numCh + for (size_t i = 0; i < _model.state.gps.numCh; i++) { + r.writeU8(_model.state.gps.svinfo[i].gnssId); // GPS_svinfo_chn[i] + r.writeU8(_model.state.gps.svinfo[i].id); // GPS_svinfo_svid[i] + r.writeU8(static_cast(_model.state.gps.svinfo[i].quality.value & 0xff)); // GPS_svinfo_quality[i] + r.writeU8(_model.state.gps.svinfo[i].cno); // GPS_svinfo_cno[i] + } + break; + case MSP_EEPROM_WRITE: _model.save(); break; diff --git a/lib/Espfc/src/Connect/StatusLed.cpp b/lib/Espfc/src/Connect/StatusLed.cpp new file mode 100644 index 00000000..e1865fb5 --- /dev/null +++ b/lib/Espfc/src/Connect/StatusLed.cpp @@ -0,0 +1,80 @@ +#include "StatusLed.hpp" +#include + +namespace Espfc::Connect +{ + +static int LED_OFF_PATTERN[] = {0}; +static int LED_OK_PATTERN[] = {100, 900, 0}; +static int LED_ERROR_PATTERN[] = {100, 100, 100, 100, 100, 1500, 0}; +static int LED_ON_PATTERN[] = {100, 0}; + +StatusLed::StatusLed() : _pin(-1), _invert(0), _status(LED_OFF), _next(0), _state(LOW), _step(0), _pattern(LED_OFF_PATTERN) {} + +void StatusLed::begin(int8_t pin, uint8_t invert) +{ + if(pin == -1) return; + _pin = pin; + _invert = invert; + pinMode(_pin, OUTPUT); + setStatus(LED_ON, true); +} + +void StatusLed::setStatus(LedStatus newStatus, bool force) +{ + if(_pin == -1) return; + if(!force && newStatus == _status) return; + + _status = newStatus; + _state = LOW; + _step = 0; + _next = millis(); + + switch (_status) + { + case LED_OK: + _pattern = LED_OK_PATTERN; + break; + case LED_ERROR: + _pattern = LED_ERROR_PATTERN; + break; + case LED_ON: + _pattern = LED_ON_PATTERN; + _state = HIGH; + break; + case LED_OFF: + default: + _pattern = LED_OFF_PATTERN; + break; + } + _write(_state); +} + +void StatusLed::update() +{ + if(_pin == -1 || !_pattern) return; + + uint32_t now = millis(); + + if(now < _next) return; + + if (!_pattern[_step]) + { + _step = 0; + _next = now + 20; + return; + } + + _state = !(_step & 1); + _write(_state); + + _next = now + _pattern[_step]; + _step++; +} + +void StatusLed::_write(uint8_t val) +{ + digitalWrite(_pin, val ^ _invert); +} + +} diff --git a/lib/Espfc/src/Connect/StatusLed.hpp b/lib/Espfc/src/Connect/StatusLed.hpp new file mode 100644 index 00000000..f2cbd103 --- /dev/null +++ b/lib/Espfc/src/Connect/StatusLed.hpp @@ -0,0 +1,36 @@ +#pragma once +#include +#include + +namespace Espfc::Connect +{ + +enum LedStatus +{ + LED_OFF, + LED_OK, + LED_ERROR, + LED_ON +}; + +class StatusLed +{ + +public: + StatusLed(); + void begin(int8_t pin, uint8_t invert); + void update(); + void setStatus(LedStatus newStatus, bool force = false); + +private: + void _write(uint8_t val); + int8_t _pin; + int8_t _invert; + LedStatus _status; + uint32_t _next; + bool _state; + size_t _step; + int * _pattern; +}; + +} \ No newline at end of file diff --git a/lib/Espfc/src/Control/Actuator.cpp b/lib/Espfc/src/Control/Actuator.cpp index 41fc7015..f9989360 100644 --- a/lib/Espfc/src/Control/Actuator.cpp +++ b/lib/Espfc/src/Control/Actuator.cpp @@ -1,9 +1,7 @@ #include "Control/Actuator.h" #include "Utils/Math.hpp" -namespace Espfc { - -namespace Control { +namespace Espfc::Control { Actuator::Actuator(Model& model): _model(model) {} @@ -37,6 +35,7 @@ int Actuator::update() updateBuzzer(); updateDynLpf(); updateRescueConfig(); + updateLed(); if(_model.config.debug.mode == DEBUG_PIDLOOP) { @@ -90,13 +89,17 @@ void Actuator::updateArmingDisabled() int errors = _model.state.i2cErrorDelta; _model.state.i2cErrorDelta = 0; - _model.setArmingDisabled(ARMING_DISABLED_NO_GYRO, !_model.state.gyro.present || errors); - _model.setArmingDisabled(ARMING_DISABLED_FAILSAFE, _model.state.failsafe.phase != FC_FAILSAFE_IDLE); - _model.setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE, _model.state.input.rxLoss || _model.state.input.rxFailSafe); - _model.setArmingDisabled(ARMING_DISABLED_THROTTLE, !_model.isThrottleLow()); - _model.setArmingDisabled(ARMING_DISABLED_CALIBRATING, _model.calibrationActive()); - _model.setArmingDisabled(ARMING_DISABLED_MOTOR_PROTOCOL, _model.config.output.protocol == ESC_PROTOCOL_DISABLED); + _model.setArmingDisabled(ARMING_DISABLED_NO_GYRO, !_model.state.gyro.present || errors); + _model.setArmingDisabled(ARMING_DISABLED_FAILSAFE, _model.state.failsafe.phase != FC_FAILSAFE_IDLE); + _model.setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE, _model.state.input.rxLoss || _model.state.input.rxFailSafe); + _model.setArmingDisabled(ARMING_DISABLED_THROTTLE, !_model.isThrottleLow()); + _model.setArmingDisabled(ARMING_DISABLED_CALIBRATING, _model.calibrationActive()); + _model.setArmingDisabled(ARMING_DISABLED_MOTOR_PROTOCOL, _model.config.output.protocol == ESC_PROTOCOL_DISABLED); _model.setArmingDisabled(ARMING_DISABLED_REBOOT_REQUIRED, _model.state.mode.rescueConfigMode == RESCUE_CONFIG_ACTIVE); + if(_model.isFeatureActive(FEATURE_GPS)) + { + _model.setArmingDisabled(ARMING_DISABLED_GPS, !_model.state.gps.present || _model.state.gps.numSats < _model.config.gps.minSats); + } } void Actuator::updateModeMask() @@ -161,7 +164,7 @@ bool Actuator::canActivateMode(FlightMode mode) void Actuator::updateArmed() { - if((_model.hasChanged(MODE_ARMED))) + if(_model.hasChanged(MODE_ARMED)) { bool armed = _model.isModeActive(MODE_ARMED); if(armed) @@ -173,6 +176,7 @@ void Actuator::updateArmed() { _model.state.mode.disarmReason = DISARM_REASON_SWITCH; } + if(armed) _model.setGpsHome(); } } @@ -207,6 +211,11 @@ void Actuator::updateBuzzer() { _model.state.buzzer.push(_model.isModeActive(MODE_ARMED) ? BUZZER_ARMING : BUZZER_DISARMING); } + if(!_model.state.gps.wasLocked && _model.state.gps.numSats >= _model.config.gps.minSats) + { + _model.state.buzzer.play(BUZZER_READY_BEEP); + _model.state.gps.wasLocked = true; + } } void Actuator::updateDynLpf() @@ -249,6 +258,20 @@ void Actuator::updateRescueConfig() } } +void Actuator::updateLed() +{ + if(_model.armingDisabled()) + { + _model.state.led.setStatus(Connect::LED_ERROR); + } + else if(_model.isModeActive(MODE_ARMED)) + { + _model.state.led.setStatus(Connect::LED_ON); + } + else + { + _model.state.led.setStatus(Connect::LED_OK); + } } } diff --git a/lib/Espfc/src/Control/Actuator.h b/lib/Espfc/src/Control/Actuator.h index 5d80c973..d8b692eb 100644 --- a/lib/Espfc/src/Control/Actuator.h +++ b/lib/Espfc/src/Control/Actuator.h @@ -2,9 +2,7 @@ #include "Model.h" -namespace Espfc { - -namespace Control { +namespace Espfc::Control { class Actuator { @@ -27,10 +25,9 @@ class Actuator void updateBuzzer(); void updateDynLpf(); void updateRescueConfig(); + void updateLed(); Model& _model; }; } - -} diff --git a/lib/Espfc/src/Device/SerialDevice.h b/lib/Espfc/src/Device/SerialDevice.h index 2eadc696..16acf68a 100644 --- a/lib/Espfc/src/Device/SerialDevice.h +++ b/lib/Espfc/src/Device/SerialDevice.h @@ -111,6 +111,7 @@ class SerialDevice: public Stream { public: virtual void begin(const SerialDeviceConfig& conf) = 0; + virtual void updateBadRate(int baud) = 0; virtual int available() = 0; virtual int read() = 0; virtual size_t readMany(uint8_t * c, size_t l) = 0; diff --git a/lib/Espfc/src/Device/SerialDeviceAdapter.h b/lib/Espfc/src/Device/SerialDeviceAdapter.h index 3da031be..7dd914c9 100644 --- a/lib/Espfc/src/Device/SerialDeviceAdapter.h +++ b/lib/Espfc/src/Device/SerialDeviceAdapter.h @@ -15,10 +15,11 @@ class SerialDeviceAdapter: public SerialDevice { public: SerialDeviceAdapter(T& dev): _dev(dev) {} - virtual void begin(const SerialDeviceConfig& conf) { targetSerialInit(_dev, conf); } - virtual int available() { return _dev.available(); } - virtual int read() { return _dev.read(); } - virtual size_t readMany(uint8_t * c, size_t l) { + void begin(const SerialDeviceConfig& conf) override { targetSerialInit(_dev, conf); } + void updateBadRate(int baud) override { _dev.updateBaudRate(baud); }; + int available() override { return _dev.available(); } + int read() override { return _dev.read(); } + size_t readMany(uint8_t * c, size_t l) override { #if defined(ARCH_RP2040) size_t count = std::min(l, (size_t)available()); for(size_t i = 0; i < count; i++) @@ -30,14 +31,14 @@ class SerialDeviceAdapter: public SerialDevice return _dev.read(c, l); #endif } - virtual int peek() { return _dev.peek(); } - virtual void flush() { _dev.flush(); } - virtual size_t write(uint8_t c) { return _dev.write(c); } - virtual size_t write(const uint8_t * c, size_t l) { return _dev.write(c, l); } - virtual bool isSoft() const { return false; }; - virtual int availableForWrite() { return _dev.availableForWrite(); } - virtual bool isTxFifoEmpty() { return _dev.availableForWrite() >= SERIAL_TX_FIFO_SIZE; } - virtual operator bool() const { return (bool)_dev; } + int peek() override { return _dev.peek(); } + void flush() override { _dev.flush(); } + size_t write(uint8_t c) override { return _dev.write(c); } + size_t write(const uint8_t * c, size_t l) override { return _dev.write(c, l); } + bool isSoft() const override { return false; }; + int availableForWrite() override { return _dev.availableForWrite(); } + bool isTxFifoEmpty() override { return _dev.availableForWrite() >= SERIAL_TX_FIFO_SIZE; } + operator bool() const override { return (bool)_dev; } private: T& _dev; }; @@ -60,6 +61,31 @@ inline bool SerialDeviceAdapter::isTxFifoEmpty() { return true; } + +template<> +inline void SerialDeviceAdapter::updateBadRate(int baud) {} + +#endif + +#if defined(ESP32C3) || defined(ESP32S3) +template<> +inline void SerialDeviceAdapter::updateBadRate(int baud) {} +#endif + +#if defined(ESP32S2) +template<> +inline void SerialDeviceAdapter::updateBadRate(int baud) {} +#endif + +#if defined(ARCH_RP2040) +template<> +inline void SerialDeviceAdapter::updateBadRate(int baud) +{ + _dev.begin(baud); +} + +template<> +inline void SerialDeviceAdapter::updateBadRate(int baud) {} #endif } diff --git a/lib/Espfc/src/Espfc.cpp b/lib/Espfc/src/Espfc.cpp index 7d996f2d..ab650f19 100644 --- a/lib/Espfc/src/Espfc.cpp +++ b/lib/Espfc/src/Espfc.cpp @@ -1,4 +1,5 @@ #include "Espfc.h" +#include "Hal/Gpio.h" #include "Debug_Espfc.h" namespace Espfc { @@ -18,6 +19,8 @@ int Espfc::load() int Espfc::begin() { + _model.state.led.begin(_model.config.pin[PIN_LED_BLINK], _model.config.led.invert); + _serial.begin(); // requires _model.load() //_model.logStorageResult(); _hardware.begin(); // requires _model.load() @@ -29,6 +32,7 @@ int Espfc::begin() _controller.begin(); _blackbox.begin(); // requires _serial.begin(), _actuator.begin() _buzzer.begin(); + _model.state.buzzer.push(BUZZER_SYSTEM_INIT); return 1; @@ -58,10 +62,6 @@ int FAST_CODE_ATTR Espfc::update(bool externalTrigger) _actuator.update(); } - _serial.update(); - _buzzer.update(); - _model.state.stats.update(); - #else _sensor.update(); @@ -84,10 +84,12 @@ int FAST_CODE_ATTR Espfc::update(bool externalTrigger) } _sensor.updateDelayed(); +#endif + _serial.update(); _buzzer.update(); + _model.state.led.update(); _model.state.stats.update(); -#endif return 1; } diff --git a/lib/Espfc/src/Hardware.cpp b/lib/Espfc/src/Hardware.cpp index 021c8184..6c37292a 100644 --- a/lib/Espfc/src/Hardware.cpp +++ b/lib/Espfc/src/Hardware.cpp @@ -69,12 +69,12 @@ void Hardware::initBus() { #if defined(ESPFC_SPI_0) int spiResult = spiBus.begin(_model.config.pin[PIN_SPI_0_SCK], _model.config.pin[PIN_SPI_0_MOSI], _model.config.pin[PIN_SPI_0_MISO]); - _model.logger.info().log(F("SPI SETUP")).log(_model.config.pin[PIN_SPI_0_SCK]).log(_model.config.pin[PIN_SPI_0_MOSI]).log(_model.config.pin[PIN_SPI_0_MISO]).logln(spiResult); + _model.logger.info().log(F("SPI")).log(_model.config.pin[PIN_SPI_0_SCK]).log(_model.config.pin[PIN_SPI_0_MOSI]).log(_model.config.pin[PIN_SPI_0_MISO]).logln(spiResult); #endif #if defined(ESPFC_I2C_0) int i2cResult = i2cBus.begin(_model.config.pin[PIN_I2C_0_SDA], _model.config.pin[PIN_I2C_0_SCL], _model.config.i2cSpeed * 1000ul); i2cBus.onError = std::bind(&Hardware::onI2CError, this); - _model.logger.info().log(F("I2C SETUP")).log(_model.config.pin[PIN_I2C_0_SDA]).log(_model.config.pin[PIN_I2C_0_SCL]).log(_model.config.i2cSpeed).logln(i2cResult); + _model.logger.info().log(F("I2C")).log(_model.config.pin[PIN_I2C_0_SDA]).log(_model.config.pin[PIN_I2C_0_SCL]).log(_model.config.i2cSpeed).logln(i2cResult); #endif } diff --git a/lib/Espfc/src/Hardware.h b/lib/Espfc/src/Hardware.h index c8539488..dcd4c0e6 100644 --- a/lib/Espfc/src/Hardware.h +++ b/lib/Espfc/src/Hardware.h @@ -29,7 +29,7 @@ class Hardware { typename Dev::DeviceType type = dev.getType(); bool status = dev.begin(&bus, cs); - _model.logger.info().log(F("SPI DETECT")).log(FPSTR(Dev::getName(type))).logln(status ? "Y" : ""); + _model.logger.info().log(F("SPI")).log(FPSTR(Dev::getName(type))).logln(status ? "Y" : ""); return status; } #endif @@ -40,7 +40,7 @@ class Hardware { typename Dev::DeviceType type = dev.getType(); bool status = dev.begin(&bus); - _model.logger.info().log(F("I2C DETECT")).log(FPSTR(Dev::getName(type))).logln(status ? "Y" : ""); + _model.logger.info().log(F("I2C")).log(FPSTR(Dev::getName(type))).logln(status ? "Y" : ""); return status; } #endif @@ -50,7 +50,7 @@ class Hardware { typename Dev::DeviceType type = dev.getType(); bool status = dev.begin(&bus); - _model.logger.info().log(F("SLV DETECT")).log(FPSTR(Dev::getName(type))).logln(status ? "Y" : ""); + _model.logger.info().log(F("SLV")).log(FPSTR(Dev::getName(type))).logln(status ? "Y" : ""); return status; } diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 4e55372a..fe207f34 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -96,6 +96,11 @@ class Model return state.gyro.present && config.gyro.dev != GYRO_NONE; } + bool gpsActive() const /* IRAM_ATTR */ + { + return state.gps.present; + } + bool accelActive() const { return state.accel.present && config.accel.dev != GYRO_NONE; @@ -200,6 +205,18 @@ class Model state.debug[index] = value; } + void setGpsHome(bool force = false) + { + if(force || (state.gps.fix && state.gps.numSats >= config.gps.minSats)) + { + if(!state.gps.homeSet || !config.gps.setHomeOnce) + { + state.gps.location.home = state.gps.location.raw; + state.gps.homeSet = true; + } + } + } + Device::SerialDevice * getSerialStream(SerialPort i) { return state.serial[i].stream; @@ -375,8 +392,10 @@ class Model } // configure serial ports - constexpr uint32_t serialFunctionAllowedMask = SERIAL_FUNCTION_MSP | SERIAL_FUNCTION_RX_SERIAL | SERIAL_FUNCTION_BLACKBOX | SERIAL_FUNCTION_TELEMETRY_FRSKY | SERIAL_FUNCTION_TELEMETRY_HOTT | SERIAL_FUNCTION_TELEMETRY_IBUS; - uint32_t featureAllowMask = FEATURE_RX_SERIAL | FEATURE_RX_PPM | FEATURE_RX_SPI | FEATURE_SOFTSERIAL | FEATURE_MOTOR_STOP | FEATURE_TELEMETRY;// | FEATURE_AIRMODE; + constexpr uint32_t serialFunctionAllowedMask = SERIAL_FUNCTION_MSP | SERIAL_FUNCTION_RX_SERIAL | SERIAL_FUNCTION_BLACKBOX | + SERIAL_FUNCTION_GPS | SERIAL_FUNCTION_TELEMETRY_FRSKY | SERIAL_FUNCTION_TELEMETRY_HOTT | SERIAL_FUNCTION_TELEMETRY_IBUS; + uint32_t featureAllowMask = FEATURE_RX_PPM | FEATURE_RX_SERIAL | FEATURE_MOTOR_STOP | FEATURE_SOFTSERIAL | FEATURE_GPS | + FEATURE_TELEMETRY | FEATURE_RX_SPI;// | FEATURE_AIRMODE; // allow dynamic filter only above 1k sampling rate if(state.loopRate >= DynamicFilterConfig::MIN_FREQ) diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 5196b5a0..b17af612 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -210,6 +210,7 @@ enum Feature { FEATURE_RX_SERIAL = 1 << 3, FEATURE_MOTOR_STOP = 1 << 4, FEATURE_SOFTSERIAL = 1 << 6, + FEATURE_GPS = 1 << 7, FEATURE_TELEMETRY = 1 << 10, FEATURE_AIRMODE = 1 << 22, FEATURE_RX_SPI = 1 << 25, @@ -256,7 +257,9 @@ enum PinFunction { #if ESPFC_OUTPUT_COUNT > 7 PIN_OUTPUT_7, #endif + PIN_BUTTON, PIN_BUZZER, + PIN_LED_BLINK, #ifdef ESPFC_SERIAL_0 PIN_SERIAL_0_TX, PIN_SERIAL_0_RX, @@ -648,6 +651,17 @@ struct ControllerConfig int16_t tpaBreakpoint = 1650; }; +struct GpsConfig +{ + uint8_t minSats = 8; + uint8_t setHomeOnce = 1; +}; + +struct LedConfig +{ + uint8_t invert = 0; +}; + // persistent data class ModelConfig { @@ -662,6 +676,7 @@ class ModelConfig FusionConfig fusion; VBatConfig vbat; IBatConfig ibat; + GpsConfig gps; ActuatorCondition conditions[ACTUATOR_CONDITIONS]; ScalerConfig scaler[SCALER_COUNT]; @@ -684,7 +699,6 @@ class ModelConfig DtermConfig dterm; ItermConfig iterm; ControllerConfig controller; - // hardware int8_t pin[PIN_COUNT] = { #ifdef ESPFC_INPUT @@ -706,7 +720,9 @@ class ModelConfig #if ESPFC_OUTPUT_COUNT > 7 [PIN_OUTPUT_7] = ESPFC_OUTPUT_7, #endif + [PIN_BUTTON] = ESPFC_BUTTON_PIN, [PIN_BUZZER] = ESPFC_BUZZER_PIN, + [PIN_LED_BLINK] = ESPFC_LED_PIN, #ifdef ESPFC_SERIAL_0 [PIN_SERIAL_0_TX] = ESPFC_SERIAL_0_TX, [PIN_SERIAL_0_RX] = ESPFC_SERIAL_0_RX, @@ -733,8 +749,6 @@ class ModelConfig [PIN_SPI_0_SCK] = ESPFC_SPI_0_SCK, [PIN_SPI_0_MOSI] = ESPFC_SPI_0_MOSI, [PIN_SPI_0_MISO] = ESPFC_SPI_0_MISO, -#endif -#ifdef ESPFC_SPI_0 [PIN_SPI_CS0] = ESPFC_SPI_CS_GYRO, [PIN_SPI_CS1] = ESPFC_SPI_CS_BARO, [PIN_SPI_CS2] = -1, @@ -757,6 +771,8 @@ class ModelConfig [SERIAL_SOFT_0] = { .id = SERIAL_ID_SOFTSERIAL_1, .functionMask = ESPFC_SERIAL_SOFT_0_FN, .baud = SERIAL_SPEED_115200, .blackboxBaud = SERIAL_SPEED_NONE }, #endif }; + + LedConfig led; BuzzerConfig buzzer; WirelessConfig wireless; diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 0f38d7cb..8a9fdfd4 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -14,6 +14,7 @@ #include "Utils/Stats.h" #include "Device/SerialDevice.h" #include "Connect/Msp.hpp" +#include "Connect/StatusLed.hpp" namespace Espfc { @@ -320,6 +321,126 @@ struct ModeState bool airmodeAllowed; }; +enum GpsDeviceVersion +{ + GPS_UNKNOWN, + GPS_M8, + GPS_M9, + GPS_F9, +}; + +struct GpsSupportState +{ + GpsDeviceVersion version = GPS_UNKNOWN; + bool glonass = false; + bool galileo = false; + bool beidou = false; + bool sbas = false; +}; + +template +struct GpsCoordinate +{ + T lat = T{}; // deg * 1e7 + T lon = T{}; // deg * 1e7 + T height = T{}; // mm (1e3) +}; + +struct GpsPosition +{ + GpsCoordinate raw; + GpsCoordinate home; +}; + +template +struct GpsSpeed +{ + 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 +{ + GpsSpeed raw; +}; + +struct GpsAccuracy +{ + 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 +{ + uint8_t gnssId = 0; + uint8_t id = 0; + uint8_t cno = 0; + union { + uint32_t value; + struct { + uint8_t qualityInd: 3; // quality indicatopr: 0-no signal, 1-searching, 2-aquired, 3-unstable, 4-code locked, 5,6,7-code and carrier locked + uint8_t svUsed: 1; // used for navigation + uint8_t health: 2; // signal health 0-unknown, 1-healthy, 2-unhealty + uint8_t difCorr: 1; // differential correction available for this SV + uint8_t smoothed: 1; // carrier smotthed pseudorange used + uint8_t orbitSource: 3; // orbit source: 0-no inform, 1-ephemeris, 2-almanac, 3-assistnow offline, 4-assistnow autonomous, 5,6,7-other + uint8_t ephAvail: 1; // ephemeris available + uint8_t elmAvail: 1; // almanac available + uint8_t enoAvail: 1; // assistnow offline available + uint8_t eopAvail: 1; // assistnow autonomous available + uint8_t reserved: 1; + uint8_t sbasCorrUsed: 1; // SBAS corrections used + uint8_t rtcmCorrUsed: 1; // RTCM corrections used + uint8_t slasCorrUsed: 1; // SLAS corrections used + uint8_t spartnCorrUsed: 1; // SPARTN corrections used + uint8_t prCorrUsed: 1; // Pseudorange corrections used + uint8_t crCorrUsed: 1; // Carrier range corrections used + uint8_t doCorrUsed: 1; // Range rate (Doppler) corrections used + uint8_t clasCorrUsed: 1; // CLAS corrections used + }; + } quality = { .value = 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; + bool present = false; + bool frameError = false; + bool wasLocked = false; + bool homeSet = false; + uint32_t interval; + uint32_t lastMsgTs; + GpsSupportState support; + GpsPosition location; + GpsVelocity velocity; + GpsAccuracy accuracy; + GpsDateTime dateTime; + GpsSatelite svinfo[SAT_MAX]; +}; + // runtime data struct ModelState { @@ -327,6 +448,7 @@ struct ModelState AccelState accel; MagState mag; BaroState baro; + GpsState gps; InputState input; FailsafeState failsafe; @@ -353,6 +475,7 @@ struct ModelState int16_t debug[DEBUG_VALUE_COUNT]; BuzzerState buzzer; + Connect::StatusLed led; BatteryState battery; diff --git a/lib/Espfc/src/Output/Mixer.cpp b/lib/Espfc/src/Output/Mixer.cpp index 739d8eae..e0e8eb1d 100644 --- a/lib/Espfc/src/Output/Mixer.cpp +++ b/lib/Espfc/src/Output/Mixer.cpp @@ -20,7 +20,7 @@ int Mixer::begin() }; escMotor.begin(motorConf); _model.state.mixer.escMotor = _motor = &escMotor; - _model.logger.info().log(F("MOTOR CONF")).log(_model.config.output.protocol).log(_model.config.output.async).log(_model.config.output.rate).log(_model.config.output.dshotTelemetry).logln(ESC_DRIVER_MOTOR_TIMER); + _model.logger.info().log(F("MOTOR")).log(FPSTR(EscDriver::getProtocolName((EscProtocol)_model.config.output.protocol))).log(_model.config.output.async).log(_model.config.output.rate).log(_model.config.output.dshotTelemetry).logln(ESC_DRIVER_MOTOR_TIMER); if(_model.config.output.servoRate) { @@ -33,7 +33,7 @@ int Mixer::begin() }; escServo.begin(servoConf); _model.state.mixer.escServo = _servo = &escServo; - _model.logger.info().log(F("SERVO CONF")).log(ESC_PROTOCOL_PWM).log(true).logln(_model.config.output.servoRate).logln(ESC_DRIVER_SERVO_TIMER); + _model.logger.info().log(F("SERVO")).log(FPSTR(EscDriver::getProtocolName(ESC_PROTOCOL_PWM))).log(true).logln(_model.config.output.servoRate).logln(ESC_DRIVER_SERVO_TIMER); } _erpmToHz = EscDriver::getErpmToHzRatio(_model.config.output.motorPoles); _statsCounterMax = _model.state.mixer.timer.rate / 2; @@ -47,13 +47,13 @@ int Mixer::begin() if(_servo) { _servo->attach(i, _model.config.pin[PIN_OUTPUT_0 + i], 1500); - _model.logger.info().log(F("SERVO PIN")).log(i).logln(_model.config.pin[PIN_OUTPUT_0 + i]); + _model.logger.info().log(F("SERVO")).log(i).logln(_model.config.pin[PIN_OUTPUT_0 + i]); } } else { _motor->attach(i, _model.config.pin[PIN_OUTPUT_0 + i], 1000); - _model.logger.info().log(F("MOTOR PIN")).log(i).logln(_model.config.pin[PIN_OUTPUT_0 + i]); + _model.logger.info().log(F("MOTOR")).log(i).logln(_model.config.pin[PIN_OUTPUT_0 + i]); } _model.state.output.telemetry.errors[i] = 0; _model.state.output.telemetry.errorsSum[i] = 0; diff --git a/lib/Espfc/src/Rc/Crsf.h b/lib/Espfc/src/Rc/Crsf.h index a3768ba1..77eb87d8 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/BaseSensor.h b/lib/Espfc/src/Sensor/BaseSensor.h index fe98aa87..45644625 100644 --- a/lib/Espfc/src/Sensor/BaseSensor.h +++ b/lib/Espfc/src/Sensor/BaseSensor.h @@ -3,9 +3,7 @@ #include #include -namespace Espfc { - -namespace Sensor { +namespace Espfc::Sensor { class BaseSensor { @@ -15,5 +13,3 @@ class BaseSensor }; } - -} diff --git a/lib/Espfc/src/Sensor/GpsSensor.cpp b/lib/Espfc/src/Sensor/GpsSensor.cpp new file mode 100644 index 00000000..0309719d --- /dev/null +++ b/lib/Espfc/src/Sensor/GpsSensor.cpp @@ -0,0 +1,457 @@ +#include "Sensor/GpsSensor.hpp" +#include +#include +#include +#include + +namespace Espfc::Sensor +{ + +static constexpr std::array BAUDS{ + 9600, 115200, 230400, 57600, 38400, 19200, +}; + +static constexpr std::array NMEA_MSG_OFF{ + Gps::NMEA_MSG_GGA, Gps::NMEA_MSG_GLL, Gps::NMEA_MSG_GSA, + Gps::NMEA_MSG_GSV, Gps::NMEA_MSG_RMC, Gps::NMEA_MSG_VTG, +}; + +static constexpr std::array, 2> UBX_MSG_ON{ + std::make_tuple(Gps::UBX_NAV_PVT, 1u), + std::make_tuple(Gps::UBX_NAV_SAT, 10u), +}; + +GpsSensor::GpsSensor(Model& model): _model(model) {} + +int GpsSensor::begin(Device::SerialDevice* port, int baud) +{ + _port = port; + _targetBaud = _currentBaud = baud; + _timer.setRate(50); + + _state = DETECT_BAUD; + _timeout = micros() + DETECT_TIMEOUT; + _counter = 0; + setBaud(BAUDS[_counter]); + + return 1; +} + +int GpsSensor::update() +{ + if(!_port) return 0; + + if(!_timer.check()) return 0; + + Utils::Stats::Measure measure(_model.state.stats, COUNTER_GPS_READ); + + bool updated = false; + uint8_t buff[32]; + size_t read = 0; + while ((read = _port->readMany(buff, sizeof(buff)))) + { + for (size_t i = 0; i < read; i++) + { + updated |= processUbx(buff[i]); + processNmea(buff[i]); + } + } + + if (!updated) handle(); + + return 1; +} + +bool GpsSensor::processUbx(uint8_t c) +{ + _ubxParser.parse(c, _ubxMsg); + if (!_ubxMsg.isReady()) return false; + + onMessage(); + + handle(); + _ubxMsg = Gps::UbxMessage(); + + return true; +} + +void GpsSensor::processNmea(uint8_t c) +{ + _nmeaParser.parse(c, _nmeaMsg); + if (!_nmeaMsg.isReady()) return; + + //$GNTXT,01,01,01,More than 100 frame errors, UART RX was disabled*70 + static const char * msg = "GNTXT,01,01,01,More than 100 frame errors"; + + if(!_model.state.gps.frameError && std::strncmp(_nmeaMsg.payload, msg, std::strlen(msg)) == 0) + { + _model.state.gps.frameError = true; + if(!_model.isModeActive(MODE_ARMED)) _model.logger.err().logln("GPS RX Frame Err"); + } + + onMessage(); + + _nmeaMsg = Gps::NmeaMessage(); +} + +void GpsSensor::onMessage() +{ + if(_state == DETECT_BAUD) + { + _state = SET_BAUD; + _model.logger.info().log("GPS DET").logln(_currentBaud); + } +} + +void GpsSensor::handle() +{ + switch (_state) + { + case DETECT_BAUD: + if(micros() > _timeout) + { + // on timeout check next baud + _counter++; + if(_counter < BAUDS.size()) + { + setBaud(BAUDS[_counter]); + } + else + { + _state = SET_BAUD; + _counter = 0; + } + _timeout = micros() + DETECT_TIMEOUT; + } + break; + + case SET_BAUD: + send(Gps::UbxCfgPrt20{ + .portId = 1, + .resered1 = 0, + .txReady = 0, + .mode = 0x08c0, // 8N1 + .baudRate = (uint32_t)_targetBaud, // baud + .inProtoMask = 0x07, + .outProtoMask = 0x07, + .flags = 0, + .resered2 = 0, + }, DISABLE_NMEA, DISABLE_NMEA); + delay(30); // wait until transmission complete at 9600bps + setBaud(_targetBaud); + delay(5); + break; + + case DISABLE_NMEA: + { + const Gps::UbxCfgMsg3 m{ + .msgId = NMEA_MSG_OFF[_counter], + .rate = 0, + }; + _counter++; + if (_counter < NMEA_MSG_OFF.size()) + { + send(m, _state); + } + else + { + _counter = 0; + send(m, GET_VERSION); + _model.logger.info().log(F("GPS NMEA")).logln(0); + } + } + break; + + case GET_VERSION: + send(Gps::UbxMonVer{}, ENABLE_UBX); + _timeout = micros() + 3 * TIMEOUT; + break; + + case ENABLE_UBX: + { + const Gps::UbxCfgMsg3 m{ + .msgId = std::get<0>(UBX_MSG_ON[_counter]), + .rate = std::get<1>(UBX_MSG_ON[_counter]), + }; + _counter++; + if (_counter < UBX_MSG_ON.size()) + { + send(m, _state); + } + else + { + send(m, ENABLE_NAV5); + _counter = 0; + _timeout = micros() + 10 * TIMEOUT; + _model.logger.info().logln(F("GPS UBX")); + } + } + break; + + case ENABLE_NAV5: + send(Gps::UbxCfgNav5{ + .mask = { .value = 0xffff }, // all + .dynModel = 8, // airborne + .fixMode = 3, + .fixedAlt = 0, + .fixedAltVar = 10000, + .minElev = 5, + .drLimit = 0, + .pDOP = 250, + .tDOP = 250, + .pAcc = 100, + .tAcc = 300, + .staticHoldThresh = 0, + .dgnssTimeout = 60, + .cnoThreshNumSVs = 0, + .cnoThresh = 0, + .reserved0 = {0, 0}, + .staticHoldMaxDist = 200, + .utcStandard = 0, + .reserved1 = {0, 0, 0, 0, 0}, + }, ENABLE_SBAS, ENABLE_SBAS); + _model.logger.info().logln(F("GPS NAV5")); + break; + + case ENABLE_SBAS: + if (_model.state.gps.support.sbas) + { + send(Gps::UbxCfgSbas8{ + .mode = 1, + .usage = 1, + .maxSbas = 3, + .scanmode2 = 0, + .scanmode1 = 0, + }, SET_RATE, SET_RATE); + } + else + { + setState(SET_RATE); + } + _model.logger.info().log(F("GPS SBAS")).logln(_model.state.gps.support.sbas); + break; + + case SET_RATE: + { + const uint16_t mRate = _currentBaud > 100000 ? 100 : 200; + const uint16_t nRate = 1; + const Gps::UbxCfgRate6 m{ + .measRate = mRate, + .navRate = nRate, + .timeRef = 0, // utc + }; + send(m, RECEIVE); + _model.logger.info().log(F("GPS RATE")).log(mRate).logln(nRate); + } + break; + + case ERROR: + if (_counter == 0) + { + _model.logger.err().logln(F("GPS ERROR")); + _counter++; + } + handleError(); + break; + + case RECEIVE: + _model.state.gps.present = true; + case WAIT: + default: + if (_ubxMsg.isReady()) + { + if (_ubxMsg.isAck()) + { + _state = _ackState; + } + else if (_ubxMsg.isNak()) + { + _state = _timeoutState; + } + else if (_ubxMsg.isResponse(Gps::UbxMonVer::ID)) + { + handleVersion(); + _state = _ackState; + _counter = 0; + } + else if (_ubxMsg.isResponse(Gps::UbxNavPvt92::ID)) + { + handleNavPvt(); + } + else if (_ubxMsg.isResponse(Gps::UbxNavSat::ID)) + { + handleNavSat(); + } + else + { + } + } + else if (_state == WAIT && micros() > _timeout) + { + // timeout + _state = _timeoutState; + _model.state.gps.present = false; + } + break; + } +} + +void GpsSensor::setBaud(int baud) +{ + if(baud != _currentBaud) + { + _port->updateBadRate(baud); + _currentBaud = baud; + _model.logger.info().log(F("GPS BAUD")).logln(baud); + } +} + +void GpsSensor::setState(State state, State ackState, State timeoutState) +{ + setState(state); + _ackState = ackState; + _timeoutState = timeoutState; +} + +void GpsSensor::setState(State state) +{ + _state = state; + _timeout = micros() + TIMEOUT; +} + +void GpsSensor::handleError() const +{ + _model.state.gps.present = false; +} + +void GpsSensor::handleNavPvt() const +{ + const auto &m = *_ubxMsg.getAs(); + + _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; // deg * 1e5 + + _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; + int32_t msec = m.nano / 1000000; + if(msec < 0) { + msec += 1000; + } + _model.state.gps.dateTime.msec = msec; + } + + uint32_t now = micros(); + _model.state.gps.interval = now - _model.state.gps.lastMsgTs; + _model.state.gps.lastMsgTs = now; +} + +void GpsSensor::handleNavSat() const +{ + const auto &m = *_ubxMsg.getAs(); + _model.state.gps.numCh = m.numSvs; + for (uint8_t i = 0; i < SAT_MAX; i++) + { + if(i < m.numSvs) + { + _model.state.gps.svinfo[i].id = m.sats[i].svId; + _model.state.gps.svinfo[i].gnssId = m.sats[i].gnssId; + _model.state.gps.svinfo[i].cno = m.sats[i].cno; + _model.state.gps.svinfo[i].quality.value = m.sats[i].flags.value; + } + else + { + _model.state.gps.svinfo[i] = GpsSatelite{}; + } + } +} + +void GpsSensor::handleVersion() const +{ + const char *payload = (const char *)_ubxMsg.payload; + + _model.logger.info().log(F("GPS VER")).logln(payload); + _model.logger.info().log(F("GPS VER")).logln(payload + 30); + + if (std::strcmp(payload + 30, "00080000") == 0) + { + _model.state.gps.support.version = GPS_M8; + } + else if (std::strcmp(payload + 30, "00090000") == 0) + { + _model.state.gps.support.version = GPS_M9; + } + else if (std::strcmp(payload + 30, "00190000") == 0) + { + _model.state.gps.support.version = GPS_F9; + } + if (_ubxMsg.length >= 70) + { + checkSupport(payload + 40); + _model.logger.info().log(F("GPS EXT")).logln(payload + 40); + } + if (_ubxMsg.length >= 100) + { + checkSupport(payload + 70); + _model.logger.info().log(F("GPS EXT")).logln(payload + 70); + } + if (_ubxMsg.length >= 130) + { + checkSupport(payload + 100); + _model.logger.info().log(F("GPS EXT")).logln(payload + 100); + } + if (_ubxMsg.length >= 160) + { + checkSupport(payload + 130); + _model.logger.info().log(F("GPS EXT")).logln(payload + 130); + } +} + +void GpsSensor::checkSupport(const char *payload) const +{ + if (std::strstr(payload, "SBAS") != nullptr) + { + _model.state.gps.support.sbas = true; + } + if (std::strstr(payload, "GLO") != nullptr) + { + _model.state.gps.support.glonass = true; + } + if (std::strstr(payload, "GAL") != nullptr) + { + _model.state.gps.support.galileo = true; + } + if (std::strstr(payload, "BDS") != nullptr) + { + _model.state.gps.support.beidou = true; + } +} + +} diff --git a/lib/Espfc/src/Sensor/GpsSensor.hpp b/lib/Espfc/src/Sensor/GpsSensor.hpp new file mode 100644 index 00000000..34a98467 --- /dev/null +++ b/lib/Espfc/src/Sensor/GpsSensor.hpp @@ -0,0 +1,88 @@ +#pragma once + +#include "Model.h" +#include "Device/SerialDevice.h" +#include "Utils/Timer.h" +#include +#include +#include +#include + +namespace Espfc::Sensor { + +class GpsSensor +{ +public: + GpsSensor(Model& model); + + int begin(Device::SerialDevice* port, int baud); + + int update(); + +private: + enum State { + DETECT_BAUD, + SET_BAUD, + DISABLE_NMEA, + GET_VERSION, + ENABLE_UBX, + ENABLE_NAV5, + ENABLE_SBAS, + SET_RATE, + WAIT, + RECEIVE, + ERROR, + }; + + void handle(); + + bool processUbx(uint8_t c); + void processNmea(uint8_t c); + void setBaud(int baud); + + void onMessage(); + + template + void send(const MsgType m, State ackState, State timeoutState = ERROR) + { + Gps::UbxFrame frame{m}; + const uint8_t* ptr = reinterpret_cast(&frame); + _port->write(ptr, sizeof(frame)); + + setState(WAIT, ackState, timeoutState); + } + + void setState(State state, State ackState, State timeoutState); + + void setState(State state); + + void handleError() const; + void handleNavPvt() const; + void handleNavSat() const; + void handleVersion() const; + void checkSupport(const char* payload) const; + + static constexpr uint32_t TIMEOUT = 300000; + static constexpr uint32_t DETECT_TIMEOUT = 2200000; + + Model& _model; + + State _state = WAIT; + State _ackState = WAIT; + State _timeoutState = DETECT_BAUD; + size_t _counter = 0; + uint32_t _timeout = 0; + int _currentBaud = 0; + int _targetBaud = 0; + + Gps::UbxParser _ubxParser; + Gps::UbxMessage _ubxMsg; + + Gps::NmeaParser _nmeaParser; + Gps::NmeaMessage _nmeaMsg; + + Device::SerialDevice* _port; + Utils::Timer _timer; +}; + +} diff --git a/lib/Espfc/src/SerialManager.cpp b/lib/Espfc/src/SerialManager.cpp index e05c4f9c..56b57d79 100644 --- a/lib/Espfc/src/SerialManager.cpp +++ b/lib/Espfc/src/SerialManager.cpp @@ -2,6 +2,7 @@ #include "Device/SerialDeviceAdapter.h" #include "Debug_Espfc.h" +// TODO: move to target #ifdef ESPFC_SERIAL_0 static Espfc::Device::SerialDeviceAdapter _uart0(ESPFC_SERIAL_0_DEV); #endif @@ -20,27 +21,22 @@ namespace Espfc { -SerialManager::SerialManager(Model& model, TelemetryManager& telemetry): _model(model), _msp(model), _cli(model), +SerialManager::SerialManager(Model& model, TelemetryManager& telemetry): _model(model), _current(0), _msp(model), _cli(model), + _telemetry(telemetry), _gps(model) #ifdef ESPFC_SERIAL_SOFT_0_WIFI -_wireless(model), + , _wireless(model) #endif -_telemetry(telemetry), _current(0) {} +{} int SerialManager::begin() { for(int i = 0; i < SERIAL_UART_COUNT; i++) { Device::SerialDevice * port = getSerialPortById((SerialPort)i); - if(!port) - { - //D("uart-no-port", i, (bool)port); - continue; - } - const SerialPortConfig& spc = _model.config.serial[i]; - if(!spc.functionMask) + + if(!port || !spc.functionMask) { - //D("uart-no-func", i, spc.id, spc.functionMask, spc.baud); continue; } @@ -63,7 +59,6 @@ int SerialManager::begin() sdc.rx_pin = _model.config.pin[pin_idx + PIN_SERIAL_0_RX]; if(sdc.tx_pin == -1 && sdc.rx_pin == -1) { - //D("uart-no-pins", i, spc.id, spc.functionMask, spc.baud); continue; } } @@ -102,27 +97,13 @@ int SerialManager::begin() else if(spc.functionMask & SERIAL_FUNCTION_TELEMETRY_IBUS) { sdc.baud = 115200; - _ibus.begin(port); } - /*if(spc.functionMask & SERIAL_FUNCTION_TELEMETRY_FRSKY) - { - sdc.baud = 420000ul; - }*/ - if(!sdc.baud) { - //D("uart-no-baud", i, spc.id, spc.functionMask, spc.baud); continue; } - //if(true || !isUsbPort) { - //D("uart-flush", i, spc.id, spc.functionMask, spc.baud); - //port->flush(); - //delay(10); - //} - - //D("uart-begin", i, spc.id, spc.functionMask, spc.baud, sdc.tx_pin, sdc.rx_pin); port->begin(sdc); _model.state.serial[i].stream = port; @@ -131,6 +112,15 @@ int SerialManager::begin() initDebugStream(port); } + if(spc.functionMask & SERIAL_FUNCTION_TELEMETRY_IBUS) + { + _ibus.begin(port); + } + else if(spc.functionMask & SERIAL_FUNCTION_GPS) + { + _gps.begin(port, sdc.baud); + } + _model.logger.info().log(F("UART")).log(i).log(spc.id).log(spc.functionMask).log(sdc.baud).log(i == ESPFC_SERIAL_DEBUG_PORT).log(sdc.tx_pin).logln(sdc.rx_pin); } @@ -143,57 +133,28 @@ int SerialManager::begin() int FAST_CODE_ATTR SerialManager::update() { - SerialPortState& ss = _model.state.serial[_current]; const SerialPortConfig& sc = _model.config.serial[_current]; - Device::SerialDevice * stream = ss.stream; + SerialPortState& ss = _model.state.serial[_current]; - bool serialRx = sc.functionMask & SERIAL_FUNCTION_RX_SERIAL; - if(stream) + if(ss.stream && !(sc.functionMask & SERIAL_FUNCTION_RX_SERIAL)) { - if(!serialRx) + Utils::Stats::Measure measure(_model.state.stats, COUNTER_SERIAL); + if (sc.functionMask & SERIAL_FUNCTION_MSP) { - Utils::Stats::Measure measure(_model.state.stats, COUNTER_SERIAL); - size_t len = stream->available(); - if(len > 0) - { - uint8_t buff[64] = {0}; - len = std::min(len, (size_t)sizeof(buff)); - stream->readMany(buff, len); - char * c = (char*)&buff[0]; - while(len--) - { - if(sc.functionMask & SERIAL_FUNCTION_MSP) - { - bool consumed = _msp.parse(*c, ss.mspRequest); - if(consumed) - { - if(ss.mspRequest.isReady() && ss.mspRequest.isCmd()) - { - _msp.processCommand(ss.mspRequest, ss.mspResponse, *stream); - _msp.sendResponse(ss.mspResponse, *stream); - _msp.postCommand(); - ss.mspRequest = Connect::MspMessage(); - ss.mspResponse = Connect::MspResponse(); - } - } - else - { - _cli.process(*c, ss.cliCmd, *stream); - } - } - c++; - } - } + processMsp(ss); } if(sc.functionMask & SERIAL_FUNCTION_TELEMETRY_FRSKY && _model.state.telemetryTimer.check()) { - _telemetry.process(*stream, TELEMETRY_PROTOCOL_TEXT); + _telemetry.process(*ss.stream, TELEMETRY_PROTOCOL_TEXT); + } + if(sc.functionMask & SERIAL_FUNCTION_TELEMETRY_IBUS) + { + _ibus.update(); + } + if(sc.functionMask & SERIAL_FUNCTION_GPS) + { + _gps.update(); } - } - - if(sc.functionMask & SERIAL_FUNCTION_TELEMETRY_IBUS) - { - _ibus.update(); } #ifdef ESPFC_SERIAL_SOFT_0_WIFI @@ -208,6 +169,37 @@ int FAST_CODE_ATTR SerialManager::update() return 1; } +void SerialManager::processMsp(SerialPortState& ss) +{ + size_t len = ss.stream->available(); + if(!len) return; + + uint8_t buff[64] = {0}; + len = std::min(len, (size_t)sizeof(buff)); + ss.stream->readMany(buff, len); + char * c = (char*)&buff[0]; + while(len--) + { + bool consumed = _msp.parse(*c, ss.mspRequest); + if(consumed) + { + if(ss.mspRequest.isReady() && ss.mspRequest.isCmd()) + { + _msp.processCommand(ss.mspRequest, ss.mspResponse, *ss.stream); + _msp.sendResponse(ss.mspResponse, *ss.stream); + _msp.postCommand(); + ss.mspRequest = Connect::MspMessage(); + ss.mspResponse = Connect::MspResponse(); + } + } + else + { + _cli.process(*c, ss.cliCmd, *ss.stream); + } + c++; + } +} + Device::SerialDevice * SerialManager::getSerialPortById(SerialPort portId) { switch(portId) diff --git a/lib/Espfc/src/SerialManager.h b/lib/Espfc/src/SerialManager.h index e9b7f00a..36144af2 100644 --- a/lib/Espfc/src/SerialManager.h +++ b/lib/Espfc/src/SerialManager.h @@ -6,6 +6,7 @@ #include "Connect/Cli.hpp" #include "TelemetryManager.h" #include "Output/OutputIBUS.hpp" +#include "Sensor/GpsSensor.hpp" #ifdef ESPFC_SERIAL_SOFT_0_WIFI #include "Wireless.h" #endif @@ -14,30 +15,32 @@ namespace Espfc { class SerialManager { - public: - SerialManager(Model& model, TelemetryManager& telemetry); - - int begin(); - int update(); - - static Device::SerialDevice * getSerialPortById(SerialPort portId); - - private: - void next() - { - _current++; - if(_current >= SERIAL_UART_COUNT) _current = 0; - } - - Model& _model; - Connect::MspProcessor _msp; - Connect::Cli _cli; +public: + SerialManager(Model& model, TelemetryManager& telemetry); + + int begin(); + int update(); + +private: + static Device::SerialDevice * getSerialPortById(SerialPort portId); + void processMsp(SerialPortState& ss); + + void next() + { + _current++; + if(_current >= SERIAL_UART_COUNT) _current = 0; + } + + Model& _model; + size_t _current; + Connect::MspProcessor _msp; + Connect::Cli _cli; + TelemetryManager& _telemetry; + Output::OutputIBUS _ibus; + Sensor::GpsSensor _gps; #ifdef ESPFC_SERIAL_SOFT_0_WIFI - Wireless _wireless; + Wireless _wireless; #endif - TelemetryManager& _telemetry; - size_t _current; - Output::OutputIBUS _ibus; }; } diff --git a/lib/Espfc/src/Target/TargetESP32c3.h b/lib/Espfc/src/Target/TargetESP32c3.h index 555e2348..cd3ad6a1 100644 --- a/lib/Espfc/src/Target/TargetESP32c3.h +++ b/lib/Espfc/src/Target/TargetESP32c3.h @@ -60,6 +60,8 @@ #define ESPFC_I2C_0_SOFT #define ESPFC_BUZZER_PIN -1 +#define ESPFC_BUTTON_PIN -1 +#define ESPFC_LED_PIN -1 #define ESPFC_ADC_0 #define ESPFC_ADC_0_PIN 0 diff --git a/lib/Espfc/src/Target/TargetESP32s2.h b/lib/Espfc/src/Target/TargetESP32s2.h index 1d302662..85d19fc7 100644 --- a/lib/Espfc/src/Target/TargetESP32s2.h +++ b/lib/Espfc/src/Target/TargetESP32s2.h @@ -57,6 +57,8 @@ #define ESPFC_I2C_0_SOFT #define ESPFC_BUZZER_PIN 5 +#define ESPFC_BUTTON_PIN -1 +#define ESPFC_LED_PIN -1 #define ESPFC_ADC_0 #define ESPFC_ADC_0_PIN 1 diff --git a/lib/Espfc/src/Target/TargetESP32s3.h b/lib/Espfc/src/Target/TargetESP32s3.h index 15efedc4..4abb4e82 100644 --- a/lib/Espfc/src/Target/TargetESP32s3.h +++ b/lib/Espfc/src/Target/TargetESP32s3.h @@ -72,6 +72,8 @@ #define ESPFC_I2C_0_SOFT #define ESPFC_BUZZER_PIN 5 +#define ESPFC_BUTTON_PIN -1 +#define ESPFC_LED_PIN -1 #define ESPFC_ADC_0 #define ESPFC_ADC_0_PIN 1 diff --git a/lib/Espfc/src/Target/TargetESP8266.h b/lib/Espfc/src/Target/TargetESP8266.h index 3e3e841b..7f50d25e 100644 --- a/lib/Espfc/src/Target/TargetESP8266.h +++ b/lib/Espfc/src/Target/TargetESP8266.h @@ -42,6 +42,8 @@ #define ESPFC_I2C_0_SOFT #define ESPFC_BUZZER_PIN 16 // D0 +#define ESPFC_BUTTON_PIN -1 +#define ESPFC_LED_PIN -1 #define ESPFC_ADC_0 #define ESPFC_ADC_0_PIN 17 // A0 diff --git a/lib/Espfc/src/Target/TargetRP2040.h b/lib/Espfc/src/Target/TargetRP2040.h index c73de2a1..a5bba5ae 100644 --- a/lib/Espfc/src/Target/TargetRP2040.h +++ b/lib/Espfc/src/Target/TargetRP2040.h @@ -37,7 +37,7 @@ #define ESPFC_SERIAL_USB_FN (SERIAL_FUNCTION_MSP) #define ESPFC_SERIAL_REMAP_PINS -#define SERIAL_TX_FIFO_SIZE 128 +#define SERIAL_TX_FIFO_SIZE 256 #define ESPFC_SERIAL_DEBUG_PORT SERIAL_USB #define ESPFC_SPI_0 @@ -55,6 +55,9 @@ #define ESPFC_I2C_0_SCL 17 #define ESPFC_BUZZER_PIN -1 +#define ESPFC_BUTTON_PIN -1 +#define ESPFC_LED_PIN -1 + #define ESPFC_ADC_0 #define ESPFC_ADC_0_PIN 26 @@ -109,12 +112,13 @@ inline int targetSerialInit(T& dev, const SerialDeviceConfig& conf) dev.setFIFOSize(SERIAL_TX_FIFO_SIZE); dev.setPinout(conf.tx_pin, conf.rx_pin); - dev.begin(conf.baud, sc); - if(conf.inverted) { - gpio_set_inover(conf.rx_pin, GPIO_OVERRIDE_INVERT); - gpio_set_outover(conf.tx_pin, GPIO_OVERRIDE_INVERT); + //gpio_set_inover(conf.rx_pin, GPIO_OVERRIDE_INVERT); + //gpio_set_outover(conf.tx_pin, GPIO_OVERRIDE_INVERT); + dev.setInvertRX(); + dev.setInvertTX(); } + dev.begin(conf.baud, sc); return 1; } diff --git a/lib/Espfc/src/Target/TargetUnitTest.h b/lib/Espfc/src/Target/TargetUnitTest.h index f4793aaa..4cb6254c 100644 --- a/lib/Espfc/src/Target/TargetUnitTest.h +++ b/lib/Espfc/src/Target/TargetUnitTest.h @@ -21,6 +21,8 @@ #define ESPFC_SERIAL_DEBUG_PORT 0 #define ESPFC_BUZZER_PIN -1 +#define ESPFC_BUTTON_PIN -1 +#define ESPFC_LED_PIN -1 inline void targetReset() { diff --git a/lib/Espfc/src/Telemetry/TelemetryCRSF.h b/lib/Espfc/src/Telemetry/TelemetryCRSF.h index 728ba984..d98ab290 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 + 500) / 1000)); // in km/h * 10 + msg.writeU16(Utils::toBigEndian16((_model.state.gps.velocity.raw.heading + 500) / 1000)); // deg * 10 + uint16_t altitude = std::clamp((_model.state.gps.location.raw.height + 500) / 1000, (int32_t)-900, (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/Espfc/src/Utils/FFTAnalyzer.hpp b/lib/Espfc/src/Utils/FFTAnalyzer.hpp index 1186db4f..0df15288 100644 --- a/lib/Espfc/src/Utils/FFTAnalyzer.hpp +++ b/lib/Espfc/src/Utils/FFTAnalyzer.hpp @@ -47,17 +47,17 @@ class FFTAnalyzer float _bin_width; // fft input - __attribute__((aligned(16))) float _in[SAMPLES]; + //__attribute__((aligned(16))) float _in[SAMPLES]; // fft output - __attribute__((aligned(16))) float _out[SAMPLES]; + //__attribute__((aligned(16))) float _out[SAMPLES]; // Window coefficients - __attribute__((aligned(16))) float _win[SAMPLES]; + //__attribute__((aligned(16))) float _win[SAMPLES]; - // float* _in; - // float* _out; - // float* _win; + float* _in; + float* _out; + float* _win; }; } diff --git a/lib/Espfc/src/Utils/FFTAnalyzer.ipp b/lib/Espfc/src/Utils/FFTAnalyzer.ipp index 5a2476ac..bbb18b77 100644 --- a/lib/Espfc/src/Utils/FFTAnalyzer.ipp +++ b/lib/Espfc/src/Utils/FFTAnalyzer.ipp @@ -14,24 +14,24 @@ namespace Espfc { namespace Utils { template -FFTAnalyzer::FFTAnalyzer(): _idx(0), _phase(PHASE_COLLECT), _begin(0), _end(0)/*, _in(nullptr), _out(nullptr), _win(nullptr)*/ +FFTAnalyzer::FFTAnalyzer(): _idx(0), _phase(PHASE_COLLECT), _begin(0), _end(0), _in(nullptr), _out(nullptr), _win(nullptr) { } template FFTAnalyzer::~FFTAnalyzer() { - // if(_in) heap_caps_free(_in); - // if(_out) heap_caps_free(_out); - // if(_win) heap_caps_free(_win); + if(_in) heap_caps_free(_in); + if(_out) heap_caps_free(_out); + if(_win) heap_caps_free(_win); } template int FFTAnalyzer::begin(int16_t rate, const DynamicFilterConfig& config, size_t axis) { - // if(!_in) _in = static_cast(heap_caps_aligned_alloc(16u, SAMPLES * sizeof(float), MALLOC_CAP_DEFAULT)); - // if(!_out) _out = static_cast(heap_caps_aligned_alloc(16u, SAMPLES * sizeof(float), MALLOC_CAP_DEFAULT)); - // if(!_win) _win = static_cast(heap_caps_aligned_alloc(16u, SAMPLES * sizeof(float), MALLOC_CAP_DEFAULT)); + if(!_in) _in = static_cast(heap_caps_aligned_alloc(16u, SAMPLES * sizeof(float), MALLOC_CAP_DEFAULT)); + if(!_out) _out = static_cast(heap_caps_aligned_alloc(16u, SAMPLES * sizeof(float), MALLOC_CAP_DEFAULT)); + if(!_win) _win = static_cast(heap_caps_aligned_alloc(16u, SAMPLES * sizeof(float), MALLOC_CAP_DEFAULT)); int16_t nyquistLimit = rate / 2; _rate = rate; diff --git a/lib/Espfc/src/Utils/Logger.h b/lib/Espfc/src/Utils/Logger.h index 9a896d43..e930e0c6 100644 --- a/lib/Espfc/src/Utils/Logger.h +++ b/lib/Espfc/src/Utils/Logger.h @@ -21,7 +21,7 @@ class Logger _tail = 0; } - int begin(size_t size = 1024) + int begin(size_t size = 2048) { _size = size; _tail = 0; diff --git a/lib/Espfc/src/Utils/Stats.cpp b/lib/Espfc/src/Utils/Stats.cpp index 2df91755..3efc97c8 100644 --- a/lib/Espfc/src/Utils/Stats.cpp +++ b/lib/Espfc/src/Utils/Stats.cpp @@ -131,6 +131,7 @@ const char * Stats::getName(StatCounter c) const case COUNTER_MAG_READ: return PSTR(" mag_r"); case COUNTER_MAG_FILTER: return PSTR(" mag_f"); case COUNTER_BARO: return PSTR("baro_p"); + case COUNTER_GPS_READ: return PSTR(" gps_r"); case COUNTER_IMU_FUSION: return PSTR(" imu_p"); case COUNTER_IMU_FUSION2: return PSTR(" imu_c"); case COUNTER_OUTER_PID: return PSTR(" pid_o"); diff --git a/lib/Espfc/src/Utils/Stats.h b/lib/Espfc/src/Utils/Stats.h index 26666f48..cc12fdba 100644 --- a/lib/Espfc/src/Utils/Stats.h +++ b/lib/Espfc/src/Utils/Stats.h @@ -31,6 +31,7 @@ enum StatCounter { COUNTER_SERIAL, COUNTER_WIFI, COUNTER_BATTERY, + COUNTER_GPS_READ, COUNTER_CPU_0, COUNTER_CPU_1, COUNTER_COUNT diff --git a/lib/Gps/library.json b/lib/Gps/library.json new file mode 100644 index 00000000..1dc76ea9 --- /dev/null +++ b/lib/Gps/library.json @@ -0,0 +1,4 @@ +{ + "name": "Gps", + "version": "1.0.0" +} diff --git a/lib/Gps/src/Gps.hpp b/lib/Gps/src/Gps.hpp new file mode 100644 index 00000000..f45fb7ce --- /dev/null +++ b/lib/Gps/src/Gps.hpp @@ -0,0 +1,4 @@ +#pragma once + +#include "GpsProtocol.hpp" +#include "GpsParser.hpp" diff --git a/lib/Gps/src/GpsParser.hpp b/lib/Gps/src/GpsParser.hpp new file mode 100644 index 00000000..626ca80e --- /dev/null +++ b/lib/Gps/src/GpsParser.hpp @@ -0,0 +1,102 @@ +#pragma once + +#include "GpsProtocol.hpp" +#include + +namespace Gps { + +class UbxParser +{ +public: + void parse(uint8_t c, UbxMessage& m) + { + switch (m.status) + { + case UBX_STATE_READY: + case UBX_STATE_IDLE: + if (c == UBX_SYNC0) m.status = UBX_STATE_SYNC; + else m.status = UBX_STATE_IDLE; + break; + + case UBX_STATE_SYNC: + if (c == UBX_SYNC1) m.status = UBX_STATE_CLASS; + else m.status = UBX_STATE_IDLE; + break; + + case UBX_STATE_CLASS: + m.msgId = c; + m.status = UBX_STATE_ID; + break; + + case UBX_STATE_ID: + m.msgId |= c << 8; + m.status = UBX_STATE_LEN0; + break; + + case UBX_STATE_LEN0: + m.length = c; + m.status = UBX_STATE_LEN1; + break; + + case UBX_STATE_LEN1: + m.length |= ((uint16_t)c << 8); + if(m.length == 0) m.status = UBX_STATE_CRC0; + else if(m.length >= 511) m.status = UBX_STATE_IDLE; + else m.status = UBX_STATE_PAYLOAD; + break; + + case UBX_STATE_PAYLOAD: + if(m.written < 511) { + m.payload[m.written] = c; + m.written++; + } + if(m.written == m.length) m.status = UBX_STATE_CRC0; + break; + + case UBX_STATE_CRC0: + m.crc = c; + m.status = UBX_STATE_CRC1; + break; + + case UBX_STATE_CRC1: + m.crc |= c << 8; + if(m.crc == m.checksum()) m.status = UBX_STATE_READY; + else m.status = UBX_STATE_IDLE; + break; + } + } +}; + +class NmeaParser +{ +public: + void parse(uint8_t c, NmeaMessage& m) + { + switch(m.status) + { + case NMEA_STATE_READY: + case NMEA_STATE_IDLE: + if(c == '$') m.status = NMEA_STATE_PAYLOAD; + break; + + case NMEA_STATE_PAYLOAD: + if (c == '\r') m.status = NMEA_STATE_END; + else if (c == '\n') m.status = NMEA_STATE_IDLE; + else { + if (m.length < 511) { + m.payload[m.length++] = c; + m.payload[m.length] = '\0'; + } + } + break; + + case NMEA_STATE_END: + if (c == '\n') { + m.status = NMEA_STATE_READY; + } + break; + } + } +}; + +} diff --git a/lib/Gps/src/GpsProtocol.hpp b/lib/Gps/src/GpsProtocol.hpp new file mode 100644 index 00000000..187356ab --- /dev/null +++ b/lib/Gps/src/GpsProtocol.hpp @@ -0,0 +1,433 @@ +#pragma once + +#include +#include +#include + +namespace Gps { + +enum DeviceVersion { + GPS_UNKNOWN, + GPS_M8, + GPS_M9, + GPS_F9, +}; + +constexpr uint8_t UBX_SYNC0 = 0xB5; +constexpr uint8_t UBX_SYNC1 = 0x62; +constexpr uint16_t UBX_SYNC = UBX_SYNC1 << 8 | UBX_SYNC0; + +enum MsgClass: uint8_t +{ + UBX_ACK = 0x05, + UBX_CFG = 0x06, + UBX_MON = 0x0A, + UBX_NAV = 0x01, + UBX_HNR = 0xB5, + NMEA_MSG = 0xF0, +}; + +enum MsgId: uint16_t +{ + UBX_ACK_NAK = 0x00 << 8 | UBX_ACK, // Message not acknowledged + UBX_ACK_ACK = 0x01 << 8 | UBX_ACK, // Message acknowledged + + UBX_MON_VER = 0x04 << 8 | UBX_MON, // Receiver and software version + + UBX_CFG_CFG = 0x09 << 8 | UBX_CFG, // Clear, save and load configurations + UBX_CFG_DAT = 0x06 << 8 | UBX_CFG, // Set/Set datum + UBX_CFG_PRT = 0x00 << 8 | UBX_CFG, // Port configuration for UART ports + UBX_CFG_MSG = 0x01 << 8 | UBX_CFG, // Set message rate(s) + UBX_CFG_INF = 0x02 << 8 | UBX_CFG, // Information message configuration + UBX_CFG_RST = 0x04 << 8 | UBX_CFG, // Reset receiver / Clear backup data + UBX_CFG_RATE = 0x08 << 8 | UBX_CFG, // Navigation/measurement rate settings + UBX_CFG_SBAS = 0x16 << 8 | UBX_CFG, // SBAS configuration + UBX_CFG_NAV5 = 0x24 << 8 | UBX_CFG, // Navigation engine settings + + UBX_NAV_HPOSECEF = 0x13 << 8 | UBX_NAV, // High precision position solution in ECEF (28 Bytes) + UBX_NAV_HPOSLLH = 0x14 << 8 | UBX_NAV, // High precision geodetic position solution (36 Bytes) + UBX_NAV_POSECEF = 0x01 << 8 | UBX_NAV, // Position solution in ECEF (20 Bytes) + UBX_NAV_POSLLH = 0x02 << 8 | UBX_NAV, // Geodetic position solution (28 Bytes) + UBX_NAV_PVT = 0x07 << 8 | UBX_NAV, // Navigation position velocity time solution (92 Bytes) + UBX_NAV_SOL = 0x06 << 8 | UBX_NAV, // Navigation solution information (52 Bytes) + UBX_NAV_STATUS = 0x03 << 8 | UBX_NAV, // Receiver navigation status (16 Bytes) + UBX_NAV_SVINFO = 0x30 << 8 | UBX_NAV, // Space vehicle information (8 + 12xNumCh Bytes) + UBX_NAV_DOP = 0x04 << 8 | UBX_NAV, // Dilution of precision (18 Bytes) + UBX_NAV_SAT = 0x35 << 8 | UBX_NAV, // Satellite information (8 + 12xNumSats Bytes) + UBX_NAV_TIMEUTC = 0x21 << 8 | UBX_NAV, // UTC time solution (20 Bytes) + UBX_NAV_VELNED = 0x12 << 8 | UBX_NAV, // Velocity solution in NED frame (36 Bytes) + + UBX_HNR_PVT = 0x00 << 8 | UBX_HNR, // High rate output of PVT solution (72 Bytes) + + NMEA_MSG_GGA = 0x00 << 8 | NMEA_MSG, // Global positioning system fix data + NMEA_MSG_GLL = 0x01 << 8 | NMEA_MSG, // Latitude and longitude, with time of position fix and status + NMEA_MSG_GSA = 0x02 << 8 | NMEA_MSG, // GNSS DOP and active satellites + NMEA_MSG_GSV = 0x03 << 8 | NMEA_MSG, // GNSS satellites in view + NMEA_MSG_RMC = 0x04 << 8 | NMEA_MSG, // Recommended minimum data + NMEA_MSG_VTG = 0x05 << 8 | NMEA_MSG, // Course over ground and ground speed +}; + +inline uint16_t ubxChecksum(uint16_t init, uint8_t v) +{ + uint8_t a = ((init & 0xff) + v) & 0xff; + uint8_t b = ((init >> 8) + a) & 0xff; + return b << 8 | a; +} + +inline uint16_t ubxChecksum(uint16_t init, const uint8_t* it, size_t len) +{ + while(len--) + { + init = ubxChecksum(init, *it++); + } + return init; +} + +enum UbxState: uint8_t +{ + UBX_STATE_IDLE, + UBX_STATE_SYNC, + UBX_STATE_CLASS, + UBX_STATE_ID, + UBX_STATE_LEN0, + UBX_STATE_LEN1, + UBX_STATE_PAYLOAD, + UBX_STATE_CRC0, + UBX_STATE_CRC1, + UBX_STATE_READY, +}; + +class UbxMessage +{ +public: + UbxMessage() = default; + UbxMessage(const UbxMessage& m) = default; + UbxMessage& operator=(const UbxMessage& m) = default; + + UbxMessage(uint8_t mId): msgId(mId), length(0), written(0) {} + + uint16_t msgId = 0; + uint16_t length = 0; + uint8_t payload[512] = {0}; + uint16_t crc = 0; + + UbxState status{UBX_STATE_IDLE}; + uint16_t written = 0; + + uint16_t checksum() const + { + uint16_t c = 0; + c = ubxChecksum(c, msgId & 0xff); + c = ubxChecksum(c, msgId >> 8); + c = ubxChecksum(c, length & 0xff); + c = ubxChecksum(c, length >> 8); + for(size_t i = 0; i < length; i++) { + c = ubxChecksum(c, payload[i]); + } + return c; + } + + template + const Target* getAs() const + { + return reinterpret_cast(payload); + } + + bool isReady() const { return status == UBX_STATE_READY; } + bool isIdle() const { return status == UBX_STATE_IDLE; } + + bool isResponse(MsgId id) const { return msgId == id; } + + bool isAck() const { return msgId == UBX_ACK_ACK; } + bool isAck(MsgId id) const { return isAck() && id == (payload[1] << 8 | payload[0]); } + + bool isNak() const { return msgId == UBX_ACK_NAK; } + bool isNak(MsgId id) const { return isNak() && id == (payload[1] << 8 | payload[0]); } +}; + +// ----------------------------------------------------------------- + +enum NmeaState: uint8_t +{ + NMEA_STATE_IDLE, + NMEA_STATE_PAYLOAD, + NMEA_STATE_END, + NMEA_STATE_READY, +}; + +class NmeaMessage +{ +public: + char payload[512] = {0}; + + size_t length = 0; + NmeaState status{NMEA_STATE_IDLE}; + + bool isReady() const { return status == NMEA_STATE_READY; } + bool isIdle() const { return status == NMEA_STATE_IDLE; } +}; + +// -------------------------------------------------------------- + +class UbxMonVer +{ +public: + static constexpr MsgId ID = UBX_MON_VER; +} __attribute__((packed)); + +class UbxCfgMsg2 +{ +public: + static constexpr MsgId ID = UBX_CFG_MSG; + uint16_t msgId; +} __attribute__((packed)); + +class UbxCfgMsg3 +{ +public: + static constexpr MsgId ID = UBX_CFG_MSG; + uint16_t msgId; + uint8_t rate; +} __attribute__((packed)); + +class UbxCfgMsg8 +{ +public: + static constexpr MsgId ID = UBX_CFG_MSG; + uint16_t msgId; + uint8_t channels[6]; +} __attribute__((packed)); + +class UbxCfgPrt1 +{ +public: + static constexpr MsgId ID = UBX_CFG_PRT; + uint8_t portId; +} __attribute__((packed)); + +class UbxCfgPrt20 +{ +public: + static constexpr MsgId ID = UBX_CFG_PRT; + uint8_t portId; + uint8_t resered1; + uint16_t txReady; + uint32_t mode; + uint32_t baudRate; + uint16_t inProtoMask; + uint16_t outProtoMask; + uint16_t flags; + uint16_t resered2; +} __attribute__((packed)); + +class UbxCfgRate6 +{ +public: + static constexpr MsgId ID = UBX_CFG_RATE; + uint16_t measRate; // in ms + uint16_t navRate; // ratio to measRate + uint16_t timeRef; // 0-utc, 1-gps, 2-glonass (18+), 3-BeiDou (18+), 4-Galileo (18+), 5-NavIC (29+) +} __attribute__((packed)); + +class UbxCfgSbas8 +{ +public: + static constexpr MsgId ID = UBX_CFG_SBAS; + uint8_t mode; // SBAS mode flags + uint8_t usage; // SBAS usage flags + uint8_t maxSbas; // Maximum number of SBAS prioritized tracking channels (valid range: 0 - 3) to use + uint8_t scanmode2; // Continuation of scanmode bitmask + uint32_t scanmode1; // Which SBAS PRN numbers to search for (bitmask).If all bits are set to zero, auto-scan (i.e. allvalid PRNs) are searched. Every bit corresponds to a PRN number +} __attribute__((packed)); + +class UbxCfgNav5 +{ +public: + static constexpr MsgId ID = UBX_CFG_NAV5; + union { // Parameters bitmask. Only the masked parameters will be applied. + uint16_t value; + struct { + uint8_t dyn: 1; // Apply dynamic model settings + uint8_t minEl: 1; // Apply minimum elevation settings + uint8_t posFixMode: 1; // Apply fix mode settings + uint8_t drLim: 1; // Reserved + uint8_t posMask: 1; // Apply position mask settings + uint8_t timeMask: 1; // Apply time mask settings + uint8_t staticHoldMask: 1; // Apply static hold settings + uint8_t dgpsMask: 1; // Apply DGPS settings + uint8_t cnoThreshold: 1; // Apply CNO threshold settings (cnoThresh, cnoThreshNumSVs) + uint8_t reserved: 1; + uint8_t utc: 1; // Apply UTC settings + }; + } mask; + uint8_t dynModel; // Dynamic model, 0:portable, 2:stationary, 3:pedestrian, 4:automotive, 5:sea, 6:airbone<1g, 7:airbone<2g, 8: airbone<4g, 9:wrist watch, 10:motorbike, 11: lawnmower, 12: electric scooter + uint8_t fixMode; // Position fixing mode, 1: 2D only, 2: 3D only, 3: auto 2D/3D + int32_t fixedAlt; // Fixed altitude (mean sea level) for 2D fix mode, [m * 0.01] + uint32_t fixedAltVar; // Fixed altitude variance for 2D mode, [m^2 * 0.0001] + int8_t minElev; // Minimum elevation for a GNSS satellite to be used in NAV, [deg] + uint8_t drLimit; // reserved [s] + uint16_t pDOP; // Position DOP mask to use * 0.1 + uint16_t tDOP; // Time DOP mask to use * 0.1 + uint16_t pAcc; // Position accuracy mask [m] + uint16_t tAcc; // Time accuracy mask [m] + uint8_t staticHoldThresh; // Static hold threshold [cm/s] + uint8_t dgnssTimeout; // DGNSS timeout [s] + uint8_t cnoThreshNumSVs; // Number of satellites required to have C/N0 above cnoThresh for a fix to be attempted + uint8_t cnoThresh; // C/N0 threshold for deciding whether to attempt a fix + uint8_t reserved0[2]; + uint16_t staticHoldMaxDist; // Static hold distance threshold + uint8_t utcStandard; // UTC standard to be used, 0:auto, 3:USNaval Obs, 5: EURLabs, 6:Soviet, 7: NTSC (China), 8: NTPL (India) + uint8_t reserved1[5]; +} __attribute__((packed)); + +class UbxNavPvt92 +{ +public: + static constexpr MsgId ID = UBX_NAV_PVT; + uint32_t iTow; // ms + uint16_t year; // UTC year + uint8_t month; // 1..12 + uint8_t day; // 1..31 + uint8_t hour; // 0..23 + uint8_t min; // 0..59 + uint8_t sec; // 0..60 + union { + uint8_t value; + struct { + uint8_t validDate: 1; // 1 = valid UTC Date + uint8_t validTime: 1; // 1 = valid UTC time of day + uint8_t fullyResolved: 1; // 1 = UTC time of day has been fully resolved (no seconds uncertainty). Cannot be used to check if time is completely solved. + uint8_t validMag: 1; // 1 = valid magnetic declination + }; + } valid; + uint32_t tAcc; // time accuracy (ns) + int32_t nano; // Fraction of seconds (ns x -1e9..1e9) + uint8_t fixType; // 0: no-fix, 1: dead-recon, 2: 2D, 3: 3D, 4: GNSS+DR, 5: time fix only + union { + uint8_t value; + struct { + uint8_t gnssFixOk: 1; // 1 = valid fix (i.e within DOP & accuracy masks) + uint8_t diffSoln: 1; // 1 = differential corrections were applied + uint8_t psmState: 3; // Power save mode state + uint8_t headVehValid: 1; // 1 = heading of vehicle is valid, only set if the receiver is in sensor fusion mode + uint8_t carrSoln: 2; // Carrier phase range solution status: 0: no carrier, 1: carrier with floating ambiguities, 2: with fixed ambiguities + }; + } flags; // fix status flags + union { + uint8_t value; + struct { + uint8_t reserved : 5; + uint8_t confirmedAvai: 1; // 1 = information about UTC Date and Time of Day validity confirmation is available + uint8_t confirmedDate: 1; // 1 = UTC Date validity could be confirmed + uint8_t confirmedTime: 1; // 1 = UTC Time of Day could be confirmed + }; + } flags2; // additional flags + uint8_t numSV; // Number of satelites in Nav Sol + int32_t lon; // Longnitude, deg x 1e-7 + int32_t lat; // Latitude, deg x 1e-7 + int32_t height; // Height above allipsoid (mm) + int32_t hSML; // Height above mean sea level (mm) + uint32_t hAcc; // horizontal accuracy (mm) + uint32_t vAcc; // vertical accuracy (mm) + int32_t velN; // notrh velocity (mm/s) + int32_t velE; // east velocity (mm/s) + int32_t velD; // down velocity (mm/s) + int32_t gSpeed; // ground speed (mm/s, 2D) + int32_t headMot; // heading of motion (deg x 1e-5) + uint32_t sAcc; // speed accuracy (mm/s) + uint32_t headAcc; // heading accuracy (deg x 1e-5) + uint16_t pDOP; // position Dillution DOP (x 0.01) + union { + uint16_t value; + struct { + uint8_t invalidLlh: 1; // 1 = Invalid lon, lat, height and hMSL + uint8_t lastCorrectionAge: 4; // Age of the most recently received differential correction: 0: ?, 1: 0-1, 2: 1-2, 3: 2-5, 4: 5-10, 5: 10-15... + }; + } flags3; // additional flags + uint32_t reserved1; // reserved + int32_t headVeh; // heading of vehicle (deg x 1e-5), only if headVehValid flag is set + int16_t magDec; // Magnetic declination (deg x 1e-2), only in ADR4.10+ + uint16_t magAcc; // Magnetic declination accuracy (deg x 1e-2), only in ADR4.10+ +} __attribute__((packed)); + +class UbxNavSat +{ +public: + static constexpr MsgId ID = UBX_NAV_SAT; + uint32_t iTow; // time of week + uint8_t version; // message version (0x01) + uint8_t numSvs; // number of satelites + uint16_t reserved1; + struct { + uint8_t gnssId; // GNSS ID + uint8_t svId; // Satelite ID + uint8_t cno; // dBHz + int8_t elev; // deg +/-90 + int16_t azim; // deg 0-360 + int16_t prRes; // Pseudorange residual + union { + uint32_t value; + struct { + uint8_t qualityInd: 3; // quality indicatopr: 0-no signal, 1-searching, 2-aquired, 3-unstable, 4-code locked, 5,6,7-code and carrier locked + uint8_t svUsed: 1; // used for navigation + uint8_t health: 2; // signal health 0-unknown, 1-healthy, 2-unhealty + uint8_t difCorr: 1; // differential correction available for this SV + uint8_t smoothed: 1; // carrier smotthed pseudorange used + uint8_t orbitSource: 3; // orbit source: 0-no inform, 1-ephemeris, 2-almanac, 3-assistnow offline, 4-assistnow autonomous, 5,6,7-other + uint8_t ephAvail: 1; // ephemeris available + uint8_t elmAvail: 1; // almanac available + uint8_t enoAvail: 1; // assistnow offline available + uint8_t eopAvail: 1; // assistnow autonomous available + uint8_t reserved: 1; + uint8_t sbasCorrUsed: 1; // SBAS corrections used + uint8_t rtcmCorrUsed: 1; // RTCM corrections used + uint8_t slasCorrUsed: 1; // SLAS corrections used + uint8_t spartnCorrUsed: 1; // SPARTN corrections used + uint8_t prCorrUsed: 1; // Pseudorange corrections used + uint8_t crCorrUsed: 1; // Carrier range corrections used + uint8_t doCorrUsed: 1; // Range rate (Doppler) corrections used + uint8_t clasCorrUsed: 1; // CLAS corrections used + }; + } flags; + } sats[]; +} __attribute__((packed)); + + +// ---------------------------------------------------------------------------------------- + +template +class UbxFrame +{ + static_assert(sizeof(MsgType) != 0, "MsgType cannot be empty!"); +public: + UbxFrame(const MsgType& m): msg(m) + { + const uint8_t* it = reinterpret_cast(this) + 2; // start after sync headers + crc = ubxChecksum(0, it, sizeof(MsgType) + 4); // plus msgClass + msgId + length + } + + const uint16_t hdr = UBX_SYNC; + const MsgId id = MsgType::ID; + const uint16_t length = sizeof(MsgType); + const MsgType msg; + uint16_t crc; +} __attribute__((packed)); + +// specialization for zero-size messages +template +class UbxFrame::value>::type> +{ +public: + UbxFrame(const MsgType& m) + { + const uint8_t* it = reinterpret_cast(this) + 2; // start after sync headers + crc = ubxChecksum(0, it, 4); // only msgClass + msgId + length + } + + const uint16_t hdr = UBX_SYNC; + const MsgId id = MsgType::ID; + const uint16_t length = 0; + uint16_t crc; +} __attribute__((packed)); + +} diff --git a/lib/betaflight/src/platform.c b/lib/betaflight/src/platform.c index 9274a8a1..b65de362 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 f5e31b18..1419d65e 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" diff --git a/src/main.cpp b/src/main.cpp index 45f14bce..7cea1755 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #if defined(ESPFC_ESPNOW) #include #endif diff --git a/test/test_esc/test_esc.cpp b/test/test_esc/test_esc.cpp index 547237c3..71d4112c 100644 --- a/test/test_esc/test_esc.cpp +++ b/test/test_esc/test_esc.cpp @@ -1,10 +1,6 @@ #include #include #include -#include -#include -#include "msp/msp_protocol.h" -#include void test_esc_dshot_encode() { diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index cfac4ebd..32ad1fd4 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -1,11 +1,11 @@ #include #include -#include #include "Utils/Timer.h" #include "Model.h" #include "Control/Controller.h" #include "Control/Actuator.h" #include "Output/Mixer.h" +#include using namespace fakeit; using namespace Espfc; diff --git a/test/test_input_crsf/test_input_crsf.cpp b/test/test_input_crsf/test_input_crsf.cpp index 4863647c..3bcf8980 100644 --- a/test/test_input_crsf/test_input_crsf.cpp +++ b/test/test_input_crsf/test_input_crsf.cpp @@ -2,11 +2,8 @@ #include #include "Device/InputCRSF.h" #include "Device/InputIBUS.hpp" -#include -#include -#include #include "msp/msp_protocol.h" -#include +#include using namespace Espfc; using namespace Espfc::Device; diff --git a/test/test_math/test_math.cpp b/test/test_math/test_math.cpp index bef47759..218155d1 100644 --- a/test/test_math/test_math.cpp +++ b/test/test_math/test_math.cpp @@ -1,15 +1,14 @@ #include #include -#include #include -#include "msp/msp_protocol.h" +#include #include "Utils/Math.hpp" #include "Utils/Bits.hpp" #include "Utils/Filter.h" #include "Control/Pid.h" #include "Target/QueueAtomic.h" #include "Utils/RingBuf.h" -#include +#include // void setUp(void) { // // set stuff up here diff --git a/test/test_msp/test_msp.cpp b/test/test_msp/test_msp.cpp index 68688bc1..79a02e3e 100644 --- a/test/test_msp/test_msp.cpp +++ b/test/test_msp/test_msp.cpp @@ -8,6 +8,7 @@ #include "Connect/Msp.hpp" #include "Connect/MspParser.hpp" #include "msp/msp_protocol.h" +#include using namespace fakeit; using namespace Espfc;