Gps snapshot 1

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

lib/EscDriver/src/EscDriverBase.cpp

@@ -1,6 +1,22 @@
 #include "EscDriverBase.hpp"
 #include <Arduino.h>
 
+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;

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() {}

lib/EscDriver/src/EscDriverEsp32.cpp

@@ -2,7 +2,7 @@
 
 #include <Arduino.h>
 #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"

lib/Espfc/src/Blackbox/Blackbox.cpp

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

lib/Espfc/src/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)

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,19 +74,18 @@ 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++)
       {
         write(c[i]);
       }
       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;

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