diff --git a/README.md b/README.md index 1b3d127b..33875ca1 100644 --- a/README.md +++ b/README.md @@ -2,32 +2,34 @@ The mini, DIY, ~$5 cost, ESP8266/ESP32 based, high performance flight controller. ## Features +* Espressif targets (ESP32, ESP8266, ESP32-S3, ESP32-S2, ESP32-C3) +* ESC protocols (PWM, Oneshot125/42, Multishot, Brushed, Dshot150/300/600 bidirectional) +* PPM, SBUS and CRSF Receivers +* Builtin ESP-NOW receiver and WiFi configuration [read more...](/docs/wireless.md) +* SPI and I2C gyro modules support (MPU6050, MPU9250, ICM20602, BMI160) +* Flight modes (ACRO, ANGLE, AIRMODE) * Frames (Quad X) -* Betaflight configuration tool compatible (v10.8 or v10.9) -* Receiver protocol (8 channel PPM) -* SBUS and CRSF Serial Rx protocols on ESP32 and RP2040 -* ESC protocols (PWM, Oneshot125, Brushed, Dshot150, Dshot300, Dshot600) -* Configurable Gyro Filters (LPF, Notch, dTerm) -* Blackbox recording (OpenLog/Opelager serial) -* In flight PID Tuning -* Flight modes (ACRO, ANGLE, AIRMODE, ARM) -* Up to 8kHz gyro/loop on ESP32 with SPI gyro +* Betaflight configuration tool compatible (v10.8-v10.10) +* Configurable Gyro Filters (LPF, Notch, dTerm, RPM) +* Blackbox recording (OpenLog/OpenLager/Flash) +* Up to 4kHz gyro/loop on ESP32 with SPI gyro * MSP protocol interface * CLI Interface * Resorce/Pin mapping +* In flight PID Tuning * Buzzer * Lipo voltage monitor -* Failsafe +* Failsafe mode ## Requirements Hardware: -* ESP8266 Wemos D1 Mini or ESP32 mini board -* MPU6050 I2C gyro (GY-88, GY-91, GY-521 or similar) or MPU9250 SPI on ESP32 +* ESP32 mini board or ESP8266 Wemos D1 mini or similar +* MPU9250 SPI or MPU6050 I2C gyro (GY-88, GY-91, GY-521 or similar) * PDB with 5V BEC * Buzzer and some electronic components (optional). Software: -* [Betaflight Configurator](https://github.com/betaflight/betaflight-configurator/releases) (v10.8 or v10.9) +* [Betaflight Configurator](https://github.com/betaflight/betaflight-configurator/releases) (v10.10 or v10.9 or v10.8) * [CH340 usb-serial converter driver](https://sparks.gogo.co.nz/ch340.html) ## Flashing @@ -38,6 +40,8 @@ Software: 5. Click "Program" 6. After success power cycle board +Note: only ESP32 and ESP8266 can be flashed in this way. + ![ESP-FC Flashing](/docs/images/esptool-js-flash-connect.png) ## Setup @@ -75,9 +79,8 @@ Here are more details about [how to setup](/docs/setup.md). | MSP | Yes | Yes | Yes | | CLI | Yes | Yes | Yes | | PPM (IN) | Yes | Yes | Yes | -| SBUS | - | Yes | Yes | -| IBUS | - | - | - | -| CRSF (ELRS) | - | Yes | Yes | +| SBUS | Yes | Yes | Yes | +| CRSF (ELRS) | Yes | Yes | Yes | | BLACKBOX | Yes | Yes | Yes | | PWM (OUT) | Yes | Yes | Yes | | ONESHOT125 | Yes | Yes | Yes | @@ -124,7 +127,6 @@ You can also join to our [Discord Channel](https://discord.gg/jhyPPM5UEH) * Serial Rx (IBUS) * ELRS telemetry * ESP32-S2/S3/C3 targets -* BiDir Dshot * Baro (MS5611) * GPS navigation diff --git a/docs/calculations.ods b/docs/calculations.ods index 66bdd179..d33bd375 100644 Binary files a/docs/calculations.ods and b/docs/calculations.ods differ diff --git a/docs/images/espfc_receiver.png b/docs/images/espfc_receiver.png new file mode 100644 index 00000000..c8523c7b Binary files /dev/null and b/docs/images/espfc_receiver.png differ diff --git a/docs/images/espfc_wifi_ap_connect.png b/docs/images/espfc_wifi_ap_connect.png new file mode 100644 index 00000000..a1fbcc7a Binary files /dev/null and b/docs/images/espfc_wifi_ap_connect.png differ diff --git a/docs/images/espfc_wifi_ap_enable.png b/docs/images/espfc_wifi_ap_enable.png new file mode 100644 index 00000000..08c0a7d6 Binary files /dev/null and b/docs/images/espfc_wifi_ap_enable.png differ diff --git a/docs/setup.md b/docs/setup.md index 6665f0d7..e6eb00c8 100644 --- a/docs/setup.md +++ b/docs/setup.md @@ -44,6 +44,13 @@ If you want to use Serial based receiver (SBUS,CRSF), you need to allocate UART Then go to the `Receiver` tab, and select `Receiver mode` and `Serial Receiver Provider`. +To use Serial RX on ESP8266, you need to connect reciver to primary UART. In this case you lose ability to configure through UART. +To deal with it activate "SOFTSERIAL" feature. In this case if board stay in failsafe mode for 30 seconds (transmitter is turned off), then automatically WiFi access point will be started. +Default name is "ESP-FC". After connecting to this AP open configurator, select "manual" port and type port: `tcp://192.168.4.1:1111`. +You can also configure board to automatically connect existing wifi network. In this case set `wifi_ssid` and `wifi_pass` parameters in CLI. + +To use ESP-NOW receiver, choose "SPI Rx (e.g. built-in Rx)" receiver mode in Receiver tab. You need compatible transmitter module. Read more about [ESP-FC Wireless Functions](/docs/wireless.md) + ## Motor setup In `Motors` tab you must configure @@ -92,7 +99,7 @@ This option allows to separate PWM frequency. If your pid loop is set to 1k, but ## Flight modes -Flight modes can be configured in modes tab +Flight modes can be configured in `Modes` tab ### Arm @@ -125,8 +132,7 @@ Logging using serial device is possible, like [D-ronin OpenLager](https://github Recommended settings -- To log with 1k rate, you need device that is capable to receive data with 250kbps. -- To log with 1k rate with debug, baro and mag data, 500kbps is recommended. +- To log with 1k rate, you need device that is capable to receive data with 500kbps. - To log with 2k rate, you need 1Mbps OpenLager can handle it easily. If you plan to use OpenLog, you might need to flash [blackbox-firmware](https://github.com/cleanflight/blackbox-firmware) to be able to handle more than 115.2kbps. Either 250kbps and 500kbps works well with modern SD cards up to 32GB size. diff --git a/docs/wireless.md b/docs/wireless.md new file mode 100644 index 00000000..ad69a505 --- /dev/null +++ b/docs/wireless.md @@ -0,0 +1,44 @@ +# ESP-FC Wireless functions + +Espressif modules have a built-in WiFi module that can be used for configuration and control. + +## WiFi configuration + +> [!NOTE] To be able to configure device using WiFi connection, you need to enable `SOFTSERIAL` function. WiFi function can only be used to configure device. Whilst WiFi is active, it is not possible to ARM controller and reboot is required. + +![Enable WiFi](/docs/images/espfc_wifi_ap_enable.png) + +WiFi will start its own Access-Point if board will not detect receiver signal for at least 30 seconds (stay in failsafe mode since boot). Name of this AP is `ESP-FC` and it is open network. If you connect to this AP, then choose `Manual Selection` and type `tcp://192.168.4.1:1111` + +![Connect to ESP-FC AP](/docs/images/espfc_wifi_ap_connect.png) + +You can also automatically connect ESP-FC to your home network. To achive that, go to the CLI tab, and enter your network name and secret. +``` +set wifi_ssid MY-HOME-NET +set wifi_pass MY-HOME-PASS +``` +> [!NOTE] Network name and pass cannot contains spaces. + +then you can check status of connection by typing `wifi` in CLI tab. +``` +wifi +ST IP4: tcp://0.0.0.0:1111 +ST MAC: 30:30:F9:6E:10:74 +AP IP4: tcp://192.168.4.1:1111 +AP MAC: 32:30:F9:6E:10:74 +``` +In this mode you need to discover IP address granted by DHCP. In line `ST IP4` is the address that you need to use in configurator. If there is `0.0.0.0`, that means that FC was not able to connect to home network. + +## ESP-NOW control + +ESP-NOW is a proprietary wireless communication protocol defined by Espressif, which enables the direct, quick and low-power control of smart devices, without the need of a router. + +As there are no real transmitters usng this protocol on the market, you have to build your own transmitting module first. If you alredy own RC transmitter with JR bay, you can use another ESP32 module. In this case follow [espnow-rclink-tx](https://github.com/rtlopez/espnow-rclink-tx) instructions. + +In ESP-FC you have to choose `SPI Rx (e.g. built-in Rx)` as Receiver mode in Receiver tab. + +![ESP-FC ESP-NOW Reciever](/docs/images/espfc_receiver.png) + +Transmitter and receiver binds automatically after power up, you don't need to do anything. Recomended startup procedure is: +1. turn on transmitter first +2. next power up receiver/flight controller diff --git a/lib/AHRS/src/Mahony.cpp b/lib/AHRS/src/Mahony.cpp index 6b74fc62..c112b5a1 100644 --- a/lib/AHRS/src/Mahony.cpp +++ b/lib/AHRS/src/Mahony.cpp @@ -22,6 +22,13 @@ // Header files #include "Mahony.h" +#include + +#ifdef ESP32 +#define FAST_CODE_IMU_ATTR IRAM_ATTR +#else +#define FAST_CODE_IMU_ATTR +#endif //------------------------------------------------------------------------------------------- // Definitions @@ -165,7 +172,7 @@ void Mahony::update(float gx, float gy, float gz, float ax, float ay, float az, //------------------------------------------------------------------------------------------- // IMU algorithm update -void Mahony::update(float gx, float gy, float gz, float ax, float ay, float az) +void FAST_CODE_IMU_ATTR Mahony::update(float gx, float gy, float gz, float ax, float ay, float az) { float recipNorm; float halfvx, halfvy, halfvz; diff --git a/lib/EscDriver/src/EscDriverEsp32.cpp b/lib/EscDriver/src/EscDriverEsp32.cpp index b1f237b6..9700f5e4 100644 --- a/lib/EscDriver/src/EscDriverEsp32.cpp +++ b/lib/EscDriver/src/EscDriverEsp32.cpp @@ -60,6 +60,16 @@ static void IRAM_ATTR _rmt_zero_mem(rmt_channel_t channel, size_t len) } } +// using map() in isr cause crash +static long IRAM_ATTR _map(long x, long in_min, long in_max, long out_min, long out_max) +{ + const long in_range = in_max - in_min; + if(in_range == 0) return out_min; + const long out_range = out_max - out_min; + const long delta = x - in_min; + return (delta * out_range) / in_range + out_min; +} + bool EscDriverEsp32::_tx_end_installed = false; EscDriverEsp32 *EscDriverEsp32::instances[] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL}; @@ -205,18 +215,14 @@ void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse) void EscDriverEsp32::modeTx(rmt_channel_t channel) { - //PIN_DEBUG(HIGH); disableRx(channel); enableTx(channel); - //PIN_DEBUG(LOW); } void EscDriverEsp32::modeRx(rmt_channel_t channel) { - //PIN_DEBUG(HIGH); disableTx(channel); enableRx(channel); - //PIN_DEBUG(LOW); } void EscDriverEsp32::enableTx(rmt_channel_t channel) @@ -266,7 +272,6 @@ void EscDriverEsp32::disableRx(rmt_channel_t channel) void EscDriverEsp32::txDoneCallback(rmt_channel_t channel, void *arg) { - //PIN_DEBUG(HIGH); auto instance = instances[channel]; if(!instance) return; if(instance->_async) @@ -277,7 +282,6 @@ void EscDriverEsp32::txDoneCallback(rmt_channel_t channel, void *arg) { instance->modeRx(channel); } - //PIN_DEBUG(LOW); } void EscDriverEsp32::transmitOne(uint8_t i) @@ -318,48 +322,25 @@ void EscDriverEsp32::transmitAll() void EscDriverEsp32::readTelemetry() { - //PIN_DEBUG(HIGH); for (size_t i = 0; i < ESC_CHANNEL_COUNT; i++) { if(!_digital || !_dshot_tlm) continue; if(!_channel[i].attached()) continue; - //HardwareSerial* s = (i == 0 && _timer.check()) ? &Serial : nullptr; - RingbufHandle_t rb = NULL; if(ESP_OK != rmt_get_ringbuf_handle((rmt_channel_t)i, &rb)) continue; size_t rmt_len = 0; rmt_item32_t* data = (rmt_item32_t*)xRingbufferReceive(rb, &rmt_len, 0); - //if(s) { s->print((uint8_t)rmt_len); } if (data) { - /*for(size_t j = 0; j < rmt_len >> 2; j++) - { - if(!data[j].duration0) break; - if(s) s->print(' '); - if(s) s->print(data[j].level0); - if(s) s->print(':'); - if(s) s->print(data[j].duration0); - - if(!data[j].duration1) break; - if(s) s->print(' '); - if(s) s->print(data[j].level1); - if(s) s->print(':'); - if(s) s->print(data[j].duration1); - }*/ uint32_t value = extractTelemetryGcr((uint32_t*)data, rmt_len >> 2, _channel[i].dshot_tlm_bit_len); - //if(s) s->print(' '); - //if(s) s->print(value, HEX); - vRingbufferReturnItem(rb, (void *)data); _channel[i].telemetryValue = value; } - //if(s) s->print('\n'); } - //PIN_DEBUG(LOW); } void EscDriverEsp32::writeAnalogCommand(uint8_t channel, int32_t pulse) @@ -373,7 +354,7 @@ void EscDriverEsp32::writeAnalogCommand(uint8_t channel, int32_t pulse) maxPulse = 2000; } pulse = constrain(pulse, minPulse, maxPulse); - int duration = map(pulse, 1000, 2000, slot.pulse_min, slot.pulse_max); + int duration = _map(pulse, 1000, 2000, slot.pulse_min, slot.pulse_max); int count = 2; if (_async) diff --git a/lib/EscDriver/src/EscDriverEsp32.h b/lib/EscDriver/src/EscDriverEsp32.h index c79b110c..2b48cc95 100644 --- a/lib/EscDriver/src/EscDriverEsp32.h +++ b/lib/EscDriver/src/EscDriverEsp32.h @@ -82,10 +82,10 @@ class EscDriverEsp32: public EscDriverBase int begin(const EscConfig& conf); void end(); int attach(size_t channel, int pin, int pulse); - int write(size_t channel, int pulse); - void apply(); + int write(size_t channel, int pulse) IRAM_ATTR; + void apply() IRAM_ATTR; int pin(size_t channel) const; - uint32_t telemetry(size_t channel) const; + uint32_t telemetry(size_t channel) const IRAM_ATTR; private: void initChannel(int channel, gpio_num_t pin, int pulse); @@ -97,8 +97,8 @@ class EscDriverEsp32: public EscDriverBase void disableRx(rmt_channel_t channel) IRAM_ATTR; static void txDoneCallback(rmt_channel_t channel, void *arg) IRAM_ATTR; void transmitOne(uint8_t i) IRAM_ATTR; - void transmitAll(); - void readTelemetry(); + void transmitAll() IRAM_ATTR; + void readTelemetry() IRAM_ATTR; void writeAnalogCommand(uint8_t channel, int32_t pulse) IRAM_ATTR; void writeDshotCommand(uint8_t channel, int32_t pulse) IRAM_ATTR; void transmitCommand(uint8_t channel) IRAM_ATTR; diff --git a/lib/EspGpio/library.json b/lib/EspGpio/library.json deleted file mode 100644 index 3b304555..00000000 --- a/lib/EspGpio/library.json +++ /dev/null @@ -1,3 +0,0 @@ -{ - "name": "EspGpio" -} diff --git a/lib/EspGpio/src/EspGpio.h b/lib/EspGpio/src/EspGpio.h deleted file mode 100644 index 1dc9c157..00000000 --- a/lib/EspGpio/src/EspGpio.h +++ /dev/null @@ -1,65 +0,0 @@ -#ifndef _ESP_GPIO_H_ -#define _ESP_GPIO_H_ - -#include -#if defined(ESP32) -#include "hal/gpio_ll.h" -#endif - -#if defined(ARCH_RP2040) -typedef PinStatus pin_status_t; -#else -typedef uint8_t pin_status_t; -#endif - -class EspGpio -{ - public: - static inline void IRAM_ATTR digitalWrite(uint8_t pin, pin_status_t val) - { -#if defined(ESP8266) - if(pin < 16) - { - if(val) GPOS = (1 << pin); - else GPOC = (1 << pin); - } - else if(pin == 16) - { - if(val) GP16O |= 1; - else GP16O &= ~1; - } -#elif defined(ESP32) - //::gpio_set_level((gpio_num_t)pin, val); - gpio_ll_set_level(&GPIO, (gpio_num_t)pin, val); -#elif defined(UNIT_TEST) - // do nothing -#else - ::digitalWrite(pin, val); -#endif - } - - static inline pin_status_t IRAM_ATTR digitalRead(uint8_t pin) - { -#if defined(ESP8266) - if(pin < 16) - { - return GPIP(pin); - } - else if(pin == 16) - { - return GP16I & 0x01; - } - return 0; -#elif defined(ESP32) - //return ::gpio_get_level((gpio_num_t)pin); - return ::gpio_ll_get_level(&GPIO, (gpio_num_t)pin); -#elif defined(UNIT_TEST) - return 0; - // do nothing -#else - return ::digitalRead(pin); -#endif - } -}; - -#endif // _ESP_GPIO_H_ diff --git a/lib/EspSoftSerial/library.json b/lib/EspSoftSerial/library.json deleted file mode 100644 index 66f6aabf..00000000 --- a/lib/EspSoftSerial/library.json +++ /dev/null @@ -1,3 +0,0 @@ -{ - "name": "EspSoftSerial" -} diff --git a/lib/EspSoftSerial/src/Buffer.h b/lib/EspSoftSerial/src/Buffer.h deleted file mode 100644 index ad3baaae..00000000 --- a/lib/EspSoftSerial/src/Buffer.h +++ /dev/null @@ -1,78 +0,0 @@ -#ifndef _BUFFER_H_ -#define _BUFFER_H_ - -#include - -template -class Buffer { -public: - Buffer(): _head(0), _tail(0), _count(0) {} - - void push(const T& item) - { - _buf[_head] = item; - _head = (_head + 1) % S; - if(_head == _tail) { - _tail = (_tail + 1) % S; - } - _count++; - if((size_t)_count > S - 1) { - _count = S - 1; - } - } - - T get() - { - if(empty()) return T(); - - //Read data and advance the tail (we now have a free space) - T val = _buf[_tail]; - _tail = (_tail + 1) % S; - - _count--; - if(_count < 0) _count = 0; - - return val; - } - - T peek(void) const - { - if(empty()) return T(); - return _buf[_tail]; - } - - void reset(void) - { - _head = _tail; - } - - bool empty(void) const - { - //if head and tail are equal, we are empty - return _head == _tail; - } - - bool full(void) const - { - //If tail is ahead the head by 1, we are full - return ((_head + 1) % S) == _tail; - } - - int count() const - { - return _count; - } - - size_t size() const - { - return S - 1; - } - -private: - size_t _head; - size_t _tail; - int _count; - T _buf[S]; -}; - -#endif diff --git a/lib/EspSoftSerial/src/EspSoftSerial.cpp b/lib/EspSoftSerial/src/EspSoftSerial.cpp deleted file mode 100644 index 1681b597..00000000 --- a/lib/EspSoftSerial/src/EspSoftSerial.cpp +++ /dev/null @@ -1,219 +0,0 @@ -#include "EspSoftSerial.h" - -#if defined(ESP8266) - -#define TIMER0_US_CLOCKS (F_CPU / 1000000L) -#define TIMER1_US_CLOCKS (APB_CLK_FREQ / 1000000L) - -//#define USE_TIMER0 -#define USE_TIMER1 -#define USE_TIMER1_NMI - -#if defined(USE_TIMER0) - #define TIMER_DELAY_MIN 200 -#elif defined(USE_TIMER1) - #define TIMER_DELAY_MIN 100 -#else - #error "Timer not defined" -#endif - -uint32_t EspSoftSerial::nsToTicks(uint32_t ns) -{ -#if defined(USE_TIMER0) - return (ns * TIMER0_US_CLOCKS) / 1000L; // timer0 -#elif defined(USE_TIMER1) - return (ns * TIMER1_US_CLOCKS) / 1000L; // timer1 -#else - #error "Timer not defined" -#endif -} - -#if defined(USE_TIMER1) && !defined(USE_TIMER1_NMI) -static void IRAM_ATTR _rx_read_bit_isr(void * p) -{ - EspSoftSerial::rx_read_bit_isr(); -} -#endif - -void EspSoftSerial::timerInit() -{ -#if defined(USE_TIMER0) - noInterrupts(); - timer0_isr_init(); - timer0_attachInterrupt(EspSoftSerial::rx_read_bit_isr); - timer0_write(ESP.getCycleCount() + nsToTicks(10000000)); // 10ms - interrupts(); -#elif defined(USE_TIMER1) - ETS_INTR_LOCK(); - ETS_INTR_DISABLE(ETS_FRC_TIMER1_INUM); - T1C = 0; - T1I = 0; - #if defined(USE_TIMER1_NMI) - NmiTimSetFunc(EspSoftSerial::rx_read_bit_isr); - #else - ets_isr_attach(ETS_FRC_TIMER1_INUM, _rx_read_bit_isr, NULL); - #endif - ETS_INTR_ENABLE(ETS_FRC_TIMER1_INUM); - T1C = (1 << TCTE) | (TIM_DIV1 << TCPD) | (TIM_EDGE << TCIT) | (TIM_SINGLE << TCAR); - T1I = 0; - ETS_INTR_UNLOCK(); -#else - #error "Timer not defined" -#endif -} - -void EspSoftSerial::timerStop() -{ -#if defined(USE_TIMER0) - noInterrupts(); - timer0_detachInterrupt(); - interrupts(); -#elif defined(USE_TIMER1) - ETS_INTR_LOCK(); - ETS_INTR_DISABLE(ETS_FRC_TIMER1_INUM); - ETS_INTR_UNLOCK(); -#else - #error "Timer not defined" -#endif -} - -static void IRAM_ATTR timerWrite(uint32_t ticks) -{ -#if defined(USE_TIMER0) - timer0_write(ESP.getCycleCount() + ticks); -#elif defined(USE_TIMER1) - //timer1_write(ticks); - T1L = ((ticks) & 0x7FFFFF); //23 - TEIE |= TEIE1; //13 -#else - #error "Timer not defined" -#endif -} - -static inline bool IRAM_ATTR _readPin(int pin, bool invert) -{ - return (bool)(GPI & (1 << pin)) ^ invert; - /*int c = 3; - int v = 0; - while(c--) - { - //v += (bool)GPIP(pin); - v += (bool)(GPI & (1 << pin)); - } - v = v >> 1; // calc avg of 3 samples - return invert ? !v : v;*/ -} - -void EspSoftSerial::rxReadBit() -{ -#if defined(USE_TIMER1) - TEIE &= ~TEIE1; //14 - T1I = 0; //9 -#endif - - //GP16O = 1; - switch(_rx_state) - { - case STATE_DATA: - timerWrite(_delay); - _rx_data >>= 1; - if(_readPin(_conf.rx_pin, _conf.inverted)) _rx_data |= 0x80; - if(++_rx_data_count >= _conf.data_bits) - { - _rx_state =_conf.parity_type ? STATE_PARITY : STATE_STOP; - _rx_data_count = 0; - } - break; - - case STATE_PARITY: - timerWrite(_delay); - if(_readPin(_conf.rx_pin, _conf.inverted)) _rx_data |= 0x100; - _rx_state = STATE_STOP; - break; - - case STATE_STOP: - _rx_buff.push((int16_t)(_rx_data & 0xffff)); - _rx_data = 0; - _rx_state = STATE_IDLE; - // no break here, call idle - - case STATE_IDLE: - default: -#if defined(USE_TIMER0) - timerWrite(_delay * 5000); // avoid wdt reset and wait ~5k bits for rxStart() call -#endif - break; - } - //GP16O = 0; -} - -void EspSoftSerial::rxStart() -{ - switch(_rx_state) - { - case STATE_IDLE: - _rx_state = STATE_DATA; - if(_delay_start > TIMER_DELAY_MIN) { // needs at least 200 cpu clocks - timerWrite(_delay_start); - } else { - rxReadBit(); - } - break; - default: - break; - } -} - -void EspSoftSerial::rx_read_bit_isr() -{ - if(_instance) _instance->rxReadBit(); -} - -void EspSoftSerial::rx_start_isr() -{ - if(_instance) _instance->rxStart(); -} - -int EspSoftSerial::begin(int baud) -{ - EspSoftSerialConfig conf(baud, -1); - begin(conf); - return 1; -} - -int EspSoftSerial::begin(const EspSoftSerialConfig& conf) -{ - if(_instance && _conf.rx_pin != -1) - { - timerStop(); - detachInterrupt(_conf.rx_pin); - _instance = NULL; - } - if(conf.rx_pin != -1 && conf.baud > 0) - { - _instance = this; - _conf = conf; - _delay = nsToTicks((1000000000UL / _conf.baud) - 1500UL); // ~1.5us margin for isr call and computation - _delay_start = _delay + (_delay >> 1); // start bit half delay - if(_delay_start > nsToTicks(2700) + TIMER_DELAY_MIN) // leave at least 300 cpu clocks margin - { - _delay_start -= nsToTicks(2700); // pin isr call compensation - } - else - { - _delay_start = 0; - } - pinMode(_conf.rx_pin, INPUT); - attachInterrupt(_conf.rx_pin, &EspSoftSerial::rx_start_isr, _conf.inverted ? RISING : FALLING); - timerInit(); - - //pinMode(16, OUTPUT); - - return 1; - } - return 0; -} - -EspSoftSerial * EspSoftSerial::_instance = NULL; - -#endif //ESP8266 diff --git a/lib/EspSoftSerial/src/EspSoftSerial.h b/lib/EspSoftSerial/src/EspSoftSerial.h deleted file mode 100644 index da4d96fc..00000000 --- a/lib/EspSoftSerial/src/EspSoftSerial.h +++ /dev/null @@ -1,118 +0,0 @@ -#ifndef _ESP_SOFT_SERIAL_H_ -#define _ESP_SOFT_SERIAL_H_ - -#if defined(ESP8266) - -#include -#include "Buffer.h" - -class EspSoftSerialConfig -{ - public: - EspSoftSerialConfig(): EspSoftSerialConfig(0, -1) {} - EspSoftSerialConfig(uint32_t baud_, int8_t rx_pin_, int8_t parity_type_ = 0, int8_t stop_bits_ = 1, bool inverted_ = false, int8_t data_bits_ = 8): - baud(baud_), rx_pin(rx_pin_), parity_type(parity_type_), stop_bits(stop_bits_), inverted(inverted_), data_bits(data_bits_) { - data_bits = constrain(data_bits, 5, 8); - } - int32_t baud; - int8_t rx_pin; - int8_t parity_type; - int8_t stop_bits; - bool inverted; - int8_t data_bits; -}; - -class EspSoftSerial: public Stream -{ - public: - enum State - { - STATE_IDLE, - STATE_DATA, - STATE_PARITY, - STATE_STOP, - }; - - EspSoftSerial(): _rx_state(STATE_IDLE), _rx_data(0) {} - - int begin(int baud); - - int begin(const EspSoftSerialConfig& conf); - - int available() override - { - return _rx_buff.count(); - } - - int read() override - { - return _rx_buff.get(); - } - - int read(uint8_t * buff, size_t len) override - { - size_t count = std::min(len, (size_t)available()); - for(size_t i = 0; i < count; i++) - { - buff[i] = read(); - } - return count; - } - - int peek() override - { - return _rx_buff.peek(); - } - - virtual size_t write(uint8_t c) override - { - return 0; - } - - virtual size_t write(const uint8_t* c, size_t l) override - { - return 0; - } - - void flush() override - { - - } - - int availableForWrite() - { - return 0; - } - - operator bool() const - { - return (bool)_instance; - } - - static void rx_read_bit_isr() IRAM_ATTR; - static void rx_start_isr() IRAM_ATTR; - - private: - void rxReadBit() IRAM_ATTR; - void rxStart() IRAM_ATTR; - - uint32_t nsToTicks(uint32_t ns); - void timerInit(); - void timerStop(); - - EspSoftSerialConfig _conf; - uint32_t _delay; - uint32_t _delay_start; - - State _rx_state; - int32_t _rx_data_count; - int32_t _rx_data; - - Buffer _rx_buff; - - static EspSoftSerial * _instance; -}; - -#endif // ESP8266 - -#endif diff --git a/lib/EspWire/src/EspWire.cpp b/lib/EspWire/src/EspWire.cpp index 0ef8317a..f6353bd9 100644 --- a/lib/EspWire/src/EspWire.cpp +++ b/lib/EspWire/src/EspWire.cpp @@ -95,21 +95,21 @@ void EspTwoWire::setClockStretchLimit(uint32_t limit){ esp_twi_setClockStretchLimit(limit); } -size_t EspTwoWire::requestFrom(uint8_t address, size_t size, bool sendStop){ +size_t IRAM_ATTR EspTwoWire::requestFrom(uint8_t address, size_t size, bool sendStop){ if(size > ESPWIRE_BUFFER_LENGTH){ size = ESPWIRE_BUFFER_LENGTH; } - size_t read = (esp_twi_readFrom(address, rxBuffer, size, sendStop) == 0)?size:0; + size_t read = (esp_twi_readFrom(address, rxBuffer, size, sendStop) == 0) ? size : 0; rxBufferIndex = 0; rxBufferLength = read; return read; } -uint8_t EspTwoWire::requestFrom(uint8_t address, uint8_t quantity, uint8_t sendStop){ +uint8_t IRAM_ATTR EspTwoWire::requestFrom(uint8_t address, uint8_t quantity, uint8_t sendStop){ return requestFrom(address, static_cast(quantity), static_cast(sendStop)); } -uint8_t EspTwoWire::requestFrom(uint8_t address, uint8_t quantity){ +uint8_t IRAM_ATTR EspTwoWire::requestFrom(uint8_t address, uint8_t quantity){ return requestFrom(address, static_cast(quantity), true); } @@ -121,18 +121,18 @@ uint8_t EspTwoWire::requestFrom(int address, int quantity, int sendStop){ return requestFrom(static_cast(address), static_cast(quantity), static_cast(sendStop)); } -void EspTwoWire::beginTransmission(uint8_t address){ +void IRAM_ATTR EspTwoWire::beginTransmission(uint8_t address){ transmitting = 1; txAddress = address; txBufferIndex = 0; txBufferLength = 0; } -void EspTwoWire::beginTransmission(int address){ +void IRAM_ATTR EspTwoWire::beginTransmission(int address){ beginTransmission((uint8_t)address); } -uint8_t EspTwoWire::endTransmission(uint8_t sendStop){ +uint8_t IRAM_ATTR EspTwoWire::endTransmission(uint8_t sendStop){ int8_t ret = esp_twi_writeTo(txAddress, txBuffer, txBufferLength, sendStop); txBufferIndex = 0; txBufferLength = 0; @@ -140,11 +140,11 @@ uint8_t EspTwoWire::endTransmission(uint8_t sendStop){ return ret; } -uint8_t EspTwoWire::endTransmission(void){ +uint8_t IRAM_ATTR EspTwoWire::endTransmission(void){ return endTransmission(true); } -size_t EspTwoWire::write(uint8_t data){ +size_t IRAM_ATTR EspTwoWire::write(uint8_t data){ if(transmitting){ if(txBufferLength >= ESPWIRE_BUFFER_LENGTH){ setWriteError(); @@ -159,7 +159,7 @@ size_t EspTwoWire::write(uint8_t data){ return 1; } -size_t EspTwoWire::write(const uint8_t *data, size_t quantity){ +size_t IRAM_ATTR EspTwoWire::write(const uint8_t *data, size_t quantity){ if(transmitting){ for(size_t i = 0; i < quantity; ++i){ if(!write(data[i])) return i; diff --git a/lib/EspWire/src/esp_twi.cpp b/lib/EspWire/src/esp_twi.cpp index 4740dfcb..8ada32fc 100644 --- a/lib/EspWire/src/esp_twi.cpp +++ b/lib/EspWire/src/esp_twi.cpp @@ -276,7 +276,7 @@ static IRAM_ATTR unsigned char esp_twi_read_byte(bool nack) { return byte; } -unsigned char esp_twi_writeTo(unsigned char address, unsigned char * buf, unsigned int len, unsigned char sendStop){ +unsigned char IRAM_ATTR esp_twi_writeTo(unsigned char address, unsigned char * buf, unsigned int len, unsigned char sendStop){ unsigned int i; if(!esp_twi_write_start()) return 4;//line busy if(!esp_twi_write_byte(((address << 1) | 0) & 0xFF)) { @@ -300,7 +300,7 @@ unsigned char esp_twi_writeTo(unsigned char address, unsigned char * buf, unsign return 0; } -unsigned char esp_twi_readFrom(unsigned char address, unsigned char* buf, unsigned int len, unsigned char sendStop){ +unsigned char IRAM_ATTR esp_twi_readFrom(unsigned char address, unsigned char* buf, unsigned int len, unsigned char sendStop){ unsigned int i; if(!esp_twi_write_start()) return 4;//line busy if(!esp_twi_write_byte(((address << 1) | 1) & 0xFF)) { diff --git a/lib/EspWire/src/esp_twi.h b/lib/EspWire/src/esp_twi.h index 1eab5e46..97f2908d 100644 --- a/lib/EspWire/src/esp_twi.h +++ b/lib/EspWire/src/esp_twi.h @@ -36,8 +36,8 @@ void esp_twi_init(unsigned char sda, unsigned char scl); void esp_twi_stop(void); void esp_twi_setClock(unsigned int freq); void esp_twi_setClockStretchLimit(uint32_t limit); -uint8_t esp_twi_writeTo(unsigned char address, unsigned char * buf, unsigned int len, unsigned char sendStop) IRAM_ATTR; -uint8_t esp_twi_readFrom(unsigned char address, unsigned char * buf, unsigned int len, unsigned char sendStop) IRAM_ATTR; +uint8_t esp_twi_writeTo(unsigned char address, unsigned char * buf, unsigned int len, unsigned char sendStop); +uint8_t esp_twi_readFrom(unsigned char address, unsigned char * buf, unsigned int len, unsigned char sendStop); uint8_t esp_twi_status(); #ifdef __cplusplus diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h index 26945cd3..d750272a 100644 --- a/lib/Espfc/src/Actuator.h +++ b/lib/Espfc/src/Actuator.h @@ -14,7 +14,18 @@ class Actuator int begin() { _model.state.modeMask = 0; + _model.state.modeMaskPrev = 0; + _model.state.modeMaskPresent = 0; + _model.state.modeMaskSwitch = 0; + for(size_t i = 0; i < ACTUATOR_CONDITIONS; i++) + { + const auto &c = _model.config.conditions[i]; + if(!(c.min < c.max)) continue; // inactive + if(c.ch < AXIS_AUX_1 || c.ch >= AXIS_COUNT) continue; // invalid channel + _model.state.modeMaskPresent |= 1 << c.id; + } _model.state.airmodeAllowed = false; + _model.state.rescueConfigMode = RESCUE_CONFIG_PENDING; return 1; } @@ -29,6 +40,7 @@ class Actuator updateScaler(); updateBuzzer(); updateDynLpf(); + updateRescueConfig(); if(_model.config.debugMode == DEBUG_PIDLOOP) { @@ -92,6 +104,7 @@ class Actuator _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.rescueConfigMode == RESCUE_CONFIG_ACTIVE); } void updateModeMask() @@ -162,6 +175,7 @@ class Actuator if(armed) { _model.state.disarmReason = DISARM_REASON_SYSTEM; + _model.state.rescueConfigMode = RESCUE_CONFIG_DISABLED; } else if(!armed && _model.state.disarmReason == DISARM_REASON_SYSTEM) { @@ -187,19 +201,19 @@ class Actuator { if(_model.isModeActive(MODE_FAILSAFE)) { - _model.state.buzzer.play(BEEPER_RX_LOST); + _model.state.buzzer.play(BUZZER_RX_LOST); } if(_model.state.battery.warn(_model.config.vbatCellWarning)) { - _model.state.buzzer.play(BEEPER_BAT_LOW); + _model.state.buzzer.play(BUZZER_BAT_LOW); } if(_model.isModeActive(MODE_BUZZER)) { - _model.state.buzzer.play(BEEPER_RX_SET); + _model.state.buzzer.play(BUZZER_RX_SET); } if((_model.hasChanged(MODE_ARMED))) { - _model.state.buzzer.push(_model.isModeActive(MODE_ARMED) ? BEEPER_ARMING : BEEPER_DISARMING); + _model.state.buzzer.push(_model.isModeActive(MODE_ARMED) ? BUZZER_ARMING : BUZZER_DISARMING); } } @@ -221,6 +235,28 @@ class Actuator } } + void updateRescueConfig() + { + switch(_model.state.rescueConfigMode) + { + case RESCUE_CONFIG_PENDING: + // if some rc frames are received, disable to prevent activate later + if(_model.state.inputFrameCount > 100) + { + _model.state.rescueConfigMode = RESCUE_CONFIG_DISABLED; + } + if(_model.state.failsafe.phase != FC_FAILSAFE_IDLE && _model.config.rescueConfigDelay > 0 && millis() > _model.config.rescueConfigDelay * 1000) + { + _model.state.rescueConfigMode = RESCUE_CONFIG_ACTIVE; + } + break; + case RESCUE_CONFIG_ACTIVE: + case RESCUE_CONFIG_DISABLED: + // nothing to do here + break; + } + } + Model& _model; }; diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h deleted file mode 100644 index 8267c3b5..00000000 --- a/lib/Espfc/src/Blackbox.h +++ /dev/null @@ -1,474 +0,0 @@ -#ifndef _ESPFC_BLACKBOX_H_ -#define _ESPFC_BLACKBOX_H_ - -#include "Model.h" -#include "Hardware.h" -#include "EscDriver.h" -#include "Math/Utils.h" -#include "Device/SerialDevice.h" - -extern "C" { -#include "blackbox/blackbox.h" -#include "blackbox/blackbox_fielddefs.h" -} - -class BlackboxBuffer: public Espfc::Device::SerialDevice -{ - public: - BlackboxBuffer(): _dev(nullptr), _idx(0) {} - - virtual void wrap(Espfc::Device::SerialDevice * s) - { - _dev = s; - } - - virtual void begin(const Espfc::SerialDeviceConfig& conf) - { - //_dev->begin(conf); - } - - virtual size_t write(uint8_t c) - { - _data[_idx++] = c; - if(_idx >= SIZE) flush(); - return 1; - } - - virtual void flush() - { - if(_dev) _dev->write(_data, _idx); - _idx = 0; - } - - virtual int availableForWrite() - { - //return _dev->availableForWrite(); - return SIZE - _idx; - } - - virtual bool isTxFifoEmpty() - { - //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) { -#ifdef TARGET_RP2040 - size_t count = std::min(l, (size_t)available()); - for(size_t i = 0; i < count; i++) - { - c[i] = read(); - } - return count; -#else - return _dev->readMany(c, l); -#endif - } - virtual int peek() { return _dev->peek(); } - - virtual size_t write(const uint8_t * c, size_t l) - { - 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); } - - static const size_t SIZE = SERIAL_TX_FIFO_SIZE;//128; - - Espfc::Device::SerialDevice * _dev; - size_t _idx; - uint8_t _data[SIZE]; -}; - -static Espfc::Model * _model_ptr = nullptr; - -void initBlackboxModel(Espfc::Model * m) -{ - _model_ptr = m; -} - -uint16_t getBatteryVoltageLatest(void) -{ - if(!_model_ptr) return 0; - float v = (*_model_ptr).state.battery.voltageUnfiltered; - return constrain(lrintf(v * 100.0f), 0, 32000); -} - -int32_t getAmperageLatest(void) -{ - if(!_model_ptr) return 0; - float v = (*_model_ptr).state.battery.currentUnfiltered; - return constrain(lrintf(v * 100.0f), 0, 32000); -} - -bool rxIsReceivingSignal(void) -{ - if(!_model_ptr) return false; - return !((*_model_ptr).state.inputRxLoss || (*_model_ptr).state.inputRxFailSafe); -} - -bool isRssiConfigured(void) -{ - if(!_model_ptr) return false; - return (*_model_ptr).config.input.rssiChannel > 0; -} - -uint16_t getRssi(void) -{ - if(!_model_ptr) return 0; - return (*_model_ptr).getRssi(); -} - -failsafePhase_e failsafePhase() -{ - if(!_model_ptr) return ::FAILSAFE_IDLE; - 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; -} - -bool sensors(uint32_t mask) -{ - return enabledSensors & mask; -} - -float pidGetPreviousSetpoint(int axis) -{ - return Espfc::Math::toDeg(_model_ptr->state.desiredRate[axis]); -} - -float mixerGetThrottle(void) -{ - return (_model_ptr->state.output[Espfc::AXIS_THRUST] + 1.0f) * 0.5f; -} - -int16_t getMotorOutputLow() -{ - return _model_ptr->state.digitalOutput ? PWM_TO_DSHOT(1000) : 1000; -} - -int16_t getMotorOutputHigh() -{ - return _model_ptr->state.digitalOutput ? PWM_TO_DSHOT(2000) : 2000; -} - -bool areMotorsRunning(void) -{ - return _model_ptr->areMotorsRunning(); -} - -uint16_t getDshotErpm(uint8_t i) -{ - return _model_ptr->state.outputTelemetryErpm[i]; -} - -namespace Espfc { - -class Blackbox -{ - public: - Blackbox(Model& model): _model(model) {} - - int begin() - { - initBlackboxModel(&_model); - - if(!_model.blackboxEnabled()) return 0; - - _serial = _model.getSerialStream(SERIAL_FUNCTION_BLACKBOX); - if(!_serial) return 0; - - _buffer.wrap(_serial); - serialDeviceInit(&_buffer, 0); - //serialDeviceInit(_serial, 0); - - systemConfigMutable()->activeRateProfile = 0; - systemConfigMutable()->debug_mode = debugMode = _model.config.debugMode; - - controlRateConfig_t *rp = controlRateProfilesMutable(systemConfig()->activeRateProfile); - for(int i = 0; i <= AXIS_YAW; i++) - { - rp->rcRates[i] = _model.config.input.rate[i]; - rp->rcExpo[i] = _model.config.input.expo[i]; - rp->rates[i] = _model.config.input.superRate[i]; - rp->rate_limit[i] = _model.config.input.rateLimit[i]; - } - rp->thrMid8 = 50; - rp->thrExpo8 = 0; - rp->dynThrPID = _model.config.tpaScale; - rp->tpa_breakpoint = _model.config.tpaBreakpoint; - rp->rates_type = _model.config.input.rateType; - - pidProfile_s * cp = currentPidProfile = &_pidProfile; - for(size_t i = 0; i < FC_PID_ITEM_COUNT; i++) - { - cp->pid[i].P = _model.config.pid[i].P; - cp->pid[i].I = _model.config.pid[i].I; - cp->pid[i].D = _model.config.pid[i].D; - cp->pid[i].F = _model.config.pid[i].F; - if(i <= AXIS_YAW) { - cp->d_min[i] = _model.config.pid[i].D; - } - } - cp->pidAtMinThrottle = 1; - cp->dterm_lpf1_type = _model.config.dtermFilter.type; - cp->dterm_lpf1_static_hz = _model.config.dtermFilter.freq; - cp->dterm_lpf1_dyn_min_hz = _model.config.dtermDynLpfFilter.cutoff; - cp->dterm_lpf1_dyn_max_hz = _model.config.dtermDynLpfFilter.freq; - cp->dterm_lpf2_type = _model.config.dtermFilter2.type; - cp->dterm_lpf2_static_hz = _model.config.dtermFilter2.freq; - cp->dterm_notch_hz = _model.config.dtermNotchFilter.freq; - cp->dterm_notch_cutoff = _model.config.dtermNotchFilter.cutoff; - cp->yaw_lowpass_hz = _model.config.yawFilter.freq; - cp->itermWindupPointPercent = _model.config.itermWindupPointPercent; - cp->antiGravityMode = 0; - cp->pidSumLimit = 500; - cp->pidSumLimitYaw = 500; - cp->ff_boost = 0; - cp->feedForwardTransition = 0; - cp->tpa_mode = 0; // PD - cp->tpa_rate = _model.config.tpaScale; - cp->tpa_breakpoint = _model.config.tpaBreakpoint; - cp->motor_output_limit = _model.config.output.motorLimit; - cp->throttle_boost = 0; - cp->throttle_boost_cutoff = 100; - cp->anti_gravity_gain = 0; - cp->anti_gravity_p_gain = 0; - cp->anti_gravity_cutoff_hz = 100; - cp->d_min_gain = 0; - cp->d_min_advance = 0; - cp->angle_limit = _model.config.angleLimit; - cp->angle_earth_ref = 100; - cp->horizon_limit_degrees = 135; - cp->horizon_delay_ms = 500; - cp->thrustLinearization = 0; - cp->iterm_relax = _model.config.itermRelax; - cp->iterm_relax_type = 1; - cp->iterm_relax_cutoff = _model.config.itermRelaxCutoff; - cp->dterm_lpf1_dyn_expo = 5; - cp->tpa_low_rate = 20; - cp->tpa_low_breakpoint = 1050; - cp->tpa_low_always = 0; - cp->ez_landing_threshold = 25; - cp->ez_landing_limit = 5; - - rcControlsConfigMutable()->deadband = _model.config.input.deadband; - rcControlsConfigMutable()->yaw_deadband = _model.config.input.deadband; - - gyroConfigMutable()->gyro_hardware_lpf = _model.config.gyroDlpf; - gyroConfigMutable()->gyro_lpf1_type = _model.config.gyroFilter.type; - gyroConfigMutable()->gyro_lpf1_static_hz = _model.config.gyroFilter.freq; - gyroConfigMutable()->gyro_lpf1_dyn_min_hz = _model.config.gyroDynLpfFilter.cutoff; - gyroConfigMutable()->gyro_lpf1_dyn_max_hz = _model.config.gyroDynLpfFilter.freq; - gyroConfigMutable()->gyro_lpf1_dyn_expo = 5; - gyroConfigMutable()->gyro_lpf2_type = _model.config.gyroFilter2.type; - gyroConfigMutable()->gyro_lpf2_static_hz = _model.config.gyroFilter2.freq; - gyroConfigMutable()->gyro_soft_notch_cutoff_1 = _model.config.gyroNotch1Filter.cutoff; - gyroConfigMutable()->gyro_soft_notch_hz_1 = _model.config.gyroNotch1Filter.freq; - gyroConfigMutable()->gyro_soft_notch_cutoff_2 = _model.config.gyroNotch2Filter.cutoff; - gyroConfigMutable()->gyro_soft_notch_hz_2 = _model.config.gyroNotch2Filter.freq; - gyroConfigMutable()->gyro_sync_denom = 1; - - dynNotchConfigMutable()->dyn_notch_count = _model.config.dynamicFilter.width; - dynNotchConfigMutable()->dyn_notch_q = _model.config.dynamicFilter.q; - dynNotchConfigMutable()->dyn_notch_min_hz = _model.config.dynamicFilter.min_freq; - dynNotchConfigMutable()->dyn_notch_max_hz = _model.config.dynamicFilter.max_freq; - - accelerometerConfigMutable()->acc_lpf_hz = _model.config.accelFilter.freq; - accelerometerConfigMutable()->acc_hardware = _model.config.accelDev; - barometerConfigMutable()->baro_hardware = _model.config.baroDev; - compassConfigMutable()->mag_hardware = _model.config.magDev; - - motorConfigMutable()->dev.useUnsyncedPwm = _model.config.output.async; - motorConfigMutable()->dev.motorPwmProtocol = _model.config.output.protocol; - motorConfigMutable()->dev.motorPwmRate = _model.config.output.rate; - motorConfigMutable()->mincommand = _model.config.output.minCommand; - motorConfigMutable()->digitalIdleOffsetValue = _model.config.output.dshotIdle; - motorConfigMutable()->minthrottle = _model.state.minThrottle; - motorConfigMutable()->maxthrottle = _model.state.maxThrottle; - motorConfigMutable()->dev.useDshotTelemetry = _model.config.output.dshotTelemetry; - motorConfigMutable()->motorPoleCount = _model.config.output.motorPoles; - - pidConfigMutable()->pid_process_denom = _model.config.loopSync; - - mixerConfigMutable()->mixer_type = 0; - - if(_model.accelActive()) enabledSensors |= SENSOR_ACC; - if(_model.magActive()) enabledSensors |= SENSOR_MAG; - if(_model.baroActive()) enabledSensors |= SENSOR_BARO; - - gyro.sampleLooptime = _model.state.gyroTimer.interval; - targetPidLooptime = _model.state.loopTimer.interval; - activePidLoopDenom = _model.config.loopSync; - - if(_model.config.blackboxPdenom >= 0 && _model.config.blackboxPdenom <= 4) - { - blackboxConfigMutable()->sample_rate = _model.config.blackboxPdenom; - } - else - { - blackboxConfigMutable()->sample_rate = blackboxCalculateSampleRate(_model.config.blackboxPdenom); - } - blackboxConfigMutable()->device = _model.config.blackboxDev; - blackboxConfigMutable()->fields_disabled_mask = _model.config.blackboxFieldsDisabledMask; - blackboxConfigMutable()->mode = _model.config.blackboxMode; - - featureConfigMutable()->enabledFeatures = _model.config.featureMask; - - batteryConfigMutable()->currentMeterSource = (currentMeterSource_e)_model.config.ibatSource; - batteryConfigMutable()->voltageMeterSource = (voltageMeterSource_e)_model.config.vbatSource; - batteryConfigMutable()->vbatwarningcellvoltage = _model.config.vbatCellWarning; - batteryConfigMutable()->vbatmaxcellvoltage = 420; - batteryConfigMutable()->vbatmincellvoltage = 340; - - rxConfigMutable()->rcInterpolation = _model.config.input.interpolationMode; - rxConfigMutable()->rcInterpolationInterval = _model.config.input.interpolationInterval; - rxConfigMutable()->rssi_channel = _model.config.input.rssiChannel; - rxConfigMutable()->airModeActivateThreshold = 40; - rxConfigMutable()->serialrx_provider = _model.config.input.serialRxProvider; - - rpmFilterConfigMutable()->rpm_filter_harmonics = _model.config.rpmFilterHarmonics; - rpmFilterConfigMutable()->rpm_filter_q = _model.config.rpmFilterQ; - rpmFilterConfigMutable()->rpm_filter_min_hz = _model.config.rpmFilterMinFreq; - rpmFilterConfigMutable()->rpm_filter_fade_range_hz = _model.config.rpmFilterFade; - rpmFilterConfigMutable()->rpm_filter_lpf_hz = _model.config.rpmFilterFreqLpf; - rpmFilterConfigMutable()->rpm_filter_weights[0] = _model.config.rpmFilterWeights[0]; - rpmFilterConfigMutable()->rpm_filter_weights[1] = _model.config.rpmFilterWeights[1]; - rpmFilterConfigMutable()->rpm_filter_weights[2] = _model.config.rpmFilterWeights[2]; - - blackboxInit(); - - return 1; - } - - int update() - { - if(!_model.blackboxEnabled()) return 0; - if(!_serial) return 0; - Stats::Measure measure(_model.state.stats, COUNTER_BLACKBOX); - - uint32_t startTime = micros(); - updateArmed(); - updateMode(); - updateData(); - blackboxUpdate(_model.state.loopTimer.last); - _buffer.flush(); - - if(_model.config.debugMode == DEBUG_PIDLOOP) - { - _model.state.debug[5] = micros() - startTime; - } - - return 1; - } - - private: - void updateData() - { - for(size_t i = 0; i < 3; i++) - { - gyro.gyroADCf[i] = degrees(_model.state.gyro[i]); - gyro.gyroADC[i] = degrees(_model.state.gyroScaled[i]); - pidData[i].P = _model.state.innerPid[i].pTerm * 1000.f; - pidData[i].I = _model.state.innerPid[i].iTerm * 1000.f; - pidData[i].D = _model.state.innerPid[i].dTerm * 1000.f; - pidData[i].F = _model.state.innerPid[i].fTerm * 1000.f; - rcCommand[i] = (_model.state.inputBuffer[i] - 1500) * (i == AXIS_YAW ? -1 : 1); - if(_model.accelActive()) { - acc.accADC[i] = _model.state.accel[i] * ACCEL_G_INV * acc.dev.acc_1G; - } - if(_model.magActive()) { - mag.magADC[i] = _model.state.mag[i] * 1090; - } - if(_model.baroActive()) { - baro.altitude = lrintf(_model.state.baroAltitude * 100.f); // cm - } - } - rcCommand[AXIS_THRUST] = _model.state.inputBuffer[AXIS_THRUST]; - for(size_t i = 0; i < 4; i++) - { - motor[i] = Math::clamp(_model.state.outputUs[i], (int16_t)1000, (int16_t)2000); - if(_model.state.digitalOutput) - { - motor[i] = PWM_TO_DSHOT(motor[i]); - } - } - if(_model.config.debugMode != DEBUG_NONE && _model.config.debugMode != DEBUG_BLACKBOX_OUTPUT) - { - for(size_t i = 0; i < 8; i++) - { - debug[i] = _model.state.debug[i]; - } - } - } - - void updateArmed() - { - // log arming beep event - static uint32_t beep = 0; - if(beep != 0 && _model.state.loopTimer.last > beep) - { - setArmingBeepTimeMicros(_model.state.loopTimer.last); - beep = 0; - } - - // stop logging - static uint32_t stop = 0; - if(stop != 0 && _model.state.loopTimer.last > stop) - { - blackboxFinish(); - stop = 0; - } - - bool armed = _model.isActive(MODE_ARMED); - if(armed == ARMING_FLAG(ARMED)) return; - if(armed) - { - ENABLE_ARMING_FLAG(ARMED); - beep = _model.state.loopTimer.last + 200000; // schedule arming beep event ~200ms - } - else - { - DISABLE_ARMING_FLAG(ARMED); - flightLogEventData_t eventData; - eventData.disarm.reason = _model.state.disarmReason; - blackboxLogEvent(FLIGHT_LOG_EVENT_DISARM, &eventData); - stop = _model.state.loopTimer.last + 500000; // schedule stop in 500ms - } - } - - void updateMode() - { - if(_model.isSwitchActive(MODE_ARMED)) bitArraySet(&rcModeActivationMask, BOXARM); - else bitArrayClr(&rcModeActivationMask, BOXARM); - - if(_model.isSwitchActive(MODE_ANGLE)) bitArraySet(&rcModeActivationMask, BOXANGLE); - else bitArrayClr(&rcModeActivationMask, BOXANGLE); - - if(_model.isSwitchActive(MODE_AIRMODE)) bitArraySet(&rcModeActivationMask, BOXAIRMODE); - else bitArrayClr(&rcModeActivationMask, BOXAIRMODE); - - if(_model.isSwitchActive(MODE_FAILSAFE)) bitArraySet(&rcModeActivationMask, BOXFAILSAFE); - else bitArrayClr(&rcModeActivationMask, BOXFAILSAFE); - } - - Model& _model; - pidProfile_s _pidProfile; - Device::SerialDevice * _serial; - BlackboxBuffer _buffer; -}; - -} -#endif diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp new file mode 100644 index 00000000..f87306a6 --- /dev/null +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -0,0 +1,322 @@ +#include "Blackbox.h" +#include "Hardware.h" +#include "EscDriver.h" +#include "Math/Utils.h" +#include "BlackboxBridge.h" + +namespace Espfc { + +namespace Blackbox { + +static void updateModeFlag(boxBitmask_t *mask, boxId_e id, bool value) +{ + if(value) bitArraySet(mask, id); + else bitArrayClr(mask, id); +} + +Blackbox::Blackbox(Model& model): _model(model) {} + +int Blackbox::begin() +{ + initBlackboxModel(&_model); + +#ifdef USE_FLASHFS + int res = flashfsInit(); + _model.logger.info().log(F("FLASHFS")).log(res).logln(flashfsGetOffset()); +#endif + + if(!_model.blackboxEnabled()) return 0; + + if(_model.config.blackboxDev == 3) + { + _serial = _model.getSerialStream(SERIAL_FUNCTION_BLACKBOX); + if(!_serial) return 0; + + _buffer.wrap(_serial); + serialDeviceInit(&_buffer, 0); + //serialDeviceInit(_serial, 0); + } + + systemConfigMutable()->activeRateProfile = 0; + systemConfigMutable()->debug_mode = debugMode = _model.config.debugMode; + + controlRateConfig_t *rp = controlRateProfilesMutable(systemConfig()->activeRateProfile); + for(int i = 0; i <= AXIS_YAW; i++) + { + rp->rcRates[i] = _model.config.input.rate[i]; + rp->rcExpo[i] = _model.config.input.expo[i]; + rp->rates[i] = _model.config.input.superRate[i]; + rp->rate_limit[i] = _model.config.input.rateLimit[i]; + } + rp->thrMid8 = 50; + rp->thrExpo8 = 0; + rp->dynThrPID = _model.config.tpaScale; + rp->tpa_breakpoint = _model.config.tpaBreakpoint; + rp->rates_type = _model.config.input.rateType; + + pidProfile_s * cp = currentPidProfile = &_pidProfile; + for(size_t i = 0; i < FC_PID_ITEM_COUNT; i++) + { + cp->pid[i].P = _model.config.pid[i].P; + cp->pid[i].I = _model.config.pid[i].I; + cp->pid[i].D = _model.config.pid[i].D; + cp->pid[i].F = _model.config.pid[i].F; + if(i <= AXIS_YAW) { + cp->d_min[i] = _model.config.pid[i].D; + } + } + cp->pidAtMinThrottle = 1; + cp->dterm_lpf1_type = _model.config.dtermFilter.type; + cp->dterm_lpf1_static_hz = _model.config.dtermFilter.freq; + cp->dterm_lpf1_dyn_min_hz = _model.config.dtermDynLpfFilter.cutoff; + cp->dterm_lpf1_dyn_max_hz = _model.config.dtermDynLpfFilter.freq; + cp->dterm_lpf2_type = _model.config.dtermFilter2.type; + cp->dterm_lpf2_static_hz = _model.config.dtermFilter2.freq; + cp->dterm_notch_hz = _model.config.dtermNotchFilter.freq; + cp->dterm_notch_cutoff = _model.config.dtermNotchFilter.cutoff; + cp->yaw_lowpass_hz = _model.config.yawFilter.freq; + cp->itermWindupPointPercent = 80; + cp->antiGravityMode = 0; + cp->pidSumLimit = 660; + cp->pidSumLimitYaw = 660; + cp->ff_boost = 0; + cp->feedForwardTransition = 0; + cp->tpa_mode = 0; // PD + cp->tpa_rate = _model.config.tpaScale; + cp->tpa_breakpoint = _model.config.tpaBreakpoint; + cp->motor_output_limit = _model.config.output.motorLimit; + cp->throttle_boost = 0; + cp->throttle_boost_cutoff = 100; + cp->anti_gravity_gain = 0; + cp->anti_gravity_p_gain = 0; + cp->anti_gravity_cutoff_hz = 100; + cp->d_min_gain = 0; + cp->d_min_advance = 0; + cp->angle_limit = _model.config.angleLimit; + cp->angle_earth_ref = 100; + cp->horizon_limit_degrees = 135; + cp->horizon_delay_ms = 500; + cp->thrustLinearization = 0; + cp->iterm_relax = _model.config.itermRelax; + cp->iterm_relax_type = 1; + cp->iterm_relax_cutoff = _model.config.itermRelaxCutoff; + cp->dterm_lpf1_dyn_expo = 5; + cp->tpa_low_rate = 20; + cp->tpa_low_breakpoint = 1050; + cp->tpa_low_always = 0; + cp->ez_landing_threshold = 25; + cp->ez_landing_limit = 5; + + rcControlsConfigMutable()->deadband = _model.config.input.deadband; + rcControlsConfigMutable()->yaw_deadband = _model.config.input.deadband; + + gyroConfigMutable()->gyro_hardware_lpf = _model.config.gyroDlpf; + gyroConfigMutable()->gyro_lpf1_type = _model.config.gyroFilter.type; + gyroConfigMutable()->gyro_lpf1_static_hz = _model.config.gyroFilter.freq; + gyroConfigMutable()->gyro_lpf1_dyn_min_hz = _model.config.gyroDynLpfFilter.cutoff; + gyroConfigMutable()->gyro_lpf1_dyn_max_hz = _model.config.gyroDynLpfFilter.freq; + gyroConfigMutable()->gyro_lpf1_dyn_expo = 5; + gyroConfigMutable()->gyro_lpf2_type = _model.config.gyroFilter2.type; + gyroConfigMutable()->gyro_lpf2_static_hz = _model.config.gyroFilter2.freq; + gyroConfigMutable()->gyro_soft_notch_cutoff_1 = _model.config.gyroNotch1Filter.cutoff; + gyroConfigMutable()->gyro_soft_notch_hz_1 = _model.config.gyroNotch1Filter.freq; + gyroConfigMutable()->gyro_soft_notch_cutoff_2 = _model.config.gyroNotch2Filter.cutoff; + gyroConfigMutable()->gyro_soft_notch_hz_2 = _model.config.gyroNotch2Filter.freq; + gyroConfigMutable()->gyro_sync_denom = 1; + + dynNotchConfigMutable()->dyn_notch_count = _model.config.dynamicFilter.width; + dynNotchConfigMutable()->dyn_notch_q = _model.config.dynamicFilter.q; + dynNotchConfigMutable()->dyn_notch_min_hz = _model.config.dynamicFilter.min_freq; + dynNotchConfigMutable()->dyn_notch_max_hz = _model.config.dynamicFilter.max_freq; + + accelerometerConfigMutable()->acc_lpf_hz = _model.config.accelFilter.freq; + accelerometerConfigMutable()->acc_hardware = _model.config.accelDev; + barometerConfigMutable()->baro_hardware = _model.config.baroDev; + compassConfigMutable()->mag_hardware = _model.config.magDev; + + motorConfigMutable()->dev.useUnsyncedPwm = _model.config.output.async; + motorConfigMutable()->dev.motorPwmProtocol = _model.config.output.protocol; + motorConfigMutable()->dev.motorPwmRate = _model.config.output.rate; + motorConfigMutable()->mincommand = _model.config.output.minCommand; + motorConfigMutable()->digitalIdleOffsetValue = _model.config.output.dshotIdle; + motorConfigMutable()->minthrottle = _model.state.minThrottle; + motorConfigMutable()->maxthrottle = _model.state.maxThrottle; + motorConfigMutable()->dev.useDshotTelemetry = _model.config.output.dshotTelemetry; + motorConfigMutable()->motorPoleCount = _model.config.output.motorPoles; + + pidConfigMutable()->pid_process_denom = _model.config.loopSync; + + mixerConfigMutable()->mixer_type = 0; + + if(_model.accelActive()) sensorsSet(SENSOR_ACC); + if(_model.magActive()) sensorsSet(SENSOR_MAG); + if(_model.baroActive()) sensorsSet(SENSOR_BARO); + + gyro.sampleLooptime = _model.state.gyroTimer.interval; + targetPidLooptime = _model.state.loopTimer.interval; + activePidLoopDenom = _model.config.loopSync; + + if(_model.config.blackboxPdenom >= 0 && _model.config.blackboxPdenom <= 4) + { + blackboxConfigMutable()->sample_rate = _model.config.blackboxPdenom; + } + else + { + blackboxConfigMutable()->sample_rate = blackboxCalculateSampleRate(_model.config.blackboxPdenom); + } + blackboxConfigMutable()->device = _model.config.blackboxDev; + blackboxConfigMutable()->fields_disabled_mask = _model.config.blackboxFieldsDisabledMask; + blackboxConfigMutable()->mode = _model.config.blackboxMode; + + featureConfigMutable()->enabledFeatures = _model.config.featureMask; + + batteryConfigMutable()->currentMeterSource = (currentMeterSource_e)_model.config.ibatSource; + batteryConfigMutable()->voltageMeterSource = (voltageMeterSource_e)_model.config.vbatSource; + batteryConfigMutable()->vbatwarningcellvoltage = _model.config.vbatCellWarning; + batteryConfigMutable()->vbatmaxcellvoltage = 420; + batteryConfigMutable()->vbatmincellvoltage = 340; + + rxConfigMutable()->rcInterpolation = _model.config.input.interpolationMode; + rxConfigMutable()->rcInterpolationInterval = _model.config.input.interpolationInterval; + rxConfigMutable()->rssi_channel = _model.config.input.rssiChannel; + rxConfigMutable()->airModeActivateThreshold = 40; + rxConfigMutable()->serialrx_provider = _model.config.input.serialRxProvider; + + rpmFilterConfigMutable()->rpm_filter_harmonics = _model.config.rpmFilterHarmonics; + rpmFilterConfigMutable()->rpm_filter_q = _model.config.rpmFilterQ; + rpmFilterConfigMutable()->rpm_filter_min_hz = _model.config.rpmFilterMinFreq; + rpmFilterConfigMutable()->rpm_filter_fade_range_hz = _model.config.rpmFilterFade; + rpmFilterConfigMutable()->rpm_filter_lpf_hz = _model.config.rpmFilterFreqLpf; + rpmFilterConfigMutable()->rpm_filter_weights[0] = _model.config.rpmFilterWeights[0]; + rpmFilterConfigMutable()->rpm_filter_weights[1] = _model.config.rpmFilterWeights[1]; + rpmFilterConfigMutable()->rpm_filter_weights[2] = _model.config.rpmFilterWeights[2]; + + updateModeFlag(&rcModeActivationPresent, BOXARM, _model.state.modeMaskPresent & 1 << MODE_ARMED); + updateModeFlag(&rcModeActivationPresent, BOXANGLE, _model.state.modeMaskPresent & 1 << MODE_ANGLE); + updateModeFlag(&rcModeActivationPresent, BOXAIRMODE, _model.state.modeMaskPresent & 1 << MODE_AIRMODE); + updateModeFlag(&rcModeActivationPresent, BOXFAILSAFE, _model.state.modeMaskPresent & 1 << MODE_FAILSAFE); + updateModeFlag(&rcModeActivationPresent, BOXBLACKBOX, _model.state.modeMaskPresent & 1 << MODE_BLACKBOX); + updateModeFlag(&rcModeActivationPresent, BOXBLACKBOXERASE, _model.state.modeMaskPresent & 1 << MODE_BLACKBOX_ERASE); + + blackboxInit(); + + return 1; +} + +int FAST_CODE_ATTR Blackbox::update() +{ + if(!_model.blackboxEnabled()) return 0; + if(_model.config.blackboxDev == 3 && !_serial) return 0; + + Stats::Measure measure(_model.state.stats, COUNTER_BLACKBOX); + + uint32_t startTime = micros(); + updateArmed(); + updateMode(); + if(blackboxShouldLogIFrame() || blackboxShouldLogPFrame()) + { + updateData(); + } + //PIN_DEBUG(HIGH); + blackboxUpdate(_model.state.loopTimer.last); + if(_model.config.blackboxDev == 3) + { + _buffer.flush(); + } + //PIN_DEBUG(LOW); + + if(_model.config.debugMode == DEBUG_PIDLOOP) + { + _model.state.debug[5] = micros() - startTime; + } + + return 1; +} + +void FAST_CODE_ATTR Blackbox::updateData() +{ + for(size_t i = 0; i < 3; i++) + { + gyro.gyroADCf[i] = degrees(_model.state.gyro[i]); + gyro.gyroADC[i] = degrees(_model.state.gyroScaled[i]); + pidData[i].P = _model.state.innerPid[i].pTerm * 1000.f; + pidData[i].I = _model.state.innerPid[i].iTerm * 1000.f; + pidData[i].D = _model.state.innerPid[i].dTerm * 1000.f; + pidData[i].F = _model.state.innerPid[i].fTerm * 1000.f; + rcCommand[i] = (_model.state.inputBuffer[i] - 1500) * (i == AXIS_YAW ? -1 : 1); + if(_model.accelActive()) { + acc.accADC[i] = _model.state.accel[i] * ACCEL_G_INV * acc.dev.acc_1G; + } + if(_model.magActive()) { + mag.magADC[i] = _model.state.mag[i] * 1090; + } + if(_model.baroActive()) { + baro.altitude = lrintf(_model.state.baroAltitude * 100.f); // cm + } + } + rcCommand[AXIS_THRUST] = _model.state.inputBuffer[AXIS_THRUST]; + for(size_t i = 0; i < 4; i++) + { + motor[i] = Math::clamp(_model.state.outputUs[i], (int16_t)1000, (int16_t)2000); + if(_model.state.digitalOutput) + { + motor[i] = PWM_TO_DSHOT(motor[i]); + } + } + if(_model.config.debugMode != DEBUG_NONE && _model.config.debugMode != DEBUG_BLACKBOX_OUTPUT) + { + for(size_t i = 0; i < 8; i++) + { + debug[i] = _model.state.debug[i]; + } + } +} + +void FAST_CODE_ATTR Blackbox::updateArmed() +{ + // log arming beep event + static uint32_t beep = 0; + if(beep != 0 && _model.state.loopTimer.last > beep) + { + setArmingBeepTimeMicros(_model.state.loopTimer.last); + beep = 0; + } + + // stop logging + static uint32_t stop = 0; + if(stop != 0 && _model.state.loopTimer.last > stop) + { + blackboxFinish(); + stop = 0; + } + + bool armed = _model.isActive(MODE_ARMED); + if(armed == ARMING_FLAG(ARMED)) return; + if(armed) + { + ENABLE_ARMING_FLAG(ARMED); + beep = _model.state.loopTimer.last + 200000; // schedule arming beep event ~200ms + } + else + { + DISABLE_ARMING_FLAG(ARMED); + flightLogEventData_t eventData; + eventData.disarm.reason = _model.state.disarmReason; + blackboxLogEvent(FLIGHT_LOG_EVENT_DISARM, &eventData); + stop = _model.state.loopTimer.last + 500000; // schedule stop in 500ms + } +} + +void FAST_CODE_ATTR Blackbox::updateMode() +{ + updateModeFlag(&rcModeActivationMask, BOXARM, _model.isSwitchActive(MODE_ARMED)); + updateModeFlag(&rcModeActivationMask, BOXANGLE, _model.isSwitchActive(MODE_ANGLE)); + updateModeFlag(&rcModeActivationMask, BOXAIRMODE, _model.isSwitchActive(MODE_AIRMODE)); + updateModeFlag(&rcModeActivationMask, BOXFAILSAFE, _model.isSwitchActive(MODE_FAILSAFE)); + updateModeFlag(&rcModeActivationMask, BOXBLACKBOX, _model.isSwitchActive(MODE_BLACKBOX)); + updateModeFlag(&rcModeActivationMask, BOXBLACKBOXERASE, _model.isSwitchActive(MODE_BLACKBOX_ERASE)); +} + +} + +} diff --git a/lib/Espfc/src/Blackbox/Blackbox.h b/lib/Espfc/src/Blackbox/Blackbox.h new file mode 100644 index 00000000..82583417 --- /dev/null +++ b/lib/Espfc/src/Blackbox/Blackbox.h @@ -0,0 +1,34 @@ +#pragma once + +#include "Model.h" +#include "Device/SerialDevice.h" +#include "BlackboxSerialBuffer.h" +extern "C" { +#include +} + +namespace Espfc { + +namespace Blackbox { + +class Blackbox +{ + public: + Blackbox(Model& model); + int begin(); + int update(); + + private: + void updateData(); + void updateArmed(); + void updateMode(); + + Model& _model; + pidProfile_s _pidProfile; + Device::SerialDevice * _serial; + BlackboxSerialBuffer _buffer; +}; + +} + +} diff --git a/lib/Espfc/src/Blackbox/BlackboxBridge.cpp b/lib/Espfc/src/Blackbox/BlackboxBridge.cpp new file mode 100644 index 00000000..c07a4114 --- /dev/null +++ b/lib/Espfc/src/Blackbox/BlackboxBridge.cpp @@ -0,0 +1,94 @@ +#include "BlackboxBridge.h" + +static Espfc::Model * _model_ptr = nullptr; + +void initBlackboxModel(Espfc::Model * m) +{ + _model_ptr = m; +} + +uint16_t getBatteryVoltageLatest(void) +{ + if(!_model_ptr) return 0; + float v = (*_model_ptr).state.battery.voltageUnfiltered; + return constrain(lrintf(v * 100.0f), 0, 32000); +} + +int32_t getAmperageLatest(void) +{ + if(!_model_ptr) return 0; + float v = (*_model_ptr).state.battery.currentUnfiltered; + return constrain(lrintf(v * 100.0f), 0, 32000); +} + +bool rxIsReceivingSignal(void) +{ + if(!_model_ptr) return false; + return !((*_model_ptr).state.inputRxLoss || (*_model_ptr).state.inputRxFailSafe); +} + +bool isRssiConfigured(void) +{ + if(!_model_ptr) return false; + return (*_model_ptr).config.input.rssiChannel > 0; +} + +uint16_t getRssi(void) +{ + if(!_model_ptr) return 0; + return (*_model_ptr).getRssi(); +} + +failsafePhase_e failsafePhase() +{ + if(!_model_ptr) return ::FAILSAFE_IDLE; + 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; +} + +void sensorsSet(uint32_t mask) +{ + enabledSensors |= mask; +} + +bool sensors(uint32_t mask) +{ + return enabledSensors & mask; +} + +float pidGetPreviousSetpoint(int axis) +{ + return Espfc::Math::toDeg(_model_ptr->state.desiredRate[axis]); +} + +float mixerGetThrottle(void) +{ + return (_model_ptr->state.output[Espfc::AXIS_THRUST] + 1.0f) * 0.5f; +} + +int16_t getMotorOutputLow() +{ + return _model_ptr->state.digitalOutput ? PWM_TO_DSHOT(1000) : 1000; +} + +int16_t getMotorOutputHigh() +{ + return _model_ptr->state.digitalOutput ? PWM_TO_DSHOT(2000) : 2000; +} + +bool areMotorsRunning(void) +{ + return _model_ptr->areMotorsRunning(); +} + +uint16_t getDshotErpm(uint8_t i) +{ + return _model_ptr->state.outputTelemetryErpm[i]; +} diff --git a/lib/Espfc/src/Blackbox/BlackboxBridge.h b/lib/Espfc/src/Blackbox/BlackboxBridge.h new file mode 100644 index 00000000..ef92621d --- /dev/null +++ b/lib/Espfc/src/Blackbox/BlackboxBridge.h @@ -0,0 +1,12 @@ +#pragma once + +#include "Model.h" +#include +extern "C" { +#include +#include +bool blackboxShouldLogPFrame(void); +bool blackboxShouldLogIFrame(void); +} + +void initBlackboxModel(Espfc::Model * m); diff --git a/lib/Espfc/src/Blackbox/BlackboxFlashfs.cpp b/lib/Espfc/src/Blackbox/BlackboxFlashfs.cpp new file mode 100644 index 00000000..8e810a08 --- /dev/null +++ b/lib/Espfc/src/Blackbox/BlackboxFlashfs.cpp @@ -0,0 +1,231 @@ +#include +#include + +#ifdef USE_FLASHFS + +#include "Device/FlashDevice.h" +#include "Utils/RingBuf.h" +#include "Math/Utils.h" + +static const uint32_t FLASHFS_ERASED_VAL = 0xffffffff; + +typedef Espfc::Utils::RingBuf BufferType; +static BufferType buff; + +static FlashfsRuntime flashfs; + +const FlashfsRuntime * flashfsGetRuntime() +{ + return &flashfs; +} + +static uint32_t IRAM_ATTR flashfsJournalAddress(size_t index) +{ + uint32_t base = reinterpret_cast(flashfs.partition)->size - FLASHFS_JOURNAL_SIZE; + return base + (index * sizeof(FlashfsJournalItem)); +} + +void flashfsJournalLoad(FlashfsJournalItem * data, size_t start, size_t num) +{ + size_t size = num * sizeof(FlashfsJournalItem); + flashfsReadAbs(flashfsJournalAddress(start), (uint8_t*)data, size); +} + +static uint32_t flashfsLogLoad() +{ + if(!flashfs.partition) return 0; + + uint32_t address = 0; + flashfsJournalLoad(flashfs.journal, 0, FLASHFS_JOURNAL_ITEMS); + for(size_t i = 0; i < FLASHFS_JOURNAL_ITEMS; i++) + { + const auto& it = flashfs.journal[i]; + if(it.logEnd == FLASHFS_ERASED_VAL) break; + address = it.logEnd; + flashfs.journalIdx++; + } + return address; +} + +static void IRAM_ATTR flashfsLogBegin() +{ + uint32_t beginAddr = flashfs.address; + size_t idx = flashfs.journalIdx; + + flashfs.journal[idx].logBegin = beginAddr; + + if(!flashfs.partition) return; + + size_t address = flashfsJournalAddress(idx); + + flashfsWriteAbs(address, (uint8_t*)&beginAddr, sizeof(uint32_t)); +} + +static bool IRAM_ATTR flashfsLogStarted() +{ + return flashfs.journal[flashfs.journalIdx].logBegin != FLASHFS_ERASED_VAL; +} + +static void IRAM_ATTR flashfsLogEnd() +{ + uint32_t endAddr = flashfs.address; + size_t idx = flashfs.journalIdx; + + flashfs.journal[idx].logEnd = endAddr; + + flashfs.journalIdx++; + + if(!flashfs.partition) return; + + size_t address = flashfsJournalAddress(idx) + sizeof(uint32_t); + + flashfsWriteAbs(address, (uint8_t*)&endAddr, sizeof(uint32_t)); +} + +int flashfsInit(void) +{ + flashfs.partition = Espfc::Device::FlashDevice::findPartition(); + if(!flashfs.partition) return 0; + + flashfs.buffer = (void*)&buff; + flashfs.journalIdx = 0; + flashfs.address = flashfsLogLoad(); + return 1; +} + +bool IRAM_ATTR flashfsFlushAsync(bool force) +{ + if(!flashfsLogStarted()) flashfsLogBegin(); + + auto buffer = reinterpret_cast(flashfs.buffer); + const size_t size = buffer->size(); + if(flashfs.partition && size > 0) + { + //uint32_t newAddress = force ? (flashfs.address + size) : Espfc::Math::alignAddressToWrite(flashfs.address, size, FLASHFS_FLUSH_BUFFER_SIZE); + //size_t toWrite = newAddress - flashfs.address; + uint8_t tmp[FLASHFS_FLUSH_BUFFER_SIZE]; + size_t chunks = (size / FLASHFS_FLUSH_BUFFER_SIZE) + 1; + while(chunks--) + { + size_t len = buffer->pop(tmp, FLASHFS_FLUSH_BUFFER_SIZE); + flashfsWriteAbs(flashfs.address, tmp, len); + flashfs.address += len; + } + return true; + } + return false; +} + +void IRAM_ATTR flashfsWriteByte(uint8_t byte) +{ + if(flashfsIsEOF()) return; + + auto buffer = reinterpret_cast(flashfs.buffer); + buffer->push(byte); + const size_t size = buffer->size(); + + if(flashfs.address + size >= flashfsGetSize()) + { + flashfsClose(); + return; + } + + if((flashfs.address + size) % SPI_FLASH_SEC_SIZE == 0) + { + flashfsFlushAsync(true); + return; + } + + if(size >= FLASHFS_FLUSH_BUFFER_SIZE) + { + flashfsFlushAsync(false); + } +} + +void IRAM_ATTR flashfsWrite(const uint8_t *data, unsigned int len, bool sync) +{ + (void)sync; + while(len) + { + flashfsWriteByte(*data); + data++; + len--; + } +} + +void IRAM_ATTR flashfsWriteAbs(uint32_t address, const uint8_t *data, unsigned int len) +{ + if(!flashfs.partition) return; + const auto p = reinterpret_cast(flashfs.partition); + esp_partition_write_raw(p, address, data, len); +} + +int flashfsReadAbs(uint32_t address, uint8_t *data, unsigned int len) +{ + if(!flashfs.partition) return 0; + + const auto p = reinterpret_cast(flashfs.partition); + len = std::min((uint32_t)len, p->size - address); + if(esp_partition_read_raw(p, address, data, len) == ESP_OK) + { + return len; + } + return 0; +} + +bool IRAM_ATTR flashfsIsSupported(void) +{ + return (bool)flashfs.partition; +} + +bool IRAM_ATTR flashfsIsReady(void) +{ + return flashfsIsSupported(); +} + +bool IRAM_ATTR flashfsIsEOF(void) +{ + return !flashfs.partition || flashfs.journalIdx >= FLASHFS_JOURNAL_ITEMS || flashfs.address >= flashfsGetSize(); +} + +void flashfsEraseCompletely(void) +{ + const auto p = reinterpret_cast(flashfs.partition); + esp_partition_erase_range(p, 0, p->size); + flashfsInit(); +} + +void IRAM_ATTR flashfsClose(void) +{ + flashfsFlushAsync(true); + flashfsLogEnd(); +} + +uint32_t IRAM_ATTR flashfsGetWriteBufferFreeSpace(void) +{ + auto buffer = reinterpret_cast(flashfs.buffer); + return buffer->available(); +} + +uint32_t IRAM_ATTR flashfsGetWriteBufferSize(void) +{ + return FLASHFS_WRITE_BUFFER_SIZE; +} + +uint32_t IRAM_ATTR flashfsGetSize(void) +{ + const auto p = reinterpret_cast(flashfs.partition); + return p ? p->size - FLASHFS_JOURNAL_SIZE : 0; +} + +uint32_t flashfsGetOffset(void) +{ + return flashfs.address; +} + +uint32_t flashfsGetSectors(void) +{ + return flashfsGetSize() / SPI_FLASH_SEC_SIZE; +} + +#endif diff --git a/lib/Espfc/src/Blackbox/BlackboxSerialBuffer.h b/lib/Espfc/src/Blackbox/BlackboxSerialBuffer.h new file mode 100644 index 00000000..b41c2cbf --- /dev/null +++ b/lib/Espfc/src/Blackbox/BlackboxSerialBuffer.h @@ -0,0 +1,86 @@ +#pragma once + +#include "Target/Target.h" +#include "Device/SerialDevice.h" + +namespace Espfc { + +namespace Blackbox { + +class BlackboxSerialBuffer: public Device::SerialDevice +{ + public: + BlackboxSerialBuffer(): _dev(nullptr), _idx(0) {} + + virtual void wrap(Espfc::Device::SerialDevice * s) + { + _dev = s; + } + + virtual void begin(const Espfc::SerialDeviceConfig& conf) + { + //_dev->begin(conf); + } + + virtual size_t write(uint8_t c) + { + _data[_idx++] = c; + if(_idx >= SIZE) flush(); + return 1; + } + + virtual void flush() + { + if(_dev) _dev->write(_data, _idx); + _idx = 0; + } + + virtual int availableForWrite() + { + //return _dev->availableForWrite(); + return SIZE - _idx; + } + + virtual bool isTxFifoEmpty() + { + //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) { +#ifdef TARGET_RP2040 + size_t count = std::min(l, (size_t)available()); + for(size_t i = 0; i < count; i++) + { + c[i] = read(); + } + return count; +#else + return _dev->readMany(c, l); +#endif + } + virtual int peek() { return _dev->peek(); } + + virtual size_t write(const uint8_t * c, size_t l) + { + 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); } + + static const size_t SIZE = SERIAL_TX_FIFO_SIZE;//128; + + Device::SerialDevice * _dev; + size_t _idx; + uint8_t _data[SIZE]; +}; + +} + +} diff --git a/lib/Espfc/src/Buzzer.h b/lib/Espfc/src/Buzzer.h index e012a3d7..7b184fc6 100644 --- a/lib/Espfc/src/Buzzer.h +++ b/lib/Espfc/src/Buzzer.h @@ -19,12 +19,12 @@ enum BuzzerPlayStatus class Buzzer { public: - Buzzer(Model& model): _model(model), _status(BUZZER_STATUS_IDLE), _wait(0), _scheme(NULL), _e(BEEPER_SILENCE) {} + Buzzer(Model& model): _model(model), _status(BUZZER_STATUS_IDLE), _wait(0), _scheme(NULL), _e(BUZZER_SILENCE) {} int begin() { - if(_model.config.pin[PIN_BUZZER] == -1) return 0; #ifndef UNIT_TEST + if(_model.config.pin[PIN_BUZZER] == -1) return 0; pinMode(_model.config.pin[PIN_BUZZER], OUTPUT); digitalWrite(_model.config.pin[PIN_BUZZER], _model.config.buzzer.inverted); #endif @@ -39,7 +39,9 @@ class Buzzer //_model.state.debug[1] = _status; //_model.state.debug[2] = (int16_t)(millis() - _wait); +#ifndef UNIT_TEST if(_model.config.pin[PIN_BUZZER] == -1) return 0; +#endif if(!_model.state.buzzer.timer.check()) return 0; if(_wait > millis()) return 0; @@ -58,7 +60,7 @@ class Buzzer { _play(false, 10, BUZZER_STATUS_IDLE); _scheme = NULL; - _e = BEEPER_SILENCE; + _e = BUZZER_SILENCE; } else { @@ -86,7 +88,9 @@ class Buzzer void _write(bool v) { +#ifndef UNIT_TEST digitalWrite(_model.config.pin[PIN_BUZZER], _model.config.buzzer.inverted ? !v : v); +#endif } void _delay(int time) @@ -106,57 +110,57 @@ class Buzzer static const uint8_t beeperBatteryCritical[] = { 50, 0 }; static const uint8_t* beeperSchemes[] = { - //BEEPER_SILENCE + //BUZZER_SILENCE beeperSilence, - //BEEPER_GYRO_CALIBRATED, + //BUZZER_GYRO_CALIBRATED, beeperGyroCalibrated, - //BEEPER_RX_LOST, // Beeps when TX is turned off or signal lost (repeat until TX is okay) + //BUZZER_RX_LOST, // Beeps when TX is turned off or signal lost (repeat until TX is okay) beeperRxLost, - //BEEPER_RX_LOST_LANDING, // Beeps SOS when armed and TX is turned off or signal lost (autolanding/autodisarm) + //BUZZER_RX_LOST_LANDING, // Beeps SOS when armed and TX is turned off or signal lost (autolanding/autodisarm) beeperSilence, - //BEEPER_DISARMING, // Beep when disarming the board + //BUZZER_DISARMING, // Beep when disarming the board beeperDisarming, - //BEEPER_ARMING, // Beep when arming the board + //BUZZER_ARMING, // Beep when arming the board beeperArming, - //BEEPER_ARMING_GPS_FIX, // Beep a special tone when arming the board and GPS has fix + //BUZZER_ARMING_GPS_FIX, // Beep a special tone when arming the board and GPS has fix beeperSilence, - //BEEPER_BAT_CRIT_LOW, // Longer warning beeps when battery is critically low (repeats) + //BUZZER_BAT_CRIT_LOW, // Longer warning beeps when battery is critically low (repeats) beeperBatteryCritical, - //BEEPER_BAT_LOW, // Warning beeps when battery is getting low (repeats) + //BUZZER_BAT_LOW, // Warning beeps when battery is getting low (repeats) beeperBatteryLow, - //BEEPER_GPS_STATUS, // FIXME **** Disable beeper when connected to USB **** + //BUZZER_GPS_STATUS, // FIXME **** Disable beeper when connected to USB **** beeperSilence, - //BEEPER_RX_SET, // Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled + //BUZZER_RX_SET, // Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled beeperRxLost, - //BEEPER_ACC_CALIBRATION, // ACC inflight calibration completed confirmation + //BUZZER_ACC_CALIBRATION, // ACC inflight calibration completed confirmation beeperSilence, - //BEEPER_ACC_CALIBRATION_FAIL, // ACC inflight calibration failed + //BUZZER_ACC_CALIBRATION_FAIL, // ACC inflight calibration failed beeperSilence, - //BEEPER_READY_BEEP, // Ring a tone when GPS is locked and ready + //BUZZER_READY_BEEP, // Ring a tone when GPS is locked and ready beeperSilence, - //BEEPER_MULTI_BEEPS, // Internal value used by 'beeperConfirmationBeeps()'. + //BUZZER_MULTI_BEEPS, // Internal value used by 'beeperConfirmationBeeps()'. beeperSilence, - //BEEPER_DISARM_REPEAT, // Beeps sounded while stick held in disarm position + //BUZZER_DISARM_REPEAT, // Beeps sounded while stick held in disarm position beeperSilence, - //BEEPER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased) + //BUZZER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased) beeperSilence, - //BEEPER_SYSTEM_INIT, // Initialisation beeps when board is powered on + //BUZZER_SYSTEM_INIT, // Initialisation beeps when board is powered on beeperSystemInit, - //BEEPER_USB, // Some boards have beeper powered USB connected + //BUZZER_USB, // Some boards have beeper powered USB connected beeperSilence, - //BEEPER_BLACKBOX_ERASE, // Beep when blackbox erase completes + //BUZZER_BLACKBOX_ERASE, // Beep when blackbox erase completes beeperSilence, - //BEEPER_CRASH_FLIP_MODE, // Crash flip mode is active + //BUZZER_CRASH_FLIP_MODE, // Crash flip mode is active beeperSilence, - //BEEPER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated + //BUZZER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated beeperSilence, - //BEEPER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop + //BUZZER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop beeperSilence, - //BEEPER_ALL, // Turn ON or OFF all beeper conditions + //BUZZER_ALL, // Turn ON or OFF all beeper conditions beeperSilence, - //BEEPER_PREFERENCE, // Save preferred beeper configuration + //BUZZER_PREFERENCE, // Save preferred beeper configuration beeperSilence - // BEEPER_ALL and BEEPER_PREFERENCE must remain at the bottom of this enum + // BUZZER_ALL and BUZZER_PREFERENCE must remain at the bottom of this enum }; return beeperSchemes; diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 0912e277..f0f3d5bd 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -3,12 +3,23 @@ #include #include +#include #include "Model.h" #include "Hardware.h" #include "Logger.h" #include "Device/GyroDevice.h" -#include "platform.h" +#include "Hal/Pgm.h" + +#ifdef USE_FLASHFS +#include "Device/FlashDevice.h" +#endif + +#if defined(ESPFC_WIFI_ALT) +#include +#elif defined(ESPFC_WIFI) +#include +#endif #ifdef ESPFC_FREE_RTOS #include @@ -42,7 +53,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): name(n), type(t), addr(a), choices(c) {} + 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, 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) {} @@ -203,47 +214,59 @@ class Cli void update(const char ** args) const { const char * v = args[2]; - if(!addr || !v) return; + if(!addr) return; switch(type) { case PARAM_BOOL: + if(!v) return; if(*v == '0') *addr = 0; if(*v == '1') *addr = 1; break; case PARAM_BYTE: + if(!v) return; write((int8_t)parse(v)); break; case PARAM_BYTE_U: + if(!v) return; write((uint8_t)parse(v)); break; case PARAM_SHORT: + if(!v) return; write((int16_t)parse(v)); break; case PARAM_INT: + if(!v) return; write((int32_t)parse(v)); break; case PARAM_FLOAT: + if(!v) return; write(String(v).toFloat()); break; case PARAM_STRING: - write(String(v)); + write(String(v ? v : "")); break; case PARAM_OUTPUT_CHANNEL: + if(!v) return; write(*reinterpret_cast(addr), args); break; case PARAM_INPUT_CHANNEL: + if(!v) return; write(*reinterpret_cast(addr), args); break; case PARAM_SCALER: + if(!v) return; write(*reinterpret_cast(addr), args); break; case PARAM_MODE: + if(!v) return; write(*reinterpret_cast(addr), args); break; case PARAM_MIXER: + if(!v) return; write(*reinterpret_cast(addr), args); break; case PARAM_SERIAL: + if(!v) return; write(*reinterpret_cast(addr), args); break; case PARAM_NONE: @@ -311,7 +334,7 @@ class Cli void write(const String& v) const { *addr = 0; - strncat(addr, v.c_str(), 16); + strncat(addr, v.c_str(), maxLen); } int32_t parse(const char * v) const @@ -331,6 +354,7 @@ class Cli ParamType type; char * addr; const char ** choices; + size_t maxLen; }; Cli(Model& model): _model(model), _ignore(false), _active(false) @@ -376,10 +400,6 @@ class Cli static const char* currentSourceChoices[] = { PSTR("NONE"), PSTR("ADC"), NULL }; static const char* blackboxModeChoices[] = { PSTR("NORMAL"), PSTR("TEST"), PSTR("ALWAYS"), NULL }; -#ifdef ESPFC_SERIAL_SOFT_0_WIFI - const char ** wifiModeChoices = WirelessConfig::getModeNames(); -#endif - size_t i = 0; static const Param params[] = { @@ -488,9 +508,9 @@ class Cli Param(PSTR("input_filter_type"), &c.input.filterType, inputFilterChoices), Param(PSTR("input_lpf_type"), &c.input.filter.type, filterTypeChoices), Param(PSTR("input_lpf_freq"), &c.input.filter.freq), + Param(PSTR("input_lpf_factor"), &c.input.filterAutoFactor), Param(PSTR("input_ff_lpf_type"), &c.input.filterDerivative.type, filterTypeChoices), Param(PSTR("input_ff_lpf_freq"), &c.input.filterDerivative.freq), - Param(PSTR("input_lpf_factor"), &c.input.filterAutoFactor), Param(PSTR("input_rssi_channel"), &c.input.rssiChannel), @@ -579,7 +599,7 @@ class Cli Param(PSTR("pid_dterm_dyn_lpf_max"), &c.dtermDynLpfFilter.freq), Param(PSTR("pid_dterm_weight"), &c.dtermSetpointWeight), - Param(PSTR("pid_iterm_limit"), &c.itermWindupPointPercent), + Param(PSTR("pid_iterm_limit"), &c.itermLimit), Param(PSTR("pid_iterm_zero"), &c.lowThrottleZeroIterm), Param(PSTR("pid_iterm_relax"), &c.itermRelax, inputItermRelaxChoices), Param(PSTR("pid_iterm_relax_cutoff"), &c.itermRelaxCutoff), @@ -683,10 +703,10 @@ class Cli #ifdef ESPFC_I2C_0 Param(PSTR("i2c_speed"), &c.i2cSpeed), #endif + Param(PSTR("rescue_config_delay"), &c.rescueConfigDelay), + //Param(PSTR("telemetry"), &c.telemetry), Param(PSTR("telemetry_interval"), &c.telemetryInterval), - //Param(PSTR("soft_serial_guard"), &c.softSerialGuard), - //Param(PSTR("serial_rx_guard"), &c.serialRxGuard), Param(PSTR("blackbox_dev"), &c.blackboxDev), Param(PSTR("blackbox_mode"), &c.blackboxMode, blackboxModeChoices), @@ -694,11 +714,8 @@ class Cli Param(PSTR("blackbox_mask"), &c.blackboxFieldsDisabledMask), #ifdef ESPFC_SERIAL_SOFT_0_WIFI - Param(PSTR("wifi_mode"), &c.wireless.mode, wifiModeChoices), - Param(PSTR("wifi_ssid"), PARAM_STRING, &c.wireless.ssid[0], NULL), - Param(PSTR("wifi_pass"), PARAM_STRING, &c.wireless.pass[0], NULL), - Param(PSTR("wifi_ssid_ap"), PARAM_STRING, &c.wireless.ssidAp[0], NULL), - Param(PSTR("wifi_pass_ap"), PARAM_STRING, &c.wireless.passAp[0], NULL), + Param(PSTR("wifi_ssid"), PARAM_STRING, &c.wireless.ssid[0], NULL, 32), + Param(PSTR("wifi_pass"), PARAM_STRING, &c.wireless.pass[0], NULL, 32), Param(PSTR("wifi_tcp_port"), &c.wireless.port), #endif @@ -780,8 +797,20 @@ class Cli { //FIXME: detect disconnection _active = true; + stream.println(); + stream.println(F("Entering CLI Mode, type 'exit' to return, or 'help'")); + stream.print(F("# ")); printVersion(stream); - stream.println(F(", CLI mode, type help")); + stream.println(); + _model.setArmingDisabled(ARMING_DISABLED_CLI, true); + cmd = CliCmd(); + return true; + } + if(_active && c == 4) // CTRL-D + { + stream.println(); + stream.println(F(" #leaving CLI mode, unsaved changes lost")); + _active = false; cmd = CliCmd(); return true; } @@ -791,8 +820,6 @@ class Cli { parse(cmd); execute(cmd, stream); - //cmd.index = 0; - //cmd.buff[cmd.index] = '\0'; cmd = CliCmd(); return true; } @@ -861,14 +888,31 @@ class Cli printVersion(s); s.println(); } +#if defined(ESPFC_WIFI) || defined(ESPFC_WIFI_ALT) else if(strcmp_P(cmd.args[0], PSTR("wifi")) == 0) { - s.print(F("IPv4 : tcp://")); - s.print(_model.state.localIp); + s.print(F("ST IP4: tcp://")); + s.print(WiFi.localIP()); s.print(F(":")); s.println(_model.config.wireless.port); + s.print(F("ST MAC: ")); + s.println(WiFi.macAddress()); + s.print(F("AP IP4: tcp://")); + s.print(WiFi.softAPIP()); + s.print(F(":")); + s.println(_model.config.wireless.port); + s.print(F("AP MAC: ")); + s.println(WiFi.softAPmacAddress()); + s.print(F("STATUS: ")); + s.println(WiFi.status()); + s.print(F(" MODE: ")); + s.println(WiFi.getMode()); + s.print(F("CHANNEL: ")); + s.println(WiFi.channel()); + //WiFi.printDiag(s); } - #if defined(ESPFC_FREE_RTOS) +#endif +#if defined(ESPFC_FREE_RTOS) else if(strcmp_P(cmd.args[0], PSTR("tasks")) == 0) { printVersion(s); @@ -880,7 +924,7 @@ class Cli s.print(numTasks); s.println(); } - #endif +#endif else if(strcmp_P(cmd.args[0], PSTR("devinfo")) == 0) { printVersion(s); @@ -942,9 +986,9 @@ class Cli } else if(strcmp_P(cmd.args[0], PSTR("dump")) == 0) { - s.print('#'); - printVersion(s); - s.println(); + //s.print(F("# ")); + //printVersion(s); + //s.println(); for(size_t i = 0; _params[i].name; ++i) { print(_params[i], s); @@ -1149,7 +1193,6 @@ class Cli } else if(strcmp_P(cmd.args[0], PSTR("status")) == 0) { - printVersion(s); s.println(); s.println(F("STATUS: ")); @@ -1159,48 +1202,48 @@ class Cli Device::GyroDevice * gyro = _model.state.gyroDev; Device::BaroDevice * baro = _model.state.baroDev; Device::MagDevice * mag = _model.state.magDev; + s.print(F(" devices: ")); if(gyro) { - s.print(F(" gyro device: ")); s.print(FPSTR(Device::GyroDevice::getName(gyro->getType()))); s.print('/'); - s.println(FPSTR(Device::BusDevice::getName(gyro->getBus()->getType()))); + s.print(FPSTR(Device::BusDevice::getName(gyro->getBus()->getType()))); } else { - s.println(F(" gyro device: NONE")); + s.print(F("NO_GYRO")); } if(baro) { - s.print(F(" baro device: ")); + s.print(F(", ")); s.print(FPSTR(Device::BaroDevice::getName(baro->getType()))); s.print('/'); - s.println(FPSTR(Device::BusDevice::getName(baro->getBus()->getType()))); + s.print(FPSTR(Device::BusDevice::getName(baro->getBus()->getType()))); } else { - s.println(F(" baro device: NONE")); + s.print(F(", NO_BARO")); } if(mag) { - s.print(F(" mag device: ")); + s.print(F(", ")); s.print(FPSTR(Device::MagDevice::getName(mag->getType()))); s.print('/'); - s.println(FPSTR(Device::BusDevice::getName(mag->getBus()->getType()))); + s.print(FPSTR(Device::BusDevice::getName(mag->getBus()->getType()))); } else { - s.println(F(" mag device: NONE")); + s.print(F(", NO_MAG")); } + s.println(); - s.print(F(" rx rate: ")); - s.println(_model.state.inputFrameRate); - - s.print(F(" rx lpfs: ")); + s.print(F(" input: ")); + s.print(_model.state.inputFrameRate); + s.print(F(" Hz, ")); s.print(_model.state.inputAutoFreq); - s.print(F(", ")); + s.print(F(" Hz, ")); s.println(_model.state.inputAutoFactor); static const char* armingDisableNames[] = { @@ -1214,8 +1257,7 @@ class Cli }; const size_t armingDisableNamesLength = sizeof(armingDisableNames) / sizeof(armingDisableNames[0]); - s.println(); - s.print(F(" arming disabled:")); + s.print(F("arming flags:")); for(size_t i = 0; i < armingDisableNamesLength; i++) { if(_model.state.armingDisabledFlags & (1 << i)) { @@ -1224,6 +1266,13 @@ class Cli } } s.println(); + s.print(F(" rescue mode: ")); + s.print(_model.state.rescueConfigMode); + s.println(); + + s.print(F(" uptime: ")); + s.print(millis() * 0.001, 1); + s.println(); } else if(strcmp_P(cmd.args[0], PSTR("stats")) == 0) { @@ -1270,18 +1319,15 @@ class Cli s.print(F("%")); s.println(); } - else if(strcmp_P(cmd.args[0], PSTR("reboot")) == 0) + else if(strcmp_P(cmd.args[0], PSTR("reboot")) == 0 || strcmp_P(cmd.args[0], PSTR("exit")) == 0) { + _active = false; Hardware::restart(_model); } else if(strcmp_P(cmd.args[0], PSTR("defaults")) == 0) { _model.reset(); } - else if(strcmp_P(cmd.args[0], PSTR("exit")) == 0) - { - _active = false; - } else if(strcmp_P(cmd.args[0], PSTR("motors")) == 0) { s.print(PSTR("count: ")); @@ -1308,6 +1354,66 @@ class Cli s.print(PSTR("total: ")); s.println(_model.logger.length()); } +#ifdef USE_FLASHFS + else if(strcmp_P(cmd.args[0], PSTR("flash")) == 0) + { + if(!cmd.args[1]) + { + size_t total = flashfsGetSize(); + size_t used = flashfsGetOffset(); + s.printf("total: %d\r\n", total); + s.printf(" used: %d\r\n", used); + s.printf(" free: %d\r\n", total - used); + } + else if(strcmp_P(cmd.args[1], PSTR("partitions")) == 0) + { + Device::FlashDevice::partitions(s); + } + else if(strcmp_P(cmd.args[1], PSTR("journal")) == 0) + { + const FlashfsRuntime* flashfs = flashfsGetRuntime(); + FlashfsJournalItem journal[16]; + flashfsJournalLoad(journal, 0, 16); + for(size_t i = 0; i < 16; i++) + { + const auto& it = journal[i]; + const auto& itr = flashfs->journal[i]; + s.printf("%02d: %08X : %08X / %08X : %08X\r\n",i , it.logBegin, it.logEnd, itr.logBegin, itr.logEnd); + } + s.printf("current: %d\r\n", flashfs->journalIdx); + } + else if(strcmp_P(cmd.args[1], PSTR("erase")) == 0) + { + flashfsEraseCompletely(); + } + else if(strcmp_P(cmd.args[1], PSTR("test")) == 0) + { + const char * data = "flashfs-test"; + flashfsWrite((const uint8_t*)data, strlen(data), true); + flashfsFlushAsync(true); + flashfsClose(); + } + else if(strcmp_P(cmd.args[1], PSTR("print")) == 0) + { + size_t addr = 0; + if(cmd.args[2]) + { + addr = String(cmd.args[2]).toInt(); + } + uint8_t data[16]; + flashfsReadAbs(addr, data, sizeof(data)); + for(size_t i = 0; i < sizeof(data); i++) + { + s.printf("%02x ", data[i]); + } + s.println(); + } + else + { + s.println(F("wrong param!")); + } + } +#endif else { s.print(F("unknown command: ")); @@ -1340,6 +1446,10 @@ class Cli s.print(' '); s.print(buildTime); s.print(' '); + s.print(API_VERSION_MAJOR); + s.print('.'); + s.print(API_VERSION_MINOR); + s.print(' '); s.print(__VERSION__); s.print(' '); s.print(__cplusplus); diff --git a/lib/Espfc/src/Control/Pid.cpp b/lib/Espfc/src/Control/Pid.cpp new file mode 100644 index 00000000..9a0c522f --- /dev/null +++ b/lib/Espfc/src/Control/Pid.cpp @@ -0,0 +1,90 @@ +#include "Pid.h" +#include "Math/Utils.h" +#include "Utils/MemoryHelper.h" + +namespace Espfc { + +namespace Control { + +Pid::Pid(): + rate(1.0f), dt(1.0f), Kp(0.1), Ki(0.f), Kd(0.f), Kf(0.0f), iLimit(0.3f), oLimit(1.f), + pScale(1.f), iScale(1.f), dScale(1.f), fScale(1.f), + error(0.f), iTermError(0.f), + pTerm(0.f), iTerm(0.f), dTerm(0.f), fTerm(0.f), + prevMeasurement(0.f), prevError(0.f), prevSetpoint(0.f), + outputSaturated(false), + itermRelax(ITERM_RELAX_OFF), itermRelaxFactor(1.0f), itermRelaxBase(0.f) + {} + +void Pid::begin() +{ + dt = 1.f / rate; +} + +float FAST_CODE_ATTR Pid::update(float setpoint, float measurement) +{ + error = setpoint - measurement; + + // P-term + pTerm = Kp * error * pScale; + pTerm = ptermFilter.update(pTerm); + + // I-term + iTermError = error; + if(Ki > 0.f && iScale > 0.f) + { + if(!outputSaturated) + { + // I-term relax + if(itermRelax) + { + const bool increasing = (iTerm > 0 && iTermError > 0) || (iTerm < 0 && iTermError < 0); + const bool incrementOnly = itermRelax == ITERM_RELAX_RP_INC || itermRelax == ITERM_RELAX_RPY_INC; + itermRelaxBase = setpoint - itermRelaxFilter.update(setpoint); + itermRelaxFactor = std::max(0.0f, 1.0f - std::abs(Math::toDeg(itermRelaxBase)) * 0.025f); // (itermRelaxBase / 40) + if(!incrementOnly || increasing) iTermError *= itermRelaxFactor; + } + iTerm += Ki * iScale * iTermError * dt; + iTerm = Math::clamp(iTerm, -iLimit, iLimit); + } + } + else + { + iTerm = 0; // zero integral + } + + // D-term + if(Kd > 0.f && dScale > 0.f) + { + //dTerm = (Kd * dScale * (((error - prevError) * dGamma) + (prevMeasurement - measure) * (1.f - dGamma)) / dt); + dTerm = Kd * dScale * ((prevMeasurement - measurement) * rate); + dTerm = dtermNotchFilter.update(dTerm); + dTerm = dtermFilter.update(dTerm); + dTerm = dtermFilter2.update(dTerm); + } + else + { + dTerm = 0; + } + + // F-term + if(Kf > 0.f && fScale > 0.f) + { + fTerm = Kf * fScale * (setpoint - prevSetpoint) * rate; + fTerm = ftermFilter.update(fTerm); + } + else + { + fTerm = 0; + } + + prevMeasurement = measurement; + prevError = error; + prevSetpoint = setpoint; + + return Math::clamp(pTerm + iTerm + dTerm + fTerm, -oLimit, oLimit); +} + +} + +} diff --git a/lib/Espfc/src/Control/Pid.h b/lib/Espfc/src/Control/Pid.h index 17bc7911..465cf87a 100644 --- a/lib/Espfc/src/Control/Pid.h +++ b/lib/Espfc/src/Control/Pid.h @@ -1,7 +1,6 @@ -#ifndef _ESPFC_CONTROL_PID_H_ -#define _ESPFC_CONTROL_PID_H_ +#pragma once -#include "Math/Utils.h" +#include #include "Filter.h" // bataflight scalers @@ -36,84 +35,9 @@ namespace Control { class Pid { public: - Pid(): - rate(1.0f), dt(1.0f), Kp(0.1), Ki(0.f), Kd(0.f), Kf(0.0f), iLimit(0.3f), oLimit(1.f), - pScale(1.f), iScale(1.f), dScale(1.f), fScale(1.f), - error(0.f), iTermError(0.f), - pTerm(0.f), iTerm(0.f), dTerm(0.f), fTerm(0.f), - prevMeasure(0.f), prevError(0.f), prevSetpoint(0.f), - outputSaturated(false), - itermRelax(ITERM_RELAX_OFF), itermRelaxFactor(1.0f), itermRelaxBase(0.f) - {} - - void begin() - { - dt = 1.f / rate; - } - - float update(float setpoint, float measure) - { - error = setpoint - measure; - - // P-term - pTerm = Kp * error * pScale; - pTerm = ptermFilter.update(pTerm); - - // I-term - iTermError = error; - if(Ki > 0.f && iScale > 0.f) - { - if(!outputSaturated) - { - // I-term relax - if(itermRelax) - { - const bool increasing = (iTerm > 0 && iTermError > 0) || (iTerm < 0 && iTermError < 0); - const bool incrementOnly = itermRelax == ITERM_RELAX_RP_INC || itermRelax == ITERM_RELAX_RPY_INC; - itermRelaxBase = setpoint - itermRelaxFilter.update(setpoint); - itermRelaxFactor = std::max(0.0f, 1.0f - abs(Math::toDeg(itermRelaxBase)) * 0.025f); // (itermRelaxBase / 40) - if(!incrementOnly || increasing) iTermError *= itermRelaxFactor; - } - iTerm += Ki * iScale * iTermError * dt; - iTerm = Math::clamp(iTerm, -iLimit, iLimit); - } - } - else - { - iTerm = 0; // zero integral - } - - // D-term - if(Kd > 0.f && dScale > 0.f) - { - //dTerm = (Kd * dScale * (((error - prevError) * dGamma) + (prevMeasure - measure) * (1.f - dGamma)) / dt); - dTerm = Kd * dScale * ((prevMeasure - measure) * rate); - dTerm = dtermNotchFilter.update(dTerm); - dTerm = dtermFilter.update(dTerm); - dTerm = dtermFilter2.update(dTerm); - } - else - { - dTerm = 0; - } - - // F-term - if(Kf > 0.f && fScale > 0.f) - { - fTerm = Kf * fScale * (setpoint - prevSetpoint) * rate; - fTerm = ftermFilter.update(fTerm); - } - else - { - fTerm = 0; - } - - prevMeasure = measure; - prevError = error; - prevSetpoint = setpoint; - - return Math::clamp(pTerm + iTerm + dTerm + fTerm, -oLimit, oLimit); - } + Pid(); + void begin(); + float update(float setpoint, float measure); float rate; float dt; @@ -125,7 +49,6 @@ class Pid float iLimit; float oLimit; - //float dGamma; float pScale; float iScale; @@ -147,7 +70,7 @@ class Pid Filter ftermFilter; Filter itermRelaxFilter; - float prevMeasure; + float prevMeasurement; float prevError; float prevSetpoint; @@ -160,5 +83,3 @@ class Pid } } - -#endif diff --git a/lib/Espfc/src/Control/Rates.cpp b/lib/Espfc/src/Control/Rates.cpp new file mode 100644 index 00000000..0b44b1d1 --- /dev/null +++ b/lib/Espfc/src/Control/Rates.cpp @@ -0,0 +1,110 @@ +#include "Rates.h" +#include "Utils/MemoryHelper.h" + +namespace Espfc { + +void Rates::begin(const InputConfig& config) +{ + rateType = (RateType)config.rateType; + for(size_t i = 0; i < 3; i++) + { + rcExpo[i] = config.expo[i]; + rcRates[i] = config.rate[i]; + rates[i] = config.superRate[i]; + rateLimit[i] = config.rateLimit[i]; + } +} + +float FAST_CODE_ATTR Rates::getSetpoint(const int axis, float input) const +{ + input = Math::clamp(input, -0.995f, 0.995f); // limit input + const float inputAbs = fabsf(input); + float result = 0; + switch(rateType) + { + case RATES_TYPE_BETAFLIGHT: + result = betaflight(axis, input, inputAbs); + case RATES_TYPE_RACEFLIGHT: + result = raceflight(axis, input, inputAbs); + case RATES_TYPE_KISS: + result = kiss(axis, input, inputAbs); + case RATES_TYPE_ACTUAL: + result = actual(axis, input, inputAbs); + case RATES_TYPE_QUICK: + result = quick(axis, input, inputAbs); + } + return Math::toRad(Math::clamp(result, -(float)rateLimit[axis], (float)rateLimit[axis])); +} + +float FAST_CODE_ATTR Rates::betaflight(const int axis, float rcCommandf, const float rcCommandfAbs) const +{ + if (this->rcExpo[axis]) + { + const float expof = this->rcExpo[axis] / 100.0f; + rcCommandf = rcCommandf * power3(rcCommandfAbs) * expof + rcCommandf * (1 - expof); + } + + float rcRate = this->rcRates[axis] / 100.0f; + if (rcRate > 2.0f) + { + rcRate += RC_RATE_INCREMENTAL * (rcRate - 2.0f); + } + float angleRate = 200.0f * rcRate * rcCommandf; + if (this->rates[axis]) + { + const float rcSuperfactor = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (this->rates[axis] / 100.0f)), 0.01f, 1.00f)); + angleRate *= rcSuperfactor; + } + + return angleRate; +} + +float FAST_CODE_ATTR Rates::raceflight(const int axis, float rcCommandf, const float rcCommandfAbs) const +{ + // -1.0 to 1.0 ranged and curved + rcCommandf = ((1.0f + 0.01f * this->rcExpo[axis] * (rcCommandf * rcCommandf - 1.0f)) * rcCommandf); + // convert to -2000 to 2000 range using acro+ modifier + float angleRate = 10.0f * this->rcRates[axis] * rcCommandf; + angleRate = angleRate * (1 + rcCommandfAbs * (float)this->rates[axis] * 0.01f); + + return angleRate; +} + +float FAST_CODE_ATTR Rates::kiss(const int axis, float rcCommandf, const float rcCommandfAbs) const +{ + const float rcCurvef = this->rcExpo[axis] / 100.0f; + + float kissRpyUseRates = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (this->rates[axis] / 100.0f)), 0.01f, 1.00f)); + float kissRcCommandf = (power3(rcCommandf) * rcCurvef + rcCommandf * (1 - rcCurvef)) * (this->rcRates[axis] / 1000.0f); + float kissAngle = constrainf(((2000.0f * kissRpyUseRates) * kissRcCommandf), -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); + + return kissAngle; +} + +float FAST_CODE_ATTR Rates::actual(const int axis, float rcCommandf, const float rcCommandfAbs) const +{ + float expof = this->rcExpo[axis] / 100.0f; + expof = rcCommandfAbs * (power5(rcCommandf) * expof + rcCommandf * (1 - expof)); + + const float centerSensitivity = this->rcRates[axis] * 10.0f; + const float stickMovement = std::max(0.f, this->rates[axis] * 10.0f - centerSensitivity); + const float angleRate = rcCommandf * centerSensitivity + stickMovement * expof; + + return angleRate; +} + +float FAST_CODE_ATTR Rates::quick(const int axis, float rcCommandf, const float rcCommandfAbs) const +{ + const float rcRate = this->rcRates[axis] * 2; + const float maxDPS = std::max(this->rates[axis] * 10.f, rcRate); + const float linearity = this->rcExpo[axis] / 100.0f; + const float superFactorConfig = (maxDPS / rcRate - 1) / (maxDPS / rcRate); + + float curve = power3(rcCommandfAbs) * linearity + rcCommandfAbs * (1 - linearity); + float superfactor = 1.0f / (constrainf(1.0f - (curve * superFactorConfig), 0.01f, 1.00f)); + float angleRate = constrainf(rcCommandf * rcRate * superfactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); + + return angleRate; +} + +} diff --git a/lib/Espfc/src/Control/Rates.h b/lib/Espfc/src/Control/Rates.h index d5c302cb..3378d0eb 100644 --- a/lib/Espfc/src/Control/Rates.h +++ b/lib/Espfc/src/Control/Rates.h @@ -1,7 +1,7 @@ -#ifndef _ESPFC_RATES_H_ -#define _ESPFC_RATES_H_ +#pragma once #include "Math/Utils.h" +#include "ModelConfig.h" #define SETPOINT_RATE_LIMIT 1998.0f #define RC_RATE_INCREMENTAL 14.54f @@ -20,110 +20,15 @@ enum RateType { class Rates { public: - void begin(const InputConfig& config) - { - rateType = (RateType)config.rateType; - for(size_t i = 0; i < 3; i++) - { - rcExpo[i] = config.expo[i]; - rcRates[i] = config.rate[i]; - rates[i] = config.superRate[i]; - rateLimit[i] = config.rateLimit[i]; - } - } - - float getSetpoint(const int axis, float input) - { - input = Math::clamp(input, -0.995f, 0.995f); // limit input - const float inputAbs = fabsf(input); - float result = 0; - switch(rateType) - { - case RATES_TYPE_BETAFLIGHT: - result = betaflight(axis, input, inputAbs); - case RATES_TYPE_RACEFLIGHT: - result = raceflight(axis, input, inputAbs); - case RATES_TYPE_KISS: - result = kiss(axis, input, inputAbs); - case RATES_TYPE_ACTUAL: - result = actual(axis, input, inputAbs); - case RATES_TYPE_QUICK: - result = quick(axis, input, inputAbs); - } - return Math::toRad(Math::clamp(result, -(float)rateLimit[axis], (float)rateLimit[axis])); - } + void begin(const InputConfig& config); + float getSetpoint(const int axis, float input) const; private: - float betaflight(const int axis, float rcCommandf, const float rcCommandfAbs) - { - if (this->rcExpo[axis]) - { - const float expof = this->rcExpo[axis] / 100.0f; - rcCommandf = rcCommandf * power3(rcCommandfAbs) * expof + rcCommandf * (1 - expof); - } - - float rcRate = this->rcRates[axis] / 100.0f; - if (rcRate > 2.0f) - { - rcRate += RC_RATE_INCREMENTAL * (rcRate - 2.0f); - } - float angleRate = 200.0f * rcRate * rcCommandf; - if (this->rates[axis]) - { - const float rcSuperfactor = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (this->rates[axis] / 100.0f)), 0.01f, 1.00f)); - angleRate *= rcSuperfactor; - } - - return angleRate; - } - - float raceflight(const int axis, float rcCommandf, const float rcCommandfAbs) - { - // -1.0 to 1.0 ranged and curved - rcCommandf = ((1.0f + 0.01f * this->rcExpo[axis] * (rcCommandf * rcCommandf - 1.0f)) * rcCommandf); - // convert to -2000 to 2000 range using acro+ modifier - float angleRate = 10.0f * this->rcRates[axis] * rcCommandf; - angleRate = angleRate * (1 + rcCommandfAbs * (float)this->rates[axis] * 0.01f); - - return angleRate; - } - - float kiss(const int axis, float rcCommandf, const float rcCommandfAbs) - { - const float rcCurvef = this->rcExpo[axis] / 100.0f; - - float kissRpyUseRates = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (this->rates[axis] / 100.0f)), 0.01f, 1.00f)); - float kissRcCommandf = (power3(rcCommandf) * rcCurvef + rcCommandf * (1 - rcCurvef)) * (this->rcRates[axis] / 1000.0f); - float kissAngle = constrainf(((2000.0f * kissRpyUseRates) * kissRcCommandf), -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); - - return kissAngle; - } - - float actual(const int axis, float rcCommandf, const float rcCommandfAbs) - { - float expof = this->rcExpo[axis] / 100.0f; - expof = rcCommandfAbs * (power5(rcCommandf) * expof + rcCommandf * (1 - expof)); - - const float centerSensitivity = this->rcRates[axis] * 10.0f; - const float stickMovement = std::max(0.f, this->rates[axis] * 10.0f - centerSensitivity); - const float angleRate = rcCommandf * centerSensitivity + stickMovement * expof; - - return angleRate; - } - - float quick(const int axis, float rcCommandf, const float rcCommandfAbs) - { - const float rcRate = this->rcRates[axis] * 2; - const float maxDPS = std::max(this->rates[axis] * 10.f, rcRate); - const float linearity = this->rcExpo[axis] / 100.0f; - const float superFactorConfig = (maxDPS / rcRate - 1) / (maxDPS / rcRate); - - float curve = power3(rcCommandfAbs) * linearity + rcCommandfAbs * (1 - linearity); - float superfactor = 1.0f / (constrainf(1.0f - (curve * superFactorConfig), 0.01f, 1.00f)); - float angleRate = constrainf(rcCommandf * rcRate * superfactor, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); - - return angleRate; - } + float betaflight(const int axis, float rcCommandf, const float rcCommandfAbs) const; + float raceflight(const int axis, float rcCommandf, const float rcCommandfAbs) const; + float kiss(const int axis, float rcCommandf, const float rcCommandfAbs) const; + float actual(const int axis, float rcCommandf, const float rcCommandfAbs) const; + float quick(const int axis, float rcCommandf, const float rcCommandfAbs) const; inline float power3(float x) const { @@ -149,5 +54,3 @@ class Rates }; } // namespace Espfc - -#endif diff --git a/lib/Espfc/src/Controller.cpp b/lib/Espfc/src/Controller.cpp new file mode 100644 index 00000000..e3e81014 --- /dev/null +++ b/lib/Espfc/src/Controller.cpp @@ -0,0 +1,187 @@ +#include "Controller.h" +#include "Math/Utils.h" + +namespace Espfc { + +Controller::Controller(Model& model): _model(model) {} + +int Controller::begin() +{ + _rates.begin(_model.config.input); + _speedFilter.begin(FilterConfig(FILTER_BIQUAD, 10), _model.state.loopTimer.rate); + return 1; +} + +int FAST_CODE_ATTR Controller::update() +{ + uint32_t startTime = 0; + if(_model.config.debugMode == DEBUG_PIDLOOP) + { + startTime = micros(); + _model.state.debug[0] = startTime - _model.state.loopTimer.last; + } + + { + Stats::Measure(_model.state.stats, COUNTER_OUTER_PID); + resetIterm(); + if(_model.config.mixerType == FC_MIXER_GIMBAL) + { + outerLoopRobot(); + } + else + { + outerLoop(); + } + } + + { + Stats::Measure(_model.state.stats, COUNTER_INNER_PID); + if(_model.config.mixerType == FC_MIXER_GIMBAL) + { + innerLoopRobot(); + } + else + { + innerLoop(); + } + } + + if(_model.config.debugMode == DEBUG_PIDLOOP) + { + _model.state.debug[2] = micros() - startTime; + } + + return 1; +} + +void Controller::outerLoopRobot() +{ + const float speedScale = 2.f; + const float gyroScale = 0.1f; + const float speed = _speedFilter.update(_model.state.output[AXIS_PITCH] * speedScale + _model.state.gyro[AXIS_PITCH] * gyroScale); + float angle = 0; + + if(true || _model.isActive(MODE_ANGLE)) + { + angle = _model.state.input[AXIS_PITCH] * radians(_model.config.angleLimit); + } + else + { + angle = _model.state.outerPid[AXIS_PITCH].update(_model.state.input[AXIS_PITCH], speed) * radians(_model.config.angleRateLimit); + } + _model.state.desiredAngle.set(AXIS_PITCH, angle); + _model.state.desiredRate[AXIS_YAW] = _model.state.input[AXIS_YAW] * radians(_model.config.angleRateLimit); + + if(_model.config.debugMode == DEBUG_ANGLERATE) + { + _model.state.debug[0] = speed * 1000; + _model.state.debug[1] = lrintf(degrees(angle) * 10); + } +} + +void Controller::innerLoopRobot() +{ + //VectorFloat v(0.f, 0.f, 1.f); + //v.rotate(_model.state.angleQ); + //const float angle = acos(v.z); + const float angle = std::max(abs(_model.state.angle[AXIS_PITCH]), abs(_model.state.angle[AXIS_ROLL])); + + const bool stabilize = angle < radians(_model.config.angleLimit); + if(stabilize) + { + _model.state.output[AXIS_PITCH] = _model.state.innerPid[AXIS_PITCH].update(_model.state.desiredAngle[AXIS_PITCH], _model.state.angle[AXIS_PITCH]); + _model.state.output[AXIS_YAW] = _model.state.innerPid[AXIS_YAW].update(_model.state.desiredRate[AXIS_YAW], _model.state.gyro[AXIS_YAW]); + } + else + { + resetIterm(); + _model.state.output[AXIS_PITCH] = 0.f; + _model.state.output[AXIS_YAW] = 0.f; + } + + if(_model.config.debugMode == DEBUG_ANGLERATE) + { + _model.state.debug[2] = lrintf(degrees(_model.state.angle[AXIS_PITCH]) * 10); + _model.state.debug[3] = lrintf(_model.state.output[AXIS_PITCH] * 1000); + } +} + +void FAST_CODE_ATTR Controller::outerLoop() +{ + if(_model.isActive(MODE_ANGLE)) + { + _model.state.desiredAngle = VectorFloat( + _model.state.input[AXIS_ROLL] * radians(_model.config.angleLimit), + _model.state.input[AXIS_PITCH] * radians(_model.config.angleLimit), + _model.state.angle[AXIS_YAW] + ); + _model.state.desiredRate[AXIS_ROLL] = _model.state.outerPid[AXIS_ROLL].update(_model.state.desiredAngle[AXIS_ROLL], _model.state.angle[AXIS_ROLL]); + _model.state.desiredRate[AXIS_PITCH] = _model.state.outerPid[AXIS_PITCH].update(_model.state.desiredAngle[AXIS_PITCH], _model.state.angle[AXIS_PITCH]); + // disable fterm in angle mode + _model.state.innerPid[AXIS_ROLL].fScale = 0.f; + _model.state.innerPid[AXIS_PITCH].fScale = 0.f; + } + else + { + _model.state.desiredRate[AXIS_ROLL] = calculateSetpointRate(AXIS_ROLL, _model.state.input[AXIS_ROLL]); + _model.state.desiredRate[AXIS_PITCH] = calculateSetpointRate(AXIS_PITCH, _model.state.input[AXIS_PITCH]); + } + _model.state.desiredRate[AXIS_YAW] = calculateSetpointRate(AXIS_YAW, _model.state.input[AXIS_YAW]); + _model.state.desiredRate[AXIS_THRUST] = _model.state.input[AXIS_THRUST]; + + if(_model.config.debugMode == DEBUG_ANGLERATE) + { + for(size_t i = 0; i < 3; ++i) + { + _model.state.debug[i] = lrintf(degrees(_model.state.desiredRate[i])); + } + } +} + +void FAST_CODE_ATTR Controller::innerLoop() +{ + const float tpaFactor = getTpaFactor(); + for(size_t i = 0; i <= AXIS_YAW; ++i) + { + _model.state.output[i] = _model.state.innerPid[i].update(_model.state.desiredRate[i], _model.state.gyro[i]) * tpaFactor; + //_model.state.debug[i] = lrintf(_model.state.innerPid[i].fTerm * 1000); + } + _model.state.output[AXIS_THRUST] = _model.state.desiredRate[AXIS_THRUST]; + + if(_model.config.debugMode == DEBUG_ITERM_RELAX) + { + _model.state.debug[0] = lrintf(Math::toDeg(_model.state.innerPid[0].itermRelaxBase)); + _model.state.debug[1] = lrintf(_model.state.innerPid[0].itermRelaxFactor * 100.0f); + _model.state.debug[2] = lrintf(Math::toDeg(_model.state.innerPid[0].iTermError)); + _model.state.debug[3] = lrintf(_model.state.innerPid[0].iTerm * 1000.0f); + } +} + +float Controller::getTpaFactor() const +{ + if(_model.config.tpaScale == 0) return 1.f; + float t = Math::clamp(_model.state.inputUs[AXIS_THRUST], (float)_model.config.tpaBreakpoint, 2000.f); + return Math::map(t, (float)_model.config.tpaBreakpoint, 2000.f, 1.f, 1.f - ((float)_model.config.tpaScale * 0.01f)); +} + +void Controller::resetIterm() +{ + if(!_model.isActive(MODE_ARMED) // when not armed + || (!_model.isAirModeActive() && _model.config.lowThrottleZeroIterm && _model.isThrottleLow()) // on low throttle (not in air mode) + ) + { + for(size_t i = 0; i < AXES; i++) + { + _model.state.innerPid[i].iTerm = 0; + _model.state.outerPid[i].iTerm = 0; + } + } +} + +float Controller::calculateSetpointRate(int axis, float input) +{ + if(axis == AXIS_YAW) input *= -1.f; + return _rates.getSetpoint(axis, input); +} + +} diff --git a/lib/Espfc/src/Controller.h b/lib/Espfc/src/Controller.h index 9f557989..e24c6782 100644 --- a/lib/Espfc/src/Controller.h +++ b/lib/Espfc/src/Controller.h @@ -2,7 +2,6 @@ #define _ESPFC_CONTROLLER_H_ #include "Model.h" -#include "Math/Utils.h" #include "Control/Rates.h" namespace Espfc { @@ -10,192 +9,23 @@ namespace Espfc { class Controller { public: - Controller(Model& model): _model(model) {} + Controller(Model& model); + int begin(); + int update(); - int begin() - { - _rates.begin(_model.config.input); - _speedFilter.begin(FilterConfig(FILTER_BIQUAD, 10), _model.state.loopTimer.rate); - return 1; - } + void outerLoopRobot(); + void innerLoopRobot(); + void outerLoop(); + void innerLoop(); - int update() - { - uint32_t startTime = 0; - if(_model.config.debugMode == DEBUG_PIDLOOP) - { - startTime = micros(); - _model.state.debug[0] = startTime - _model.state.loopTimer.last; - } - - { - Stats::Measure(_model.state.stats, COUNTER_OUTER_PID); - resetIterm(); - if(_model.config.mixerType == FC_MIXER_GIMBAL) - { - outerLoopRobot(); - } - else - { - outerLoop(); - } - } - - { - Stats::Measure(_model.state.stats, COUNTER_INNER_PID); - if(_model.config.mixerType == FC_MIXER_GIMBAL) - { - innerLoopRobot(); - } - else - { - innerLoop(); - } - } - - if(_model.config.debugMode == DEBUG_PIDLOOP) - { - _model.state.debug[2] = micros() - startTime; - } - - return 1; - } - - void outerLoopRobot() - { - const float speedScale = 2.f; - const float gyroScale = 0.1f; - const float speed = _speedFilter.update(_model.state.output[AXIS_PITCH] * speedScale + _model.state.gyro[AXIS_PITCH] * gyroScale); - float angle = 0; - - if(true || _model.isActive(MODE_ANGLE)) - { - angle = _model.state.input[AXIS_PITCH] * radians(_model.config.angleLimit); - } - else - { - angle = _model.state.outerPid[AXIS_PITCH].update(_model.state.input[AXIS_PITCH], speed) * radians(_model.config.angleRateLimit); - } - _model.state.desiredAngle.set(AXIS_PITCH, angle); - _model.state.desiredRate[AXIS_YAW] = _model.state.input[AXIS_YAW] * radians(_model.config.angleRateLimit); - - if(_model.config.debugMode == DEBUG_ANGLERATE) - { - _model.state.debug[0] = speed * 1000; - _model.state.debug[1] = lrintf(degrees(angle) * 10); - } - } - - void innerLoopRobot() - { - //VectorFloat v(0.f, 0.f, 1.f); - //v.rotate(_model.state.angleQ); - //const float angle = acos(v.z); - const float angle = std::max(abs(_model.state.angle[AXIS_PITCH]), abs(_model.state.angle[AXIS_ROLL])); - - const bool stabilize = angle < radians(_model.config.angleLimit); - if(stabilize) - { - _model.state.output[AXIS_PITCH] = _model.state.innerPid[AXIS_PITCH].update(_model.state.desiredAngle[AXIS_PITCH], _model.state.angle[AXIS_PITCH]); - _model.state.output[AXIS_YAW] = _model.state.innerPid[AXIS_YAW].update(_model.state.desiredRate[AXIS_YAW], _model.state.gyro[AXIS_YAW]); - } - else - { - resetIterm(); - _model.state.output[AXIS_PITCH] = 0.f; - _model.state.output[AXIS_YAW] = 0.f; - } - - if(_model.config.debugMode == DEBUG_ANGLERATE) - { - _model.state.debug[2] = lrintf(degrees(_model.state.angle[AXIS_PITCH]) * 10); - _model.state.debug[3] = lrintf(_model.state.output[AXIS_PITCH] * 1000); - } - } - - void outerLoop() - { - if(_model.isActive(MODE_ANGLE)) - { - _model.state.desiredAngle = VectorFloat( - _model.state.input[AXIS_ROLL] * radians(_model.config.angleLimit), - _model.state.input[AXIS_PITCH] * radians(_model.config.angleLimit), - _model.state.angle[AXIS_YAW] - ); - _model.state.desiredRate[AXIS_ROLL] = _model.state.outerPid[AXIS_ROLL].update(_model.state.desiredAngle[AXIS_ROLL], _model.state.angle[AXIS_ROLL]); - _model.state.desiredRate[AXIS_PITCH] = _model.state.outerPid[AXIS_PITCH].update(_model.state.desiredAngle[AXIS_PITCH], _model.state.angle[AXIS_PITCH]); - // disable fterm in angle mode - _model.state.innerPid[AXIS_ROLL].fScale = 0.f; - _model.state.innerPid[AXIS_PITCH].fScale = 0.f; - } - else - { - _model.state.desiredRate[AXIS_ROLL] = calculateSetpointRate(AXIS_ROLL, _model.state.input[AXIS_ROLL]); - _model.state.desiredRate[AXIS_PITCH] = calculateSetpointRate(AXIS_PITCH, _model.state.input[AXIS_PITCH]); - } - _model.state.desiredRate[AXIS_YAW] = calculateSetpointRate(AXIS_YAW, _model.state.input[AXIS_YAW]); - _model.state.desiredRate[AXIS_THRUST] = _model.state.input[AXIS_THRUST]; - - if(_model.config.debugMode == DEBUG_ANGLERATE) - { - for(size_t i = 0; i < 3; ++i) - { - _model.state.debug[i] = lrintf(degrees(_model.state.desiredRate[i])); - } - } - } - - void innerLoop() - { - const float tpaFactor = getTpaFactor(); - for(size_t i = 0; i <= AXIS_YAW; ++i) - { - _model.state.output[i] = _model.state.innerPid[i].update(_model.state.desiredRate[i], _model.state.gyro[i]) * tpaFactor; - //_model.state.debug[i] = lrintf(_model.state.innerPid[i].fTerm * 1000); - } - _model.state.output[AXIS_THRUST] = _model.state.desiredRate[AXIS_THRUST]; - - if(_model.config.debugMode == DEBUG_ITERM_RELAX) - { - _model.state.debug[0] = lrintf(Math::toDeg(_model.state.innerPid[0].itermRelaxBase)); - _model.state.debug[1] = lrintf(_model.state.innerPid[0].itermRelaxFactor * 100.0f); - _model.state.debug[2] = lrintf(Math::toDeg(_model.state.innerPid[0].iTermError)); - _model.state.debug[3] = lrintf(_model.state.innerPid[0].iTerm * 1000.0f); - } - } - - float getTpaFactor() const - { - if(_model.config.tpaScale == 0) return 1.f; - float t = Math::clamp(_model.state.inputUs[AXIS_THRUST], (float)_model.config.tpaBreakpoint, 2000.f); - return Math::map(t, (float)_model.config.tpaBreakpoint, 2000.f, 1.f, 1.f - ((float)_model.config.tpaScale * 0.01f)); - } - - void resetIterm() - { - if(!_model.isActive(MODE_ARMED) // when not armed - || (!_model.isAirModeActive() && _model.config.lowThrottleZeroIterm && _model.isThrottleLow()) // on low throttle (not in air mode) - ) - { - for(size_t i = 0; i < AXES; i++) - { - _model.state.innerPid[i].iTerm = 0; - _model.state.outerPid[i].iTerm = 0; - } - } - } - - float calculateSetpointRate(int axis, float input) - { - if(axis == AXIS_YAW) input *= -1.f; - return _rates.getSetpoint(axis, input); - } + inline float getTpaFactor() const; + inline void resetIterm(); + float calculateSetpointRate(int axis, float input); private: Model& _model; Rates _rates; Filter _speedFilter; - }; } diff --git a/lib/Espfc/src/Debug_Espfc.h b/lib/Espfc/src/Debug_Espfc.h index 12772148..f90b3a91 100644 --- a/lib/Espfc/src/Debug_Espfc.h +++ b/lib/Espfc/src/Debug_Espfc.h @@ -2,19 +2,19 @@ #define _ESPFC_DEBUG_H_ #include -#include - -namespace Espfc -{ +#include "Hal/Gpio.h" #ifdef ESPFC_DEBUG_PIN - #define PIN_DEBUG(v) EspGpio::digitalWrite(ESPFC_DEBUG_PIN, v) - #define PIN_DEBUG_INIT() pinMode(ESPFC_DEBUG_PIN, OUTPUT) + #define PIN_DEBUG(v) ::Espfc::Hal::Gpio::digitalWrite(ESPFC_DEBUG_PIN, v) + #define PIN_DEBUG_INIT() ::pinMode(ESPFC_DEBUG_PIN, OUTPUT) #else #define PIN_DEBUG(v) #define PIN_DEBUG_INIT() #endif +namespace Espfc +{ + #ifdef ESPFC_DEBUG_SERIAL extern Stream * _debugStream; @@ -28,9 +28,9 @@ template void D(T t) { if(!_debugStream) return; - _debugStream->print(t); - _debugStream->print('\r'); - _debugStream->print('\n'); + _debugStream->println(t); + //_debugStream->print('\r'); + //_debugStream->print('\n'); } template diff --git a/lib/Espfc/src/Device/BusDevice.h b/lib/Espfc/src/Device/BusDevice.h index 79ead4ea..dd3c3e69 100644 --- a/lib/Espfc/src/Device/BusDevice.h +++ b/lib/Espfc/src/Device/BusDevice.h @@ -2,7 +2,7 @@ #define _ESPFC_DEVICE_BUSDEVICE_H_ #include -#include +#include #include "Math/Bits.h" #define ESPFC_BUS_TIMEOUT 100 diff --git a/lib/Espfc/src/Device/BusI2C.cpp b/lib/Espfc/src/Device/BusI2C.cpp new file mode 100644 index 00000000..217c027e --- /dev/null +++ b/lib/Espfc/src/Device/BusI2C.cpp @@ -0,0 +1,77 @@ +#include "Target/Target.h" + +#if defined(ESPFC_I2C_0) + +#include "BusI2C.h" + +namespace Espfc { + +namespace Device { + +BusI2C::BusI2C(WireClass& i2c): _dev(i2c) {} + +BusType BusI2C::getType() const { return BUS_I2C; } + +int BusI2C::begin(int sda, int scl, uint32_t speed) +{ + if(sda == -1 || scl == -1) return 0; + + targetI2CInit(_dev, sda, scl, speed); + + return 1; +} + +int8_t FAST_CODE_ATTR BusI2C::readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) +{ + return read(devAddr, regAddr, length, data); +} + +int8_t FAST_CODE_ATTR BusI2C::read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) +{ + int8_t count = 0; + uint32_t t1 = millis(); + + //D("i2c:r0", devAddr, regAddr, length); + + _dev.beginTransmission(devAddr); + _dev.write(regAddr); + _dev.endTransmission(); + _dev.requestFrom(devAddr, length); + + for (; _dev.available() && (_timeout == 0 || millis() - t1 < _timeout); count++) + { + data[count] = _dev.read(); + //D("i2c:r1", count, data[count]); + } + + //D("i2c:r3", length, count); + if (_timeout > 0 && millis() - t1 >= _timeout && count < length) count = -1; // timeout + + if(onError && count != length) onError(); + + return count; +} + +bool BusI2C::write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) +{ + //Serial.print("I2C W "); Serial.print(devAddr, HEX); Serial.print(' '); Serial.print(regAddr, HEX); Serial.print(' '); Serial.println(length); + + _dev.beginTransmission(devAddr); + _dev.write((uint8_t) regAddr); // send address + for (uint8_t i = 0; i < length; i++) + { + _dev.write(data[i]); + //Serial.print("I2C W "); Serial.print(i); Serial.print(' '); Serial.println(data[i], HEX); + } + uint8_t status = _dev.endTransmission(); + + if(onError && status != 0) onError(); + + return status == 0; +} + +} + +} + +#endif diff --git a/lib/Espfc/src/Device/BusI2C.h b/lib/Espfc/src/Device/BusI2C.h index 1cc372b7..2b68b452 100644 --- a/lib/Espfc/src/Device/BusI2C.h +++ b/lib/Espfc/src/Device/BusI2C.h @@ -1,5 +1,4 @@ -#ifndef _ESPFC_DEVICE_BUSI2C_H_ -#define _ESPFC_DEVICE_BUSI2C_H_ +#pragma once #include "BusDevice.h" @@ -10,67 +9,13 @@ namespace Device { class BusI2C: public BusDevice { public: - BusI2C(WireClass& i2c): _dev(i2c) {} + BusI2C(WireClass& i2c); + BusType getType() const override; - BusType getType() const override { return BUS_I2C; } - - int begin(int sda, int scl, uint32_t speed) - { - if(sda == -1 || scl == -1) return 0; - - targetI2CInit(_dev, sda, scl, speed); - - return 1; - } - - int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override - { - return read(devAddr, regAddr, length, data); - } - - int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override - { - int8_t count = 0; - uint32_t t1 = millis(); - - //D("i2c:r0", devAddr, regAddr, length); - - _dev.beginTransmission(devAddr); - _dev.write(regAddr); - _dev.endTransmission(); - _dev.requestFrom(devAddr, length); - - for (; _dev.available() && (_timeout == 0 || millis() - t1 < _timeout); count++) - { - data[count] = _dev.read(); - //D("i2c:r1", count, data[count]); - } - - //D("i2c:r3", length, count); - if (_timeout > 0 && millis() - t1 >= _timeout && count < length) count = -1; // timeout - - if(onError && count != length) onError(); - - return count; - } - - bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) override - { - //Serial.print("I2C W "); Serial.print(devAddr, HEX); Serial.print(' '); Serial.print(regAddr, HEX); Serial.print(' '); Serial.println(length); - - _dev.beginTransmission(devAddr); - _dev.write((uint8_t) regAddr); // send address - for (uint8_t i = 0; i < length; i++) - { - _dev.write(data[i]); - //Serial.print("I2C W "); Serial.print(i); Serial.print(' '); Serial.println(data[i], HEX); - } - uint8_t status = _dev.endTransmission(); - - if(onError && status != 0) onError(); - - return status == 0; - } + int begin(int sda, int scl, uint32_t speed); + int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override; + int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override; + bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) override; private: WireClass& _dev; @@ -79,5 +24,3 @@ class BusI2C: public BusDevice } } - -#endif diff --git a/lib/Espfc/src/Device/BusSPI.cpp b/lib/Espfc/src/Device/BusSPI.cpp new file mode 100644 index 00000000..d9bb5fe2 --- /dev/null +++ b/lib/Espfc/src/Device/BusSPI.cpp @@ -0,0 +1,65 @@ +#include "Target/Target.h" + +#if defined(ESPFC_SPI_0) + +#include "BusSPI.h" +#include + +namespace Espfc { + +namespace Device { + +BusSPI::BusSPI(ESPFC_SPI_0_DEV_T& spi): _dev(spi) {} + +BusType BusSPI::getType() const { return BUS_SPI; } + +int BusSPI::begin(int8_t sck, int8_t mosi, int8_t miso, int8_t ss) +{ + if(sck == -1 || miso == -1 || mosi == -1) return 0; + + targetSPIInit(_dev, sck, mosi, miso, ss); + + return 1; +} + +int8_t BusSPI::read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) +{ + //D("spi:r", regAddr, length); + transfer(devAddr, regAddr | SPI_READ, length, NULL, data, SPI_SPEED_NORMAL); + return length; +} + +int8_t FAST_CODE_ATTR BusSPI::readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) +{ + //D("spi:r", regAddr, length); + transfer(devAddr, regAddr | SPI_READ, length, NULL, data, SPI_SPEED_FAST); + return length; +} + +bool BusSPI::write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) +{ + //D("spi:w", regAddr, length, *data); + transfer(devAddr, regAddr & SPI_WRITE, length, data, NULL, SPI_SPEED_NORMAL); + return true; +} + +void FAST_CODE_ATTR BusSPI::transfer(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t *in, uint8_t *out, uint32_t speed) +{ + _dev.beginTransaction(SPISettings(speed, MSBFIRST, SPI_MODE0)); + Hal::Gpio::digitalWrite(devAddr, LOW); +#if defined (ARCH_RP2040) + _dev.transfer(regAddr); + _dev.transfer(in, out, length); +#else + _dev.transfer(regAddr); + _dev.transferBytes(in, out, length); +#endif + Hal::Gpio::digitalWrite(devAddr, HIGH); + _dev.endTransaction(); +} + +} + +} + +#endif \ No newline at end of file diff --git a/lib/Espfc/src/Device/BusSPI.h b/lib/Espfc/src/Device/BusSPI.h index 10e54ee8..1192057c 100644 --- a/lib/Espfc/src/Device/BusSPI.h +++ b/lib/Espfc/src/Device/BusSPI.h @@ -10,7 +10,7 @@ namespace Device { class BusSPI: public BusDevice { public: - BusSPI(ESPFC_SPI_0_DEV_T& spi): _dev(spi) {} + BusSPI(ESPFC_SPI_0_DEV_T& spi); static const uint8_t SPI_READ = 0x80; static const uint8_t SPI_WRITE = 0x7f; @@ -18,53 +18,19 @@ class BusSPI: public BusDevice static const uint32_t SPI_SPEED_NORMAL = 1000000; static const uint32_t SPI_SPEED_FAST = 16000000; - BusType getType() const override { return BUS_SPI; } + BusType getType() const override; - int begin(int8_t sck = -1, int8_t mosi = -1, int8_t miso = -1, int8_t ss = -1) - { - if(sck == -1 || miso == -1 || mosi == -1) return 0; + int begin(int8_t sck = -1, int8_t mosi = -1, int8_t miso = -1, int8_t ss = -1); - targetSPIInit(_dev, sck, mosi, miso, ss); + int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override; - return 1; - } + int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override; - int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override - { - //D("spi:r", regAddr, length); - transfer(devAddr, regAddr | SPI_READ, length, NULL, data, SPI_SPEED_NORMAL); - return length; - } - - int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override - { - //D("spi:r", regAddr, length); - transfer(devAddr, regAddr | SPI_READ, length, NULL, data, SPI_SPEED_FAST); - return length; - } - - bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) override - { - //D("spi:w", regAddr, length, *data); - transfer(devAddr, regAddr & SPI_WRITE, length, data, NULL, SPI_SPEED_NORMAL); - return true; - } + bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) override; private: - void transfer(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t *in, uint8_t *out, uint32_t speed) - { - _dev.beginTransaction(SPISettings(speed, MSBFIRST, SPI_MODE0)); - EspGpio::digitalWrite(devAddr, LOW); -#if defined (ARCH_RP2040) - _dev.transfer(regAddr); - _dev.transfer(in, out, length); -#else - _dev.transfer(regAddr); - _dev.transferBytes(in, out, length); -#endif - EspGpio::digitalWrite(devAddr, HIGH); - _dev.endTransaction(); - } + void transfer(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t *in, uint8_t *out, uint32_t speed); + ESPFC_SPI_0_DEV_T& _dev; }; diff --git a/lib/Espfc/src/Device/BusSlave.cpp b/lib/Espfc/src/Device/BusSlave.cpp new file mode 100644 index 00000000..f65c3054 --- /dev/null +++ b/lib/Espfc/src/Device/BusSlave.cpp @@ -0,0 +1,93 @@ +#include +#include "BusSlave.h" + +#define MPU6050_I2C_SLV0_ADDR 0x25 +#define MPU6050_I2C_SLV0_REG 0x26 +#define MPU6050_I2C_SLV0_DO 0x63 +#define MPU6050_I2C_SLV0_CTRL 0x27 +#define MPU6050_I2C_SLV0_EN 0x80 +#define MPU6050_EXT_SENS_DATA_00 0x49 + +namespace Espfc { + +namespace Device { + +BusSlave::BusSlave() {} + +int BusSlave::begin(BusDevice * dev, int addr) +{ + setBus(dev, addr); + + return 1; +} + +BusType BusSlave::getType() const { return BUS_SLV; } + +int8_t BusSlave::read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) +{ + // set slave 0 to the AK8963 and set for read + if(!writeMaster(MPU6050_I2C_SLV0_ADDR, devAddr | 0x80)) { + return 0; + } + // set the register to the desired AK8963 sub address + if(!writeMaster(MPU6050_I2C_SLV0_REG, regAddr)) { + return 0; + } + // enable I2C and request the bytes + if(!writeMaster(MPU6050_I2C_SLV0_CTRL, MPU6050_I2C_SLV0_EN | length)) { + return 0; + } + + // takes some time for these registers to fill + delay(1); + + // read the bytes off the EXT_SENS_DATA registers + int8_t res = readMaster(MPU6050_EXT_SENS_DATA_00, length, data); + + return res; +} + +// readFast() ignores devAddr and regAddr args and read ext sensor data reg from master +int8_t IRAM_ATTR BusSlave::readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) +{ + return _bus->readFast(_addr, MPU6050_EXT_SENS_DATA_00, length, data); +} + +// writes only one byte, length is ignored +bool BusSlave::write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) +{ + // set slave 0 to the AK8963 and set for write + if(!writeMaster(MPU6050_I2C_SLV0_ADDR, devAddr)) { + return false; + } + // set the register to the desired AK8963 sub address + if(!writeMaster(MPU6050_I2C_SLV0_REG, regAddr)) { + return false; + } + // store the data for write + if(!writeMaster(MPU6050_I2C_SLV0_DO, *data)) { + return false; + } + // enable I2C and send 1 byte + if(!writeMaster(MPU6050_I2C_SLV0_CTRL, MPU6050_I2C_SLV0_EN | 0x01)) { + return false; + } + + return true; +} + +int8_t BusSlave::writeMaster(uint8_t regAddr, uint8_t data) +{ + int8_t res = _bus->write(_addr, regAddr, 1, &data); + delay(10); + return res; +} + +int8_t BusSlave::readMaster(uint8_t regAddr, uint8_t length, uint8_t *data) +{ + return _bus->read(_addr, regAddr, length, data); +} + +} + +} diff --git a/lib/Espfc/src/Device/BusSlave.h b/lib/Espfc/src/Device/BusSlave.h index 91c65842..14887f58 100644 --- a/lib/Espfc/src/Device/BusSlave.h +++ b/lib/Espfc/src/Device/BusSlave.h @@ -1,11 +1,8 @@ #pragma once -#define MPU6050_I2C_SLV0_ADDR 0x25 -#define MPU6050_I2C_SLV0_REG 0x26 -#define MPU6050_I2C_SLV0_DO 0x63 -#define MPU6050_I2C_SLV0_CTRL 0x27 -#define MPU6050_I2C_SLV0_EN 0x80 -#define MPU6050_EXT_SENS_DATA_00 0x49 +#include +#include "BusDevice.h" +#include "BusAwareDevice.h" namespace Espfc { @@ -14,83 +11,25 @@ namespace Device { class BusSlave: public BusDevice, public BusAwareDevice { public: - BusSlave() {} + BusSlave(); - int begin(BusDevice * dev, int addr) - { - setBus(dev, addr); + int begin(BusDevice * dev, int addr); - return 1; - } + BusType getType() const override; - BusType getType() const override { return BUS_SLV; } - - virtual int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override - { - // set slave 0 to the AK8963 and set for read - if(!writeMaster(MPU6050_I2C_SLV0_ADDR, devAddr | 0x80)) { - return 0; - } - // set the register to the desired AK8963 sub address - if(!writeMaster(MPU6050_I2C_SLV0_REG, regAddr)) { - return 0; - } - // enable I2C and request the bytes - if(!writeMaster(MPU6050_I2C_SLV0_CTRL, MPU6050_I2C_SLV0_EN | length)) { - return 0; - } - - // takes some time for these registers to fill - delay(1); - - // read the bytes off the EXT_SENS_DATA registers - int8_t res = readMaster(MPU6050_EXT_SENS_DATA_00, length, data); - - return res; - } + virtual int8_t read(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override; // readFast() ignores devAddr and regAddr args and read ext sensor data reg from master - virtual int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override - { - return _bus->readFast(_addr, MPU6050_EXT_SENS_DATA_00, length, data); - } + virtual int8_t readFast(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) override; // writes only one byte, length is ignored - virtual bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) override - { - // set slave 0 to the AK8963 and set for write - if(!writeMaster(MPU6050_I2C_SLV0_ADDR, devAddr)) { - return false; - } - // set the register to the desired AK8963 sub address - if(!writeMaster(MPU6050_I2C_SLV0_REG, regAddr)) { - return false; - } - // store the data for write - if(!writeMaster(MPU6050_I2C_SLV0_DO, *data)) { - return false; - } - // enable I2C and send 1 byte - if(!writeMaster(MPU6050_I2C_SLV0_CTRL, MPU6050_I2C_SLV0_EN | 0x01)) { - return false; - } - - return true; - } + virtual bool write(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t* data) override; - int8_t writeMaster(uint8_t regAddr, uint8_t data) - { - int8_t res = _bus->write(_addr, regAddr, 1, &data); - delay(10); - return res; - } + int8_t writeMaster(uint8_t regAddr, uint8_t data); - int8_t readMaster(uint8_t regAddr, uint8_t length, uint8_t *data) - { - return _bus->read(_addr, regAddr, length, data); - } + int8_t readMaster(uint8_t regAddr, uint8_t length, uint8_t *data); }; } -} \ No newline at end of file +} diff --git a/lib/Espfc/src/Device/FlashDevice.h b/lib/Espfc/src/Device/FlashDevice.h new file mode 100644 index 00000000..1df5d874 --- /dev/null +++ b/lib/Espfc/src/Device/FlashDevice.h @@ -0,0 +1,76 @@ +#pragma once + +#include +#include +#include +#include "esp_partition.h" + +namespace Espfc { + +namespace Device { + +class FlashPartition +{ +public: + FlashPartition(esp_partition_t* p): _p(p) {} + + size_t read(uint32_t address, uint8_t * data, size_t len) + { + len = std::min((uint32_t)len, _p->size - address); + if(esp_partition_read_raw(_p, address, data, len) == ESP_OK) + { + return len; + } + return 0; + } + + size_t write(uint32_t address, const uint8_t * data, size_t len) + { + esp_partition_write_raw(_p, address, data, len); + return len; + } + +private: + esp_partition_t* _p; +}; + +class FlashDevice +{ +public: + static void partitions(Stream& s) + { + s.printf("ESP32 Partition table:\r\n"); + s.printf("| Type | Sub | Offset | Size | Label |\r\n"); + s.printf("| ---- | --- | -------- | -------- | ---------------- |\r\n"); + + esp_partition_iterator_t pi = esp_partition_find(ESP_PARTITION_TYPE_ANY, ESP_PARTITION_SUBTYPE_ANY, nullptr); + if (pi != NULL) { + do { + const esp_partition_t* p = esp_partition_get(pi); + s.printf("| %02x | %02x | 0x%06X | 0x%06X | %-16s |\r\n", + p->type, p->subtype, p->address, p->size, p->label); + } while ((pi = esp_partition_next(pi))); + } + } + + static const esp_partition_t* findPartition() + { + return esp_partition_find_first(ESP_PARTITION_TYPE_DATA, ESP_PARTITION_SUBTYPE_DATA_SPIFFS, nullptr); + } + + static void journal(Stream& s) + { + FlashfsJournalItem journal[8]; + flashfsJournalLoad(journal, 0, 8); + for(size_t i = 0; i < 8; i++) + { + const auto& it = journal[i]; + s.printf("%08x => %08x\r\n", it.logBegin, it.logEnd); + } + } + +}; + +} + +} \ No newline at end of file diff --git a/lib/Espfc/src/Device/GyroBMI160.h b/lib/Espfc/src/Device/GyroBMI160.h index 200bec07..90145c06 100644 --- a/lib/Espfc/src/Device/GyroBMI160.h +++ b/lib/Espfc/src/Device/GyroBMI160.h @@ -190,7 +190,7 @@ class GyroBMI160: public GyroDevice return GYRO_BMI160; } - int readGyro(VectorInt16& v) override + int FAST_CODE_ATTR readGyro(VectorInt16& v) override { uint8_t buffer[6]; diff --git a/lib/Espfc/src/Device/GyroDevice.h b/lib/Espfc/src/Device/GyroDevice.h index 7967cd07..1abf53e9 100644 --- a/lib/Espfc/src/Device/GyroDevice.h +++ b/lib/Espfc/src/Device/GyroDevice.h @@ -1,7 +1,7 @@ #ifndef _ESPFC_DEVICE_GYRO_DEVICE_H_ #define _ESPFC_DEVICE_GYRO_DEVICE_H_ -#include "helper_3dmath.h" +#include #include "BusDevice.h" #include "BusAwareDevice.h" diff --git a/lib/Espfc/src/Device/GyroLSM6DSO.h b/lib/Espfc/src/Device/GyroLSM6DSO.h index 06ac9e9e..6adb79d8 100644 --- a/lib/Espfc/src/Device/GyroLSM6DSO.h +++ b/lib/Espfc/src/Device/GyroLSM6DSO.h @@ -111,7 +111,7 @@ class GyroLSM6DSO: public GyroDevice return GYRO_LSM6DSO; } - int readGyro(VectorInt16& v) override + int FAST_CODE_ATTR readGyro(VectorInt16& v) override { int16_t buffer[3]; diff --git a/lib/Espfc/src/Device/GyroMPU6050.h b/lib/Espfc/src/Device/GyroMPU6050.h index 92e3b9ba..d00c87c6 100644 --- a/lib/Espfc/src/Device/GyroMPU6050.h +++ b/lib/Espfc/src/Device/GyroMPU6050.h @@ -189,7 +189,7 @@ class GyroMPU6050: public GyroDevice return GYRO_MPU6050; } - int readGyro(VectorInt16& v) override + int FAST_CODE_ATTR readGyro(VectorInt16& v) override { uint8_t buffer[6]; diff --git a/lib/Espfc/src/Device/InputCRSF.cpp b/lib/Espfc/src/Device/InputCRSF.cpp new file mode 100644 index 00000000..809c70d7 --- /dev/null +++ b/lib/Espfc/src/Device/InputCRSF.cpp @@ -0,0 +1,158 @@ + +#include "InputCRSF.h" +#include "Utils/MemoryHelper.h" + +namespace Espfc { + +namespace Device { + +using namespace Espfc::Rc; + +InputCRSF::InputCRSF(): _serial(NULL), _state(CRSF_ADDR), _idx(0), _new_data(false) {} + +int InputCRSF::begin(Device::SerialDevice * serial) +{ + _serial = serial; + for(size_t i = 0; i < CRSF_FRAME_SIZE_MAX; i++) + { + _frame.data[i] = 0; + if(i < CHANNELS) _channels[i] = 0; + } + return 1; +} + +InputStatus FAST_CODE_ATTR InputCRSF::update() +{ + if(!_serial) return INPUT_IDLE; + + size_t len = _serial->available(); + if(len) + { + uint8_t buff[64] = {0}; + len = std::min(len, sizeof(buff)); + _serial->readMany(buff, len); + size_t i = 0; + while(i < len) + { + parse(_frame, buff[i++]); + } + } + + if(_new_data) + { + _new_data = false; + return INPUT_RECEIVED; + } + + return INPUT_IDLE; +} + +uint16_t FAST_CODE_ATTR InputCRSF::get(uint8_t i) const +{ + return _channels[i]; +} + +void FAST_CODE_ATTR InputCRSF::get(uint16_t * data, size_t len) const +{ + const uint16_t * src = _channels; + while(len--) + { + *data++ = *src++; + } +} + +size_t InputCRSF::getChannelCount() const { return CHANNELS; } + +bool InputCRSF::needAverage() const { return false; } + + +void FAST_CODE_ATTR InputCRSF::parse(CrsfFrame& frame, int d) +{ + uint8_t c = (uint8_t)(d & 0xff); + switch(_state) + { + case CRSF_ADDR: + if(c == CRSF_ADDRESS_FLIGHT_CONTROLLER) + { + frame.data[_idx++] = c; + _state = CRSF_SIZE; + } + break; + case CRSF_SIZE: + if(c > 3 && c <= CRSF_PAYLOAD_SIZE_MAX) + { + frame.data[_idx++] = c; + _state = CRSF_TYPE; + } else { + reset(); + } + break; + case CRSF_TYPE: + if(c == CRSF_FRAMETYPE_RC_CHANNELS_PACKED || c == CRSF_FRAMETYPE_LINK_STATISTICS) + { + frame.data[_idx++] = c; + _state = CRSF_DATA; + } else { + reset(); + } + break; + case CRSF_DATA: + frame.data[_idx++] = c; + if(_idx > frame.message.size) // _idx is incremented here and operator > accounts as size - 2 + { + _state = CRSF_CRC; + } + break; + case CRSF_CRC: + frame.data[_idx++] = c; + reset(); + uint8_t crc = Crsf::crc(frame); + if(c == crc) { + apply(frame); + } + break; + } +} + +void FAST_CODE_ATTR InputCRSF::reset() +{ + _state = CRSF_ADDR; + _idx = 0; +} + +void FAST_CODE_ATTR InputCRSF::apply(const CrsfFrame& frame) +{ + switch (frame.message.type) + { + case CRSF_FRAMETYPE_RC_CHANNELS_PACKED: + applyChannels(frame); + break; + + case CRSF_FRAMETYPE_LINK_STATISTICS: + applyLinkStats(frame); + break; + + default: + break; + } +} + +void FAST_CODE_ATTR InputCRSF::applyLinkStats(const CrsfFrame f) +{ + const CrsfLinkStats* frame = reinterpret_cast(f.message.payload); + (void)frame; + // TODO: +} + +void FAST_CODE_ATTR InputCRSF::applyChannels(const CrsfFrame f) +{ + const CrsfData* frame = reinterpret_cast(f.message.payload); + Crsf::decodeRcDataShift8(_channels, frame); + //Crsf::decodeRcData(_channels, frame); + _new_data = true; +} + +} + +} + diff --git a/lib/Espfc/src/Device/InputCRSF.h b/lib/Espfc/src/Device/InputCRSF.h index c077b2f7..b9fb0eb3 100644 --- a/lib/Espfc/src/Device/InputCRSF.h +++ b/lib/Espfc/src/Device/InputCRSF.h @@ -1,5 +1,4 @@ -#ifndef _INPUT_DEVICE_INPUT_CRSF_H_ -#define _INPUT_DEVICE_INPUT_CRSF_H_ +#pragma once #include "Device/SerialDevice.h" #include "Device/InputDevice.h" @@ -13,8 +12,6 @@ namespace Espfc { namespace Device { -using namespace Espfc::Rc; - class InputCRSF: public InputDevice { public: @@ -26,154 +23,23 @@ class InputCRSF: public InputDevice CRSF_CRC }; - InputCRSF(): _serial(NULL), _state(CRSF_ADDR), _idx(0), _new_data(false) {} - - int begin(Device::SerialDevice * serial) - { - _serial = serial; - for(size_t i = 0; i < CRSF_FRAME_SIZE_MAX; i++) - { - _frame.data[i] = 0; - if(i < CHANNELS) _channels[i] = 0; - } - return 1; - } - - InputStatus update() override - { - if(!_serial) return INPUT_IDLE; - - size_t len = _serial->available(); - if(len) - { - uint8_t buff[64] = {0}; - len = std::min(len, sizeof(buff)); - _serial->readMany(buff, len); - size_t i = 0; - while(i < len) - { - parse(_frame, buff[i++]); - } - } - - if(_new_data) - { - _new_data = false; - return INPUT_RECEIVED; - } - - return INPUT_IDLE; - } - - uint16_t get(uint8_t i) const override - { - return _channels[i]; - } - - void get(uint16_t * data, size_t len) const override - { - const uint16_t * src = _channels; - while(len--) - { - *data++ = *src++; - } - } + InputCRSF(); - size_t getChannelCount() const override { return CHANNELS; } + int begin(Device::SerialDevice * serial); + virtual InputStatus update() override; + virtual uint16_t get(uint8_t i) const override; + virtual void get(uint16_t * data, size_t len) const override; + virtual size_t getChannelCount() const override; + virtual bool needAverage() const override; - bool needAverage() const override { return false; } - - void print(char c) const - { - //Serial.write(c); - } - - void parse(CrsfFrame& frame, int d) - { - uint8_t c = (uint8_t)(d & 0xff); - //print(c); - switch(_state) - { - case CRSF_ADDR: - if(c == CRSF_ADDRESS_FLIGHT_CONTROLLER) - { - frame.data[_idx++] = c; - _state = CRSF_SIZE; - } - break; - case CRSF_SIZE: - if(c > 3 && c <= CRSF_PAYLOAD_SIZE_MAX) - { - frame.data[_idx++] = c; - _state = CRSF_TYPE; - } else { - reset(); - } - break; - case CRSF_TYPE: - if(c == CRSF_FRAMETYPE_RC_CHANNELS_PACKED || c == CRSF_FRAMETYPE_LINK_STATISTICS) - { - frame.data[_idx++] = c; - _state = CRSF_DATA; - } else { - reset(); - } - break; - case CRSF_DATA: - frame.data[_idx++] = c; - if(_idx > frame.message.size) // _idx is incremented here and operator > accounts as size - 2 - { - _state = CRSF_CRC; - } - break; - case CRSF_CRC: - frame.data[_idx++] = c; - reset(); - uint8_t crc = Crsf::crc(frame); - if(c == crc) { - apply(frame); - } - break; - } - } + void print(char c) const; + void parse(Rc::CrsfFrame& frame, int d); private: - void reset() - { - _state = CRSF_ADDR; - _idx = 0; - } - - void apply(const CrsfFrame& frame) - { - switch (frame.message.type) - { - case CRSF_FRAMETYPE_RC_CHANNELS_PACKED: - applyChannels(frame); - break; - - case CRSF_FRAMETYPE_LINK_STATISTICS: - applyLinkStats(frame); - break; - - default: - break; - } - } - - void applyLinkStats(const CrsfFrame f) - { - const CrsfLinkStats* frame = reinterpret_cast(f.message.payload); - (void)frame; - // TODO: - } - - void applyChannels(const CrsfFrame f) - { - const CrsfData* frame = reinterpret_cast(f.message.payload); - Crsf::decodeRcDataShift8(_channels, frame); - _new_data = true; - } + void reset(); + void apply(const Rc::CrsfFrame& frame); + void applyLinkStats(const Rc::CrsfFrame f); + void applyChannels(const Rc::CrsfFrame f); static const size_t CHANNELS = 16; @@ -181,12 +47,10 @@ class InputCRSF: public InputDevice CrsfState _state; uint8_t _idx; bool _new_data; - CrsfFrame _frame; + Rc::CrsfFrame _frame; uint16_t _channels[CHANNELS]; }; } } - -#endif diff --git a/lib/Espfc/src/Device/InputEspNow.cpp b/lib/Espfc/src/Device/InputEspNow.cpp new file mode 100644 index 00000000..3ad2218c --- /dev/null +++ b/lib/Espfc/src/Device/InputEspNow.cpp @@ -0,0 +1,55 @@ +#if defined(ESP32) || defined(ESP8266) + +#include "InputEspNow.h" +#include "Utils/MemoryHelper.h" + +namespace Espfc { + +namespace Device { + +int InputEspNow::begin(void) +{ + for(size_t i = 0; i < CHANNELS; i++) + { + _channels[i] = i == 2 ? 1000 : 1500; + } + return _rx.begin(); +} + +InputStatus FAST_CODE_ATTR InputEspNow::update() +{ + _rx.update(); + if(_rx.available()) + { + for(size_t i = 0; i < CHANNELS; i++) + { + _channels[i] = _rx.getChannel(i); + } + return INPUT_RECEIVED; + } + return INPUT_IDLE; +} + +uint16_t FAST_CODE_ATTR InputEspNow::get(uint8_t i) const +{ + return _channels[i]; +} + +void FAST_CODE_ATTR InputEspNow::get(uint16_t * data, size_t len) const +{ + const uint16_t * src = _channels; + while(len--) + { + *data++ = *src++; + } +} + +size_t InputEspNow::getChannelCount() const { return CHANNELS; } + +bool InputEspNow::needAverage() const { return false; } + +} + +} + +#endif diff --git a/lib/Espfc/src/Device/InputEspNow.h b/lib/Espfc/src/Device/InputEspNow.h new file mode 100644 index 00000000..1b5c0836 --- /dev/null +++ b/lib/Espfc/src/Device/InputEspNow.h @@ -0,0 +1,34 @@ +#pragma once + +#if defined(ESP32) || defined(ESP8266) + +#include "Device/InputDevice.h" +#include +#include +#include + +namespace Espfc { + +namespace Device { + +class InputEspNow: public InputDevice +{ +public: + int begin(void); + InputStatus update() override; + uint16_t get(uint8_t i) const override; + void get(uint16_t * data, size_t len) const override; + size_t getChannelCount() const override; + bool needAverage() const override; + +private: + EspNowRcLink::Receiver _rx; + static const size_t CHANNELS = EspNowRcLink::RC_CHANNEL_MAX + 1; + uint16_t _channels[CHANNELS]; +}; + +} + +} + +#endif \ No newline at end of file diff --git a/lib/Espfc/src/Device/InputPPM.cpp b/lib/Espfc/src/Device/InputPPM.cpp index 93f691e7..8e782a9e 100644 --- a/lib/Espfc/src/Device/InputPPM.cpp +++ b/lib/Espfc/src/Device/InputPPM.cpp @@ -1,39 +1,67 @@ #include "Device/InputPPM.h" #include +#include "Utils/MemoryHelper.h" namespace Espfc { namespace Device { -InputPPM * InputPPM::_instance = NULL; - void InputPPM::begin(uint8_t pin, int mode) { - if(_instance && _pin != -1) + if(_pin != -1) { detachInterrupt(_pin); - _instance = NULL; + _pin = -1; } if(pin != -1) { - _instance = this; _pin = pin; _channel = 0; _last_tick = micros(); for(size_t i = 0; i < CHANNELS; i++) { - _channels[i] = (i == 0 || i == 1 || i == 3) ? 1500 : 1000; // ail, elev, rud + _channels[i] = (i == 2) ? 1000 : 1500; // throttle } pinMode(_pin, INPUT); -#if defined(ARCH_RP2040) - attachInterrupt(_pin, InputPPM::handle_isr, (PinStatus)mode); +#if defined(UNIT_TEST) + // no mock available +#elif defined(ARCH_RP2040) + attachInterruptParam(_pin, InputPPM::handle_isr, (PinStatus)mode, this); #else - attachInterrupt(_pin, InputPPM::handle_isr, mode); + attachInterruptArg(_pin, InputPPM::handle_isr, this, mode); #endif } } -void InputPPM::handle() +InputStatus FAST_CODE_ATTR InputPPM::update() +{ + if(_new_data) + { + _new_data = false; + return INPUT_RECEIVED; + } + return INPUT_IDLE; +} + +uint16_t FAST_CODE_ATTR InputPPM::get(uint8_t i) const +{ + return _channels[i]; +} + +void FAST_CODE_ATTR InputPPM::get(uint16_t * data, size_t len) const +{ + const uint16_t * src = const_cast(_channels); + while(len--) + { + *data++ = *src++; + } +} + +size_t InputPPM::getChannelCount() const { return CHANNELS; } + +bool InputPPM::needAverage() const { return true; } + +void IRAM_ATTR InputPPM::handle() { uint32_t now = micros(); uint32_t width = now - _last_tick; @@ -57,9 +85,9 @@ void InputPPM::handle() _channel++; } -void InputPPM::handle_isr() +void IRAM_ATTR InputPPM::handle_isr(void* args) { - if(_instance) _instance->handle(); + if(args) reinterpret_cast(args)->handle(); } } diff --git a/lib/Espfc/src/Device/InputPPM.h b/lib/Espfc/src/Device/InputPPM.h index e2872f00..4ef4cb76 100644 --- a/lib/Espfc/src/Device/InputPPM.h +++ b/lib/Espfc/src/Device/InputPPM.h @@ -1,9 +1,7 @@ -#ifndef _INPUT_DEVICE_INPUT_PPM_H_ -#define _INPUT_DEVICE_INPUT_PPM_H_ +#pragma once -#ifndef UNIT_TEST -#include -#endif +#include +#include #include "InputDevice.h" namespace Espfc { @@ -19,52 +17,26 @@ class InputPPM: public InputDevice { public: void begin(uint8_t pin, int mode = PPM_MODE_NORMAL); - void handle() IRAM_ATTR; - static void handle_isr() IRAM_ATTR; - - uint16_t get(uint8_t i) const override - { - return _channels[i]; - } - - void get(uint16_t * data, size_t len) const override - { - const uint16_t * src = const_cast(_channels); - while(len--) - { - *data++ = *src++; - } - } - - InputStatus update() override - { - if(_new_data) - { - _new_data = false; - return INPUT_RECEIVED; - } - //if(fail()) return INPUT_FAILED; - return INPUT_IDLE; - } - - size_t getChannelCount() const override { return CHANNELS; } - - bool needAverage() const override { return true; } + InputStatus update() override; + uint16_t get(uint8_t i) const override; + void get(uint16_t * data, size_t len) const override; + size_t getChannelCount() const override; + bool needAverage() const override; + private: static const size_t CHANNELS = 16; static const uint32_t BROKEN_LINK_US = 100000UL; // 100ms - private: + void handle(); + static void handle_isr(void* args); + volatile uint16_t _channels[CHANNELS]; volatile uint32_t _last_tick; volatile uint8_t _channel; volatile bool _new_data; uint8_t _pin; - static InputPPM * _instance; }; } } - -#endif diff --git a/lib/Espfc/src/Device/InputSBUS.cpp b/lib/Espfc/src/Device/InputSBUS.cpp new file mode 100644 index 00000000..af87b5c7 --- /dev/null +++ b/lib/Espfc/src/Device/InputSBUS.cpp @@ -0,0 +1,148 @@ +#include "InputSBUS.h" +#include "Math/Utils.h" +#include "Utils/MemoryHelper.h" + +namespace Espfc { + +namespace Device { + +InputSBUS::InputSBUS(): _serial(NULL), _state(SBUS_START), _idx(0), _new_data(false) {} + +int InputSBUS::begin(Device::SerialDevice * serial) +{ + _serial = serial; + for(size_t i = 0; i < SBUS_FRAME_SIZE; i++) + { + _data[i] = 0; + if(i < CHANNELS) _channels[i] = 0; + } + return 1; +} + +InputStatus FAST_CODE_ATTR InputSBUS::update() +{ + if(!_serial) return INPUT_IDLE; + + size_t len = _serial->available(); + if(len) + { + uint8_t buff[64] = {0}; + len = std::min(len, sizeof(buff)); + _serial->readMany(buff, len); + size_t i = 0; + while(i < len) + { + parse(buff[i++]); + } + } + + if(_new_data) + { + _new_data = false; + if(_flags & SBUS_FLAG_FAILSAFE_ACTIVE) return INPUT_FAILSAFE; + if(_flags & SBUS_FLAG_SIGNAL_LOSS) return INPUT_LOST; + return INPUT_RECEIVED; + } + + return INPUT_IDLE; +} + +uint16_t FAST_CODE_ATTR InputSBUS::get(uint8_t i) const +{ + return _channels[i]; +} + +void FAST_CODE_ATTR InputSBUS::get(uint16_t * data, size_t len) const +{ + const uint16_t * src = _channels; + while(len--) + { + *data++ = *src++; + } +} + +size_t InputSBUS::getChannelCount() const { return CHANNELS; } + +bool InputSBUS::needAverage() const { return false; } + +void FAST_CODE_ATTR InputSBUS::parse(int d) +{ + char c = (char)(d & 0xff); + switch(_state) + { + case SBUS_START: + if(c == 0x0F) + { + _data[_idx++] = c; + _state = SBUS_DATA; + } + break; + case SBUS_DATA: + _data[_idx] = c; + if(++_idx >= SBUS_FRAME_SIZE - 1) + { + _state = SBUS_END; + } + break; + case SBUS_END: + _data[_idx++] = c; + _state = SBUS_START; + _idx = 0; + apply(); + break; + } +} + +void FAST_CODE_ATTR InputSBUS::apply() +{ + /* + const SbusData* frame = reinterpret_cast(_data); + _channels[0] = convert(frame->chan0); + _channels[1] = convert(frame->chan1); + _channels[2] = convert(frame->chan2); + _channels[3] = convert(frame->chan3); + _channels[4] = convert(frame->chan4); + _channels[5] = convert(frame->chan5); + _channels[6] = convert(frame->chan6); + _channels[7] = convert(frame->chan7); + _channels[8] = convert(frame->chan8); + _channels[9] = convert(frame->chan9); + _channels[10] = convert(frame->chan10); + _channels[11] = convert(frame->chan11); + _channels[12] = convert(frame->chan12); + _channels[13] = convert(frame->chan13); + _channels[14] = convert(frame->chan14); + _channels[15] = convert(frame->chan15); + _flags = frame->flags; + */ + + // shifting is more efficient + _channels[0] = convert((_data[1] | _data[2] << 8) & 0x07FF); + _channels[1] = convert((_data[2] >> 3 | _data[3] << 5) & 0x07FF); + _channels[2] = convert((_data[3] >> 6 | _data[4] << 2 | _data[5] << 10) & 0x07FF); + _channels[3] = convert((_data[5] >> 1 | _data[6] << 7) & 0x07FF); + _channels[4] = convert((_data[6] >> 4 | _data[7] << 4) & 0x07FF); + _channels[5] = convert((_data[7] >> 7 | _data[8] << 1 | _data[9] << 9) & 0x07FF); + _channels[6] = convert((_data[9] >> 2 | _data[10] << 6) & 0x07FF); + _channels[7] = convert((_data[10] >> 5 | _data[11] << 3) & 0x07FF); + _channels[8] = convert((_data[12] | _data[13] << 8) & 0x07FF); + _channels[9] = convert((_data[13] >> 3 | _data[14] << 5) & 0x07FF); + _channels[10] = convert((_data[14] >> 6 | _data[15] << 2 | _data[16] << 10) & 0x07FF); + _channels[11] = convert((_data[16] >> 1 | _data[17] << 7) & 0x07FF); + _channels[12] = convert((_data[17] >> 4 | _data[18] << 4) & 0x07FF); + _channels[13] = convert((_data[18] >> 7 | _data[19] << 1 | _data[20] << 9) & 0x07FF); + _channels[14] = convert((_data[20] >> 2 | _data[21] << 6) & 0x07FF); + _channels[15] = convert((_data[21] >> 5 | _data[22] << 3) & 0x07FF); + _flags = _data[23]; + + _new_data = true; +} + +uint16_t FAST_CODE_ATTR InputSBUS::convert(int v) +{ + return Math::clamp(((v * 5) / 8) + 880, 800, 2200); +} + +} + +} diff --git a/lib/Espfc/src/Device/InputSBUS.h b/lib/Espfc/src/Device/InputSBUS.h index 4e7253ff..dbf54c7e 100644 --- a/lib/Espfc/src/Device/InputSBUS.h +++ b/lib/Espfc/src/Device/InputSBUS.h @@ -1,9 +1,9 @@ -#ifndef _INPUT_DEVICE_INPUT_SBUS_H_ -#define _INPUT_DEVICE_INPUT_SBUS_H_ +#pragma once #include "Device/SerialDevice.h" #include "Device/InputDevice.h" -#include "Math/Utils.h" +#include +#include namespace Espfc { @@ -50,170 +50,28 @@ class InputSBUS: public InputDevice SBUS_END }; - InputSBUS(): _serial(NULL), _state(SBUS_START), _idx(0), _new_data(false) {} + InputSBUS(); - int begin(Device::SerialDevice * serial) - { - _serial = serial; - for(size_t i = 0; i < SBUS_FRAME_SIZE; i++) - { - _data[i] = 0; - if(i < CHANNELS) _channels[i] = 0; - } - return 1; - } - - InputStatus update() override - { - if(!_serial) return INPUT_IDLE; - - size_t len = _serial->available(); - if(len) - { - uint8_t buff[64] = {0}; - len = std::min(len, sizeof(buff)); - _serial->readMany(buff, len); - size_t i = 0; - while(i < len) - { - parse(buff[i++]); - } - } - - if(_new_data) - { - _new_data = false; - if(_flags & SBUS_FLAG_FAILSAFE_ACTIVE) return INPUT_FAILSAFE; - if(_flags & SBUS_FLAG_SIGNAL_LOSS) return INPUT_LOST; - return INPUT_RECEIVED; - } - - return INPUT_IDLE; - } - - uint16_t get(uint8_t i) const override - { - return _channels[i]; - } - - void get(uint16_t * data, size_t len) const override - { - const uint16_t * src = _channels; - while(len--) - { - *data++ = *src++; - } - } - - size_t getChannelCount() const override { return CHANNELS; } - - bool needAverage() const override { return false; } - - void print(char c) const - { - //Serial.write(c); - } + int begin(Device::SerialDevice * serial); + InputStatus update() override; + uint16_t get(uint8_t i) const override; + void get(uint16_t * data, size_t len) const override; + size_t getChannelCount() const override; + bool needAverage() const override; private: - void parse(int d) - { - char c = (char)(d & 0xff); - //print(c); - //print(checkParityEven(d)); - switch(_state) - { - case SBUS_START: - if(c == 0x0F) - { - _data[_idx++] = c; - _state = SBUS_DATA; - } - break; - case SBUS_DATA: - _data[_idx] = c; - if(++_idx >= SBUS_FRAME_SIZE - 1) - { - _state = SBUS_END; - } - break; - case SBUS_END: - _data[_idx++] = c; - _state = SBUS_START; - _idx = 0; - apply(); - break; - } - } - - bool checkParityEven(int d) const - { - bool parity = (d >> 8) & 0x1; - for(size_t i = 0; i < 8; i++) - { - if(d & (1 << i)) parity = !parity; - } - return !parity; - } - - void apply() - { - /* - const SbusData* frame = reinterpret_cast(_data); - _channels[0] = convert(frame->chan0); - _channels[1] = convert(frame->chan1); - _channels[2] = convert(frame->chan2); - _channels[3] = convert(frame->chan3); - _channels[4] = convert(frame->chan4); - _channels[5] = convert(frame->chan5); - _channels[6] = convert(frame->chan6); - _channels[7] = convert(frame->chan7); - _channels[8] = convert(frame->chan8); - _channels[9] = convert(frame->chan9); - _channels[10] = convert(frame->chan10); - _channels[11] = convert(frame->chan11); - _channels[12] = convert(frame->chan12); - _channels[13] = convert(frame->chan13); - _channels[14] = convert(frame->chan14); - _channels[15] = convert(frame->chan15); - _flags = frame->flags; - */ - - // shifting is more efficient - _channels[0] = convert((_data[1] | _data[2] << 8) & 0x07FF); - _channels[1] = convert((_data[2] >> 3 | _data[3] << 5) & 0x07FF); - _channels[2] = convert((_data[3] >> 6 | _data[4] << 2 | _data[5] << 10) & 0x07FF); - _channels[3] = convert((_data[5] >> 1 | _data[6] << 7) & 0x07FF); - _channels[4] = convert((_data[6] >> 4 | _data[7] << 4) & 0x07FF); - _channels[5] = convert((_data[7] >> 7 | _data[8] << 1 | _data[9] << 9) & 0x07FF); - _channels[6] = convert((_data[9] >> 2 | _data[10] << 6) & 0x07FF); - _channels[7] = convert((_data[10] >> 5 | _data[11] << 3) & 0x07FF); - _channels[8] = convert((_data[12] | _data[13] << 8) & 0x07FF); - _channels[9] = convert((_data[13] >> 3 | _data[14] << 5) & 0x07FF); - _channels[10] = convert((_data[14] >> 6 | _data[15] << 2 | _data[16] << 10) & 0x07FF); - _channels[11] = convert((_data[16] >> 1 | _data[17] << 7) & 0x07FF); - _channels[12] = convert((_data[17] >> 4 | _data[18] << 4) & 0x07FF); - _channels[13] = convert((_data[18] >> 7 | _data[19] << 1 | _data[20] << 9) & 0x07FF); - _channels[14] = convert((_data[20] >> 2 | _data[21] << 6) & 0x07FF); - _channels[15] = convert((_data[21] >> 5 | _data[22] << 3) & 0x07FF); - _flags = _data[23]; - - _new_data = true; - } - - inline uint16_t convert(int v) - { - return Math::clamp(((v * 5) / 8) + 880, 800, 2200); - } + void parse(int d); + void apply(); + uint16_t convert(int v); const static size_t SBUS_FRAME_SIZE = sizeof(SbusData); + static const size_t CHANNELS = 16; Device::SerialDevice * _serial; SbusState _state; uint8_t _idx = 0; bool _new_data; - static const size_t CHANNELS = 16; - uint8_t _data[SBUS_FRAME_SIZE]; uint16_t _channels[CHANNELS]; uint8_t _flags; @@ -222,5 +80,3 @@ class InputSBUS: public InputDevice } } - -#endif diff --git a/lib/Espfc/src/Device/SerialDeviceAdapter.h b/lib/Espfc/src/Device/SerialDeviceAdapter.h index 4ada1324..9ea854fd 100644 --- a/lib/Espfc/src/Device/SerialDeviceAdapter.h +++ b/lib/Espfc/src/Device/SerialDeviceAdapter.h @@ -2,9 +2,6 @@ #define _ESPFC_SERIAL_DEVICE_ADAPTER_H_ #include "Device/SerialDevice.h" -#ifdef ESPFC_SERIAL_SOFT_0_RX -#include "EspSoftSerial.h" -#endif #ifdef ESPFC_SERIAL_SOFT_0_WIFI #include #endif @@ -18,7 +15,7 @@ class SerialDeviceAdapter: public SerialDevice { public: SerialDeviceAdapter(T& dev): _dev(dev) {} - virtual void begin(const SerialDeviceConfig& conf); + 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) { @@ -45,48 +42,21 @@ class SerialDeviceAdapter: public SerialDevice T& _dev; }; -template -void SerialDeviceAdapter::begin(const SerialDeviceConfig& conf) -{ - targetSerialInit(_dev, conf); -} - // WiFiClient specializations #ifdef ESPFC_SERIAL_SOFT_0_WIFI template<> -void SerialDeviceAdapter::begin(const SerialDeviceConfig& conf) +inline void SerialDeviceAdapter::begin(const SerialDeviceConfig& conf) { } template<> -int SerialDeviceAdapter::availableForWrite() +inline int SerialDeviceAdapter::availableForWrite() { return SERIAL_TX_FIFO_SIZE; } template<> -bool SerialDeviceAdapter::isTxFifoEmpty() -{ - return true; -} -#endif - -#ifdef ESPFC_SERIAL_SOFT_0_RX -template<> -void SerialDeviceAdapter::begin(const SerialDeviceConfig& conf) -{ - EspSoftSerialConfig ec; - ec.baud = conf.baud; - ec.rx_pin = conf.rx_pin; - ec.inverted = conf.inverted; - ec.data_bits = conf.data_bits; - ec.parity_type = conf.parity; - ec.stop_bits = conf.stop_bits; - _dev.begin(ec); -} - -template<> -bool SerialDeviceAdapter::isSoft() const +inline bool SerialDeviceAdapter::isTxFifoEmpty() { return true; } diff --git a/lib/Espfc/src/Espfc.cpp b/lib/Espfc/src/Espfc.cpp new file mode 100644 index 00000000..c8c8494e --- /dev/null +++ b/lib/Espfc/src/Espfc.cpp @@ -0,0 +1,145 @@ +#include "Espfc.h" +#include "Debug_Espfc.h" + +namespace Espfc { + +Espfc::Espfc(): + _hardware(_model), _controller(_model), _input(_model), _actuator(_model), _sensor(_model), + _mixer(_model), _blackbox(_model) +#ifdef ESPFC_BUZER + , _buzzer(_model) +#endif + , _serial(_model) + {} + +int Espfc::load() +{ + PIN_DEBUG_INIT(); + _model.load(); + _model.state.appQueue.begin(); + return 1; +} + +int Espfc::begin() +{ + _serial.begin(); + _model.logStorageResult(); + _hardware.begin(); // requires _model.load() + _model.begin(); // requires _hardware.begin() + _mixer.begin(); + _sensor.begin(); // requires _hardware.begin() + _input.begin(); // requires _serial.begin() + _actuator.begin(); // requires _model.begin() + _controller.begin(); + _blackbox.begin(); // requires _serial.begin(), _actuator.begin() +#ifdef ESPFC_BUZER + _buzzer.begin(); +#endif + _model.state.buzzer.push(BUZZER_SYSTEM_INIT); + + return 1; +} + +int FAST_CODE_ATTR Espfc::update(bool externalTrigger) +{ + if(externalTrigger) + { + _model.state.gyroTimer.update(); + } + else + { + if(!_model.state.gyroTimer.check()) return 0; + } + Stats::Measure measure(_model.state.stats, COUNTER_CPU_0); + +#if defined(ESPFC_MULTI_CORE) + + _sensor.read(); + if(_model.state.inputTimer.syncTo(_model.state.gyroTimer, 1u)) + { + _input.update(); + } + if(_model.state.actuatorTimer.check()) + { + _actuator.update(); + } + + _serial.update(); +#ifdef ESPFC_BUZER + _buzzer.update(); +#endif + _model.state.stats.update(); + +#else + + _sensor.update(); + if(_model.state.loopTimer.syncTo(_model.state.gyroTimer)) + { + _controller.update(); + if(_model.state.mixerTimer.syncTo(_model.state.loopTimer)) + { + _mixer.update(); + } + _blackbox.update(); + if(_model.state.inputTimer.syncTo(_model.state.gyroTimer, 1u)) + { + _input.update(); + } + if(_model.state.actuatorTimer.check()) + { + _actuator.update(); + } + } + _sensor.updateDelayed(); + + _serial.update(); +#ifdef ESPFC_BUZER + _buzzer.update(); +#endif + _model.state.stats.update(); +#endif + + return 1; +} + +// other task +int FAST_CODE_ATTR Espfc::updateOther() +{ +#if defined(ESPFC_MULTI_CORE) + if(_model.state.appQueue.isEmpty()) + { + return 0; + } + Event e = _model.state.appQueue.receive(); + + Stats::Measure measure(_model.state.stats, COUNTER_CPU_1); + + switch(e.type) + { + case EVENT_GYRO_READ: + _sensor.preLoop(); + _controller.update(); + // skip mixer and bb if earlier than half cycle, possible delay in previous iteration, + // to keep space to receive dshot erpm frame, but process rest + if(_loop_next < micros()) + { + _loop_next = micros() + _model.state.loopTimer.interval / 2; + _mixer.update(); + _blackbox.update(); + } + _sensor.postLoop(); + break; + case EVENT_ACCEL_READ: + _sensor.fusion(); + break; + default: + break; + // nothing + } +#endif + + return 1; +} + +} + diff --git a/lib/Espfc/src/Espfc.h b/lib/Espfc/src/Espfc.h index 4ce7a28d..06f0ea64 100644 --- a/lib/Espfc/src/Espfc.h +++ b/lib/Espfc/src/Espfc.h @@ -1,7 +1,5 @@ -#ifndef _ESPFC_ESPFC_H_ -#define _ESPFC_ESPFC_H_ +#pragma once -#include "Target/Target.h" #include "Model.h" #include "Hardware.h" #include "Controller.h" @@ -9,148 +7,27 @@ #include "Actuator.h" #include "SensorManager.h" #include "SerialManager.h" -#include "Fusion.h" #include "Output/Mixer.h" -#include "Blackbox.h" -#include "Cli.h" +#include "Blackbox/Blackbox.h" +#ifdef ESPFC_BUZER #include "Buzzer.h" +#endif namespace Espfc { class Espfc { public: - Espfc(): - _hardware(_model), _controller(_model), _input(_model), _actuator(_model), _sensor(_model), - _mixer(_model), _blackbox(_model), _telemetry(_model), _buzzer(_model), _serial(_model) - {} - - int load() - { - PIN_DEBUG_INIT(); - _model.load(); - _model.state.appQueue.begin(); - return 1; - } - - int begin() - { - _serial.begin(); - _model.logStorageResult(); - _hardware.begin(); - _model.begin(); - //_mixer.begin(); - _sensor.begin(); - _input.begin(); - _actuator.begin(); - _controller.begin(); - _blackbox.begin(); - - return 1; - } - - int beginOther() - { - _mixer.begin(); - _buzzer.begin(); - _model.state.buzzer.push(BEEPER_SYSTEM_INIT); - return 1; - } - - int update() - { -#if defined(ESPFC_MULTI_CORE) - if(!_model.state.gyroTimer.check()) - { - return 0; - } - Stats::Measure measure(_model.state.stats, COUNTER_CPU_0); - - _sensor.read(); - if(_model.state.inputTimer.check()) - { - _input.update(); - } - if(_model.state.actuatorTimer.check()) - { - _actuator.update(); - } - if(_model.state.serialTimer.check()) - { - _serial.update(); - _buzzer.update(); - _model.state.stats.update(); - } - - return 1; -#else - if(_model.state.gyroTimer.check()) - { - Stats::Measure measure(_model.state.stats, COUNTER_CPU_0); - _sensor.update(); - if(_model.state.loopTimer.syncTo(_model.state.gyroTimer)) - { - _controller.update(); - if(_model.state.mixerTimer.syncTo(_model.state.loopTimer)) - { - _mixer.update(); - } - _blackbox.update(); - if(_model.state.inputTimer.check()) - { - _input.update(); - } - if(_model.state.actuatorTimer.check()) - { - _actuator.update(); - } - } - _sensor.updateDelayed(); - } -#endif + Espfc(); - return 1; - } + int load(); + int begin(); + int update(bool externalTrigger = false); + int updateOther(); - // other task - int updateOther() + int getGyroInterval() const { -#if defined(ESPFC_MULTI_CORE) - if(_model.state.appQueue.isEmpty()) - { - return 0; - } - Event e = _model.state.appQueue.receive(); - - Stats::Measure measure(_model.state.stats, COUNTER_CPU_1); - - switch(e.type) - { - case EVENT_GYRO_READ: - _sensor.preLoop(); - _controller.update(); - _mixer.update(); - _blackbox.update(); - _sensor.postLoop(); - break; - case EVENT_ACCEL_READ: - _sensor.fusion(); - break; - default: - break; - // nothing - } -#else - if(_model.state.serialTimer.check()) - { - Stats::Measure measure(_model.state.stats, COUNTER_CPU_1); - _serial.update(); - _buzzer.update(); - _model.state.stats.update(); - } -#endif - - return 1; + return _model.state.gyroTimer.interval; } private: @@ -161,12 +38,12 @@ class Espfc Actuator _actuator; SensorManager _sensor; Output::Mixer _mixer; - Blackbox _blackbox; - Telemetry _telemetry; + Blackbox::Blackbox _blackbox; +#ifdef ESPFC_BUZER Buzzer _buzzer; +#endif SerialManager _serial; + uint32_t _loop_next; }; } - -#endif diff --git a/lib/Espfc/src/Filter.cpp b/lib/Espfc/src/Filter.cpp new file mode 100644 index 00000000..77039617 --- /dev/null +++ b/lib/Espfc/src/Filter.cpp @@ -0,0 +1,474 @@ +#include +#include "Filter.h" +#include "Math/Utils.h" +#include "Utils/MemoryHelper.h" + +// Quick median filter implementation +// (c) N. Devillard - 1998 +// http://ndevilla.free.fr/median/median.pdf +#define QMF_SORT(a,b) { if ((a)>(b)) QMF_SWAP((a),(b)); } +#define QMF_SWAP(a,b) { int32_t temp=(a);(a)=(b);(b)=temp; } +#define QMF_COPY(p,v,n) { for (size_t i=0; i(b)) QMF_SWAPF((a),(b)); } +#define QMF_SWAPF(a,b) { float temp=(a);(a)=(b);(b)=temp; } + +static float pt1Gain(float rate, float freq) +{ + float rc = 1.f / (2.f * Espfc::Math::pi() * freq); + float dt = 1.f / rate; + return dt / (dt + rc); +} + +namespace Espfc { + +FilterConfig::FilterConfig(): type(FILTER_NONE), freq(0), cutoff(0) {} +FilterConfig::FilterConfig(FilterType t, int16_t f, int16_t c): type(t), freq(f), cutoff(c) {} + +FilterConfig FAST_CODE_ATTR FilterConfig::sanitize(int rate) const +{ + const int halfRate = rate * 0.49f; + FilterType t = (FilterType)type; + int16_t f = Math::clamp((int)freq, 0, halfRate); // adj cut freq below nyquist rule + int16_t c = Math::clamp((int)cutoff, 0, (int)(f * 0.98f)); // sanitize cutoff to be slightly below filter freq + + bool biquad = type == FILTER_NOTCH || type == FILTER_NOTCH_DF1 || type == FILTER_BPF; + if(f == 0 || (biquad && c == 0)) t = FILTER_NONE; // if freq is zero or cutoff for biquad, turn off + + return FilterConfig(t, f, c); +} + +void FilterStatePt1::reset() +{ + v = 0.f; +} + +void FAST_CODE_ATTR FilterStatePt1::reconfigure(const FilterStatePt1& from) +{ + k = from.k; +} + +void FilterStatePt1::init(float rate, float freq) +{ + k = pt1Gain(rate, freq); +} + +float FAST_CODE_ATTR FilterStatePt1::update(float n) +{ + v += k * (n - v); + return v; +} + +void FilterStateFir2::reset() +{ + v[0] = v[1] = 0.0f; +} + +void FilterStateFir2::init() +{ +} + +void FAST_CODE_ATTR FilterStateFir2::reconfigure(const FilterStateFir2& from) +{ +} + +float FAST_CODE_ATTR FilterStateFir2::update(float n) +{ + v[0] = (n + v[1]) * 0.5f; + v[1] = n; + return v[0]; +} + +void FilterStateBiquad::reset() +{ + x1 = x2 = y1 = y2 = 0; +} + +void FilterStateBiquad::init(BiquadFilterType filterType, float rate, float freq, float q) +{ + const float omega = (2.0f * Math::pi() * freq) / rate; + const float sn = sinf(omega); + const float cs = cosf(omega); + const float alpha = sn / (2.0f * q); + float b0 = 0.0f, b1 = 0.0f, b2 = 0.0f, a0 = 0.0f, a1 = 0.0f, a2 = 0.0f; + switch (filterType) + { + case BIQUAD_FILTER_LPF: + b0 = (1.f - cs) * 0.5f; + b1 = 1.f - cs; + b2 = (1.f - cs) * 0.5f; + a0 = 1.f + alpha; + a1 = -2.f * cs; + a2 = 1.f - alpha; + break; + case BIQUAD_FILTER_NOTCH: + b0 = 1.f; + b1 = -2.f * cs; + b2 = 1.f; + a0 = 1.f + alpha; + a1 = -2.f * cs; + a2 = 1.f - alpha; + break; + case BIQUAD_FILTER_BPF: + b0 = alpha; + b1 = 0; + b2 = -alpha; + a0 = 1.f + alpha; + a1 = -2.f * cs; + a2 = 1.f - alpha; + break; + } + + // precompute the coefficients + this->b0 = b0 / a0; + this->b1 = b1 / a0; + this->b2 = b2 / a0; + this->a1 = a1 / a0; + this->a2 = a2 / a0; +} + +void FAST_CODE_ATTR FilterStateBiquad::reconfigure(const FilterStateBiquad& from) +{ + b0 = from.b0; + b1 = from.b1; + b2 = from.b2; + a1 = from.a1; + a2 = from.a2; +} + +float FAST_CODE_ATTR FilterStateBiquad::update(float n) +{ + // DF2 + const float result = b0 * n + x1; + x1 = b1 * n - a1 * result + x2; + x2 = b2 * n - a2 * result; + return result; +} + +float FAST_CODE_ATTR FilterStateBiquad::updateDF1(float n) +{ + /* compute result */ + const float result = b0 * n + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2; + + /* shift x1 to x2, input to x1 */ + x2 = x1; x1 = n; + + /* shift y1 to y2, result to y1 */ + y2 = y1; y1 = result; + + return result; +} + +void FilterStateFirstOrder::reset() +{ + x1 = y1 = 0; +} + +void FilterStateFirstOrder::init(float rate, float freq) +{ + freq = Math::clamp(freq, 0.0f, rate * 0.48f); + + const float W = std::tan(Math::pi() * freq / rate); + + a1 = (W - 1) / (W + 1); + b1 = b0 = W / (W + 1); +} + +void FAST_CODE_ATTR FilterStateFirstOrder::reconfigure(const FilterStateFirstOrder& from) +{ + b0 = from.b0; + b1 = from.b1; + a1 = from.a1; +} + +float FAST_CODE_ATTR FilterStateFirstOrder::update(float n) +{ + // DF2 + const float result = b0 * n + x1; + x1 = b1 * n - a1 * result; + return result; +} + +float FAST_CODE_ATTR FilterStateFirstOrder::updateDF1(float n) +{ + /* compute result */ + const float result = b0 * n + b1 * x1 - a1 * y1; + + /* shift input to x1 */ + x1 = n; + + /* shift result to y1 */ + y1 = result; + + return result; +} + +void FilterStateMedian::reset() +{ + v[0] = v[1] = v[2] = 0.f; +} + +void FilterStateMedian::init() +{ +} + +void FAST_CODE_ATTR FilterStateMedian::reconfigure(const FilterStateMedian& from) +{ +} + +float FAST_CODE_ATTR FilterStateMedian::update(float n) +{ + v[0] = v[1]; + v[1] = v[2]; + v[2] = n; + float p[3]; + QMF_COPY(p, v, 3); + QMF_SORTF(p[0], p[1]); + QMF_SORTF(p[1], p[2]); + QMF_SORTF(p[0], p[1]); + return p[1]; +} + +void FilterStatePt2::reset() +{ + v[0] = v[1] = 0.f; +} + +void FilterStatePt2::init(float rate, float freq) +{ + constexpr float correction = 1.553773974f; // 1 / sqrt(2^(1/n) - 1) + k = pt1Gain(rate, freq * correction); +} + +void FAST_CODE_ATTR FilterStatePt2::reconfigure(const FilterStatePt2& from) +{ + k = from.k; +} + +float FAST_CODE_ATTR FilterStatePt2::update(float n) +{ + v[0] += k * (n - v[0]); + v[1] += k * (v[0] - v[1]); + return v[1]; +} + +void FilterStatePt3::reset() +{ + v[0] = v[1] = v[2] = 0.f; +} + +void FilterStatePt3::init(float rate, float freq) +{ + constexpr float correction = 1.961459177f; // 1 / sqrt(2^(1/n) - 1) + k = pt1Gain(rate, freq * correction); +} + +void FAST_CODE_ATTR FilterStatePt3::reconfigure(const FilterStatePt3& from) +{ + k = from.k; +} + +float FAST_CODE_ATTR FilterStatePt3::update(float n) +{ + v[0] += k * (n - v[0]); + v[1] += k * (v[0] - v[1]); + v[2] += k * (v[1] - v[2]); + return v[2]; +} + +Filter::Filter(): _conf(FilterConfig(FILTER_NONE, 0)) {} + +void Filter::begin() +{ + _conf = FilterConfig(FILTER_NONE, 0); +} + +void Filter::begin(const FilterConfig& config, int rate) +{ + reconfigure(config, rate); + reset(); +} + +float FAST_CODE_ATTR Filter::update(float v) +{ + switch(_conf.type) + { + case FILTER_PT1: + return _state.pt1.update(v); + case FILTER_BIQUAD: + case FILTER_NOTCH: + case FILTER_BPF: + return _state.bq.update(v); + case FILTER_NOTCH_DF1: + return _output_weight * _state.bq.updateDF1(v) + _input_weight * v; + case FILTER_FIR2: + return _state.fir2.update(v); + case FILTER_MEDIAN3: + return _state.median.update(v); + case FILTER_PT2: + return _state.pt2.update(v); + case FILTER_PT3: + return _state.pt3.update(v); + case FILTER_FO: + return _state.fo.update(v); + case FILTER_NONE: + default: + return v; + } +} + +void Filter::reset() +{ + switch(_conf.type) + { + case FILTER_PT1: + _state.pt1.reset(); + break; + case FILTER_BIQUAD: + case FILTER_NOTCH: + case FILTER_NOTCH_DF1: + case FILTER_BPF: + _state.bq.reset(); + break; + case FILTER_FIR2: + _state.fir2.reset(); + break; + case FILTER_MEDIAN3: + _state.median.reset(); + break; + case FILTER_PT2: + return _state.pt2.reset(); + case FILTER_PT3: + return _state.pt3.reset(); + case FILTER_FO: + return _state.fo.reset(); + case FILTER_NONE: + default: + ; + } +} + +void FAST_CODE_ATTR Filter::reconfigure(int16_t freq, int16_t cutoff) +{ + reconfigure(FilterConfig((FilterType)_conf.type, freq, cutoff), _rate); +} + +void FAST_CODE_ATTR Filter::reconfigure(int16_t freq, int16_t cutoff, float q, float weight) +{ + reconfigure(FilterConfig((FilterType)_conf.type, freq, cutoff), _rate, q, weight); +} + +void FAST_CODE_ATTR Filter::reconfigure(const FilterConfig& config, int rate) +{ + _rate = rate; + _conf = config.sanitize(_rate); + switch(_conf.type) + { + case FILTER_BIQUAD: + reconfigure(config, rate, 0.70710678118f, 1.0f); // 1.0f / sqrtf(2.0f); // quality factor for butterworth lpf + break; + case FILTER_NOTCH: + case FILTER_NOTCH_DF1: + case FILTER_BPF: + reconfigure(config, rate, getNotchQApprox(config.freq, config.cutoff), 1.0f); + break; + default: + reconfigure(config, rate, 0.0f, 1.0f); + } +} + +void FAST_CODE_ATTR Filter::reconfigure(const FilterConfig& config, int rate, float q, float weight) +{ + _rate = rate; + _conf = config.sanitize(_rate); + setWeight(weight); + switch(_conf.type) + { + case FILTER_PT1: + _state.pt1.init(_rate, _conf.freq); + break; + case FILTER_BIQUAD: + _state.bq.init(BIQUAD_FILTER_LPF, _rate, _conf.freq, q); + break; + case FILTER_NOTCH: + case FILTER_NOTCH_DF1: + _state.bq.init(BIQUAD_FILTER_NOTCH, _rate, _conf.freq, q); + break; + case FILTER_BPF: + _state.bq.init(BIQUAD_FILTER_BPF, _rate, _conf.freq, q); + break; + case FILTER_FIR2: + _state.fir2.init(); + break; + case FILTER_MEDIAN3: + _state.median.init(); + break; + case FILTER_PT2: + _state.pt2.init(_rate, _conf.freq); + break; + case FILTER_PT3: + _state.pt3.init(_rate, _conf.freq); + break; + case FILTER_FO: + _state.fo.init(_rate, _conf.freq); + break; + case FILTER_NONE: + default: + ; + } +} + +void FAST_CODE_ATTR Filter::reconfigure(const Filter& filter) +{ + _rate = filter._rate; + _conf = filter._conf; + _output_weight = filter._output_weight; + _input_weight = filter._input_weight; + switch(_conf.type) + { + case FILTER_PT1: + _state.pt1.reconfigure(filter._state.pt1); + break; + case FILTER_BIQUAD: + case FILTER_NOTCH: + case FILTER_NOTCH_DF1: + _state.bq.reconfigure(filter._state.bq); + break; + case FILTER_FIR2: + _state.fir2.reconfigure(filter._state.fir2); + break; + case FILTER_MEDIAN3: + _state.median.reconfigure(filter._state.median); + break; + case FILTER_PT2: + _state.pt2.reconfigure(filter._state.pt2); + break; + case FILTER_PT3: + _state.pt3.reconfigure(filter._state.pt3); + break; + case FILTER_FO: + _state.fo.reconfigure(filter._state.fo); + break; + case FILTER_NONE: + default: + ; + } +} + +void FAST_CODE_ATTR Filter::setWeight(float weight) +{ + _output_weight = std::max(0.0f, std::min(weight, 1.0f)); + _input_weight = 1.0f - _output_weight; +} + +float FAST_CODE_ATTR Filter::getNotchQApprox(float freq, float cutoff) +{ + return ((float)(cutoff * freq) / ((float)(freq - cutoff) * (float)(freq + cutoff))); +} + +float FAST_CODE_ATTR Filter::getNotchQ(float freq, float cutoff) +{ + float octaves = std::log2(freq / cutoff) * 2.f; + return sqrtf(std::pow(2.f, octaves)) / (std::pow(2.f, octaves) - 1); +} + +} diff --git a/lib/Espfc/src/Filter.h b/lib/Espfc/src/Filter.h index 0dabd8d4..11b3be23 100644 --- a/lib/Espfc/src/Filter.h +++ b/lib/Espfc/src/Filter.h @@ -1,28 +1,6 @@ -#ifndef _ESPFC_FILTER_H_ -#define _ESPFC_FILTER_H_ +#pragma once -#include "Math/Utils.h" -#include - -// Quick median filter implementation -// (c) N. Devillard - 1998 -// http://ndevilla.free.fr/median/median.pdf -#define QMF_SORT(a,b) { if ((a)>(b)) QMF_SWAP((a),(b)); } -#define QMF_SWAP(a,b) { int32_t temp=(a);(a)=(b);(b)=temp; } -#define QMF_COPY(p,v,n) { for (size_t i=0; i(b)) QMF_SWAPF((a),(b)); } -#define QMF_SWAPF(a,b) { float temp=(a);(a)=(b);(b)=temp; } - -namespace { - -static float pt1Gain(float rate, float freq) -{ - float rc = 1.f / (2.f * Espfc::Math::pi() * freq); - float dt = 1.f / rate; - return dt / (dt + rc); -} - -} +#include namespace Espfc { @@ -49,21 +27,9 @@ enum BiquadFilterType { class FilterConfig { public: - FilterConfig(): type(FILTER_NONE), freq(0), cutoff(0) {} - FilterConfig(FilterType t, int16_t f, int16_t c = 0): type(t), freq(f), cutoff(c) {} - - FilterConfig sanitize(int rate) const - { - const int halfRate = rate * 0.49f; - FilterType t = (FilterType)type; - int16_t f = Math::clamp((int)freq, 0, halfRate); // adj cut freq below nyquist rule - int16_t c = Math::clamp((int)cutoff, 0, (int)(f * 0.98f)); // sanitize cutoff to be slightly below filter freq - - bool biquad = type == FILTER_NOTCH || type == FILTER_NOTCH_DF1 || type == FILTER_BPF; - if(f == 0 || (biquad && c == 0)) t = FILTER_NONE; // if freq is zero or cutoff for biquad, turn off - - return FilterConfig(t, f, c); - } + FilterConfig(); + FilterConfig(FilterType t, int16_t f, int16_t c = 0); + FilterConfig sanitize(int rate) const; int8_t type; int16_t freq; @@ -83,26 +49,10 @@ class DynamicFilterConfig { class FilterStatePt1 { public: - void reset() - { - v = 0.f; - } - - void reconfigure(const FilterStatePt1& from) - { - k = from.k; - } - - void init(float rate, float freq) - { - k = pt1Gain(rate, freq); - } - - float update(float n) - { - v += k * (n - v); - return v; - } + void reset(); + void reconfigure(const FilterStatePt1& from); + void init(float rate, float freq); + float update(float n); float k; float v; @@ -110,110 +60,21 @@ class FilterStatePt1 { class FilterStateFir2 { public: - void reset() - { - v[0] = v[1] = 0.0f; - } - - void init() - { - } - - void reconfigure(const FilterStateFir2& from) - { - } - - float update(float n) - { - v[0] = (n + v[1]) * 0.5f; - v[1] = n; - return v[0]; - } + void reset(); + void init(); + void reconfigure(const FilterStateFir2& from); + float update(float n); float v[2]; }; class FilterStateBiquad { public: - void reset() - { - x1 = x2 = y1 = y2 = 0; - } - - void init(BiquadFilterType filterType, float rate, float freq, float q) - { - const float omega = (2.0f * Math::pi() * freq) / rate; - const float sn = sinf(omega); - const float cs = cosf(omega); - const float alpha = sn / (2.0f * q); - float b0 = 0.0f, b1 = 0.0f, b2 = 0.0f, a0 = 0.0f, a1 = 0.0f, a2 = 0.0f; - switch (filterType) - { - case BIQUAD_FILTER_LPF: - b0 = (1.f - cs) * 0.5f; - b1 = 1.f - cs; - b2 = (1.f - cs) * 0.5f; - a0 = 1.f + alpha; - a1 = -2.f * cs; - a2 = 1.f - alpha; - break; - case BIQUAD_FILTER_NOTCH: - b0 = 1.f; - b1 = -2.f * cs; - b2 = 1.f; - a0 = 1.f + alpha; - a1 = -2.f * cs; - a2 = 1.f - alpha; - break; - case BIQUAD_FILTER_BPF: - b0 = alpha; - b1 = 0; - b2 = -alpha; - a0 = 1.f + alpha; - a1 = -2.f * cs; - a2 = 1.f - alpha; - break; - } - - // precompute the coefficients - this->b0 = b0 / a0; - this->b1 = b1 / a0; - this->b2 = b2 / a0; - this->a1 = a1 / a0; - this->a2 = a2 / a0; - } - - void reconfigure(const FilterStateBiquad& from) - { - b0 = from.b0; - b1 = from.b1; - b2 = from.b2; - a1 = from.a1; - a2 = from.a2; - } - - float update(float n) - { - // DF2 - const float result = b0 * n + x1; - x1 = b1 * n - a1 * result + x2; - x2 = b2 * n - a2 * result; - return result; - } - - float updateDF1(float n) - { - /* compute result */ - const float result = b0 * n + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2; - - /* shift x1 to x2, input to x1 */ - x2 = x1; x1 = n; - - /* shift y1 to y2, result to y1 */ - y2 = y1; y1 = result; - - return result; - } + void reset(); + void init(BiquadFilterType filterType, float rate, float freq, float q); + void reconfigure(const FilterStateBiquad& from); + float update(float n); + float updateDF1(float n); float b0, b1, b2, a1, a2; float x1, x2, y1, y2; @@ -221,49 +82,11 @@ class FilterStateBiquad { class FilterStateFirstOrder { public: - void reset() - { - x1 = y1 = 0; - } - - void init(float rate, float freq) - { - freq = Math::clamp(freq, 0.0f, rate * 0.48f); - - const float W = std::tan(Math::pi() * freq / rate); - - a1 = (W - 1) / (W + 1); - b1 = b0 = W / (W + 1); - } - - void reconfigure(const FilterStateFirstOrder& from) - { - b0 = from.b0; - b1 = from.b1; - a1 = from.a1; - } - - float update(float n) - { - // DF2 - const float result = b0 * n + x1; - x1 = b1 * n - a1 * result; - return result; - } - - float updateDF1(float n) - { - /* compute result */ - const float result = b0 * n + b1 * x1 - a1 * y1; - - /* shift input to x1 */ - x1 = n; - - /* shift result to y1 */ - y1 = result; - - return result; - } + void reset(); + void init(float rate, float freq); + void reconfigure(const FilterStateFirstOrder& from); + float update(float n); + float updateDF1(float n); float b0, b1, a1; float x1, y1; @@ -271,59 +94,20 @@ class FilterStateFirstOrder { class FilterStateMedian { public: - void reset() - { - v[0] = v[1] = v[2] = 0.f; - } - - void init() - { - } - - void reconfigure(const FilterStateMedian& from) - { - } - - float update(float n) - { - v[0] = v[1]; - v[1] = v[2]; - v[2] = n; - float p[3]; - QMF_COPY(p, v, 3); - QMF_SORTF(p[0], p[1]); - QMF_SORTF(p[1], p[2]); - QMF_SORTF(p[0], p[1]); - return p[1]; - } + void reset(); + void init(); + void reconfigure(const FilterStateMedian& from); + float update(float n); float v[3]; }; class FilterStatePt2 { public: - void reset() - { - v[0] = v[1] = 0.f; - } - - void init(float rate, float freq) - { - constexpr float correction = 1.553773974f; // 1 / sqrt(2^(1/n) - 1) - k = pt1Gain(rate, freq * correction); - } - - void reconfigure(const FilterStatePt2& from) - { - k = from.k; - } - - float update(float n) - { - v[0] += k * (n - v[0]); - v[1] += k * (v[0] - v[1]); - return v[1]; - } + void reset(); + void init(float rate, float freq); + void reconfigure(const FilterStatePt2& from); + float update(float n); float k; float v[2]; @@ -331,29 +115,10 @@ class FilterStatePt2 { class FilterStatePt3 { public: - void reset() - { - v[0] = v[1] = v[2] = 0.f; - } - - void init(float rate, float freq) - { - constexpr float correction = 1.961459177f; // 1 / sqrt(2^(1/n) - 1) - k = pt1Gain(rate, freq * correction); - } - - void reconfigure(const FilterStatePt3& from) - { - k = from.k; - } - - float update(float n) - { - v[0] += k * (n - v[0]); - v[1] += k * (v[0] - v[1]); - v[2] += k * (v[1] - v[2]); - return v[2]; - } + void reset(); + void init(float rate, float freq); + void reconfigure(const FilterStatePt3& from); + float update(float n); float k; float v[3]; @@ -362,201 +127,20 @@ class FilterStatePt3 { class Filter { public: - Filter(): _conf(FilterConfig(FILTER_NONE, 0)) {} - - void begin() - { - _conf = FilterConfig(FILTER_NONE, 0); - } - - void begin(const FilterConfig& config, int rate) - { - reconfigure(config, rate); - reset(); - } - - float update(float v) - { - switch(_conf.type) - { - case FILTER_PT1: - return _state.pt1.update(v); - case FILTER_BIQUAD: - case FILTER_NOTCH: - case FILTER_BPF: - return _state.bq.update(v); - case FILTER_NOTCH_DF1: - return _output_weight * _state.bq.updateDF1(v) + _input_weight * v; - case FILTER_FIR2: - return _state.fir2.update(v); - case FILTER_MEDIAN3: - return _state.median.update(v); - case FILTER_PT2: - return _state.pt2.update(v); - case FILTER_PT3: - return _state.pt3.update(v); - case FILTER_FO: - return _state.fo.update(v); - case FILTER_NONE: - default: - return v; - } - } - - void reset() - { - switch(_conf.type) - { - case FILTER_PT1: - _state.pt1.reset(); - break; - case FILTER_BIQUAD: - case FILTER_NOTCH: - case FILTER_NOTCH_DF1: - case FILTER_BPF: - _state.bq.reset(); - break; - case FILTER_FIR2: - _state.fir2.reset(); - break; - case FILTER_MEDIAN3: - _state.median.reset(); - break; - case FILTER_PT2: - return _state.pt2.reset(); - case FILTER_PT3: - return _state.pt3.reset(); - case FILTER_FO: - return _state.fo.reset(); - case FILTER_NONE: - default: - ; - } - } - - void reconfigure(int16_t freq, int16_t cutoff = 0) - { - reconfigure(FilterConfig((FilterType)_conf.type, freq, cutoff), _rate); - } - - void reconfigure(int16_t freq, int16_t cutoff, float q, float weight = 1.0f) - { - reconfigure(FilterConfig((FilterType)_conf.type, freq, cutoff), _rate, q, weight); - } - - void reconfigure(const FilterConfig& config, int rate) - { - _rate = rate; - _conf = config.sanitize(_rate); - switch(_conf.type) - { - case FILTER_BIQUAD: - reconfigure(config, rate, 0.70710678118f, 1.0f); // 1.0f / sqrtf(2.0f); // quality factor for butterworth lpf - break; - case FILTER_NOTCH: - case FILTER_NOTCH_DF1: - case FILTER_BPF: - reconfigure(config, rate, getNotchQApprox(config.freq, config.cutoff), 1.0f); - break; - default: - reconfigure(config, rate, 0.0f, 1.0f); - } - } - - void reconfigure(const FilterConfig& config, int rate, float q, float weight) - { - _rate = rate; - _conf = config.sanitize(_rate); - setWeight(weight); - switch(_conf.type) - { - case FILTER_PT1: - _state.pt1.init(_rate, _conf.freq); - break; - case FILTER_BIQUAD: - _state.bq.init(BIQUAD_FILTER_LPF, _rate, _conf.freq, q); - break; - case FILTER_NOTCH: - case FILTER_NOTCH_DF1: - _state.bq.init(BIQUAD_FILTER_NOTCH, _rate, _conf.freq, q); - break; - case FILTER_BPF: - _state.bq.init(BIQUAD_FILTER_BPF, _rate, _conf.freq, q); - break; - case FILTER_FIR2: - _state.fir2.init(); - break; - case FILTER_MEDIAN3: - _state.median.init(); - break; - case FILTER_PT2: - _state.pt2.init(_rate, _conf.freq); - break; - case FILTER_PT3: - _state.pt3.init(_rate, _conf.freq); - break; - case FILTER_FO: - _state.fo.init(_rate, _conf.freq); - break; - case FILTER_NONE: - default: - ; - } - } - - void reconfigure(const Filter& filter) - { - _rate = filter._rate; - _conf = filter._conf; - _output_weight = filter._output_weight; - _input_weight = filter._input_weight; - switch(_conf.type) - { - case FILTER_PT1: - _state.pt1.reconfigure(filter._state.pt1); - break; - case FILTER_BIQUAD: - case FILTER_NOTCH: - case FILTER_NOTCH_DF1: - _state.bq.reconfigure(filter._state.bq); - break; - case FILTER_FIR2: - _state.fir2.reconfigure(filter._state.fir2); - break; - case FILTER_MEDIAN3: - _state.median.reconfigure(filter._state.median); - break; - case FILTER_PT2: - _state.pt2.reconfigure(filter._state.pt2); - break; - case FILTER_PT3: - _state.pt3.reconfigure(filter._state.pt3); - break; - case FILTER_FO: - _state.fo.reconfigure(filter._state.fo); - break; - case FILTER_NONE: - default: - ; - } - } - - void setWeight(float weight) - { - _output_weight = std::max(0.0f, std::min(weight, 1.0)); - _input_weight = 1.0f - _output_weight; - } - - float getNotchQApprox(float freq, float cutoff) - { - return ((float)(cutoff * freq) / ((float)(freq - cutoff) * (float)(freq + cutoff))); - } - - float getNotchQ(int freq, int cutoff) - { - float octaves = log2f((float)freq / (float)cutoff) * 2; - return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1); - } + Filter(); + void begin(); + void begin(const FilterConfig& config, int rate); + float update(float v); + void reset(); + + void reconfigure(int16_t freq, int16_t cutoff = 0); + void reconfigure(int16_t freq, int16_t cutoff, float q, float weight = 1.0f); + void reconfigure(const FilterConfig& config, int rate); + void reconfigure(const FilterConfig& config, int rate, float q, float weight); + void reconfigure(const Filter& filter); + void setWeight(float weight); + float getNotchQApprox(float freq, float cutoff); + float getNotchQ(float freq, float cutoff); #if !defined(UNIT_TEST) private: @@ -578,5 +162,3 @@ class Filter }; } - -#endif diff --git a/lib/Espfc/src/Fusion.cpp b/lib/Espfc/src/Fusion.cpp new file mode 100644 index 00000000..99be1d3b --- /dev/null +++ b/lib/Espfc/src/Fusion.cpp @@ -0,0 +1,332 @@ +#include "Fusion.h" +#include "Utils/MemoryHelper.h" + +namespace Espfc { + +Fusion::Fusion(Model& model): _model(model), _first(true) {} + +int Fusion::begin() +{ + _model.state.gyroPoseQ = Quaternion(); + + _madgwick.begin(_model.state.accelTimer.rate); + _madgwick.setKp(_model.config.fusion.gain * 0.05f); + + _mahony.begin(_model.state.accelTimer.rate); + _mahony.setKp(_model.config.fusion.gain * 0.1f); + _mahony.setKi(_model.config.fusion.gainI * 0.1f); + + _model.logger.info().log(F("FUSION")).log(FPSTR(FusionConfig::getModeName((FusionMode)_model.config.fusion.mode))).logln(_model.config.fusion.gain); + + return 1; +} + +void Fusion::restoreGain() +{ + _madgwick.setKp(_model.config.fusion.gain * 0.005f); + _mahony.setKp(_model.config.fusion.gain * 0.02f); + _mahony.setKi(_model.config.fusion.gainI * 0.02f); +} + +int FAST_CODE_ATTR Fusion::update() +{ + Stats::Measure measure(_model.state.stats, COUNTER_IMU_FUSION); + + if(_model.accelActive()) + { + switch(_model.config.fusion.mode) + { + case FUSION_MADGWICK: + madgwickFusion(); + break; + case FUSION_MAHONY: + mahonyFusion(); + break; + case FUSION_LERP: + lerpFusion(); + break; + case FUSION_RTQF: + updatePoseFromAccelMag(); + rtqfFusion(); + break; + case FUSION_KALMAN: + //updatePoseFromAccelMag(); + kalmanFusion(); + break; + case FUSION_COMPLEMENTARY: + //updatePoseFromAccelMag(); + complementaryFusion(); + break; + case FUSION_SIMPLE: + simpleFusion(); + break; + case FUSION_EXPERIMENTAL: + experimentalFusion(); + break; + case FUSION_NONE: + default: + ; + } + } + //else madgwickFusion1(); + + if(_model.config.debugMode == DEBUG_ALTITUDE) + { + _model.state.debug[0] = lrintf(degrees(_model.state.angle[0]) * 10); + _model.state.debug[1] = lrintf(degrees(_model.state.angle[1]) * 10); + _model.state.debug[2] = lrintf(degrees(_model.state.angle[2]) * 10); + } + return 1; +} + +void Fusion::experimentalFusion() +{ + // Experiment: workaround for 90 deg limit on pitch[y] axis + Quaternion r = Quaternion::lerp(Quaternion(), _model.state.accel.accelToQuaternion(), 0.5); + _model.state.angle.eulerFromQuaternion(r); + _model.state.angle *= 2.f; +} + +void Fusion::simpleFusion() +{ + _model.state.pose = _model.state.accel.accelToEuler(); + _model.state.angle.x = _model.state.pose.x; + _model.state.angle.y = _model.state.pose.y; + _model.state.angle.z += _model.state.gyroTimer.intervalf * _model.state.gyro.z; + if(_model.state.angle.z > PI) _model.state.angle.z -= TWO_PI; + if(_model.state.angle.z < -PI) _model.state.angle.z += TWO_PI; +} + +void Fusion::kalmanFusion() +{ + _model.state.pose = _model.state.accel.accelToEuler(); + _model.state.pose.z = _model.state.angle.z; + const float dt = _model.state.gyroTimer.intervalf; + for(size_t i = 0; i < 3; i++) + { + float angle = _model.state.kalman[i].getAngle(_model.state.pose.get(i), _model.state.gyro.get(i), dt); + _model.state.angle.set(i, angle); + //_model.state.rate.set(i, _model.state.kalman[i].getRate()); + } + _model.state.angleQ = _model.state.angle.eulerToQuaternion(); +} + +void Fusion::complementaryFusion() +{ + _model.state.pose = _model.state.accel.accelToEuler(); + _model.state.pose.z = _model.state.angle.z; + const float dt = _model.state.gyroTimer.intervalf; + const float alpha = 0.002f; + for(size_t i = 0; i < 3; i++) + { + float angle = (_model.state.angle[i] + _model.state.gyro[i] * dt) * (1.f - alpha) + _model.state.pose[i] * alpha; + if(angle > PI) angle -= TWO_PI; + if(angle < -PI) angle += TWO_PI; + _model.state.angle.set(i, angle); + } + _model.state.angleQ = _model.state.angle.eulerToQuaternion(); + //_model.state.angle.eulerFromQuaternion(_model.state.angleQ); // causes NaN +} + +void Fusion::complementaryFusionOld() +{ + const float alpha = 0.01f; + const float dt = _model.state.gyroTimer.intervalf; + _model.state.pose = _model.state.accel.accelToEuler(); + _model.state.pose.z = _model.state.angle.z; + _model.state.angle = (_model.state.angle + _model.state.gyro * dt) * (1.f - alpha) + _model.state.pose * alpha; + _model.state.angleQ = _model.state.angle.eulerToQuaternion(); +} + +void Fusion::rtqfFusion() +{ + float slerpPower = 0.001f; + if(_first) + { + _model.state.angle = _model.state.pose; + _model.state.angleQ = _model.state.poseQ; + _first = false; + return; + } + + float timeDelta = _model.state.gyroTimer.intervalf; + Quaternion measuredQPose = _model.state.poseQ; + Quaternion fusionQPose = _model.state.angleQ; + VectorFloat fusionGyro = _model.state.gyro; + + float qs, qx, qy, qz; + qs = fusionQPose.w; + qx = fusionQPose.x; + qy = fusionQPose.y; + qz = fusionQPose.z; + + float x2, y2, z2; + x2 = fusionGyro.x / 2.0; + y2 = fusionGyro.y / 2.0; + z2 = fusionGyro.z / 2.0; + + // Predict new state + fusionQPose.w = qs + (-x2 * qx - y2 * qy - z2 * qz) * timeDelta; + fusionQPose.x = qx + ( x2 * qs + z2 * qy - y2 * qz) * timeDelta; + fusionQPose.y = qy + ( y2 * qs - z2 * qx + x2 * qz) * timeDelta; + fusionQPose.z = qz + ( z2 * qs + y2 * qx - x2 * qy) * timeDelta; + + // calculate rotation delta + Quaternion rotationDelta = fusionQPose.getConjugate() * measuredQPose; + rotationDelta.normalize(); + + // take it to the power (0 to 1) to give the desired amount of correction + float theta = acos(rotationDelta.w); + float sinPowerTheta = sin(theta * slerpPower); + float cosPowerTheta = cos(theta * slerpPower); + + VectorFloat rotationUnitVector(rotationDelta.x, rotationDelta.y, rotationDelta.z); + rotationUnitVector.normalize(); + + Quaternion rotationPower; + rotationPower.w = cosPowerTheta; + rotationPower.x = sinPowerTheta * rotationUnitVector.x; + rotationPower.y = sinPowerTheta * rotationUnitVector.y; + rotationPower.z = sinPowerTheta * rotationUnitVector.z; + rotationPower.normalize(); + + // multiple this by predicted value to get result + fusionQPose = fusionQPose * rotationPower; + fusionQPose.normalize(); + + _model.state.angleQ = fusionQPose; + _model.state.angle.eulerFromQuaternion(fusionQPose); +} + +void Fusion::updatePoseFromAccelMag() +{ + _model.state.pose = _model.state.accel.accelToEuler(); + //_model.state.accelPose = _model.state.pose; + + if(_model.magActive()) + { + // Quaternion q = _model.state.pose.eulerToQuaternion(); + // since Z is always 0, it can be optimized a bit + float cosX2 = cos(_model.state.pose.x / 2.0f); + float sinX2 = sin(_model.state.pose.x / 2.0f); + float cosY2 = cos(_model.state.pose.y / 2.0f); + float sinY2 = sin(_model.state.pose.y / 2.0f); + + Quaternion q; + q.w = cosX2 * cosY2; + q.x = sinX2 * cosY2; + q.y = cosX2 * sinY2; + q.z = -sinX2 * sinY2; + + VectorFloat m = _model.state.mag.getRotated(q); + _model.state.magPose = m; + _model.state.pose.z = -atan2(m.y, m.x); + } + else + { + _model.state.pose.z = _model.state.angle.z; + } + _model.state.poseQ = _model.state.pose.eulerToQuaternion(); + _model.state.pose.eulerFromQuaternion(_model.state.poseQ); + + //return; + // check for quaternion aliasing. If the quaternion has the wrong sign + // the kalman filter will be very unhappy. + int maxIndex = -1; + float maxVal = -1000; + + for(int i = 0; i < 4; i++) { + if(fabs(_model.state.poseQ.get(i)) > maxVal) { + maxVal = fabs(_model.state.poseQ.get(i)); + maxIndex = i; + } + } + + // if the biggest component has a different sign in the measured and kalman poses, + // change the sign of the measured pose to match. + if(((_model.state.poseQ.get(maxIndex) < 0) && (_model.state.angleQ.get(maxIndex) > 0)) || + ((_model.state.poseQ.get(maxIndex) > 0) && (_model.state.angleQ.get(maxIndex) < 0))) { + _model.state.poseQ.w = -_model.state.poseQ.w; + _model.state.poseQ.x = -_model.state.poseQ.x; + _model.state.poseQ.y = -_model.state.poseQ.y; + _model.state.poseQ.z = -_model.state.poseQ.z; + _model.state.pose.eulerFromQuaternion(_model.state.poseQ); + } +} + +// experimental +void Fusion::lerpFusion() +{ + float correctionAlpha = 0.001f; // 0 - 1 => gyro - accel + + _model.state.accelPose = _model.state.accel.accelToEuler(); + _model.state.accelPoseQ = _model.state.accelPose.eulerToQuaternion(); + + if(_model.magActive()) + { + // use yaw from mag + VectorFloat mag = _model.state.mag.getRotated(_model.state.accelPoseQ); + _model.state.accelPose.z = -atan2(mag.y, mag.x); + } + else + { + _model.state.accelPose.z = _model.state.gyroPose.z; + } + _model.state.accelPoseQ = _model.state.accelPose.eulerToQuaternion(); + + //_model.state.accelPose.eulerFromQuaternion(_model.state.accelPoseQ); + + // predict new state + Quaternion rotation = (_model.state.gyro * _model.state.gyroTimer.intervalf).eulerToQuaternion(); + _model.state.gyroPoseQ = (_model.state.gyroPoseQ * rotation).getNormalized(); + + // drift compensation + _model.state.gyroPoseQ = Quaternion::lerp(_model.state.gyroPoseQ, _model.state.accelPoseQ, correctionAlpha); + + // calculate euler vectors for accel and position + _model.state.gyroPose.eulerFromQuaternion(_model.state.gyroPoseQ); +} + +void Fusion::madgwickFusion() +{ + if(false && _model.magActive()) + { + _madgwick.update( + _model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z, + _model.state.accel.x, _model.state.accel.y, _model.state.accel.z, + _model.state.mag.x, _model.state.mag.y, _model.state.mag.z + ); + } + else + { + _madgwick.update( + _model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z, + _model.state.accel.x, _model.state.accel.y, _model.state.accel.z + ); + } + _model.state.angleQ = _madgwick.getQuaternion(); + _model.state.angle = _madgwick.getEuler(); +} + +void FAST_CODE_ATTR Fusion::mahonyFusion() +{ + if(false && _model.magActive()) + { + _mahony.update( + _model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z, + _model.state.accel.x, _model.state.accel.y, _model.state.accel.z, + _model.state.mag.x, _model.state.mag.y, _model.state.mag.z + ); + } + else + { + _mahony.update( + _model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z, + _model.state.accel.x, _model.state.accel.y, _model.state.accel.z + ); + } + _model.state.angleQ = _mahony.getQuaternion(); + _model.state.angle = _mahony.getEuler(); +} + +} diff --git a/lib/Espfc/src/Fusion.h b/lib/Espfc/src/Fusion.h index fbc3a486..dc2ead12 100644 --- a/lib/Espfc/src/Fusion.h +++ b/lib/Espfc/src/Fusion.h @@ -1,5 +1,4 @@ -#ifndef _ESPFC_FUSION_H_ -#define _ESPFC_FUSION_H_ +#pragma once #include "Model.h" #include "Madgwick.h" @@ -10,392 +9,23 @@ namespace Espfc { class Fusion { public: - Fusion(Model& model): _model(model), _first(true) {} - int begin() - { - _model.state.gyroPoseQ = Quaternion(); - - _madgwick.begin(_model.state.accelTimer.rate); - _madgwick.setKp(_model.config.fusion.gain * 0.05f); - - _mahony.begin(_model.state.accelTimer.rate); - _mahony.setKp(_model.config.fusion.gain * 0.1f); - _mahony.setKi(_model.config.fusion.gainI * 0.1f); - - _model.logger.info().log(F("FUSION")).log(FPSTR(FusionConfig::getModeName((FusionMode)_model.config.fusion.mode))).logln(_model.config.fusion.gain); - - return 1; - } - - void restoreGain() - { - _madgwick.setKp(_model.config.fusion.gain * 0.005f); - _mahony.setKp(_model.config.fusion.gain * 0.02f); - _mahony.setKi(_model.config.fusion.gainI * 0.02f); - } - - int update() - { - Stats::Measure measure(_model.state.stats, COUNTER_IMU_FUSION); - - if(_model.accelActive()) - { - switch(_model.config.fusion.mode) - { - case FUSION_MADGWICK: - madgwickFusion(); - break; - case FUSION_MAHONY: - mahonyFusion(); - break; - case FUSION_LERP: - lerpFusion(); - break; - case FUSION_RTQF: - updatePoseFromAccelMag(); - rtqfFusion(); - break; - case FUSION_KALMAN: - //updatePoseFromAccelMag(); - kalmanFusion(); - break; - case FUSION_COMPLEMENTARY: - //updatePoseFromAccelMag(); - complementaryFusion(); - break; - case FUSION_SIMPLE: - simpleFusion(); - break; - case FUSION_EXPERIMENTAL: - experimentalFusion(); - break; - case FUSION_NONE: - default: - ; - } - } - //else madgwickFusion1(); - - if(_model.config.debugMode == DEBUG_ALTITUDE) - { - _model.state.debug[0] = lrintf(degrees(_model.state.angle[0]) * 10); - _model.state.debug[1] = lrintf(degrees(_model.state.angle[1]) * 10); - _model.state.debug[2] = lrintf(degrees(_model.state.angle[2]) * 10); - } - return 1; - } - - void updateDelayed() - { - Stats::Measure measure(_model.state.stats, COUNTER_IMU_FUSION2); - switch(_model.config.fusion.mode) - { - case FUSION_MADGWICK: - madgwickFusion2(); - break; - case FUSION_MAHONY: - mahonyFusion2(); - break; - } - } - - void experimentalFusion() - { - // Experiment: workaround for 90 deg limit on pitch[y] axis - Quaternion r = Quaternion::lerp(Quaternion(), _model.state.accel.accelToQuaternion(), 0.5); - _model.state.angle.eulerFromQuaternion(r); - _model.state.angle *= 2.f; - } - - void simpleFusion() - { - _model.state.pose = _model.state.accel.accelToEuler(); - _model.state.angle.x = _model.state.pose.x; - _model.state.angle.y = _model.state.pose.y; - _model.state.angle.z += _model.state.gyroTimer.intervalf * _model.state.gyro.z; - if(_model.state.angle.z > PI) _model.state.angle.z -= TWO_PI; - if(_model.state.angle.z < -PI) _model.state.angle.z += TWO_PI; - } - - void kalmanFusion() - { - _model.state.pose = _model.state.accel.accelToEuler(); - _model.state.pose.z = _model.state.angle.z; - const float dt = _model.state.gyroTimer.intervalf; - for(size_t i = 0; i < 3; i++) - { - float angle = _model.state.kalman[i].getAngle(_model.state.pose.get(i), _model.state.gyro.get(i), dt); - _model.state.angle.set(i, angle); - //_model.state.rate.set(i, _model.state.kalman[i].getRate()); - } - _model.state.angleQ = _model.state.angle.eulerToQuaternion(); - } - - void complementaryFusion() - { - _model.state.pose = _model.state.accel.accelToEuler(); - _model.state.pose.z = _model.state.angle.z; - const float dt = _model.state.gyroTimer.intervalf; - const float alpha = 0.002f; - for(size_t i = 0; i < 3; i++) - { - float angle = (_model.state.angle[i] + _model.state.gyro[i] * dt) * (1.f - alpha) + _model.state.pose[i] * alpha; - if(angle > PI) angle -= TWO_PI; - if(angle < -PI) angle += TWO_PI; - _model.state.angle.set(i, angle); - } - _model.state.angleQ = _model.state.angle.eulerToQuaternion(); - //_model.state.angle.eulerFromQuaternion(_model.state.angleQ); // causes NaN - } - - void complementaryFusionOld() - { - const float alpha = 0.01f; - const float dt = _model.state.gyroTimer.intervalf; - _model.state.pose = _model.state.accel.accelToEuler(); - _model.state.pose.z = _model.state.angle.z; - _model.state.angle = (_model.state.angle + _model.state.gyro * dt) * (1.f - alpha) + _model.state.pose * alpha; - _model.state.angleQ = _model.state.angle.eulerToQuaternion(); - } - - void rtqfFusion() - { - float slerpPower = 0.001f; - if(_first) - { - _model.state.angle = _model.state.pose; - _model.state.angleQ = _model.state.poseQ; - _first = false; - return; - } - - float timeDelta = _model.state.gyroTimer.intervalf; - Quaternion measuredQPose = _model.state.poseQ; - Quaternion fusionQPose = _model.state.angleQ; - VectorFloat fusionGyro = _model.state.gyro; - - float qs, qx, qy, qz; - qs = fusionQPose.w; - qx = fusionQPose.x; - qy = fusionQPose.y; - qz = fusionQPose.z; - - float x2, y2, z2; - x2 = fusionGyro.x / 2.0; - y2 = fusionGyro.y / 2.0; - z2 = fusionGyro.z / 2.0; - - // Predict new state - fusionQPose.w = qs + (-x2 * qx - y2 * qy - z2 * qz) * timeDelta; - fusionQPose.x = qx + ( x2 * qs + z2 * qy - y2 * qz) * timeDelta; - fusionQPose.y = qy + ( y2 * qs - z2 * qx + x2 * qz) * timeDelta; - fusionQPose.z = qz + ( z2 * qs + y2 * qx - x2 * qy) * timeDelta; - - // calculate rotation delta - Quaternion rotationDelta = fusionQPose.getConjugate() * measuredQPose; - rotationDelta.normalize(); - - // take it to the power (0 to 1) to give the desired amount of correction - float theta = acos(rotationDelta.w); - float sinPowerTheta = sin(theta * slerpPower); - float cosPowerTheta = cos(theta * slerpPower); - - VectorFloat rotationUnitVector(rotationDelta.x, rotationDelta.y, rotationDelta.z); - rotationUnitVector.normalize(); - - Quaternion rotationPower; - rotationPower.w = cosPowerTheta; - rotationPower.x = sinPowerTheta * rotationUnitVector.x; - rotationPower.y = sinPowerTheta * rotationUnitVector.y; - rotationPower.z = sinPowerTheta * rotationUnitVector.z; - rotationPower.normalize(); - - // multiple this by predicted value to get result - fusionQPose = fusionQPose * rotationPower; - fusionQPose.normalize(); - - _model.state.angleQ = fusionQPose; - _model.state.angle.eulerFromQuaternion(fusionQPose); - } - - void updatePoseFromAccelMag() - { - _model.state.pose = _model.state.accel.accelToEuler(); - //_model.state.accelPose = _model.state.pose; - - if(_model.magActive()) - { - // Quaternion q = _model.state.pose.eulerToQuaternion(); - // since Z is always 0, it can be optimized a bit - float cosX2 = cos(_model.state.pose.x / 2.0f); - float sinX2 = sin(_model.state.pose.x / 2.0f); - float cosY2 = cos(_model.state.pose.y / 2.0f); - float sinY2 = sin(_model.state.pose.y / 2.0f); - - Quaternion q; - q.w = cosX2 * cosY2; - q.x = sinX2 * cosY2; - q.y = cosX2 * sinY2; - q.z = -sinX2 * sinY2; - - VectorFloat m = _model.state.mag.getRotated(q); - _model.state.magPose = m; - _model.state.pose.z = -atan2(m.y, m.x); - } - else - { - _model.state.pose.z = _model.state.angle.z; - } - _model.state.poseQ = _model.state.pose.eulerToQuaternion(); - _model.state.pose.eulerFromQuaternion(_model.state.poseQ); - - //return; - // check for quaternion aliasing. If the quaternion has the wrong sign - // the kalman filter will be very unhappy. - int maxIndex = -1; - float maxVal = -1000; - - for(int i = 0; i < 4; i++) { - if(fabs(_model.state.poseQ.get(i)) > maxVal) { - maxVal = fabs(_model.state.poseQ.get(i)); - maxIndex = i; - } - } - - // if the biggest component has a different sign in the measured and kalman poses, - // change the sign of the measured pose to match. - if(((_model.state.poseQ.get(maxIndex) < 0) && (_model.state.angleQ.get(maxIndex) > 0)) || - ((_model.state.poseQ.get(maxIndex) > 0) && (_model.state.angleQ.get(maxIndex) < 0))) { - _model.state.poseQ.w = -_model.state.poseQ.w; - _model.state.poseQ.x = -_model.state.poseQ.x; - _model.state.poseQ.y = -_model.state.poseQ.y; - _model.state.poseQ.z = -_model.state.poseQ.z; - _model.state.pose.eulerFromQuaternion(_model.state.poseQ); - } - } + Fusion(Model& model); + int begin(); + void restoreGain(); + int update(); + + void experimentalFusion(); + void simpleFusion(); + void kalmanFusion(); + void complementaryFusion(); + void complementaryFusionOld(); + void rtqfFusion(); + void updatePoseFromAccelMag(); // experimental - void lerpFusion() - { - float correctionAlpha = 0.001f; // 0 - 1 => gyro - accel - - _model.state.accelPose = _model.state.accel.accelToEuler(); - _model.state.accelPoseQ = _model.state.accelPose.eulerToQuaternion(); - - if(_model.magActive()) - { - // use yaw from mag - VectorFloat mag = _model.state.mag.getRotated(_model.state.accelPoseQ); - _model.state.accelPose.z = -atan2(mag.y, mag.x); - } - else - { - _model.state.accelPose.z = _model.state.gyroPose.z; - } - _model.state.accelPoseQ = _model.state.accelPose.eulerToQuaternion(); - - //_model.state.accelPose.eulerFromQuaternion(_model.state.accelPoseQ); - - // predict new state - Quaternion rotation = (_model.state.gyro * _model.state.gyroTimer.intervalf).eulerToQuaternion(); - _model.state.gyroPoseQ = (_model.state.gyroPoseQ * rotation).getNormalized(); - - // drift compensation - _model.state.gyroPoseQ = Quaternion::lerp(_model.state.gyroPoseQ, _model.state.accelPoseQ, correctionAlpha); - - // calculate euler vectors for accel and position - _model.state.gyroPose.eulerFromQuaternion(_model.state.gyroPoseQ); - } - - void madgwickFusion() - { - if(false && _model.magActive()) - { - _madgwick.update( - _model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z, - _model.state.accel.x, _model.state.accel.y, _model.state.accel.z, - _model.state.mag.x, _model.state.mag.y, _model.state.mag.z - ); - } - else - { - _madgwick.update( - _model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z, - _model.state.accel.x, _model.state.accel.y, _model.state.accel.z - ); - } - _model.state.angleQ = _madgwick.getQuaternion(); - _model.state.angle = _madgwick.getEuler(); - } - - void madgwickFusion1() - { - // prediction phase - _madgwick.update( - _model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z, - 0.f, 0.f, 0.f - ); - _model.state.angleQ = _madgwick.getQuaternion(); - _model.state.angle = _madgwick.getEuler(); - } - - void madgwickFusion2() - { - //float mag = _model.state.accel.getMagnitude(); - //_model.state.debug[0] = lrintf(mag * 1000); - //if(mag > 1.5f || mag < 0.75f) return; // skip at high overloads or weightlessness - - // correction phase - _madgwick.update( - 0.f, 0.f, 0.f, - _model.state.accel.x, _model.state.accel.y, _model.state.accel.z - ); - } - - void mahonyFusion() - { - if(false && _model.magActive()) - { - _mahony.update( - _model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z, - _model.state.accel.x, _model.state.accel.y, _model.state.accel.z, - _model.state.mag.x, _model.state.mag.y, _model.state.mag.z - ); - } - else - { - _mahony.update( - _model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z, - _model.state.accel.x, _model.state.accel.y, _model.state.accel.z - ); - } - _model.state.angleQ = _mahony.getQuaternion(); - _model.state.angle = _mahony.getEuler(); - } - - void mahonyFusion1() - { - // prediction phase - _mahony.update( - _model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z, - 0.f, 0.f, 0.f - ); - _model.state.angleQ = _mahony.getQuaternion(); - _model.state.angle = _mahony.getEuler(); - } - - void mahonyFusion2() - { - //float mag = _model.state.accel.getMagnitude(); - //_model.state.debug[0] = lrintf(mag * 1000); - //if(mag > 1.5f || mag < 0.75f) return; // skip at high overloads or weightlessness - - // correction phase - _mahony.update( - 0.f, 0.f, 0.f, - _model.state.accel.x, _model.state.accel.y, _model.state.accel.z - ); - } + void lerpFusion(); + void madgwickFusion(); + void mahonyFusion(); private: Model& _model; @@ -405,5 +35,3 @@ class Fusion }; } - -#endif diff --git a/lib/Espfc/src/Hal/Gpio.cpp b/lib/Espfc/src/Hal/Gpio.cpp new file mode 100644 index 00000000..26bba426 --- /dev/null +++ b/lib/Espfc/src/Hal/Gpio.cpp @@ -0,0 +1,64 @@ +#include +#include "Gpio.h" +#include "Utils/MemoryHelper.h" +#if defined(ESP32) +#include "hal/gpio_ll.h" +#endif + +namespace Espfc { + +namespace Hal { + +void FAST_CODE_ATTR Gpio::digitalWrite(uint8_t pin, pin_status_t val) +{ +#if defined(ESP8266) + if (pin < 16) + { + if (val) + GPOS = (1 << pin); + else + GPOC = (1 << pin); + } + else if (pin == 16) + { + if (val) + GP16O |= 1; + else + GP16O &= ~1; + } +#elif defined(ESP32) + //::gpio_set_level((gpio_num_t)pin, val); + gpio_ll_set_level(&GPIO, (gpio_num_t)pin, val); +#elif defined(UNIT_TEST) + // do nothing +#else + ::digitalWrite(pin, val); +#endif +} + +pin_status_t FAST_CODE_ATTR Gpio::digitalRead(uint8_t pin) +{ +#if defined(ESP8266) + if (pin < 16) + { + return GPIP(pin); + } + else if (pin == 16) + { + return GP16I & 0x01; + } + return 0; +#elif defined(ESP32) + // return ::gpio_get_level((gpio_num_t)pin); + return ::gpio_ll_get_level(&GPIO, (gpio_num_t)pin); +#elif defined(UNIT_TEST) + return 0; + // do nothing +#else + return ::digitalRead(pin); +#endif +} + +} + +} diff --git a/lib/Espfc/src/Hal/Gpio.h b/lib/Espfc/src/Hal/Gpio.h new file mode 100644 index 00000000..5f24b992 --- /dev/null +++ b/lib/Espfc/src/Hal/Gpio.h @@ -0,0 +1,24 @@ +#pragma once + +#include + +#if defined(ARCH_RP2040) +typedef PinStatus pin_status_t; +#else +typedef uint8_t pin_status_t; +#endif + +namespace Espfc { + +namespace Hal { + +class Gpio +{ + public: + static void digitalWrite(uint8_t pin, pin_status_t val); + static pin_status_t digitalRead(uint8_t pin); +}; + +} + +} diff --git a/lib/Hal/src/Hal.h b/lib/Espfc/src/Hal/Pgm.h similarity index 73% rename from lib/Hal/src/Hal.h rename to lib/Espfc/src/Hal/Pgm.h index 8a1620f4..624a3acf 100644 --- a/lib/Hal/src/Hal.h +++ b/lib/Espfc/src/Hal/Pgm.h @@ -1,5 +1,4 @@ -#ifndef _HAL_H_ -#define _HAL_H_ +#pragma once #ifdef UNIT_TEST @@ -7,6 +6,10 @@ #define PSTR(s) (s) #endif +#ifndef FPSTR +#define FPSTR(s) (s) +#endif + #ifndef F #define F(s) (s) #endif @@ -16,7 +19,6 @@ #endif // UNIT_TEST -#undef max -#undef min +//#undef max +//#undef min -#endif // _HAL_H_ diff --git a/lib/Espfc/src/Hardware.h b/lib/Espfc/src/Hardware.h index 116e107b..3965123d 100644 --- a/lib/Espfc/src/Hardware.h +++ b/lib/Espfc/src/Hardware.h @@ -2,8 +2,12 @@ #define _ESPFC_HARDWARE_H_ #include -#include "Target/Target.h" #include "Model.h" +#if defined(ESPFC_WIFI_ALT) +#include +#elif defined(ESPFC_WIFI) +#include +#endif #include "Device/BusDevice.h" #if defined(ESPFC_I2C_0) #include "Device/BusI2C.h" @@ -89,7 +93,7 @@ class Hardware { if(_model.config.gyroDev == GYRO_NONE) return; - Espfc::Device::GyroDevice * detectedGyro = nullptr; + Device::GyroDevice * detectedGyro = nullptr; #if defined(ESPFC_SPI_0) if(_model.config.pin[PIN_SPI_CS0] != -1) { @@ -128,7 +132,7 @@ class Hardware { if(_model.config.magDev == MAG_NONE) return; - Espfc::Device::MagDevice * detectedMag = nullptr; + Device::MagDevice * detectedMag = nullptr; #if defined(ESPFC_I2C_0) if(_model.config.pin[PIN_I2C_0_SDA] != -1 && _model.config.pin[PIN_I2C_0_SCL] != -1) { @@ -150,7 +154,7 @@ class Hardware { if(_model.config.baroDev == BARO_NONE) return; - Espfc::Device::BaroDevice * detectedBaro = nullptr; + Device::BaroDevice * detectedBaro = nullptr; #if defined(ESPFC_SPI_0) if(_model.config.pin[PIN_SPI_CS1] != -1) { @@ -211,6 +215,11 @@ class Hardware { if(model.state.escMotor) model.state.escMotor->end(); if(model.state.escServo) model.state.escServo->end(); +#ifdef ESPFC_SERIAL_SOFT_0_WIFI + WiFi.disconnect(); + WiFi.softAPdisconnect(); +#endif + delay(100); targetReset(); } diff --git a/lib/Espfc/src/Input.cpp b/lib/Espfc/src/Input.cpp new file mode 100644 index 00000000..a1744894 --- /dev/null +++ b/lib/Espfc/src/Input.cpp @@ -0,0 +1,372 @@ + +#include "Input.h" +#include "Math/Utils.h" +#include "Utils/MemoryHelper.h" + +namespace Espfc { + +Input::Input(Model& model): _model(model) {} + +int Input::begin() +{ + _device = getInputDevice(); + _model.state.inputChannelCount = _device ? _device->getChannelCount() : INPUT_CHANNELS; + _model.state.inputFrameDelta = FRAME_TIME_DEFAULT_US; + _model.state.inputFrameRate = 1000000ul / _model.state.inputFrameDelta; + _model.state.inputFrameCount = 0; + _model.state.inputAutoFactor = 1.f / (2.f + _model.config.input.filterAutoFactor * 0.1f); + switch(_model.config.input.interpolationMode) + { + case INPUT_INTERPOLATION_AUTO: + _model.state.inputInterpolationDelta = Math::clamp(_model.state.inputFrameDelta, (uint32_t)4000, (uint32_t)40000) * 0.000001f; // estimate real interval + break; + case INPUT_INTERPOLATION_MANUAL: + _model.state.inputInterpolationDelta = _model.config.input.interpolationInterval * 0.001f; // manual interval + break; + case INPUT_INTERPOLATION_DEFAULT: + case INPUT_INTERPOLATION_OFF: + default: + _model.state.inputInterpolationDelta = FRAME_TIME_DEFAULT_US * 0.000001f; + break; + } + _model.state.inputInterpolationStep = _model.state.loopTimer.intervalf / _model.state.inputInterpolationDelta; + _step = 0.0f; + for(size_t c = 0; c < INPUT_CHANNELS; ++c) + { + if(_device) _filter[c].begin(FilterConfig(_device->needAverage() ? FILTER_FIR2 : FILTER_NONE, 1), _model.state.loopTimer.rate); + int16_t v = c == AXIS_THRUST ? PWM_RANGE_MIN : PWM_RANGE_MID; + _model.state.inputRaw[c] = v; + _model.state.inputBuffer[c] = v; + _model.state.inputBufferPrevious[c] = v; + setInput((Axis)c, v, true, true); + } + return 1; +} + +int16_t FAST_CODE_ATTR Input::getFailsafeValue(uint8_t c) +{ + const InputChannelConfig& ich = _model.config.input.channel[c]; + switch(ich.fsMode) + { + case FAILSAFE_MODE_AUTO: + return c == AXIS_THRUST ? PWM_RANGE_MIN : PWM_RANGE_MID; + case FAILSAFE_MODE_SET: + return ich.fsValue; + case FAILSAFE_MODE_INVALID: + case FAILSAFE_MODE_HOLD: + default: + return _model.state.inputBuffer[c]; + } +} + +void FAST_CODE_ATTR Input::setInput(Axis i, float v, bool newFrame, bool noFilter) +{ + const InputChannelConfig& ich = _model.config.input.channel[i]; + if(i <= AXIS_THRUST) + { + const float nv = noFilter ? v : _model.state.inputFilter[i].update(v); + _model.state.inputUs[i] = nv; + _model.state.input[i] = Math::map(nv, ich.min, ich.max, -1.f, 1.f); + } + else if(newFrame) + { + _model.state.inputUs[i] = v; + _model.state.input[i] = Math::map(v, ich.min, ich.max, -1.f, 1.f); + } +} + +int FAST_CODE_ATTR Input::update() +{ + if(!_device) return 0; + + uint32_t startTime = micros(); + + InputStatus status = readInputs(); + + if(!failsafe(status)) + { + filterInputs(status); + } + + if(_model.config.debugMode == DEBUG_PIDLOOP) + { + _model.state.debug[1] = micros() - startTime; + } + + return 1; +} + +InputStatus FAST_CODE_ATTR Input::readInputs() +{ + Stats::Measure readMeasure(_model.state.stats, COUNTER_INPUT_READ); + uint32_t startTime = micros(); + + InputStatus status = _device->update(); + + if(_model.config.debugMode == DEBUG_RX_TIMING) + { + _model.state.debug[0] = micros() - startTime; + } + + if(status == INPUT_IDLE) return status; + + _model.state.inputRxLoss = (status == INPUT_LOST || status == INPUT_FAILSAFE); + _model.state.inputRxFailSafe = (status == INPUT_FAILSAFE); + _model.state.inputFrameCount++; + + updateFrameRate(); + + processInputs(); + + if(_model.config.debugMode == DEBUG_RX_SIGNAL_LOSS) + { + _model.state.debug[0] = !_model.state.inputRxLoss; + _model.state.debug[1] = _model.state.inputRxFailSafe; + _model.state.debug[2] = _model.state.inputChannelsValid; + _model.state.debug[3] = _model.state.inputLossTime / (100 * 1000); + } + + return status; +} + +void FAST_CODE_ATTR Input::processInputs() +{ + if(_model.state.inputFrameCount < 5) return; // ignore few first frames that might be garbage + + uint32_t startTime = micros(); + + uint16_t channels[INPUT_CHANNELS]; + _device->get(channels, _model.state.inputChannelCount); + + _model.state.inputChannelsValid = true; + for(size_t c = 0; c < _model.state.inputChannelCount; c++) + { + const InputChannelConfig& ich = _model.config.input.channel[c]; + + // remap channels + int16_t v = _model.state.inputRaw[c] = (int16_t)channels[ich.map]; + + // adj midrc + v -= _model.config.input.midRc - PWM_RANGE_MID; + + // adj range + //float t = Math::map3((float)v, (float)ich.min, (float)ich.neutral, (float)ich.max, (float)PWM_RANGE_MIN, (float)PWM_RANGE_MID, (float)PWM_RANGE_MAX); + float t = Math::mapi(v, ich.min, ich.max, PWM_RANGE_MIN, PWM_RANGE_MAX); + + // filter if required + t = _filter[c].update(t); + v = lrintf(t); + + // apply deadband + if(c < AXIS_THRUST) + { + v = Math::deadband(v - PWM_RANGE_MID, (int)_model.config.input.deadband) + PWM_RANGE_MID; + } + + // check if inputs are valid, apply failsafe value otherwise + if(v < _model.config.input.minRc || v > _model.config.input.maxRc) + { + v = getFailsafeValue(c); + if(c <= AXIS_THRUST) _model.state.inputChannelsValid = false; + } + + // update input buffer + _model.state.inputBufferPrevious[c] = _model.state.inputBuffer[c]; + _model.state.inputBuffer[c] = v; + } + + if(_model.config.debugMode == DEBUG_RX_TIMING) + { + _model.state.debug[2] = micros() - startTime; + } +} + +bool FAST_CODE_ATTR Input::failsafe(InputStatus status) +{ + Stats::Measure readMeasure(_model.state.stats, COUNTER_FAILSAFE); + + if(_model.isSwitchActive(MODE_FAILSAFE)) + { + failsafeStage2(); + return false; // not real failsafe, rx link is still valid + } + + if(status == INPUT_RECEIVED) + { + failsafeIdle(); + return false; + } + + if(status == INPUT_FAILSAFE) + { + failsafeStage2(); + return true; + } + + // stage 2 timeout + _model.state.inputLossTime = micros() - _model.state.inputFrameTime; + if(_model.state.inputLossTime > Math::clamp((uint32_t)_model.config.failsafe.delay, (uint32_t)2u, (uint32_t)200u) * TENTH_TO_US) + { + failsafeStage2(); + return true; + } + + // stage 1 timeout (100ms) + if(_model.state.inputLossTime >= 2 * TENTH_TO_US) + { + failsafeStage1(); + return true; + } + + return false; +} + +void FAST_CODE_ATTR Input::failsafeIdle() +{ + _model.state.failsafe.phase = FC_FAILSAFE_IDLE; + _model.state.inputLossTime = 0; +} + +void FAST_CODE_ATTR Input::failsafeStage1() +{ + _model.state.failsafe.phase = FC_FAILSAFE_RX_LOSS_DETECTED; + _model.state.inputRxLoss = true; + for(size_t i = 0; i < _model.state.inputChannelCount; i++) + { + setInput((Axis)i, getFailsafeValue(i), true, true); + } +} + +void FAST_CODE_ATTR Input::failsafeStage2() +{ + _model.state.failsafe.phase = FC_FAILSAFE_RX_LOSS_DETECTED; + _model.state.inputRxLoss = true; + _model.state.inputRxFailSafe = true; + if(_model.isModeActive(MODE_ARMED)) + { + _model.state.failsafe.phase = FC_FAILSAFE_LANDED; + _model.disarm(DISARM_REASON_FAILSAFE); + } +} + +void FAST_CODE_ATTR Input::filterInputs(InputStatus status) +{ + Stats::Measure filterMeasure(_model.state.stats, COUNTER_INPUT_FILTER); + uint32_t startTime = micros(); + + const bool newFrame = status != INPUT_IDLE; + const bool interpolation = _model.config.input.interpolationMode != INPUT_INTERPOLATION_OFF && _model.config.input.filterType == INPUT_INTERPOLATION; + + if(interpolation) + { + if(newFrame) + { + _step = 0.0f; + } + if(_step < 1.f) + { + _step += _model.state.inputInterpolationStep; + } + } + + for(size_t c = 0; c < _model.state.inputChannelCount; c++) + { + float v = _model.state.inputBuffer[c]; + if(c <= AXIS_THRUST) + { + v = interpolation ? _interpolate(_model.state.inputBufferPrevious[c], v, _step) : v; + } + setInput((Axis)c, v, newFrame); + } + + if(_model.config.debugMode == DEBUG_RX_TIMING) + { + _model.state.debug[3] = micros() - startTime; + } +} + +void FAST_CODE_ATTR Input::updateFrameRate() +{ + const uint32_t now = micros(); + const uint32_t frameDelta = now - _model.state.inputFrameTime; + + _model.state.inputFrameTime = now; + _model.state.inputFrameDelta += (((int)frameDelta - (int)_model.state.inputFrameDelta) >> 3); // avg * 0.125 + _model.state.inputFrameRate = 1000000ul / _model.state.inputFrameDelta; + + if (_model.config.input.interpolationMode == INPUT_INTERPOLATION_AUTO && _model.config.input.filterType == INPUT_INTERPOLATION) + { + _model.state.inputInterpolationDelta = Math::clamp(_model.state.inputFrameDelta, (uint32_t)4000, (uint32_t)40000) * 0.000001f; // estimate real interval + _model.state.inputInterpolationStep = _model.state.loopTimer.intervalf / _model.state.inputInterpolationDelta; + } + + if(_model.config.debugMode == DEBUG_RC_SMOOTHING_RATE) + { + _model.state.debug[0] = _model.state.inputFrameDelta / 10; + _model.state.debug[1] = _model.state.inputFrameRate; + } + + // auto cutoff input freq + float freq = std::max(_model.state.inputFrameRate * _model.state.inputAutoFactor, 15.f); // no lower than 15Hz + if(freq > _model.state.inputAutoFreq * 1.1f || freq < _model.state.inputAutoFreq * 0.9f) + { + _model.state.inputAutoFreq += 0.25f * (freq - _model.state.inputAutoFreq); + if(_model.config.debugMode == DEBUG_RC_SMOOTHING_RATE) + { + _model.state.debug[2] = lrintf(freq); + _model.state.debug[3] = lrintf(_model.state.inputAutoFreq); + } + FilterConfig conf((FilterType)_model.config.input.filter.type, _model.state.inputAutoFreq); + FilterConfig confDerivative((FilterType)_model.config.input.filterDerivative.type, _model.state.inputAutoFreq); + for(size_t i = 0; i <= AXIS_THRUST; i++) + { + if(_model.config.input.filter.freq == 0) + { + _model.state.inputFilter[i].reconfigure(conf, _model.state.loopTimer.rate); + } + if(_model.config.input.filterDerivative.freq == 0) + { + _model.state.innerPid[i].ftermFilter.reconfigure(confDerivative, _model.state.loopTimer.rate); + } + } + } + + if(_model.config.debugMode == DEBUG_RX_TIMING) + { + _model.state.debug[1] = micros() - now; + } +} + +Device::InputDevice * Input::getInputDevice() +{ + Device::SerialDevice * serial = _model.getSerialStream(SERIAL_FUNCTION_RX_SERIAL); + if(serial && _model.isFeatureActive(FEATURE_RX_SERIAL) && _model.config.input.serialRxProvider == SERIALRX_SBUS) + { + _sbus.begin(serial); + _model.logger.info().logln(F("RX SBUS")); + return &_sbus; + } + if(serial && _model.isFeatureActive(FEATURE_RX_SERIAL) && _model.config.input.serialRxProvider == SERIALRX_CRSF) + { + _crsf.begin(serial); + _model.logger.info().logln(F("RX CRSF")); + return &_crsf; + } + else if(_model.isFeatureActive(FEATURE_RX_PPM) && _model.config.pin[PIN_INPUT_RX] != -1) + { + _ppm.begin(_model.config.pin[PIN_INPUT_RX], _model.config.input.ppmMode); + _model.logger.info().log(F("RX PPM")).log(_model.config.pin[PIN_INPUT_RX]).logln(_model.config.input.ppmMode); + return &_ppm; + } +#if defined(ESPFC_ESPNOW) + else if(_model.isFeatureActive(FEATURE_RX_SPI)) + { + int status = _espnow.begin(); + _model.logger.info().log(F("RX ESPNOW")).logln(status); + return &_espnow; + } +#endif + return nullptr; +} + +} diff --git a/lib/Espfc/src/Input.h b/lib/Espfc/src/Input.h index 82e25082..0a10d58c 100644 --- a/lib/Espfc/src/Input.h +++ b/lib/Espfc/src/Input.h @@ -1,5 +1,4 @@ -#ifndef _ESPFC_INPUT_H_ -#define _ESPFC_INPUT_H_ +#pragma once #include "Model.h" #include "Math/Utils.h" @@ -7,6 +6,9 @@ #include "Device/InputPPM.h" #include "Device/InputSBUS.h" #include "Device/InputCRSF.h" +#if defined(ESPFC_ESPNOW) +#include "Device/InputEspNow.h" +#endif namespace Espfc { @@ -26,363 +28,28 @@ enum InputPwmRange { class Input { public: - Input(Model& model): _model(model) {} - - int begin() - { - _device = getInputDevice(); - _model.state.inputChannelCount = _device ? _device->getChannelCount() : INPUT_CHANNELS; - _model.state.inputFrameDelta = FRAME_TIME_DEFAULT_US; - _model.state.inputFrameRate = 1000000ul / _model.state.inputFrameDelta; - _model.state.inputFrameCount = 0; - _model.state.inputAutoFactor = 1.f / (2.f + _model.config.input.filterAutoFactor * 0.1f); - switch(_model.config.input.interpolationMode) - { - case INPUT_INTERPOLATION_AUTO: - _model.state.inputInterpolationDelta = Math::clamp(_model.state.inputFrameDelta, (uint32_t)4000, (uint32_t)40000) * 0.000001f; // estimate real interval - break; - case INPUT_INTERPOLATION_MANUAL: - _model.state.inputInterpolationDelta = _model.config.input.interpolationInterval * 0.001f; // manual interval - break; - case INPUT_INTERPOLATION_DEFAULT: - case INPUT_INTERPOLATION_OFF: - default: - _model.state.inputInterpolationDelta = FRAME_TIME_DEFAULT_US * 0.000001f; - break; - } - _model.state.inputInterpolationStep = _model.state.loopTimer.intervalf / _model.state.inputInterpolationDelta; - _step = 0.0f; - for(size_t c = 0; c < INPUT_CHANNELS; ++c) - { - if(_device) _filter[c].begin(FilterConfig(_device->needAverage() ? FILTER_FIR2 : FILTER_NONE, 1), _model.state.loopTimer.rate); - int16_t v = c == AXIS_THRUST ? PWM_RANGE_MIN : PWM_RANGE_MID; - _model.state.inputRaw[c] = v; - _model.state.inputBuffer[c] = v; - _model.state.inputBufferPrevious[c] = v; - setInput((Axis)c, v, true, true); - } - return 1; - } - - int16_t getFailsafeValue(uint8_t c) - { - const InputChannelConfig& ich = _model.config.input.channel[c]; - switch(ich.fsMode) - { - case FAILSAFE_MODE_AUTO: - return c == AXIS_THRUST ? PWM_RANGE_MIN : PWM_RANGE_MID; - case FAILSAFE_MODE_SET: - return ich.fsValue; - case FAILSAFE_MODE_INVALID: - case FAILSAFE_MODE_HOLD: - default: - return _model.state.inputBuffer[c]; - } - } - - void setInput(Axis i, float v, bool newFrame, bool noDelta = false) - { - const InputChannelConfig& ich = _model.config.input.channel[i]; - if(i <= AXIS_THRUST) - { - v = noDelta ? v : _model.state.inputFilter[i].update(v); - _model.state.inputUs[i] = v; - _model.state.input[i] = Math::map(v, ich.min, ich.max, -1.f, 1.f); - } - else if(newFrame) - { - _model.state.inputUs[i] = v; - _model.state.input[i] = Math::map(v, ich.min, ich.max, -1.f, 1.f); - } - } - - int update() - { - if(!_device) return 0; - - uint32_t startTime = micros(); - - InputStatus status = readInputs(); - - if(!failsafe(status)) - { - filterInputs(status); - } - - if(_model.config.debugMode == DEBUG_PIDLOOP) - { - _model.state.debug[1] = micros() - startTime; - } - - return 1; - } - - InputStatus readInputs() - { - Stats::Measure readMeasure(_model.state.stats, COUNTER_INPUT_READ); - uint32_t startTime = micros(); + Input(Model& model); - InputStatus status = _device->update(); + int begin(); + int update(); - if(_model.config.debugMode == DEBUG_RX_TIMING) - { - _model.state.debug[0] = micros() - startTime; - } + int16_t getFailsafeValue(uint8_t c); + void setInput(Axis i, float v, bool newFrame, bool noFilter = false); - if(status == INPUT_IDLE) return status; + InputStatus readInputs(); + void processInputs(); - _model.state.inputRxLoss = (status == INPUT_LOST || status == INPUT_FAILSAFE); - _model.state.inputRxFailSafe = (status == INPUT_FAILSAFE); - _model.state.inputFrameCount++; + bool failsafe(InputStatus status); + void failsafeIdle(); + void failsafeStage1(); + void failsafeStage2(); + void filterInputs(InputStatus status); - updateFrameRate(); - - processInputs(); - - if(_model.config.debugMode == DEBUG_RX_SIGNAL_LOSS) - { - _model.state.debug[0] = !_model.state.inputRxLoss; - _model.state.debug[1] = _model.state.inputRxFailSafe; - _model.state.debug[2] = _model.state.inputChannelsValid; - _model.state.debug[3] = _model.state.inputRaw[AXIS_THRUST]; - } - - return status; - } - - void processInputs() - { - if(_model.state.inputFrameCount < 5) return; // ignore few first frames that might be garbage - - uint32_t startTime = micros(); - - uint16_t channels[INPUT_CHANNELS]; - _device->get(channels, _model.state.inputChannelCount); - - _model.state.inputChannelsValid = true; - for(size_t c = 0; c < _model.state.inputChannelCount; c++) - { - const InputChannelConfig& ich = _model.config.input.channel[c]; - - // remap channels - int16_t v = _model.state.inputRaw[c] = (int16_t)channels[ich.map]; - - // adj midrc - v -= _model.config.input.midRc - PWM_RANGE_MID; - - // adj range - //float t = Math::map3((float)v, (float)ich.min, (float)ich.neutral, (float)ich.max, (float)PWM_RANGE_MIN, (float)PWM_RANGE_MID, (float)PWM_RANGE_MAX); - float t = Math::mapi(v, ich.min, ich.max, PWM_RANGE_MIN, PWM_RANGE_MAX); - - // filter if required - t = _filter[c].update(t); - v = lrintf(t); - - // apply deadband - if(c < AXIS_THRUST) - { - v = Math::deadband(v - PWM_RANGE_MID, (int)_model.config.input.deadband) + PWM_RANGE_MID; - } - - // check if inputs are valid, apply failsafe value otherwise - if(v < _model.config.input.minRc || v > _model.config.input.maxRc) - { - v = getFailsafeValue(c); - if(c <= AXIS_THRUST) _model.state.inputChannelsValid = false; - } - - // update input buffer - _model.state.inputBufferPrevious[c] = _model.state.inputBuffer[c]; - _model.state.inputBuffer[c] = v; - } - - if(_model.config.debugMode == DEBUG_RX_TIMING) - { - _model.state.debug[2] = micros() - startTime; - } - } - - bool failsafe(InputStatus status) - { - Stats::Measure readMeasure(_model.state.stats, COUNTER_FAILSAFE); - - if(_model.isSwitchActive(MODE_FAILSAFE)) - { - failsafeStage2(); - return false; // not real failsafe, rx link is still valid - } - - if(status == INPUT_RECEIVED) - { - failsafeIdle(); - return false; - } - - if(status == INPUT_FAILSAFE) - { - failsafeStage2(); - return true; - } - - // stage 2 timeout - const uint32_t lossTime = micros() - _model.state.inputFrameTime; - if(lossTime >= Math::clamp((uint32_t)_model.config.failsafe.delay, (uint32_t)1u, (uint32_t)200u) * TENTH_TO_US) - { - failsafeStage2(); - return true; - } - - // stage 1 timeout - if(lossTime >= 1 * TENTH_TO_US) - { - failsafeStage1(); - return true; - } - - return false; - } - - void failsafeIdle() - { - _model.state.failsafe.phase = FC_FAILSAFE_IDLE; - } - - void failsafeStage1() - { - _model.state.failsafe.phase = FC_FAILSAFE_RX_LOSS_DETECTED; - _model.state.inputRxLoss = true; - for(size_t i = 0; i < _model.state.inputChannelCount; i++) - { - setInput((Axis)i, getFailsafeValue(i), true, true); - } - } - - void failsafeStage2() - { - _model.state.failsafe.phase = FC_FAILSAFE_RX_LOSS_DETECTED; - _model.state.inputRxLoss = true; - _model.state.inputRxFailSafe = true; - if(_model.isModeActive(MODE_ARMED)) - { - _model.state.failsafe.phase = FC_FAILSAFE_LANDED; - _model.disarm(DISARM_REASON_FAILSAFE); - } - } - - void filterInputs(InputStatus status) - { - Stats::Measure filterMeasure(_model.state.stats, COUNTER_INPUT_FILTER); - uint32_t startTime = micros(); - - const bool newFrame = status != INPUT_IDLE; - const bool interpolation = _model.config.input.interpolationMode != INPUT_INTERPOLATION_OFF && _model.config.input.filterType == INPUT_INTERPOLATION; - - if(interpolation) - { - if(newFrame) - { - _step = 0.0f; - } - if(_step < 1.f) - { - _step += _model.state.inputInterpolationStep; - } - } - - for(size_t c = 0; c < _model.state.inputChannelCount; c++) - { - float v = _model.state.inputBuffer[c]; - if(c <= AXIS_THRUST) - { - v = interpolation ? _interpolate(_model.state.inputBufferPrevious[c], v, _step) : v; - } - setInput((Axis)c, v, newFrame); - } - - if(_model.config.debugMode == DEBUG_RX_TIMING) - { - _model.state.debug[3] = micros() - startTime; - } - } - - void updateFrameRate() - { - const uint32_t now = micros(); - const uint32_t frameDelta = now - _model.state.inputFrameTime; - - _model.state.inputFrameTime = now; - _model.state.inputFrameDelta += (((int)frameDelta - (int)_model.state.inputFrameDelta) >> 3); // avg * 0.125 - _model.state.inputFrameRate = 1000000ul / _model.state.inputFrameDelta; - - if (_model.config.input.interpolationMode == INPUT_INTERPOLATION_AUTO && _model.config.input.filterType == INPUT_INTERPOLATION) - { - _model.state.inputInterpolationDelta = Math::clamp(_model.state.inputFrameDelta, (uint32_t)4000, (uint32_t)40000) * 0.000001f; // estimate real interval - _model.state.inputInterpolationStep = _model.state.loopTimer.intervalf / _model.state.inputInterpolationDelta; - } - - if(_model.config.debugMode == DEBUG_RC_SMOOTHING_RATE) - { - _model.state.debug[0] = _model.state.inputFrameDelta / 10; - _model.state.debug[1] = _model.state.inputFrameRate; - } - - // auto cutoff input freq - float freq = std::max(_model.state.inputFrameRate * _model.state.inputAutoFactor, 15.f); // no lower than 15Hz - if(freq > _model.state.inputAutoFreq * 1.1f || freq < _model.state.inputAutoFreq * 0.9f) - { - _model.state.inputAutoFreq += 0.25f * (freq - _model.state.inputAutoFreq); - if(_model.config.debugMode == DEBUG_RC_SMOOTHING_RATE) - { - _model.state.debug[2] = lrintf(freq); - _model.state.debug[3] = lrintf(_model.state.inputAutoFreq); - } - FilterConfig conf((FilterType)_model.config.input.filter.type, _model.state.inputAutoFreq); - FilterConfig confDerivative((FilterType)_model.config.input.filterDerivative.type, _model.state.inputAutoFreq); - for(size_t i = 0; i <= AXIS_THRUST; i++) - { - if(_model.config.input.filter.freq == 0) - { - _model.state.inputFilter[i].reconfigure(conf, _model.state.loopTimer.rate); - } - if(_model.config.input.filterDerivative.freq == 0) - { - _model.state.innerPid[i].ftermFilter.reconfigure(confDerivative, _model.state.loopTimer.rate); - } - } - } - - if(_model.config.debugMode == DEBUG_RX_TIMING) - { - _model.state.debug[1] = micros() - now; - } - } - - Device::InputDevice * getInputDevice() - { - Device::SerialDevice * serial = _model.getSerialStream(SERIAL_FUNCTION_RX_SERIAL); - if(serial && _model.isActive(FEATURE_RX_SERIAL) && _model.config.input.serialRxProvider == SERIALRX_SBUS) - { - _sbus.begin(serial); - _model.logger.info().logln(F("RX SBUS")); - return &_sbus; - } - if(serial && _model.isActive(FEATURE_RX_SERIAL) && _model.config.input.serialRxProvider == SERIALRX_CRSF) - { - _crsf.begin(serial); - _model.logger.info().logln(F("RX CRSF")); - return &_crsf; - } - else if(_model.isActive(FEATURE_RX_PPM) && _model.config.pin[PIN_INPUT_RX] != -1) - { - _ppm.begin(_model.config.pin[PIN_INPUT_RX], _model.config.input.ppmMode); - _model.logger.info().log(F("RX PPM")).log(_model.config.pin[PIN_INPUT_RX]).logln(_model.config.input.ppmMode); - return &_ppm; - } - return nullptr; - } + void updateFrameRate(); + Device::InputDevice * getInputDevice(); private: - float _interpolate(float left, float right, float step) + inline float _interpolate(float left, float right, float step) { return (left * (1.f - step) + right * step); } @@ -394,11 +61,12 @@ class Input Device::InputPPM _ppm; Device::InputSBUS _sbus; Device::InputCRSF _crsf; +#if defined(ESPFC_ESPNOW) + Device::InputEspNow _espnow; +#endif static const uint32_t TENTH_TO_US = 100000UL; // 1_000_000 / 10; static const uint32_t FRAME_TIME_DEFAULT_US = 23000; // 23 ms }; } - -#endif diff --git a/lib/Espfc/src/Math/Crc.cpp b/lib/Espfc/src/Math/Crc.cpp new file mode 100644 index 00000000..5c8ced3c --- /dev/null +++ b/lib/Espfc/src/Math/Crc.cpp @@ -0,0 +1,50 @@ +#include "Crc.h" +#include "Utils/MemoryHelper.h" + +namespace Espfc { + +namespace Math { + +uint8_t FAST_CODE_ATTR crc8_dvb_s2(uint8_t crc, const uint8_t a) +{ + crc ^= a; + for (size_t i = 0; i < 8; ++i) + { + if (crc & 0x80) + { + crc = (crc << 1) ^ 0xD5; + } + else + { + crc = crc << 1; + } + } + return crc; +} + +uint8_t FAST_CODE_ATTR crc8_dvb_s2(uint8_t crc, const uint8_t *data, size_t len) +{ + while (len-- > 0) + { + crc = crc8_dvb_s2(crc, *data++); + } + return crc; +} + +uint8_t FAST_CODE_ATTR crc8_xor(uint8_t checksum, const uint8_t a) +{ + return checksum ^ a; +} + +uint8_t FAST_CODE_ATTR crc8_xor(uint8_t checksum, const uint8_t *data, int len) +{ + while (len-- > 0) + { + checksum = crc8_xor(checksum, *data++); + } + return checksum; +} + +} + +} \ No newline at end of file diff --git a/lib/Espfc/src/Math/Crc.h b/lib/Espfc/src/Math/Crc.h index 774ff684..809ce180 100644 --- a/lib/Espfc/src/Math/Crc.h +++ b/lib/Espfc/src/Math/Crc.h @@ -1,48 +1,17 @@ -#ifndef _ESPFC_MATH_CRC_H_ -#define _ESPFC_MATH_CRC_H_ +#pragma once + +#include +#include namespace Espfc { namespace Math { - uint8_t crc8_dvb_s2(uint8_t crc, const uint8_t a) - { - crc ^= a; - for (size_t i = 0; i < 8; ++i) - { - if (crc & 0x80) - crc = (crc << 1) ^ 0xD5; - else - crc = crc << 1; - } - return crc; - } - - uint8_t crc8_dvb_s2(uint8_t crc, const uint8_t *data, size_t len) - { - while (len-- > 0) - { - crc = crc8_dvb_s2(crc, *data++); - } - return crc; - } - - uint8_t crc8_xor(uint8_t checksum, const uint8_t a) - { - return checksum ^ a; - } - - uint8_t crc8_xor(uint8_t checksum, const uint8_t *data, int len) - { - while (len-- > 0) - { - checksum = crc8_xor(checksum, *data++); - } - return checksum; - } +uint8_t crc8_dvb_s2(uint8_t crc, const uint8_t a); +uint8_t crc8_dvb_s2(uint8_t crc, const uint8_t *data, size_t len); +uint8_t crc8_xor(uint8_t checksum, const uint8_t a); +uint8_t crc8_xor(uint8_t checksum, const uint8_t *data, int len); } } - -#endif \ No newline at end of file diff --git a/lib/Espfc/src/Math/Utils.h b/lib/Espfc/src/Math/Utils.h index 34e3ab43..ce9673cb 100644 --- a/lib/Espfc/src/Math/Utils.h +++ b/lib/Espfc/src/Math/Utils.h @@ -1,6 +1,8 @@ -#ifndef _ESPFC_MATH_UTILS_H_ -#define _ESPFC_MATH_UTILS_H_ +#pragma once +#include +#include +#include #include namespace Espfc { @@ -51,7 +53,7 @@ class Peak return value; } - int alignToClock(uint32_t clock, uint32_t maxFreq) + inline int alignToClock(uint32_t clock, uint32_t maxFreq) { uint32_t result = clock; uint32_t div = 1; @@ -62,6 +64,11 @@ class Peak return result; } + inline uint32_t alignAddressToWrite(uint32_t addr, size_t size, size_t alignment) + { + return ((addr + size) / alignment) * alignment; + } + constexpr float pi() { return 3.14159265358979f; @@ -87,12 +94,12 @@ class Peak return rad * (180.0f * invPi()); } - float toAltitude(float pressure, float seaLevelPressure = 101325.f) + inline float toAltitude(float pressure, float seaLevelPressure = 101325.f) { - return 44330.f * (1.f - powf(pressure / seaLevelPressure, 0.1903)); + return 44330.f * (1.f - std::pow(pressure / seaLevelPressure, 0.1903f)); } - void peakDetect(float * samples, size_t begin_bin, size_t end_bin, float bin_width, Peak * peaks, size_t peak_count) + inline void peakDetect(float * samples, size_t begin_bin, size_t end_bin, float bin_width, Peak * peaks, size_t peak_count) { for(size_t b = begin_bin; b <= end_bin; b++) { @@ -128,7 +135,7 @@ class Peak } // sort peaks by freq, move zero to end - void peakSort(Peak * peaks, size_t peak_count) + void inline peakSort(Peak * peaks, size_t peak_count) { std::sort(peaks, peaks + peak_count, [](const Peak& a, const Peak& b) -> bool { if (a.freq == 0.f) return false; @@ -139,5 +146,3 @@ class Peak } } - -#endif \ No newline at end of file diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 1de4d519..4dde78a7 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -3,11 +3,7 @@ #include #include - -#include #include -#include - #include "Debug_Espfc.h" #include "ModelConfig.h" #include "ModelState.h" @@ -107,7 +103,8 @@ class Model bool blackboxEnabled() const { - return config.blackboxDev == 3 && config.blackboxPdenom > 0; + // serial or flash + return (config.blackboxDev == 3 || config.blackboxDev == 1) && config.blackboxPdenom > 0; } bool gyroActive() const /* IRAM_ATTR */ @@ -154,7 +151,7 @@ class Model if(state.gyroCalibrationState == CALIBRATION_SAVE) { //save(); - state.buzzer.push(BEEPER_GYRO_CALIBRATED); + state.buzzer.push(BUZZER_GYRO_CALIBRATED); logger.info().log(F("GYRO BIAS")).log(degrees(state.gyroBias.x)).log(degrees(state.gyroBias.y)).logln(degrees(state.gyroBias.z)); } if(state.accelCalibrationState == CALIBRATION_SAVE) @@ -262,7 +259,7 @@ class Model size_t channel = config.input.rssiChannel; if(channel < 4 || channel > state.inputChannelCount) return 0; float value = state.input[channel - 1]; - return Math::clamp(lrintf(Math::map(value, -1.0f, 1.0f, 0, 1023)), 0l, 1023l); + return Math::clamp(lrintf(Math::map(value, -1.0f, 1.0f, 0.0f, 1023.0f)), 0l, 1023l); } int load() @@ -391,8 +388,8 @@ class Model } // configure serial ports - uint32_t serialFunctionAllowedMask = SERIAL_FUNCTION_MSP | SERIAL_FUNCTION_BLACKBOX | SERIAL_FUNCTION_TELEMETRY_FRSKY | SERIAL_FUNCTION_TELEMETRY_HOTT; - uint32_t featureAllowMask = FEATURE_RX_PPM | FEATURE_MOTOR_STOP | FEATURE_TELEMETRY;// | FEATURE_AIRMODE; + uint32_t serialFunctionAllowedMask = SERIAL_FUNCTION_MSP | SERIAL_FUNCTION_RX_SERIAL | SERIAL_FUNCTION_BLACKBOX | SERIAL_FUNCTION_TELEMETRY_FRSKY | SERIAL_FUNCTION_TELEMETRY_HOTT; + uint32_t featureAllowMask = FEATURE_RX_SERIAL | FEATURE_RX_PPM | FEATURE_RX_SPI | FEATURE_SOFTSERIAL | FEATURE_MOTOR_STOP | FEATURE_TELEMETRY;// | FEATURE_AIRMODE; // allow dynamic filter only above 1k sampling rate if(state.loopRate >= DynamicFilterConfig::MIN_FREQ) @@ -400,32 +397,21 @@ class Model featureAllowMask |= FEATURE_DYNAMIC_FILTER; } - if(config.softSerialGuard || !ESPFC_GUARD) - { - featureAllowMask |= FEATURE_SOFTSERIAL; - } - if(config.serialRxGuard || !ESPFC_GUARD) - { - featureAllowMask |= FEATURE_RX_SERIAL; - serialFunctionAllowedMask |= SERIAL_FUNCTION_RX_SERIAL; - } config.featureMask &= featureAllowMask; for(int i = 0; i < SERIAL_UART_COUNT; i++) { config.serial[i].functionMask &= serialFunctionAllowedMask; } - //config.featureMask |= FEATURE_RX_PPM; // force ppm - //config.featureMask &= ~FEATURE_RX_PPM; // disallow ppm // only few beeper modes allowed config.buzzer.beeperMask &= - 1 << (BEEPER_GYRO_CALIBRATED - 1) | - 1 << (BEEPER_SYSTEM_INIT - 1) | - 1 << (BEEPER_RX_LOST - 1) | - 1 << (BEEPER_RX_SET - 1) | - 1 << (BEEPER_DISARMING - 1) | - 1 << (BEEPER_ARMING - 1) | - 1 << (BEEPER_BAT_LOW - 1); + 1 << (BUZZER_GYRO_CALIBRATED - 1) | + 1 << (BUZZER_SYSTEM_INIT - 1) | + 1 << (BUZZER_RX_LOST - 1) | + 1 << (BUZZER_RX_SET - 1) | + 1 << (BUZZER_DISARMING - 1) | + 1 << (BUZZER_ARMING - 1) | + 1 << (BUZZER_BAT_LOW - 1); if(config.dynamicFilter.width > 6) { @@ -440,21 +426,16 @@ class Model // init timers // sample rate = clock / ( divider + 1) state.gyroTimer.setRate(state.gyroRate); - state.accelTimer.setRate(constrain(state.gyroTimer.rate, 100, 500)); - state.accelTimer.setInterval(state.accelTimer.interval - 5); - //state.accelTimer.setRate(state.gyroTimer.rate, 2); + int accelRate = Math::alignToClock(state.gyroTimer.rate, 500); + state.accelTimer.setRate(state.gyroTimer.rate, state.gyroTimer.rate / accelRate); state.loopTimer.setRate(state.gyroTimer.rate, config.loopSync); state.mixerTimer.setRate(state.loopTimer.rate, config.mixerSync); - state.inputTimer.setRate(1000); + int inputRate = Math::alignToClock(state.gyroTimer.rate, 1000); + state.inputTimer.setRate(state.gyroTimer.rate, state.gyroTimer.rate / inputRate); state.actuatorTimer.setRate(50); state.dynamicFilterTimer.setRate(50); state.telemetryTimer.setInterval(config.telemetryInterval * 1000); - state.stats.timer.setRate(4); -#if defined(ESPFC_MULTI_CORE) - state.serialTimer.setRate(4000); -#else - state.serialTimer.setRate(1000); -#endif + state.stats.timer.setRate(3); if(magActive()) { state.magTimer.setRate(state.magRate); @@ -462,7 +443,7 @@ class Model const uint32_t gyroPreFilterRate = state.gyroTimer.rate; const uint32_t gyroFilterRate = state.loopTimer.rate; - const uint32_t inputFilterRate = state.loopTimer.rate; + const uint32_t inputFilterRate = state.inputTimer.rate; const uint32_t pidFilterRate = state.loopTimer.rate; // configure filters @@ -540,8 +521,8 @@ class Model pid.Ki = (float)pc.I * ITERM_SCALE * pidScale[i]; pid.Kd = (float)pc.D * DTERM_SCALE * pidScale[i]; pid.Kf = (float)pc.F * FTERM_SCALE * pidScale[i]; - pid.iLimit = 0.15f; - pid.oLimit = 0.5f; + pid.iLimit = config.itermLimit * 0.01f; + pid.oLimit = 0.66f; pid.rate = state.loopTimer.rate; pid.dtermNotchFilter.begin(config.dtermNotchFilter, pidFilterRate); if(config.dtermDynLpfFilter.cutoff > 0) { diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 09f181bd..cf99a171 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -86,6 +86,8 @@ enum FlightMode { MODE_AIRMODE, MODE_BUZZER, MODE_FAILSAFE, + MODE_BLACKBOX, + MODE_BLACKBOX_ERASE, MODE_COUNT }; @@ -209,6 +211,7 @@ enum Feature { FEATURE_SOFTSERIAL = 1 << 6, FEATURE_TELEMETRY = 1 << 10, FEATURE_AIRMODE = 1 << 22, + FEATURE_RX_SPI = 1 << 25, FEATURE_DYNAMIC_FILTER = 1 << 29, }; @@ -317,32 +320,32 @@ class SerialPortConfig #define BUZZER_MAX_EVENTS 8 enum BuzzerEvent { - BEEPER_SILENCE = 0, // Silence, see beeperSilence() - BEEPER_GYRO_CALIBRATED, - BEEPER_RX_LOST, // Beeps when TX is turned off or signal lost (repeat until TX is okay) - BEEPER_RX_LOST_LANDING, // Beeps SOS when armed and TX is turned off or signal lost (autolanding/autodisarm) - BEEPER_DISARMING, // Beep when disarming the board - BEEPER_ARMING, // Beep when arming the board - BEEPER_ARMING_GPS_FIX, // Beep a special tone when arming the board and GPS has fix - BEEPER_BAT_CRIT_LOW, // Longer warning beeps when battery is critically low (repeats) - BEEPER_BAT_LOW, // Warning beeps when battery is getting low (repeats) - BEEPER_GPS_STATUS, // FIXME **** Disable beeper when connected to USB **** - BEEPER_RX_SET, // Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled - BEEPER_ACC_CALIBRATION, // ACC inflight calibration completed confirmation - BEEPER_ACC_CALIBRATION_FAIL, // ACC inflight calibration failed - BEEPER_READY_BEEP, // Ring a tone when GPS is locked and ready - BEEPER_MULTI_BEEPS, // Internal value used by 'beeperConfirmationBeeps()'. - BEEPER_DISARM_REPEAT, // Beeps sounded while stick held in disarm position - BEEPER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased) - BEEPER_SYSTEM_INIT, // Initialisation beeps when board is powered on - BEEPER_USB, // Some boards have beeper powered USB connected - BEEPER_BLACKBOX_ERASE, // Beep when blackbox erase completes - BEEPER_CRASH_FLIP_MODE, // Crash flip mode is active - BEEPER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated - BEEPER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop - BEEPER_ALL, // Turn ON or OFF all beeper conditions - BEEPER_PREFERENCE, // Save preferred beeper configuration - // BEEPER_ALL and BEEPER_PREFERENCE must remain at the bottom of this enum + BUZZER_SILENCE = 0, // Silence, see beeperSilence() + BUZZER_GYRO_CALIBRATED, + BUZZER_RX_LOST, // Beeps when TX is turned off or signal lost (repeat until TX is okay) + BUZZER_RX_LOST_LANDING, // Beeps SOS when armed and TX is turned off or signal lost (autolanding/autodisarm) + BUZZER_DISARMING, // Beep when disarming the board + BUZZER_ARMING, // Beep when arming the board + BUZZER_ARMING_GPS_FIX, // Beep a special tone when arming the board and GPS has fix + BUZZER_BAT_CRIT_LOW, // Longer warning beeps when battery is critically low (repeats) + BUZZER_BAT_LOW, // Warning beeps when battery is getting low (repeats) + BUZZER_GPS_STATUS, // FIXME **** Disable beeper when connected to USB **** + BUZZER_RX_SET, // Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled + BUZZER_ACC_CALIBRATION, // ACC inflight calibration completed confirmation + BUZZER_ACC_CALIBRATION_FAIL, // ACC inflight calibration failed + BUZZER_READY_BEEP, // Ring a tone when GPS is locked and ready + BUZZER_MULTI_BEEPS, // Internal value used by 'beeperConfirmationBeeps()'. + BUZZER_DISARM_REPEAT, // Beeps sounded while stick held in disarm position + BUZZER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased) + BUZZER_SYSTEM_INIT, // Initialisation beeps when board is powered on + BUZZER_USB, // Some boards have beeper powered USB connected + BUZZER_BLACKBOX_ERASE, // Beep when blackbox erase completes + BUZZER_CRASH_FLIP_MODE, // Crash flip mode is active + BUZZER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated + BUZZER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop + BUZZER_ALL, // Turn ON or OFF all beeper conditions + BUZZER_PREFERENCE, // Save preferred beeper configuration + // BUZZER_ALL and BUZZER_PREFERENCE must remain at the bottom of this enum }; class BuzzerConfig @@ -494,36 +497,13 @@ enum ArmingDisabledFlags { static const size_t ARMING_DISABLED_FLAGS_COUNT = 25; -enum WirelessMode { - WIRELESS_MODE_NULL = 0, /**< null mode */ - WIRELESS_MODE_STA, /**< WiFi station mode */ - WIRELESS_MODE_AP, /**< WiFi soft-AP mode */ - WIRELESS_MODE_APSTA, /**< WiFi station + soft-AP mode */ - WIRELESS_MODE_MAX -}; - class WirelessConfig { public: static const size_t MAX_LEN = 32; - int8_t mode; int16_t port; char ssid[MAX_LEN + 1]; char pass[MAX_LEN + 1]; - char ssidAp[MAX_LEN + 1]; - char passAp[MAX_LEN + 1]; - - static const char * getModeName(WirelessMode mode) - { - if(mode >= WIRELESS_MODE_MAX) return PSTR("?"); - return getModeNames()[mode]; - } - - static const char ** getModeNames() - { - static const char* modeChoices[] = { PSTR("OFF"), PSTR("STA"), PSTR("AP"), PSTR("AP_STA"), NULL }; - return modeChoices; - } }; class FailsafeConfig @@ -585,7 +565,7 @@ class ModelConfig FilterConfig levelPtermFilter; int16_t dtermSetpointWeight; - int8_t itermWindupPointPercent; + int8_t itermLimit; int8_t itermRelax; int8_t itermRelaxCutoff; @@ -636,8 +616,6 @@ class ModelConfig int8_t pin[PIN_COUNT]; int16_t i2cSpeed; - bool softSerialGuard; - bool serialRxGuard; int8_t tpaScale; int16_t tpaBreakpoint; @@ -655,6 +633,8 @@ class ModelConfig uint8_t rpmFilterWeights[RPM_FILTER_HARMONICS_MAX]; uint8_t rpmFilterFade; + uint8_t rescueConfigDelay = 30; + ModelConfig() { #ifdef ESPFC_INPUT @@ -746,16 +726,6 @@ class ModelConfig dtermFilter = FilterConfig(FILTER_PT1, 128); dtermFilter2 = FilterConfig(FILTER_PT1, 128); - // ESPFC defaults => BF x 0.75 - //gyroDynLpfFilter = FilterConfig(FILTER_PT1, 375, 150); - //gyroFilter = FilterConfig(FILTER_PT1, 100); - //gyroFilter2 = FilterConfig(FILTER_PT1, 188); - //dynamicFilter = DynamicFilterConfig(5, 200, 80, 400); // 8%. q:2.0, 80-400 Hz - - //dtermDynLpfFilter = FilterConfig(FILTER_PT1, 128, 53); - //dtermFilter = FilterConfig(FILTER_PT1, 113); - //dtermFilter2 = FilterConfig(FILTER_PT1, 113); - rpmFilterHarmonics = 3; rpmFilterMinFreq = 100; rpmFilterQ = 500; @@ -779,9 +749,6 @@ class ModelConfig telemetry = 0; telemetryInterval = 1000; - softSerialGuard = false; - serialRxGuard = false; - #ifdef ESPFC_SERIAL_0 serial[SERIAL_UART_0].id = SERIAL_ID_UART_1; serial[SERIAL_UART_0].functionMask = ESPFC_SERIAL_0_FN; @@ -860,25 +827,6 @@ class ModelConfig input.channel[2].map = 3; // replace input 2 with rx channel 3, yaw input.channel[3].map = 2; // replace input 3 with rx channel 2, throttle - /* - input.rateType = 0; // betaflight - - input.rate[AXIS_ROLL] = 70; - input.expo[AXIS_ROLL] = 0; - input.superRate[AXIS_ROLL] = 80; - input.rateLimit[AXIS_ROLL] = 1998; - - input.rate[AXIS_PITCH] = 70; - input.expo[AXIS_PITCH] = 0; - input.superRate[AXIS_PITCH] = 80; - input.rateLimit[AXIS_PITCH] = 1998; - - input.rate[AXIS_YAW] = 120; - input.expo[AXIS_YAW] = 0; - input.superRate[AXIS_YAW] = 50; - input.rateLimit[AXIS_YAW] = 1998; - */ - input.rateType = 3; // actual input.rate[AXIS_ROLL] = 20; @@ -927,7 +875,7 @@ class ModelConfig pid[FC_PID_MAG] = { .P = 0, .I = 0, .D = 0, .F = 0 }; pid[FC_PID_VEL] = { .P = 0, .I = 0, .D = 0, .F = 0 }; - itermWindupPointPercent = 30; + itermLimit = 30; itermRelax = ITERM_RELAX_RP; itermRelaxCutoff = 15; dtermSetpointWeight = 30; @@ -994,11 +942,8 @@ class ModelConfig buzzer.inverted = true; - wireless.mode = WIRELESS_MODE_NULL; wireless.ssid[0] = 0; wireless.pass[0] = 0; - wireless.ssidAp[0] = 0; - wireless.passAp[0] = 0; wireless.port = 1111; modelName[0] = 0; @@ -1077,7 +1022,7 @@ class ModelConfig //accelFilter.freq = 30; // ROBOT lowThrottleZeroIterm = false; // ROBOT - itermWindupPointPercent = 10; // ROBOT + itermLimit = 10; // ROBOT dtermSetpointWeight = 0; // ROBOT angleLimit = 10; // deg // ROBOT diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index fb3d6ddd..08f7b801 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -21,7 +21,7 @@ namespace Espfc { -static const size_t CLI_BUFF_SIZE = 64; +static const size_t CLI_BUFF_SIZE = 128; static const size_t CLI_ARGS_SIZE = 12; class CliCmd @@ -64,7 +64,7 @@ class BuzzerState BuzzerEvent pop() { - if(empty()) return BEEPER_SILENCE; + if(empty()) return BUZZER_SILENCE; return events[--idx]; } @@ -134,6 +134,12 @@ class FailsafeState //#define ACCEL_G (1.f) //#define ACCEL_G_INV (1.f) +enum RescueConfigMode { + RESCUE_CONFIG_PENDING, + RESCUE_CONFIG_ACTIVE, + RESCUE_CONFIG_DISABLED, +}; + // working data struct ModelState { @@ -203,6 +209,7 @@ struct ModelState uint32_t inputFrameDelta; uint32_t inputFrameRate; uint32_t inputFrameCount; + uint32_t inputLossTime; float inputInterpolationDelta; float inputInterpolationStep; float inputAutoFactor; @@ -290,6 +297,7 @@ struct ModelState uint32_t modeMask; uint32_t modeMaskPrev; uint32_t modeMaskSwitch; + uint32_t modeMaskPresent; uint32_t disarmReason; bool airmodeAllowed; @@ -323,7 +331,7 @@ struct ModelState uint32_t armingDisabledFlags; - IPAddress localIp; + RescueConfigMode rescueConfigMode; SerialPortState serial[SERIAL_UART_COUNT]; Timer serialTimer; diff --git a/lib/Espfc/src/Msp/Msp.h b/lib/Espfc/src/Msp/Msp.h index 549741db..0fd0b5ca 100644 --- a/lib/Espfc/src/Msp/Msp.h +++ b/lib/Espfc/src/Msp/Msp.h @@ -1,8 +1,8 @@ #ifndef _ESPFC_MSP_MSP_H_ #define _ESPFC_MSP_MSP_H_ -#include -#include +#include +#include "Hal/Pgm.h" extern "C" { #include "msp/msp_protocol.h" @@ -15,6 +15,7 @@ namespace Espfc { namespace Msp { static const size_t MSP_BUF_SIZE = 192; +static const size_t MSP_BUF_OUT_SIZE = 240; enum MspState { MSP_STATE_IDLE, @@ -70,7 +71,7 @@ class MspMessage uint8_t checksum2; uint8_t buffer[MSP_BUF_SIZE]; - int remain() + int remain() const { return received - read; } @@ -113,7 +114,17 @@ class MspResponse int8_t result; uint8_t direction; uint16_t len; - uint8_t data[MSP_BUF_SIZE]; + uint8_t data[MSP_BUF_OUT_SIZE]; + + int remain() const + { + return MSP_BUF_OUT_SIZE - len; + } + + void advance(size_t size) + { + len += size; + } void writeData(const char * v, int size) { diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 6a5b7cbb..c13a17b7 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -9,6 +9,7 @@ extern "C" { #include "io/serial_4way.h" + #include "blackbox/blackbox_io.h" int blackboxCalculatePDenom(int rateNum, int rateDenom); uint8_t blackboxCalculateSampleRate(uint16_t pRatio); uint8_t blackboxGetRateDenom(void); @@ -288,7 +289,7 @@ class MspProcessor break; case MSP_BOXNAMES: - r.writeString(F("ARM;ANGLE;AIRMODE;BUZZER;FAILSAFE;")); + r.writeString(F("ARM;ANGLE;AIRMODE;BEEPER;FAILSAFE;BLACKBOX;BLACKBOXERASE;")); break; case MSP_BOXIDS: @@ -297,6 +298,8 @@ class MspProcessor r.writeU8(MODE_AIRMODE); r.writeU8(MODE_BUZZER); r.writeU8(MODE_FAILSAFE); + r.writeU8(MODE_BLACKBOX); + r.writeU8(MODE_BLACKBOX_ERASE); break; case MSP_MODE_RANGES: @@ -466,10 +469,51 @@ class MspProcessor break; case MSP_DATAFLASH_SUMMARY: - r.writeU8(0); // FlashFS is neither ready nor supported +#ifdef USE_FLASHFS + { + uint8_t flags = flashfsIsSupported() ? 2 : 0; + flags |= flashfsIsReady() ? 1 : 0; + r.writeU8(flags); + r.writeU32(flashfsGetSectors()); + r.writeU32(flashfsGetSize()); + r.writeU32(flashfsGetOffset()); + } +#else + r.writeU8(0); r.writeU32(0); r.writeU32(0); r.writeU32(0); +#endif + break; + + case MSP_DATAFLASH_ERASE: +#ifdef USE_FLASHFS + blackboxEraseAll(); +#endif + break; + + case MSP_DATAFLASH_READ: +#ifdef USE_FLASHFS + { + const unsigned int dataSize = m.remain(); + const uint32_t readAddress = m.readU32(); + uint16_t readLength; + bool allowCompression = false; + bool useLegacyFormat; + + if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) { + readLength = m.readU16(); + if (m.remain()) { + allowCompression = m.readU8(); + } + useLegacyFormat = false; + } else { + readLength = 128; + useLegacyFormat = true; + } + serializeFlashData(r, readAddress, readLength, useLegacyFormat, allowCompression); + } +#endif break; case MSP_ACC_TRIM: @@ -530,7 +574,7 @@ class MspProcessor case MSP_CF_SERIAL_CONFIG: for(int i = 0; i < SERIAL_UART_COUNT; i++) { - if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isActive(FEATURE_SOFTSERIAL)) continue; + if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isFeatureActive(FEATURE_SOFTSERIAL)) continue; r.writeU8(_model.config.serial[i].id); // identifier r.writeU16(_model.config.serial[i].functionMask); // functionMask r.writeU8(toBaudIndex(_model.config.serial[i].baud)); // msp_baudrateIndex @@ -545,13 +589,13 @@ class MspProcessor uint8_t count = 0; for (int i = 0; i < SERIAL_UART_COUNT; i++) { - if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isActive(FEATURE_SOFTSERIAL)) continue; + if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isFeatureActive(FEATURE_SOFTSERIAL)) continue; count++; } r.writeU8(count); for (int i = 0; i < SERIAL_UART_COUNT; i++) { - if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isActive(FEATURE_SOFTSERIAL)) continue; + if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isFeatureActive(FEATURE_SOFTSERIAL)) continue; r.writeU8(_model.config.serial[i].id); // identifier r.writeU32(_model.config.serial[i].functionMask); // functionMask r.writeU8(toBaudIndex(_model.config.serial[i].baud)); // msp_baudrateIndex @@ -1433,6 +1477,7 @@ class MspProcessor break; case MSP_REBOOT: + r.writeU8(0); // reboot to firmware _postCommand = std::bind(&MspProcessor::processRestart, this); break; @@ -1453,6 +1498,42 @@ class MspProcessor Hardware::restart(_model); } +#ifdef USE_FLASHFS + void serializeFlashData(MspResponse& r, uint32_t address, const uint16_t size, bool useLegacyFormat, bool allowCompression) + { + (void)allowCompression; // not supported + + const uint32_t allowedToRead = r.remain() - 16; + const uint32_t flashfsSize = flashfsGetSize(); + + uint16_t readLen = std::min(std::min((uint32_t)size, allowedToRead), flashfsSize - address); + + r.writeU32(address); + + uint16_t *readLenPtr = (uint16_t*)&r.data[r.len]; + if (!useLegacyFormat) + { + // new format supports variable read lengths + r.writeU16(readLen); + r.writeU8(0); // NO_COMPRESSION + } + + const size_t bytesRead = flashfsReadAbs(address, &r.data[r.len], readLen); + r.advance(bytesRead); + + if (!useLegacyFormat) + { + // update the 'read length' with the actual amount read from flash. + *readLenPtr = bytesRead; + } + else + { + // pad the buffer with zeros + //for (int i = bytesRead; i < allowedToRead; i++) r.writeU8(0); + } + } +#endif + void sendResponse(MspResponse& r, Device::SerialDevice& s) { debugResponse(r); @@ -1465,7 +1546,7 @@ class MspProcessor sendResponseV2(r, s); break; } - postCommand(); + //postCommand(); } void sendResponseV1(MspResponse& r, Device::SerialDevice& s) @@ -1543,12 +1624,12 @@ class MspProcessor Device::SerialDevice * s = _model.getSerialStream(SERIAL_FUNCTION_TELEMETRY_HOTT); if(!s) return; - s->print(m.dir == MSP_TYPE_REPLY ? '>' : '<'); s->print(' '); - s->print(m.cmd); s->print(' '); + s->print(m.dir == MSP_TYPE_REPLY ? '>' : '<'); + s->print(m.cmd); s->print('.'); s->print(m.expected); s->print(' '); for(size_t i = 0; i < m.expected; i++) { - s->print(m.buffer[i]); s->print(' '); + s->print(m.buffer[i], HEX); s->print(' '); } s->println(); } @@ -1559,12 +1640,12 @@ class MspProcessor Device::SerialDevice * s = _model.getSerialStream(SERIAL_FUNCTION_TELEMETRY_HOTT); if(!s) return; - s->print(r.result == 1 ? '>' : (r.result == -1 ? '!' : '@')); s->print(' '); - s->print(r.cmd); s->print(' '); + s->print(r.result == 1 ? '>' : (r.result == -1 ? '!' : '@')); + s->print(r.cmd); s->print('.'); s->print(r.len); s->print(' '); for(size_t i = 0; i < r.len; i++) { - s->print(r.data[i]); s->print(' '); + s->print(r.data[i], HEX); s->print(' '); } s->println(); } diff --git a/lib/Espfc/src/Output/Mixer.cpp b/lib/Espfc/src/Output/Mixer.cpp new file mode 100644 index 00000000..1c421db4 --- /dev/null +++ b/lib/Espfc/src/Output/Mixer.cpp @@ -0,0 +1,385 @@ + +#include "Mixer.h" +#include "Output/Mixers.h" +#include "platform.h" + +namespace Espfc { + +namespace Output { + +Mixer::Mixer(Model& model): _model(model), _motor(NULL), _servo(NULL) {} + +int Mixer::begin() +{ + EscConfig motorConf = { + .timer = ESC_DRIVER_MOTOR_TIMER, + .protocol = (EscProtocol)_model.config.output.protocol, + .rate = _model.config.output.rate, + .async = !!_model.config.output.async, + .dshotTelemetry = !!_model.config.output.dshotTelemetry, + }; + escMotor.begin(motorConf); + _model.state.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).logln(ESC_DRIVER_MOTOR_TIMER); + + if(_model.config.output.servoRate) + { + EscConfig servoConf = { + .timer = ESC_DRIVER_SERVO_TIMER, + .protocol = ESC_PROTOCOL_PWM, + .rate = _model.config.output.servoRate, + .async = true, + .dshotTelemetry = false, + }; + escServo.begin(servoConf); + _model.state.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); + } + _erpmToHz = EscDriver::getErpmToHzRatio(_model.config.output.motorPoles); + _statsCounterMax = _model.state.mixerTimer.rate / 2; + _statsCounter = 0; + + for(size_t i = 0; i < OUTPUT_CHANNELS; ++i) + { + const OutputChannelConfig& occ = _model.config.output.channel[i]; + if(occ.servo) + { + 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]); + } + } + 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.state.outputTelemetryErrors[i] = 0; + _model.state.outputTelemetryErrorsSum[i] = 0; + _model.state.outputTelemetryErrorsCount[i] = 0; + _model.state.outputTelemetryTemperature[i] = 0; + _model.state.outputTelemetryCurrent[i] = 0; + _model.state.outputTelemetryVoltage[i] = 0; + _model.state.outputTelemetryErpm[i] = 0; + _model.state.outputTelemetryRpm[i] = 0; + _model.state.outputTelemetryFreq[i] = 0; + } + motorInitEscDevice(_motor); + + _model.state.minThrottle = _model.config.output.minThrottle; + _model.state.maxThrottle = _model.config.output.maxThrottle; + _model.state.digitalOutput = _model.config.output.protocol >= ESC_PROTOCOL_DSHOT150; + if(_model.state.digitalOutput) + { + _model.state.minThrottle = (_model.config.output.dshotIdle * 0.1f) + 1001.f; + _model.state.maxThrottle = 2000.f; + } + _model.state.currentMixer = Mixers::getMixer((MixerType)_model.config.mixerType, _model.state.customMixer); + return 1; +} + +int FAST_CODE_ATTR Mixer::update() +{ + uint32_t startTime = micros(); + + float outputs[OUTPUT_CHANNELS]; + const MixerConfig& mixer = _model.state.currentMixer; + + readTelemetry(); + updateMixer(mixer, outputs); + writeOutput(mixer, outputs); + + if(_model.config.debugMode == DEBUG_PIDLOOP) + { + _model.state.debug[3] = micros() - startTime; + } + + _model.state.stats.loopTick(); + + if(_model.config.debugMode == DEBUG_CYCLETIME) + { + _model.state.debug[0] = _model.state.stats.loopTime(); + _model.state.debug[1] = lrintf(_model.state.stats.getCpuLoad()); + } + + return 1; +} + +void FAST_CODE_ATTR Mixer::updateMixer(const MixerConfig& mixer, float * outputs) +{ + Stats::Measure mixerMeasure(_model.state.stats, COUNTER_MIXER); + + float sources[MIXER_SOURCE_MAX]; + sources[MIXER_SOURCE_NULL] = 0; + + sources[MIXER_SOURCE_ROLL] = _model.state.output[AXIS_ROLL]; + sources[MIXER_SOURCE_PITCH] = _model.state.output[AXIS_PITCH]; + sources[MIXER_SOURCE_YAW] = _model.state.output[AXIS_YAW] * (_model.config.yawReverse ? 1.f : -1.f); + sources[MIXER_SOURCE_THRUST] = _model.state.output[AXIS_THRUST]; + + sources[MIXER_SOURCE_RC_ROLL] = _model.state.input[AXIS_ROLL]; + sources[MIXER_SOURCE_RC_PITCH] = _model.state.input[AXIS_PITCH]; + sources[MIXER_SOURCE_RC_YAW] = _model.state.input[AXIS_YAW]; + sources[MIXER_SOURCE_RC_THRUST] = _model.state.input[AXIS_THRUST]; + + for(size_t i = 0; i < 3; i++) + { + sources[MIXER_SOURCE_RC_AUX1 + i] = _model.state.input[AXIS_AUX_1 + i]; + } + + for(size_t i = 0; i < OUTPUT_CHANNELS; i++) + { + outputs[i] = 0.f; + } + + // mix stabilized sources first + const MixerEntry * entry = mixer.mixes; + const MixerEntry * end = mixer.mixes + MIXER_RULE_MAX; + while(entry != end) + { + if(entry->src == MIXER_SOURCE_NULL) break; // break on terminator + if(entry->src <= MIXER_SOURCE_YAW && entry->dst < mixer.count && entry->rate != 0) + { + outputs[entry->dst] += sources[entry->src] * (entry->rate * 0.01f); + } + entry++; + } + + // airmode logic + float thrust = limitThrust(sources[MIXER_SOURCE_THRUST], (ThrottleLimitType)_model.config.output.throttleLimitType, _model.config.output.throttleLimitPercent); + if(_model.isAirModeActive()) + { + float min = 0.f, max = 0.f; + for(size_t i = 0; i < mixer.count; i++) + { + max = std::max(max, outputs[i]); + min = std::min(min, outputs[i]); + } + float range = (max - min) * 0.5f; + if(range > 1.f) + { + for(size_t i = 0; i < mixer.count; i++) + { + outputs[i] /= range; + } + thrust = 0.f; //limitThrust(0.f); + } + else + { + thrust = Math::clamp(thrust, -1.f + range, 1.f - range); + } + } + + // apply other channels + entry = mixer.mixes; + while(entry != end) + { + if(entry->src == MIXER_SOURCE_NULL) break; // break on terminator + if(entry->dst < mixer.count) + { + if(entry->src == MIXER_SOURCE_THRUST) + { + outputs[entry->dst] += thrust * (entry->rate * 0.01f); + } + else if(entry->src > MIXER_SOURCE_THRUST && entry->src < MIXER_SOURCE_MAX) + { + outputs[entry->dst] += sources[entry->src] * (entry->rate * 0.01f); + } + } + entry++; + } + + bool saturated = false; + for(size_t i = 0; i < mixer.count; i++) + { + const OutputChannelConfig& occ = _model.config.output.channel[i]; + if(!occ.servo && outputs[i] >= 0.98f) saturated = true; + outputs[i] = limitOutput(outputs[i], occ, _model.config.output.motorLimit); + } + _model.setOutputSaturated(saturated); +} + +float FAST_CODE_ATTR Mixer::limitThrust(float thrust, ThrottleLimitType type, int8_t limit) +{ + if(type == THROTTLE_LIMIT_TYPE_NONE || limit >= 100 || limit < 1) return thrust; + + // thrust range is [-1, 1] + switch(type) + { + case THROTTLE_LIMIT_TYPE_SCALE: + return (thrust + 1.0f) * limit * 0.01f - 1.0f; + case THROTTLE_LIMIT_TYPE_CLIP: + return Math::clamp(thrust, -1.f, (limit * 0.02f) - 1.0f); + default: + break; + } + + return thrust; +} + +float FAST_CODE_ATTR Mixer::limitOutput(float output, const OutputChannelConfig& occ, int limit) +{ + if(limit >= 100 || limit < 1) return output; + + if(occ.servo) + { + const float factor = limit * 0.01f; + return Math::clamp(output, -factor, factor); + } + else + { + const float factor = limit * 0.02f; // *2 + return Math::clamp(output + 1.f, 0.f, factor) - 1.0f; + } +} + +void FAST_CODE_ATTR Mixer::writeOutput(const MixerConfig& mixer, float * out) +{ + Stats::Measure mixerMeasure(_model.state.stats, COUNTER_MIXER_WRITE); + + bool stop = _stop(); + for(size_t i = 0; i < OUTPUT_CHANNELS; i++) + { + const OutputChannelConfig& och = _model.config.output.channel[i]; + if(i >= mixer.count || stop) + { + _model.state.outputUs[i] = och.servo && _model.state.outputDisarmed[i] == 1000 ? och.neutral : _model.state.outputDisarmed[i]; + } + else + { + if(och.servo) + { + const int16_t tmp = lrintf(Math::map3(out[i], -1.f, 0.f, 1.f, och.reverse ? 2000 : 1000, och.neutral, och.reverse ? 1000 : 2000)); + _model.state.outputUs[i] = Math::clamp(tmp, och.min, och.max); + } + else + { + float v = Math::clamp(out[i], -1.f, 1.f); + _model.state.outputUs[i] = lrintf(Math::map(v, -1.f, 1.f, _model.state.minThrottle, _model.state.maxThrottle)); + } + } + } + + for(size_t i = 0; i < OUTPUT_CHANNELS; i++) + { + const OutputChannelConfig& och = _model.config.output.channel[i]; + if(och.servo) + { + if(_servo) _servo->write(i, _model.state.outputUs[i]); + } + else + { + if(_motor) _motor->write(i, _model.state.outputUs[i]); + } + } + + if(_motor) _motor->apply(); + if(_servo) _servo->apply(); +} + +void FAST_CODE_ATTR Mixer::readTelemetry() +{ + Stats::Measure mixerMeasure(_model.state.stats, COUNTER_MIXER_READ); + if(!_model.config.output.dshotTelemetry || !_motor) return; + + for(size_t i = 0; i < OUTPUT_CHANNELS; i++) + { + if(_model.config.output.channel[i].servo) continue; + uint32_t value = _motor->telemetry(i); + value = EscDriver::gcrToRawValue(value); + + _model.state.outputTelemetryErrorsCount[i]++; + + if(value == EscDriver::INVALID_TELEMETRY_VALUE) + { + _model.state.outputTelemetryErrorsSum[i]++; + continue; + } + + // Decode Extended DSHOT telemetry + switch (value & 0x0f00) + { + case 0x0200: + // Temperature range (in degree Celsius, just like Blheli_32 and KISS) + _model.state.outputTelemetryTemperature[i] = value & 0x00ff; + break; + case 0x0400: + // Voltage range (0-63,75V step 0,25V) + _model.state.outputTelemetryVoltage[i] = value & 0x00ff; + break; + case 0x0600: + // Current range (0-255A step 1A) + _model.state.outputTelemetryCurrent[i] = value & 0x00ff; + break; + case 0x0800: + // Debug 1 value + _model.state.outputTelemetryDebug1[i] = value & 0x00ff; + break; + case 0x0A00: + // Debug 2 value + _model.state.outputTelemetryDebug2[i] = value & 0x00ff; + break; + case 0x0C00: + // Debug 3 value + _model.state.outputTelemetryDebug3[i] = value & 0x00ff; + break; + case 0x0E00: + // State / Events + _model.state.outputTelemetryEvents[i] = value & 0x00ff; + break; + default: + value = EscDriver::convertToValue(value); + _model.state.outputTelemetryErpm[i] = EscDriver::convertToErpm(value); + _model.state.outputTelemetryRpm[i] = erpmToRpm(_model.state.outputTelemetryErpm[i]); + _model.setDebug(DEBUG_DSHOT_RPM_TELEMETRY, i, _model.state.outputTelemetryErpm[i]); + break; + } + } + + for(size_t i = 0; i < OUTPUT_CHANNELS; i++) + { + _model.state.outputTelemetryFreq[i] = _model.state.rpmFreqFilter[i].update(erpmToHz(_model.state.outputTelemetryErpm[i])); + } + + _statsCounter++; + if(_statsCounter >= _statsCounterMax) + { + for(size_t i = 0; i < OUTPUT_CHANNELS; i++) + { + if(_model.state.outputTelemetryErrorsCount[i]) + { + _model.state.outputTelemetryErrors[i] = _model.state.outputTelemetryErrorsSum[i] * 10000 / _model.state.outputTelemetryErrorsCount[i]; + if(_model.config.debugMode == DEBUG_DSHOT_RPM_ERRORS) + { + _model.state.debug[i] = _model.state.outputTelemetryErrors[i]; + } + } + _model.state.outputTelemetryErrorsCount[i] = 0; + _model.state.outputTelemetryErrorsSum[i] = 0; + } + _statsCounter = 0; + } +} + +float inline Mixer::erpmToHz(float erpm) +{ + return _erpmToHz * erpm; +} + +float inline Mixer::erpmToRpm(float erpm) +{ + return erpmToHz(erpm) * EscDriver::SECONDS_PER_MINUTE; +} + +bool Mixer::_stop(void) +{ + if(!_model.isActive(MODE_ARMED)) return true; + if(_model.isActive(FEATURE_MOTOR_STOP) && _model.isThrottleLow()) return true; + return false; +} + +} + +} + diff --git a/lib/Espfc/src/Output/Mixer.h b/lib/Espfc/src/Output/Mixer.h index 0dbb426a..97099ff2 100644 --- a/lib/Espfc/src/Output/Mixer.h +++ b/lib/Espfc/src/Output/Mixer.h @@ -1,10 +1,7 @@ -#ifndef _ESPFC_OUTPUT_MIXER_H_ -#define _ESPFC_OUTPUT_MIXER_H_ +#pragma once #include "Model.h" -#include "Output/Mixers.h" #include "EscDriver.h" -#include "platform.h" namespace Espfc { @@ -13,378 +10,20 @@ namespace Output { class Mixer { public: - Mixer(Model& model): _model(model), _motor(NULL), _servo(NULL) {} - - int begin() - { - EscConfig motorConf = { - .timer = ESC_DRIVER_MOTOR_TIMER, - .protocol = (EscProtocol)_model.config.output.protocol, - .rate = _model.config.output.rate, - .async = !!_model.config.output.async, - .dshotTelemetry = !!_model.config.output.dshotTelemetry, - }; - escMotor.begin(motorConf); - _model.state.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).logln(ESC_DRIVER_MOTOR_TIMER); - - if(_model.config.output.servoRate) - { - EscConfig servoConf = { - .timer = ESC_DRIVER_SERVO_TIMER, - .protocol = ESC_PROTOCOL_PWM, - .rate = _model.config.output.servoRate, - .async = true, - .dshotTelemetry = false, - }; - escServo.begin(servoConf); - _model.state.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); - } - _erpmToHz = EscDriver::getErpmToHzRatio(_model.config.output.motorPoles); - _statsCounterMax = _model.state.mixerTimer.rate / 2; - _statsCounter = 0; - - for(size_t i = 0; i < OUTPUT_CHANNELS; ++i) - { - const OutputChannelConfig& occ = _model.config.output.channel[i]; - if(occ.servo) - { - 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]); - } - } - 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.state.outputTelemetryErrors[i] = 0; - _model.state.outputTelemetryErrorsSum[i] = 0; - _model.state.outputTelemetryErrorsCount[i] = 0; - _model.state.outputTelemetryTemperature[i] = 0; - _model.state.outputTelemetryCurrent[i] = 0; - _model.state.outputTelemetryVoltage[i] = 0; - _model.state.outputTelemetryErpm[i] = 0; - _model.state.outputTelemetryRpm[i] = 0; - _model.state.outputTelemetryFreq[i] = 0; - } - motorInitEscDevice(_motor); - - _model.state.minThrottle = _model.config.output.minThrottle; - _model.state.maxThrottle = _model.config.output.maxThrottle; - _model.state.digitalOutput = _model.config.output.protocol >= ESC_PROTOCOL_DSHOT150; - if(_model.state.digitalOutput) - { - _model.state.minThrottle = (_model.config.output.dshotIdle * 0.1f) + 1001.f; - _model.state.maxThrottle = 2000.f; - } - _model.state.currentMixer = Mixers::getMixer((MixerType)_model.config.mixerType, _model.state.customMixer); - return 1; - } - - int update() - { - uint32_t startTime = micros(); - - float outputs[OUTPUT_CHANNELS]; - const MixerConfig& mixer = _model.state.currentMixer; - - readTelemetry(); - updateMixer(mixer, outputs); - writeOutput(mixer, outputs); - - if(_model.config.debugMode == DEBUG_PIDLOOP) - { - _model.state.debug[3] = micros() - startTime; - } - - _model.state.stats.loopTick(); - - if(_model.config.debugMode == DEBUG_CYCLETIME) - { - _model.state.debug[0] = _model.state.stats.loopTime(); - _model.state.debug[1] = lrintf(_model.state.stats.getCpuLoad()); - } - - return 1; - } - - void updateMixer(const MixerConfig& mixer, float * outputs) - { - Stats::Measure mixerMeasure(_model.state.stats, COUNTER_MIXER); - - float sources[MIXER_SOURCE_MAX]; - sources[MIXER_SOURCE_NULL] = 0; - - sources[MIXER_SOURCE_ROLL] = _model.state.output[AXIS_ROLL]; - sources[MIXER_SOURCE_PITCH] = _model.state.output[AXIS_PITCH]; - sources[MIXER_SOURCE_YAW] = _model.state.output[AXIS_YAW] * (_model.config.yawReverse ? 1.f : -1.f); - sources[MIXER_SOURCE_THRUST] = _model.state.output[AXIS_THRUST]; - - sources[MIXER_SOURCE_RC_ROLL] = _model.state.input[AXIS_ROLL]; - sources[MIXER_SOURCE_RC_PITCH] = _model.state.input[AXIS_PITCH]; - sources[MIXER_SOURCE_RC_YAW] = _model.state.input[AXIS_YAW]; - sources[MIXER_SOURCE_RC_THRUST] = _model.state.input[AXIS_THRUST]; - - for(size_t i = 0; i < 3; i++) - { - sources[MIXER_SOURCE_RC_AUX1 + i] = _model.state.input[AXIS_AUX_1 + i]; - } - - for(size_t i = 0; i < OUTPUT_CHANNELS; i++) - { - outputs[i] = 0.f; - } - - // mix stabilized sources first - const MixerEntry * entry = mixer.mixes; - const MixerEntry * end = mixer.mixes + MIXER_RULE_MAX; - while(entry != end) - { - if(entry->src == MIXER_SOURCE_NULL) break; // break on terminator - if(entry->src <= MIXER_SOURCE_YAW && entry->dst < mixer.count && entry->rate != 0) - { - outputs[entry->dst] += sources[entry->src] * (entry->rate * 0.01f); - } - entry++; - } - - // airmode logic - float thrust = limitThrust(sources[MIXER_SOURCE_THRUST], (ThrottleLimitType)_model.config.output.throttleLimitType, _model.config.output.throttleLimitPercent); - if(_model.isAirModeActive()) - { - float min = 0.f, max = 0.f; - for(size_t i = 0; i < mixer.count; i++) - { - max = std::max(max, outputs[i]); - min = std::min(min, outputs[i]); - } - float range = (max - min) * 0.5f; - if(range > 1.f) - { - for(size_t i = 0; i < mixer.count; i++) - { - outputs[i] /= range; - } - thrust = 0.f; //limitThrust(0.f); - } - else - { - thrust = Math::clamp(thrust, -1.f + range, 1.f - range); - } - } - - // apply other channels - entry = mixer.mixes; - while(entry != end) - { - if(entry->src == MIXER_SOURCE_NULL) break; // break on terminator - if(entry->dst < mixer.count) - { - if(entry->src == MIXER_SOURCE_THRUST) - { - outputs[entry->dst] += thrust * (entry->rate * 0.01f); - } - else if(entry->src > MIXER_SOURCE_THRUST && entry->src < MIXER_SOURCE_MAX) - { - outputs[entry->dst] += sources[entry->src] * (entry->rate * 0.01f); - } - } - entry++; - } - - bool saturated = false; - for(size_t i = 0; i < mixer.count; i++) - { - const OutputChannelConfig& occ = _model.config.output.channel[i]; - if(!occ.servo && outputs[i] >= 0.98f) saturated = true; - outputs[i] = limitOutput(outputs[i], occ, _model.config.output.motorLimit); - } - _model.setOutputSaturated(saturated); - } - - float limitThrust(float thrust, ThrottleLimitType type, int8_t limit) - { - if(type == THROTTLE_LIMIT_TYPE_NONE || limit >= 100 || limit < 1) return thrust; - - // thrust range is [-1, 1] - switch(type) - { - case THROTTLE_LIMIT_TYPE_SCALE: - return (thrust + 1.0f) * limit * 0.01f - 1.0f; - case THROTTLE_LIMIT_TYPE_CLIP: - return Math::clamp(thrust, -1.f, (limit * 0.02f) - 1.0f); - default: - break; - } - - return thrust; - } - - float limitOutput(float output, const OutputChannelConfig& occ, int limit) - { - if(limit >= 100 || limit < 1) return output; - - if(occ.servo) - { - const float factor = limit * 0.01f; - return Math::clamp(output, -factor, factor); - } - else - { - const float factor = limit * 0.02f; // *2 - return Math::clamp(output + 1.f, 0.f, factor) - 1.0f; - } - } - - void writeOutput(const MixerConfig& mixer, float * out) - { - Stats::Measure mixerMeasure(_model.state.stats, COUNTER_MIXER_WRITE); - - bool stop = _stop(); - for(size_t i = 0; i < OUTPUT_CHANNELS; i++) - { - const OutputChannelConfig& och = _model.config.output.channel[i]; - if(i >= mixer.count || stop) - { - _model.state.outputUs[i] = och.servo && _model.state.outputDisarmed[i] == 1000 ? och.neutral : _model.state.outputDisarmed[i]; - } - else - { - if(och.servo) - { - const int16_t tmp = lrintf(Math::map3(out[i], -1.f, 0.f, 1.f, och.reverse ? 2000 : 1000, och.neutral, och.reverse ? 1000 : 2000)); - _model.state.outputUs[i] = Math::clamp(tmp, och.min, och.max); - } - else - { - float v = Math::clamp(out[i], -1.f, 1.f); - _model.state.outputUs[i] = lrintf(Math::map(v, -1.f, 1.f, _model.state.minThrottle, _model.state.maxThrottle)); - } - } - } - - for(size_t i = 0; i < OUTPUT_CHANNELS; i++) - { - const OutputChannelConfig& och = _model.config.output.channel[i]; - if(och.servo) - { - if(_servo) _servo->write(i, _model.state.outputUs[i]); - } - else - { - if(_motor) _motor->write(i, _model.state.outputUs[i]); - } - } - - if(_motor) _motor->apply(); - if(_servo) _servo->apply(); - } - - void readTelemetry() - { - Stats::Measure mixerMeasure(_model.state.stats, COUNTER_MIXER_READ); - if(!_model.config.output.dshotTelemetry || !_motor) return; - - for(size_t i = 0; i < OUTPUT_CHANNELS; i++) - { - if(_model.config.output.channel[i].servo) continue; - uint32_t value = _motor->telemetry(i); - value = EscDriver::gcrToRawValue(value); - - _model.state.outputTelemetryErrorsCount[i]++; - - if(value == EscDriver::INVALID_TELEMETRY_VALUE) - { - _model.state.outputTelemetryErrorsSum[i]++; - continue; - } - - // Decode Extended DSHOT telemetry - switch (value & 0x0f00) - { - case 0x0200: - // Temperature range (in degree Celsius, just like Blheli_32 and KISS) - _model.state.outputTelemetryTemperature[i] = value & 0x00ff; - break; - case 0x0400: - // Voltage range (0-63,75V step 0,25V) - _model.state.outputTelemetryVoltage[i] = value & 0x00ff; - break; - case 0x0600: - // Current range (0-255A step 1A) - _model.state.outputTelemetryCurrent[i] = value & 0x00ff; - break; - case 0x0800: - // Debug 1 value - _model.state.outputTelemetryDebug1[i] = value & 0x00ff; - break; - case 0x0A00: - // Debug 2 value - _model.state.outputTelemetryDebug2[i] = value & 0x00ff; - break; - case 0x0C00: - // Debug 3 value - _model.state.outputTelemetryDebug3[i] = value & 0x00ff; - break; - case 0x0E00: - // State / Events - _model.state.outputTelemetryEvents[i] = value & 0x00ff; - break; - default: - value = EscDriver::convertToValue(value); - _model.state.outputTelemetryErpm[i] = EscDriver::convertToErpm(value); - _model.state.outputTelemetryRpm[i] = erpmToRpm(_model.state.outputTelemetryErpm[i]); - _model.setDebug(DEBUG_DSHOT_RPM_TELEMETRY, i, _model.state.outputTelemetryErpm[i]); - break; - } - } - - for(size_t i = 0; i < OUTPUT_CHANNELS; i++) - { - _model.state.outputTelemetryFreq[i] = _model.state.rpmFreqFilter[i].update(erpmToHz(_model.state.outputTelemetryErpm[i])); - } - - _statsCounter++; - if(_statsCounter >= _statsCounterMax) - { - for(size_t i = 0; i < OUTPUT_CHANNELS; i++) - { - if(_model.state.outputTelemetryErrorsCount[i]) - { - _model.state.outputTelemetryErrors[i] = _model.state.outputTelemetryErrorsSum[i] * 10000 / _model.state.outputTelemetryErrorsCount[i]; - if(_model.config.debugMode == DEBUG_DSHOT_RPM_ERRORS) - { - _model.state.debug[i] = _model.state.outputTelemetryErrors[i]; - } - } - _model.state.outputTelemetryErrorsCount[i] = 0; - _model.state.outputTelemetryErrorsSum[i] = 0; - } - _statsCounter = 0; - } - } - - float inline erpmToHz(float erpm) - { - return _erpmToHz * erpm; - } - - float inline erpmToRpm(float erpm) - { - return erpmToHz(erpm) * EscDriver::SECONDS_PER_MINUTE; - } - - bool _stop(void) - { - if(!_model.isActive(MODE_ARMED)) return true; - if(_model.isActive(FEATURE_MOTOR_STOP) && _model.isThrottleLow()) return true; - return false; - } - + Mixer(Model& model); + int begin(); + int update(); + + void updateMixer(const MixerConfig& mixer, float * outputs); + float limitThrust(float thrust, ThrottleLimitType type, int8_t limit); + float limitOutput(float output, const OutputChannelConfig& occ, int limit); + void writeOutput(const MixerConfig& mixer, float * out); + void readTelemetry(); + float inline erpmToHz(float erpm); + float inline erpmToRpm(float erpm); + bool inline _stop(void); + + private: Model& _model; EscDriver * _motor; EscDriver * _servo; @@ -399,5 +38,3 @@ class Mixer } } - -#endif diff --git a/lib/Espfc/src/Rc/Crsf.cpp b/lib/Espfc/src/Rc/Crsf.cpp new file mode 100644 index 00000000..54189e45 --- /dev/null +++ b/lib/Espfc/src/Rc/Crsf.cpp @@ -0,0 +1,124 @@ + +#include "Crsf.h" +#include "Math/Utils.h" +#include "Math/Crc.h" +#include "Utils/MemoryHelper.h" +#include + +namespace Espfc { + +namespace Rc { + +void FAST_CODE_ATTR Crsf::decodeRcData(uint16_t* channels, const CrsfData* frame) +{ + channels[0] = convert(frame->chan0); + channels[1] = convert(frame->chan1); + channels[2] = convert(frame->chan2); + channels[3] = convert(frame->chan3); + channels[4] = convert(frame->chan4); + channels[5] = convert(frame->chan5); + channels[6] = convert(frame->chan6); + channels[7] = convert(frame->chan7); + channels[8] = convert(frame->chan8); + channels[9] = convert(frame->chan9); + channels[10] = convert(frame->chan10); + channels[11] = convert(frame->chan11); + channels[12] = convert(frame->chan12); + channels[13] = convert(frame->chan13); + channels[14] = convert(frame->chan14); + channels[15] = convert(frame->chan15); +} + +void FAST_CODE_ATTR Crsf::decodeRcDataShift8(uint16_t* channels, const CrsfData* frame) +{ + // 8-bit + // 0....... ...1.... ......2. ........ .3...... ....4... .......5 ........ ..6..... .....7.. ........ + // 8....... ...9.... ......A. ........ .B...... ....C... .......D ........ ..E..... .....F.. ........ + const uint8_t * crsfData = reinterpret_cast(frame); + channels[0] = convert((crsfData[0] | crsfData[1] << 8) & 0x07FF); + channels[1] = convert((crsfData[1] >> 3 | crsfData[2] << 5) & 0x07FF); + channels[2] = convert((crsfData[2] >> 6 | crsfData[3] << 2 | crsfData[4] << 10) & 0x07FF); + channels[3] = convert((crsfData[4] >> 1 | crsfData[5] << 7) & 0x07FF); + + channels[4] = convert((crsfData[5] >> 4 | crsfData[6] << 4) & 0x07FF); + channels[5] = convert((crsfData[6] >> 7 | crsfData[7] << 1 | crsfData[8] << 9) & 0x07FF); + channels[6] = convert((crsfData[8] >> 2 | crsfData[9] << 6) & 0x07FF); + channels[7] = convert((crsfData[9] >> 5 | crsfData[10] << 3) & 0x07FF); + + channels[8] = convert((crsfData[11] | crsfData[12] << 8) & 0x07FF); + channels[9] = convert((crsfData[12] >> 3 | crsfData[13] << 5) & 0x07FF); + channels[10] = convert((crsfData[13] >> 6 | crsfData[14] << 2 | crsfData[15] << 10) & 0x07FF); + channels[11] = convert((crsfData[15] >> 1 | crsfData[16] << 7) & 0x07FF); + + channels[12] = convert((crsfData[16] >> 4 | crsfData[17] << 4) & 0x07FF); + channels[13] = convert((crsfData[17] >> 7 | crsfData[18] << 1 | crsfData[19] << 9) & 0x07FF); + channels[14] = convert((crsfData[19] >> 2 | crsfData[20] << 6) & 0x07FF); + channels[15] = convert((crsfData[20] >> 5 | crsfData[21] << 3) & 0x07FF); +} + +/*void Crsf::decodeRcDataShift32(uint16_t* channels, const CrsfData* frame) +{ + // 32-bit + // 0..........1..........2......... .3..........4..........5........ ..6..........7..........8....... + // ...9..........A..........B...... ....C..........D..........E..... .....F.......... + const uint32_t * crsfData = reinterpret_cast(frame); + channels[0] = convert((crsfData[0]) & 0x07FF); + channels[1] = convert((crsfData[0] >> 11) & 0x07FF); + channels[2] = convert((crsfData[0] >> 22 | crsfData[1] << 10) & 0x07FF); + channels[3] = convert((crsfData[1] >> 1) & 0x07FF); + + channels[4] = convert((crsfData[1] >> 12) & 0x07FF); + channels[5] = convert((crsfData[1] >> 23 | crsfData[2] << 9) & 0x07FF); + channels[6] = convert((crsfData[2] >> 2) & 0x07FF); + channels[7] = convert((crsfData[2] >> 13) & 0x07FF); + + channels[8] = convert((crsfData[2] >> 24 | crsfData[3] << 8) & 0x07FF); + channels[9] = convert((crsfData[3] >> 3) & 0x07FF); + channels[10] = convert((crsfData[3] >> 14) & 0x07FF); + channels[11] = convert((crsfData[3] >> 25 | crsfData[4] << 7) & 0x07FF); + + channels[12] = convert((crsfData[4] >> 4) & 0x07FF); + channels[13] = convert((crsfData[4] >> 15) & 0x07FF); + channels[14] = convert((crsfData[4] >> 26 | crsfData[5] << 6) & 0x07FF); + channels[15] = convert((crsfData[5] >> 5) & 0x07FF); +}*/ + +void Crsf::encodeRcData(CrsfFrame& frame, const CrsfData& data) +{ + frame.message.addr = CRSF_ADDRESS_FLIGHT_CONTROLLER; + frame.message.type = CRSF_FRAMETYPE_RC_CHANNELS_PACKED; + frame.message.size = sizeof(data) + 2; + std::memcpy(frame.message.payload, (void*)&data, sizeof(data)); + frame.message.payload[sizeof(data)] = crc(frame); +} + +uint16_t Crsf::convert(int v) +{ + /* conversion from RC value to PWM + * for 0x16 RC frame + * RC PWM + * min 172 -> 988us + * mid 992 -> 1500us + * max 1811 -> 2012us + * scale factor = (2012-988) / (1811-172) = 0.62477120195241 => 1024 / 1639 = 0.62477 + * offset = 988 - 172 * 0.62477120195241 = 880.53935326418548 => 988 - 107.46 = 880.54 + */ + return ((v * 1024) / 1639) + 881; + //return lrintf((0.62477120195241 * (float)v) + 880.54); + //return Math::map(v, 172, 1811, 988, 2012); + //return Math::mapi(v, 172, 1811, 988, 2012); +} + +uint8_t Crsf::crc(const CrsfFrame& frame) +{ + // CRC includes type and payload + uint8_t crc = Math::crc8_dvb_s2(0, frame.message.type); + for (int i = 0; i < frame.message.size - 2; i++) { // size includes type and crc + crc = Math::crc8_dvb_s2(crc, frame.message.payload[i]); + } + return crc; +} + +} + +} diff --git a/lib/Espfc/src/Rc/Crsf.h b/lib/Espfc/src/Rc/Crsf.h index 64e3bf76..39784613 100644 --- a/lib/Espfc/src/Rc/Crsf.h +++ b/lib/Espfc/src/Rc/Crsf.h @@ -1,8 +1,6 @@ -#ifndef _RC_CRSF_H_ -#define _RC_CRSF_H_ +#pragma once -#include "Math/Utils.h" -#include "Math/Crc.h" +#include namespace Espfc { @@ -127,119 +125,14 @@ union CrsfFrame class Crsf { public: - static void decodeRcData(uint16_t* channels, const CrsfData* frame) - { - channels[0] = convert(frame->chan0); - channels[1] = convert(frame->chan1); - channels[2] = convert(frame->chan2); - channels[3] = convert(frame->chan3); - channels[4] = convert(frame->chan4); - channels[5] = convert(frame->chan5); - channels[6] = convert(frame->chan6); - channels[7] = convert(frame->chan7); - channels[8] = convert(frame->chan8); - channels[9] = convert(frame->chan9); - channels[10] = convert(frame->chan10); - channels[11] = convert(frame->chan11); - channels[12] = convert(frame->chan12); - channels[13] = convert(frame->chan13); - channels[14] = convert(frame->chan14); - channels[15] = convert(frame->chan15); - } - - static void decodeRcDataShift8(uint16_t* channels, const CrsfData* frame) - { - // 8-bit - // 0....... ...1.... ......2. ........ .3...... ....4... .......5 ........ ..6..... .....7.. ........ - // 8....... ...9.... ......A. ........ .B...... ....C... .......D ........ ..E..... .....F.. ........ - const uint8_t * crsfData = reinterpret_cast(frame); - channels[0] = convert((crsfData[0] | crsfData[1] << 8) & 0x07FF); - channels[1] = convert((crsfData[1] >> 3 | crsfData[2] << 5) & 0x07FF); - channels[2] = convert((crsfData[2] >> 6 | crsfData[3] << 2 | crsfData[4] << 10) & 0x07FF); - channels[3] = convert((crsfData[4] >> 1 | crsfData[5] << 7) & 0x07FF); - - channels[4] = convert((crsfData[5] >> 4 | crsfData[6] << 4) & 0x07FF); - channels[5] = convert((crsfData[6] >> 7 | crsfData[7] << 1 | crsfData[8] << 9) & 0x07FF); - channels[6] = convert((crsfData[8] >> 2 | crsfData[9] << 6) & 0x07FF); - channels[7] = convert((crsfData[9] >> 5 | crsfData[10] << 3) & 0x07FF); - - channels[8] = convert((crsfData[11] | crsfData[12] << 8) & 0x07FF); - channels[9] = convert((crsfData[12] >> 3 | crsfData[13] << 5) & 0x07FF); - channels[10] = convert((crsfData[13] >> 6 | crsfData[14] << 2 | crsfData[15] << 10) & 0x07FF); - channels[11] = convert((crsfData[15] >> 1 | crsfData[16] << 7) & 0x07FF); - - channels[12] = convert((crsfData[16] >> 4 | crsfData[17] << 4) & 0x07FF); - channels[13] = convert((crsfData[17] >> 7 | crsfData[18] << 1 | crsfData[19] << 9) & 0x07FF); - channels[14] = convert((crsfData[19] >> 2 | crsfData[20] << 6) & 0x07FF); - channels[15] = convert((crsfData[20] >> 5 | crsfData[21] << 3) & 0x07FF); - } - - /*static void decodeRcDataShift32(uint16_t* channels, const CrsfData* frame) - { - // 32-bit - // 0..........1..........2......... .3..........4..........5........ ..6..........7..........8....... - // ...9..........A..........B...... ....C..........D..........E..... .....F.......... - const uint32_t * crsfData = reinterpret_cast(frame); - channels[0] = convert((crsfData[0]) & 0x07FF); - channels[1] = convert((crsfData[0] >> 11) & 0x07FF); - channels[2] = convert((crsfData[0] >> 22 | crsfData[1] << 10) & 0x07FF); - channels[3] = convert((crsfData[1] >> 1) & 0x07FF); - - channels[4] = convert((crsfData[1] >> 12) & 0x07FF); - channels[5] = convert((crsfData[1] >> 23 | crsfData[2] << 9) & 0x07FF); - channels[6] = convert((crsfData[2] >> 2) & 0x07FF); - channels[7] = convert((crsfData[2] >> 13) & 0x07FF); - - channels[8] = convert((crsfData[2] >> 24 | crsfData[3] << 8) & 0x07FF); - channels[9] = convert((crsfData[3] >> 3) & 0x07FF); - channels[10] = convert((crsfData[3] >> 14) & 0x07FF); - channels[11] = convert((crsfData[3] >> 25 | crsfData[4] << 7) & 0x07FF); - - channels[12] = convert((crsfData[4] >> 4) & 0x07FF); - channels[13] = convert((crsfData[4] >> 15) & 0x07FF); - channels[14] = convert((crsfData[4] >> 26 | crsfData[5] << 6) & 0x07FF); - channels[15] = convert((crsfData[5] >> 5) & 0x07FF); - }*/ - - static void encodeRcData(CrsfFrame& frame, const CrsfData& data) - { - frame.message.addr = CRSF_ADDRESS_FLIGHT_CONTROLLER; - frame.message.type = CRSF_FRAMETYPE_RC_CHANNELS_PACKED; - frame.message.size = sizeof(data) + 2; - memcpy(frame.message.payload, (void*)&data, sizeof(data)); - frame.message.payload[sizeof(data)] = crc(frame); - } - - static inline uint16_t convert(int v) - { - /* conversion from RC value to PWM - * for 0x16 RC frame - * RC PWM - * min 172 -> 988us - * mid 992 -> 1500us - * max 1811 -> 2012us - * scale factor = (2012-988) / (1811-172) = 0.62477120195241 => 1024 / 1639 = 0.62477 - * offset = 988 - 172 * 0.62477120195241 = 880.53935326418548 => 988 - 107.46 = 880.54 - */ - return ((v * 1024) / 1639) + 881; - //return lrintf((0.62477120195241 * (float)v) + 880.54); - //return Math::map(v, 172, 1811, 988, 2012); - //return Math::mapi(v, 172, 1811, 988, 2012); - } - - static uint8_t crc(const CrsfFrame& frame) - { - // CRC includes type and payload - uint8_t crc = Math::crc8_dvb_s2(0, frame.message.type); - for (int i = 0; i < frame.message.size - 2; i++) { // size includes type and crc - crc = Math::crc8_dvb_s2(crc, frame.message.payload[i]); - } - return crc; - } + static void decodeRcData(uint16_t* channels, const CrsfData* frame); + static void decodeRcDataShift8(uint16_t* channels, const CrsfData* frame); + //static void decodeRcDataShift32(uint16_t* channels, const CrsfData* frame); + static void encodeRcData(CrsfFrame& frame, const CrsfData& data); + static uint16_t convert(int v); + static uint8_t crc(const CrsfFrame& frame); }; } } - -#endif \ No newline at end of file diff --git a/lib/Espfc/src/Sensor/AccelSensor.h b/lib/Espfc/src/Sensor/AccelSensor.h index a293255a..8b7db252 100644 --- a/lib/Espfc/src/Sensor/AccelSensor.h +++ b/lib/Espfc/src/Sensor/AccelSensor.h @@ -48,7 +48,7 @@ class AccelSensor: public BaseSensor { if(!_model.accelActive()) return 0; - if(!_model.state.accelTimer.check()) return 0; + //if(!_model.state.accelTimer.check()) return 0; Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_READ); _gyro->readAccel(_model.state.accelRaw); diff --git a/lib/Espfc/src/Sensor/BaseSensor.h b/lib/Espfc/src/Sensor/BaseSensor.h index ac57d5c6..863b30de 100644 --- a/lib/Espfc/src/Sensor/BaseSensor.h +++ b/lib/Espfc/src/Sensor/BaseSensor.h @@ -1,11 +1,10 @@ #ifndef _ESPFC_SENSOR_BASE_SENSOR_H_ #define _ESPFC_SENSOR_BASE_SENSOR_H_ +#include #include "Model.h" #include "Hardware.h" -#include - namespace Espfc { namespace Sensor { @@ -13,7 +12,7 @@ namespace Sensor { class BaseSensor { public: - void align(VectorInt16& dest, uint8_t rotation) /* IRAM_ATTR */ + void FAST_CODE_ATTR align(VectorInt16& dest, uint8_t rotation) { const int16_t x = dest.x; const int16_t y = dest.y; diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp new file mode 100644 index 00000000..67815a36 --- /dev/null +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -0,0 +1,318 @@ + +#include "GyroSensor.h" +#include "Model.h" +#include "Utils/FilterHelper.h" + +#define ESPFC_FUZZY_ACCEL_ZERO 0.05 +#define ESPFC_FUZZY_GYRO_ZERO 0.20 + +namespace Espfc +{ +namespace Sensor +{ + +GyroSensor::GyroSensor(Model &model) : _dyn_notch_denom(1), _model(model) +{ +} + +int GyroSensor::begin() +{ + _gyro = _model.state.gyroDev; + if (!_gyro) return 0; + + _gyro->setDLPFMode(_model.config.gyroDlpf); + _gyro->setRate(_gyro->getRate()); + _model.state.gyroScale = radians(2000.f) / 32768.f; + + _model.state.gyroCalibrationState = CALIBRATION_START; // calibrate gyro on start + _model.state.gyroCalibrationRate = _model.state.loopTimer.rate; + _model.state.gyroBiasAlpha = 5.0f / _model.state.gyroCalibrationRate; + + _sma.begin(_model.config.loopSync); + _dyn_notch_denom = std::max((uint32_t)1, _model.state.loopTimer.rate / 1000); + _dyn_notch_sma.begin(_dyn_notch_denom); + _dyn_notch_enabled = _model.isActive(FEATURE_DYNAMIC_FILTER) && _model.config.dynamicFilter.width > 0 && _model.state.loopTimer.rate >= DynamicFilterConfig::MIN_FREQ; + _dyn_notch_debug = _model.config.debugMode == DEBUG_FFT_FREQ || _model.config.debugMode == DEBUG_FFT_TIME; + + _rpm_enabled = _model.config.rpmFilterHarmonics > 0 && _model.config.output.dshotTelemetry; + _rpm_motor_index = 0; + _rpm_fade_inv = 1.0f / _model.config.rpmFilterFade; + _rpm_min_freq = _model.config.rpmFilterMinFreq; + _rpm_max_freq = 0.48f * _model.state.loopTimer.rate; + _rpm_q = _model.config.rpmFilterQ * 0.01f; + + for (size_t i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) + { + _rpm_weights[i] = Math::clamp(0.01f * _model.config.rpmFilterWeights[i], 0.0f, 1.0f); + } + for (size_t i = 0; i < 3; i++) + { +#ifdef ESPFC_DSP + _fft[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.dynamicFilter, i); +#else + _freqAnalyzer[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.dynamicFilter); +#endif + } + + _model.logger.info().log(F("GYRO INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_gyro->getAddress()).log(_model.config.gyroDlpf).log(_gyro->getRate()).log(_model.state.gyroTimer.rate).logln(_model.state.gyroTimer.interval); + + return 1; +} + +int FAST_CODE_ATTR GyroSensor::read() +{ + if (!_model.gyroActive()) return 0; + + Stats::Measure measure(_model.state.stats, COUNTER_GYRO_READ); + + _gyro->readGyro(_model.state.gyroRaw); + + align(_model.state.gyroRaw, _model.config.gyroAlign); + + VectorFloat input = static_cast(_model.state.gyroRaw) * _model.state.gyroScale; + + if (_model.config.gyroFilter3.freq) + { + _model.state.gyroSampled = Utils::applyFilter(_model.state.gyroFilter3, input); + } + else + { + _model.state.gyroSampled = _sma.update(input); + } + + return 1; +} + +int FAST_CODE_ATTR GyroSensor::filter() +{ + if (!_model.gyroActive()) return 0; + + Stats::Measure measure(_model.state.stats, COUNTER_GYRO_FILTER); + + _model.state.gyro = _model.state.gyroSampled; + + calibrate(); + + _model.state.gyroScaled = _model.state.gyro; // must be after calibration + + for (size_t i = 0; i < 3; ++i) + { + _model.setDebug(DEBUG_GYRO_RAW, i, _model.state.gyroRaw[i]); + _model.setDebug(DEBUG_GYRO_SCALED, i, lrintf(degrees(_model.state.gyroScaled[i]))); + } + + _model.setDebug(DEBUG_GYRO_SAMPLE, 0, lrintf(degrees(_model.state.gyro[_model.config.debugAxis]))); + + _model.state.gyro = Utils::applyFilter(_model.state.gyroFilter2, _model.state.gyro); + + _model.setDebug(DEBUG_GYRO_SAMPLE, 1, lrintf(degrees(_model.state.gyro[_model.config.debugAxis]))); + + if (_rpm_enabled) + { + for (size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) + { + for (size_t n = 0; n < _model.config.rpmFilterHarmonics; n++) + { + _model.state.gyro = Utils::applyFilter(_model.state.rpmFilter[m][n], _model.state.gyro); + } + } + } + + _model.setDebug(DEBUG_GYRO_SAMPLE, 2, lrintf(degrees(_model.state.gyro[_model.config.debugAxis]))); + + _model.state.gyro = Utils::applyFilter(_model.state.gyroNotch1Filter, _model.state.gyro); + _model.state.gyro = Utils::applyFilter(_model.state.gyroNotch2Filter, _model.state.gyro); + _model.state.gyro = Utils::applyFilter(_model.state.gyroFilter, _model.state.gyro); + + _model.setDebug(DEBUG_GYRO_SAMPLE, 3, lrintf(degrees(_model.state.gyro[_model.config.debugAxis]))); + + if (_dyn_notch_enabled || _dyn_notch_debug) + { + _model.state.gyroDynNotch = _dyn_notch_sma.update(_model.state.gyro); + } + + if (_dyn_notch_enabled) + { + for (size_t p = 0; p < (size_t)_model.config.dynamicFilter.width; p++) + { + _model.state.gyro = Utils::applyFilter(_model.state.gyroDynNotchFilter[p], _model.state.gyro); + } + } + + for (size_t i = 0; i < 3; ++i) + { + _model.setDebug(DEBUG_GYRO_FILTERED, i, lrintf(degrees(_model.state.gyro[i]))); + } + + if (_model.accelActive()) + { + _model.state.gyroImu = Utils::applyFilter(_model.state.gyroImuFilter, _model.state.gyro); + } + + return 1; +} + +void FAST_CODE_ATTR GyroSensor::postLoop() +{ + rpmFilterUpdate(); + dynNotchFilterUpdate(); +} + +void FAST_CODE_ATTR GyroSensor::rpmFilterUpdate() +{ + if (!_rpm_enabled) return; + + Stats::Measure measure(_model.state.stats, COUNTER_RPM_UPDATE); + + const float motorFreq = _model.state.outputTelemetryFreq[_rpm_motor_index]; + for (size_t n = 0; n < _model.config.rpmFilterHarmonics; n++) + { + const float freq = Math::clamp(motorFreq * (n + 1), _rpm_min_freq, _rpm_max_freq); + const float freqMargin = freq - _rpm_min_freq; + float weight = _rpm_weights[n]; + if (freqMargin < _model.config.rpmFilterFade) + { + weight *= freqMargin * _rpm_fade_inv; + } + _model.state.rpmFilter[_rpm_motor_index][n][0].reconfigure(freq, freq, _rpm_q, weight); + for (size_t i = 1; i < 3; ++i) + { + // copy coefs from roll to pitch and yaw + _model.state.rpmFilter[_rpm_motor_index][n][i].reconfigure(_model.state.rpmFilter[_rpm_motor_index][n][0]); + } + } + + _model.setDebug(DEBUG_RPM_FILTER, _rpm_motor_index, lrintf(_model.state.outputTelemetryFreq[_rpm_motor_index])); + + _rpm_motor_index++; + if (_rpm_motor_index >= RPM_FILTER_MOTOR_MAX) + { + _rpm_motor_index = 0; + } +} + +void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() +{ + if (!_model.gyroActive()) return; + if (_model.state.loopTimer.rate < DynamicFilterConfig::MIN_FREQ) return; + + if (_dyn_notch_enabled || _dyn_notch_debug) + { + Stats::Measure measure(_model.state.stats, COUNTER_GYRO_FFT); + + const float q = _model.config.dynamicFilter.q * 0.01; + bool feed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0; + bool update = _model.state.dynamicFilterTimer.check(); + + for (size_t i = 0; i < 3; ++i) + { +#ifdef ESPFC_DSP + (void)update; + if (feed) + { + uint32_t startTime = micros(); + int status = _fft[i].update(_model.state.gyroDynNotch[i]); + if (_model.config.debugMode == DEBUG_FFT_TIME) + { + if (i == 0) + _model.state.debug[0] = status; + _model.state.debug[i + 1] = micros() - startTime; + } + if (_model.config.debugMode == DEBUG_FFT_FREQ && i == _model.config.debugAxis) + { + _model.state.debug[0] = lrintf(_fft[i].peaks[0].freq); + _model.state.debug[1] = lrintf(_fft[i].peaks[1].freq); + _model.state.debug[2] = lrintf(_fft[i].peaks[2].freq); + _model.state.debug[3] = lrintf(_fft[i].peaks[3].freq); + } + if (_dyn_notch_enabled && status) + { + for (size_t p = 0; p < (size_t)_model.config.dynamicFilter.width; p++) + { + float freq = _fft[i].peaks[p].freq; + if (freq >= _model.config.dynamicFilter.min_freq && freq <= _model.config.dynamicFilter.max_freq) + { + _model.state.gyroDynNotchFilter[p][i].reconfigure(freq, freq, q); + } + } + } + } +#else + if (feed) + { + uint32_t startTime = micros(); + _freqAnalyzer[i].update(_model.state.gyroDynNotch[i]); + float freq = _freqAnalyzer[i].freq; + if (_model.config.debugMode == DEBUG_FFT_TIME) + { + if (i == 0) + _model.state.debug[0] = update; + _model.state.debug[i + 1] = micros() - startTime; + } + if (_model.config.debugMode == DEBUG_FFT_FREQ) + { + if (update) + _model.state.debug[i] = lrintf(freq); + if (i == _model.config.debugAxis) + _model.state.debug[3] = lrintf(degrees(_model.state.gyroDynNotch[i])); + } + if (_dyn_notch_enabled && update) + { + if (freq >= _model.config.dynamicFilter.min_freq && freq <= _model.config.dynamicFilter.max_freq) + { + for (size_t p = 0; p < (size_t)_model.config.dynamicFilter.width; p++) + { + size_t x = (p + i) % 3; + int harmonic = (p / 3) + 1; + int16_t f = Math::clamp((int16_t)lrintf(freq * harmonic), _model.config.dynamicFilter.min_freq, _model.config.dynamicFilter.max_freq); + _model.state.gyroDynNotchFilter[p][x].reconfigure(f, f, q); + } + } + } + } +#endif + } + } +} + +void FAST_CODE_ATTR GyroSensor::calibrate() +{ + switch (_model.state.gyroCalibrationState) + { + case CALIBRATION_IDLE: + _model.state.gyro -= _model.state.gyroBias; + break; + case CALIBRATION_START: + //_model.state.gyroBias = VectorFloat(); + _model.state.gyroBiasSamples = 2 * _model.state.gyroCalibrationRate; + _model.state.gyroCalibrationState = CALIBRATION_UPDATE; + break; + case CALIBRATION_UPDATE: + { + VectorFloat deltaAccel = _model.state.accel - _model.state.accelPrev; + _model.state.accelPrev = _model.state.accel; + if (deltaAccel.getMagnitude() < ESPFC_FUZZY_ACCEL_ZERO && _model.state.gyro.getMagnitude() < ESPFC_FUZZY_GYRO_ZERO) + { + _model.state.gyroBias += (_model.state.gyro - _model.state.gyroBias) * _model.state.gyroBiasAlpha; + _model.state.gyroBiasSamples--; + } + if (_model.state.gyroBiasSamples <= 0) + _model.state.gyroCalibrationState = CALIBRATION_APPLY; + } + break; + case CALIBRATION_APPLY: + _model.state.gyroCalibrationState = CALIBRATION_SAVE; + break; + case CALIBRATION_SAVE: + _model.finishCalibration(); + _model.state.gyroCalibrationState = CALIBRATION_IDLE; + break; + default: + _model.state.gyroCalibrationState = CALIBRATION_IDLE; + break; + } +} + +} + +} diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index 9c8fcde0..b5b1d4aa 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -1,8 +1,6 @@ -#ifndef _ESPFC_SENSOR_GYRO_SENSOR_H_ -#define _ESPFC_SENSOR_GYRO_SENSOR_H_ +#pragma once #include "BaseSensor.h" -#include "Utils/FilterHelper.h" #include "Device/GyroDevice.h" #include "Math/Sma.h" #include "Math/FreqAnalyzer.h" @@ -20,310 +18,17 @@ namespace Sensor { class GyroSensor: public BaseSensor { public: - GyroSensor(Model& model): _dyn_notch_denom(1), _model(model) {} + GyroSensor(Model& model); - int begin() - { - _gyro = _model.state.gyroDev; - if(!_gyro) return 0; - - _gyro->setDLPFMode(_model.config.gyroDlpf); - _gyro->setRate(_gyro->getRate()); - _model.state.gyroScale = radians(2000.f) / 32768.f; - - _model.state.gyroCalibrationState = CALIBRATION_START; // calibrate gyro on start - _model.state.gyroCalibrationRate = _model.state.loopTimer.rate; - _model.state.gyroBiasAlpha = 5.0f / _model.state.gyroCalibrationRate; - - _sma.begin(_model.config.loopSync); - _dyn_notch_denom = std::max((uint32_t)1, _model.state.loopTimer.rate / 1000); - _dyn_notch_sma.begin(_dyn_notch_denom); - _dyn_notch_enabled = _model.isActive(FEATURE_DYNAMIC_FILTER) && _model.config.dynamicFilter.width > 0 && _model.state.loopTimer.rate >= DynamicFilterConfig::MIN_FREQ; - _dyn_notch_debug = _model.config.debugMode == DEBUG_FFT_FREQ || _model.config.debugMode == DEBUG_FFT_TIME; - - _rpm_enabled = _model.config.rpmFilterHarmonics > 0 && _model.config.output.dshotTelemetry; - _rpm_motor_index = 0; - _rpm_fade_inv = 1.0f / _model.config.rpmFilterFade; - _rpm_min_freq = _model.config.rpmFilterMinFreq; - _rpm_max_freq = 0.48f * _model.state.loopTimer.rate; - _rpm_q = _model.config.rpmFilterQ * 0.01f; - - for(size_t i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) - { - _rpm_weights[i] = Math::clamp(0.01f * _model.config.rpmFilterWeights[i], 0.0f, 1.0f); - } - for(size_t i = 0; i < 3; i++) - { -#ifdef ESPFC_DSP - _fft[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.dynamicFilter, i); -#else - _freqAnalyzer[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.dynamicFilter); -#endif - } - - _model.logger.info().log(F("GYRO INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_gyro->getAddress()).log(_model.config.gyroDlpf).log(_gyro->getRate()).log(_model.state.gyroTimer.rate).logln(_model.state.gyroTimer.interval); - - return 1; - } - - int update() - { - int status = read(); - - if (status) filter(); - - return status; - } - - int read() - { - if(!_model.gyroActive()) return 0; - - Stats::Measure measure(_model.state.stats, COUNTER_GYRO_READ); - - _gyro->readGyro(_model.state.gyroRaw); - - align(_model.state.gyroRaw, _model.config.gyroAlign); - - VectorFloat input = static_cast(_model.state.gyroRaw) * _model.state.gyroScale; - - if(_model.config.gyroFilter3.freq) - { - _model.state.gyroSampled = Utils::applyFilter(_model.state.gyroFilter3, input); - } - else - { - _model.state.gyroSampled = _sma.update(input); - } - - return 1; - } - - int filter() - { - if(!_model.gyroActive()) return 0; - - Stats::Measure measure(_model.state.stats, COUNTER_GYRO_FILTER); - - _model.state.gyro = _model.state.gyroSampled; - - calibrate(); - - _model.state.gyroScaled = _model.state.gyro; // must be after calibration - - for(size_t i = 0; i < 3; ++i) - { - _model.setDebug(DEBUG_GYRO_RAW, i, _model.state.gyroRaw[i]); - _model.setDebug(DEBUG_GYRO_SCALED, i, lrintf(degrees(_model.state.gyroScaled[i]))); - } - - _model.setDebug(DEBUG_GYRO_SAMPLE, 0, lrintf(degrees(_model.state.gyro[_model.config.debugAxis]))); - - _model.state.gyro = Utils::applyFilter(_model.state.gyroFilter2, _model.state.gyro); - - _model.setDebug(DEBUG_GYRO_SAMPLE, 1, lrintf(degrees(_model.state.gyro[_model.config.debugAxis]))); - - if(_rpm_enabled) - { - for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) - { - for(size_t n = 0; n < _model.config.rpmFilterHarmonics; n++) - { - _model.state.gyro = Utils::applyFilter(_model.state.rpmFilter[m][n], _model.state.gyro); - } - } - } - - _model.setDebug(DEBUG_GYRO_SAMPLE, 2, lrintf(degrees(_model.state.gyro[_model.config.debugAxis]))); - - _model.state.gyro = Utils::applyFilter(_model.state.gyroNotch1Filter, _model.state.gyro); - _model.state.gyro = Utils::applyFilter(_model.state.gyroNotch2Filter, _model.state.gyro); - _model.state.gyro = Utils::applyFilter(_model.state.gyroFilter, _model.state.gyro); - - _model.setDebug(DEBUG_GYRO_SAMPLE, 3, lrintf(degrees(_model.state.gyro[_model.config.debugAxis]))); - - if(_dyn_notch_enabled || _dyn_notch_debug) - { - _model.state.gyroDynNotch = _dyn_notch_sma.update(_model.state.gyro); - } - - if(_dyn_notch_enabled) - { - for(size_t p = 0; p < (size_t)_model.config.dynamicFilter.width; p++) - { - _model.state.gyro = Utils::applyFilter(_model.state.gyroDynNotchFilter[p], _model.state.gyro); - } - } - - for(size_t i = 0; i < 3; ++i) - { - _model.setDebug(DEBUG_GYRO_FILTERED, i, lrintf(degrees(_model.state.gyro[i]))); - } - - if(_model.accelActive()) - { - _model.state.gyroImu = Utils::applyFilter(_model.state.gyroImuFilter, _model.state.gyro); - } - - return 1; - } - - void postLoop() - { - rpmFilterUpdate(); - dynNotchFilterUpdate(); - } - - void rpmFilterUpdate() - { - if(!_rpm_enabled) return; - - Stats::Measure measure(_model.state.stats, COUNTER_RPM_UPDATE); - - const float motorFreq = _model.state.outputTelemetryFreq[_rpm_motor_index]; - for(size_t n = 0; n < _model.config.rpmFilterHarmonics; n++) - { - const float freq = Math::clamp(motorFreq * (n + 1), _rpm_min_freq, _rpm_max_freq); - const float freqMargin = freq - _rpm_min_freq; - float weight = _rpm_weights[n]; - if(freqMargin < _model.config.rpmFilterFade) - { - weight *= freqMargin * _rpm_fade_inv; - } - _model.state.rpmFilter[_rpm_motor_index][n][0].reconfigure(freq, freq, _rpm_q, weight); - for(size_t i = 1; i < 3; ++i) - { - // copy coefs from roll to pitch and yaw - _model.state.rpmFilter[_rpm_motor_index][n][i].reconfigure(_model.state.rpmFilter[_rpm_motor_index][n][0]); - } - } - - _model.setDebug(DEBUG_RPM_FILTER, _rpm_motor_index, lrintf(_model.state.outputTelemetryFreq[_rpm_motor_index])); - - _rpm_motor_index++; - if(_rpm_motor_index >= RPM_FILTER_MOTOR_MAX) - { - _rpm_motor_index = 0; - } - } - - void dynNotchFilterUpdate() - { - if(!_model.gyroActive()) return; - if(_model.state.loopTimer.rate < DynamicFilterConfig::MIN_FREQ) return; - - if(_dyn_notch_enabled || _dyn_notch_debug) - { - Stats::Measure measure(_model.state.stats, COUNTER_GYRO_FFT); - - const float q = _model.config.dynamicFilter.q * 0.01; - bool feed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0; - bool update = _model.state.dynamicFilterTimer.check(); - - for(size_t i = 0; i < 3; ++i) - { -#ifdef ESPFC_DSP - (void)update; - if(feed) - { - uint32_t startTime = micros(); - int status = _fft[i].update(_model.state.gyroDynNotch[i]); - if(_model.config.debugMode == DEBUG_FFT_TIME) - { - if(i == 0) _model.state.debug[0] = status; - _model.state.debug[i + 1] = micros() - startTime; - } - if(_model.config.debugMode == DEBUG_FFT_FREQ && i == _model.config.debugAxis) - { - _model.state.debug[0] = lrintf(_fft[i].peaks[0].freq); - _model.state.debug[1] = lrintf(_fft[i].peaks[1].freq); - _model.state.debug[2] = lrintf(_fft[i].peaks[2].freq); - _model.state.debug[3] = lrintf(_fft[i].peaks[3].freq); - } - if(_dyn_notch_enabled && status) - { - for(size_t p = 0; p < (size_t)_model.config.dynamicFilter.width; p++) - { - float freq = _fft[i].peaks[p].freq; - if(freq >= _model.config.dynamicFilter.min_freq && freq <= _model.config.dynamicFilter.max_freq) - { - _model.state.gyroDynNotchFilter[p][i].reconfigure(freq, freq, q); - } - } - } - } -#else - if(feed) - { - uint32_t startTime = micros(); - _freqAnalyzer[i].update(_model.state.gyroDynNotch[i]); - float freq = _freqAnalyzer[i].freq; - if(_model.config.debugMode == DEBUG_FFT_TIME) - { - if(i == 0) _model.state.debug[0] = update; - _model.state.debug[i + 1] = micros() - startTime; - } - if(_model.config.debugMode == DEBUG_FFT_FREQ) - { - if(update) _model.state.debug[i] = lrintf(freq); - if(i == _model.config.debugAxis) _model.state.debug[3] = lrintf(degrees(_model.state.gyroDynNotch[i])); - } - if(_dyn_notch_enabled && update) - { - if(freq >= _model.config.dynamicFilter.min_freq && freq <= _model.config.dynamicFilter.max_freq) - { - for(size_t p = 0; p < (size_t)_model.config.dynamicFilter.width; p++) - { - size_t x = (p + i) % 3; - int harmonic = (p / 3) + 1; - int16_t f = Math::clamp((int16_t)lrintf(freq * harmonic), _model.config.dynamicFilter.min_freq, _model.config.dynamicFilter.max_freq); - _model.state.gyroDynNotchFilter[p][x].reconfigure(f, f, q); - } - } - } - } -#endif - } - } - } + int begin(); + int read(); + int filter(); + void postLoop(); + void rpmFilterUpdate(); + void dynNotchFilterUpdate(); private: - void calibrate() - { - switch(_model.state.gyroCalibrationState) - { - case CALIBRATION_IDLE: - _model.state.gyro -= _model.state.gyroBias; - break; - case CALIBRATION_START: - //_model.state.gyroBias = VectorFloat(); - _model.state.gyroBiasSamples = 2 * _model.state.gyroCalibrationRate; - _model.state.gyroCalibrationState = CALIBRATION_UPDATE; - break; - case CALIBRATION_UPDATE: - { - VectorFloat deltaAccel = _model.state.accel - _model.state.accelPrev; - _model.state.accelPrev = _model.state.accel; - if(deltaAccel.getMagnitude() < ESPFC_FUZZY_ACCEL_ZERO && _model.state.gyro.getMagnitude() < ESPFC_FUZZY_GYRO_ZERO) - { - _model.state.gyroBias += (_model.state.gyro - _model.state.gyroBias) * _model.state.gyroBiasAlpha; - _model.state.gyroBiasSamples--; - } - if(_model.state.gyroBiasSamples <= 0) _model.state.gyroCalibrationState = CALIBRATION_APPLY; - } - break; - case CALIBRATION_APPLY: - _model.state.gyroCalibrationState = CALIBRATION_SAVE; - break; - case CALIBRATION_SAVE: - _model.finishCalibration(); - _model.state.gyroCalibrationState = CALIBRATION_IDLE; - break; - default: - _model.state.gyroCalibrationState = CALIBRATION_IDLE; - break; - } - } + void calibrate(); Math::Sma _sma; Math::Sma _dyn_notch_sma; @@ -352,4 +57,3 @@ class GyroSensor: public BaseSensor } } -#endif diff --git a/lib/Espfc/src/SensorManager.cpp b/lib/Espfc/src/SensorManager.cpp new file mode 100644 index 00000000..e74d0102 --- /dev/null +++ b/lib/Espfc/src/SensorManager.cpp @@ -0,0 +1,105 @@ +#include "SensorManager.h" + +namespace Espfc { + +SensorManager::SensorManager(Model& model): _model(model), _gyro(model), _accel(model), _mag(model), _baro(model), _voltage(model), _fusion(model), _fusionUpdate(false) {} + +int SensorManager::begin() +{ + _gyro.begin(); + _accel.begin(); + _mag.begin(); + _baro.begin(); + _voltage.begin(); + _fusion.begin(); + + return 1; +} + +int FAST_CODE_ATTR SensorManager::read() +{ + _gyro.read(); + + if(_model.state.loopTimer.syncTo(_model.state.gyroTimer)) + { + _model.state.appQueue.send(Event(EVENT_GYRO_READ)); + } + + if(_model.state.accelTimer.syncTo(_model.state.gyroTimer)) + { + _accel.update(); + _model.state.appQueue.send(Event(EVENT_ACCEL_READ)); + return 1; + } + + if(_mag.update()) return 1; + + if(_baro.update()) return 1; + + if(_voltage.update()) return 1; + + return 0; +} + +int FAST_CODE_ATTR SensorManager::preLoop() +{ + _gyro.filter(); + if(_model.state.gyroBiasSamples == 0) + { + _model.state.gyroBiasSamples = -1; + _fusion.restoreGain(); + } + return 1; +} + +int SensorManager::postLoop() +{ + _gyro.postLoop(); + return 1; +} + +int FAST_CODE_ATTR SensorManager::fusion() +{ + return _fusion.update(); +} + +// main task +int FAST_CODE_ATTR SensorManager::update() +{ + _gyro.read(); + return preLoop(); +} + +// sub task +int SensorManager::updateDelayed() +{ + _gyro.postLoop(); + + // update at most one sensor besides gyro + int status = 0; + if(_model.state.accelTimer.syncTo(_model.state.gyroTimer)) + { + _accel.update(); + status = 1; + } + + // delay imu update to next cycle + if(_fusionUpdate) + { + _fusionUpdate = false; + _fusion.update(); + } + _fusionUpdate = status; + + if(status) return 1; + + if(_mag.update()) return 1; + + if(_baro.update()) return 1; + + if(_voltage.update()) return 0; + + return 0; +} + +} diff --git a/lib/Espfc/src/SensorManager.h b/lib/Espfc/src/SensorManager.h index c140b253..5e5de836 100644 --- a/lib/Espfc/src/SensorManager.h +++ b/lib/Espfc/src/SensorManager.h @@ -1,11 +1,7 @@ -#ifndef _ESPFC_SENSOR_H_ -#define _ESPFC_SENSOR_H_ +#pragma once -#include #include "Model.h" -#include "Filter.h" #include "Fusion.h" -#include "Hardware.h" #include "Sensor/GyroSensor.h" #include "Sensor/AccelSensor.h" #include "Sensor/MagSensor.h" @@ -17,118 +13,19 @@ namespace Espfc { class SensorManager { public: - SensorManager(Model& model): _model(model), _gyro(model), _accel(model), _mag(model), _baro(model), _voltage(model), _fusion(model), _fusionUpdate(false) {} - - int begin() - { - _gyro.begin(); - _accel.begin(); - _mag.begin(); - _baro.begin(); - _voltage.begin(); - _fusion.begin(); - - return 1; - } - - int read() - { - _gyro.read(); - if(_model.state.loopTimer.syncTo(_model.state.gyroTimer)) - { - _model.state.appQueue.send(Event(EVENT_GYRO_READ)); - } - - int status = _accel.update(); - if(status) - { - _model.state.appQueue.send(Event(EVENT_ACCEL_READ)); - } - - if (!status) - { - status = _mag.update(); - } - - if(!status) - { - status = _baro.update(); - } - - if(!status) - { - status = _voltage.update(); - } - - return 1; - } - - int preLoop() - { - _gyro.filter(); - if(_model.state.gyroBiasSamples == 0) - { - _model.state.gyroBiasSamples = -1; - _fusion.restoreGain(); - } - return 1; - } - - int postLoop() - { - _gyro.postLoop(); - return 1; - } - - int fusion() - { - return _fusion.update(); - } + SensorManager(Model& model); + int begin(); + int read(); + int preLoop(); + int postLoop(); + int fusion(); // main task - int update() - { - int status = _gyro.update(); - - if(_model.state.gyroBiasSamples == 0) - { - _model.state.gyroBiasSamples = -1; - _fusion.restoreGain(); - } - - return status; - } - + int update(); // sub task - int updateDelayed() - { - _gyro.postLoop(); - int status = _accel.update(); - if(_fusionUpdate) - { - _fusionUpdate = false; - _fusion.update(); - } - _fusionUpdate = status; // update in next loop cycle - - if(!status) - { - status = _mag.update(); - } - - if(!status) - { - status = _baro.update(); - } - - if(!status) - { - _voltage.update(); - } - - return status; - } + int updateDelayed(); + private: Model& _model; Sensor::GyroSensor _gyro; Sensor::AccelSensor _accel; @@ -140,5 +37,3 @@ class SensorManager }; } - -#endif diff --git a/lib/Espfc/src/SerialManager.cpp b/lib/Espfc/src/SerialManager.cpp new file mode 100644 index 00000000..744b9c8f --- /dev/null +++ b/lib/Espfc/src/SerialManager.cpp @@ -0,0 +1,214 @@ +#include "SerialManager.h" +#include "Device/SerialDeviceAdapter.h" +#include "Debug_Espfc.h" + +#ifdef ESPFC_SERIAL_0 + static Espfc::Device::SerialDeviceAdapter _uart0(ESPFC_SERIAL_0_DEV); +#endif + +#ifdef ESPFC_SERIAL_1 + static Espfc::Device::SerialDeviceAdapter _uart1(ESPFC_SERIAL_1_DEV); +#endif + +#ifdef ESPFC_SERIAL_2 + static Espfc::Device::SerialDeviceAdapter _uart2(ESPFC_SERIAL_2_DEV); +#endif + +#ifdef ESPFC_SERIAL_USB + static Espfc::Device::SerialDeviceAdapter _usb(ESPFC_SERIAL_USB_DEV); +#endif + +namespace Espfc { + +SerialManager::SerialManager(Model& model): _model(model), _msp(model), _cli(model), +#ifdef ESPFC_SERIAL_SOFT_0_WIFI +_wireless(model), +#endif +_telemetry(model), _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) + { + //D("uart-no-func", i, spc.id, spc.functionMask, spc.baud); + continue; + } + + SerialDeviceConfig sdc; + sdc.baud = spc.baud; + +#ifdef ESPFC_SERIAL_USB + const bool hasUsbPort = true; + const bool isUsbPort = i == SERIAL_USB; +#else + const bool hasUsbPort = false; + const bool isUsbPort = false; +#endif + +#ifdef ESPFC_SERIAL_REMAP_PINS + if(!isUsbPort) + { + const size_t pin_idx = 2 * (hasUsbPort ? i - 1 : i); + sdc.tx_pin = _model.config.pin[pin_idx + PIN_SERIAL_0_TX]; + 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; + } + } +#else + (void)(isUsbPort && hasUsbPort); +#endif + + if(spc.functionMask & SERIAL_FUNCTION_RX_SERIAL) + { + switch(_model.config.input.serialRxProvider) + { + case SERIALRX_SBUS: + sdc.baud = 100000ul; + sdc.parity = SDC_SERIAL_PARITY_EVEN; + sdc.stop_bits = SDC_SERIAL_STOP_BITS_2; + sdc.inverted = true; + break; + case SERIALRX_CRSF: + sdc.baud = 420000ul; + //sdc.parity = SDC_SERIAL_PARITY_EVEN; + //sdc.stop_bits = SDC_SERIAL_STOP_BITS_2; + //sdc.inverted = true; + break; + default: + break; + } + } + else if(spc.functionMask & SERIAL_FUNCTION_BLACKBOX) + { + //sdc.baud = spc.blackboxBaud; + if(sdc.baud == 230400 || sdc.baud == 460800) + { + sdc.stop_bits = SDC_SERIAL_STOP_BITS_2; + } + } + + /*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; + if(i == ESPFC_SERIAL_DEBUG_PORT) + { + initDebugStream(port); + } + + _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); + } + +#ifdef ESPFC_SERIAL_SOFT_0_WIFI + _wireless.begin(); +#endif + + return 1; +} + +int FAST_CODE_ATTR SerialManager::update() +{ + Stats::Measure measure(_model.state.stats, COUNTER_SERIAL); + + //D("serial", _current); + SerialPortState& ss = _model.state.serial[_current]; + const SerialPortConfig& sc = _model.config.serial[_current]; + Device::SerialDevice * stream = ss.stream; + + bool serialRx = sc.functionMask & SERIAL_FUNCTION_RX_SERIAL; + if(stream && !serialRx) + { + 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.process(*c, ss.mspRequest, ss.mspResponse, *stream); + if(!consumed) + { + _cli.process(*c, ss.cliCmd, *stream); + } + } + c++; + } + } + if(!stream->available()) + { + _msp.postCommand(); + } + } + + if(sc.functionMask & SERIAL_FUNCTION_TELEMETRY_FRSKY && _model.state.telemetryTimer.check()) + { + _telemetry.process(*stream); + } + +#ifdef ESPFC_SERIAL_SOFT_0_WIFI + if(_current == SERIAL_SOFT_0) + { + _wireless.update(); + } +#endif + + next(); + + return 1; +} + +Device::SerialDevice * SerialManager::getSerialPortById(SerialPort portId) +{ + switch(portId) + { +#ifdef ESPFC_SERIAL_0 + case SERIAL_UART_0: return &_uart0; +#endif +#ifdef ESPFC_SERIAL_1 + case SERIAL_UART_1: return &_uart1; +#endif +#ifdef ESPFC_SERIAL_2 + case SERIAL_UART_2: return &_uart2; +#endif +#ifdef ESPFC_SERIAL_USB + case SERIAL_USB: return &_usb; +#endif + default: return nullptr; + } +} + +} diff --git a/lib/Espfc/src/SerialManager.h b/lib/Espfc/src/SerialManager.h index 64886ea3..d0e0721a 100644 --- a/lib/Espfc/src/SerialManager.h +++ b/lib/Espfc/src/SerialManager.h @@ -1,232 +1,25 @@ -#ifndef _ESPFC_SERIAL_MANAGER_H_ -#define _ESPFC_SERIAL_MANAGER_H_ +#pragma once #include "Model.h" #include "Device/SerialDevice.h" -#include "Device/SerialDeviceAdapter.h" -#include "EspSoftSerial.h" +#ifdef ESPFC_SERIAL_SOFT_0_WIFI +#include "Wireless.h" +#endif #include "Msp/MspProcessor.h" #include "Cli.h" -#include "Wireless.h" #include "Telemetry.h" -#include "Debug_Espfc.h" - -namespace { - -#ifdef ESPFC_SERIAL_0 - static Espfc::Device::SerialDeviceAdapter _uart0(ESPFC_SERIAL_0_DEV); -#endif - -#ifdef ESPFC_SERIAL_1 - static Espfc::Device::SerialDeviceAdapter _uart1(ESPFC_SERIAL_1_DEV); -#endif - -#ifdef ESPFC_SERIAL_2 - static Espfc::Device::SerialDeviceAdapter _uart2(ESPFC_SERIAL_2_DEV); -#endif - -#ifdef ESPFC_SERIAL_USB - static Espfc::Device::SerialDeviceAdapter _usb(ESPFC_SERIAL_USB_DEV); -#endif - -#ifdef ESPFC_SERIAL_SOFT_0_RX - static EspSoftSerial softSerial; - static Espfc::Device::SerialDeviceAdapter _soft0(softSerial); -#endif - -} namespace Espfc { class SerialManager { public: - SerialManager(Model& model): _model(model), _msp(model), _cli(model), _wireless(model), _telemetry(model), _current(SERIAL_UART_0) {} - - int begin() - { -#ifdef ESPFC_SERIAL_SOFT_0_WIFI - _wireless.begin(); -#endif - - 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) - { - //D("uart-no-func", i, spc.id, spc.functionMask, spc.baud); - continue; - } - - SerialDeviceConfig sdc; - sdc.baud = spc.baud; - -#ifdef ESPFC_SERIAL_USB - const bool hasUsbPort = true; - const bool isUsbPort = i == SERIAL_USB; -#else - const bool hasUsbPort = false; - const bool isUsbPort = false; -#endif - -#ifdef ESPFC_SERIAL_REMAP_PINS - if(!isUsbPort) - { - const size_t pin_idx = 2 * (hasUsbPort ? i - 1 : i); - sdc.tx_pin = _model.config.pin[pin_idx + PIN_SERIAL_0_TX]; - 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; - } - } -#else - (void)(isUsbPort && hasUsbPort); -#endif + SerialManager(Model& model); - if(spc.functionMask & SERIAL_FUNCTION_RX_SERIAL) - { - switch(_model.config.input.serialRxProvider) - { - case SERIALRX_SBUS: - sdc.baud = 100000ul; - sdc.parity = SDC_SERIAL_PARITY_EVEN; - sdc.stop_bits = SDC_SERIAL_STOP_BITS_2; - sdc.inverted = true; - break; - case SERIALRX_CRSF: - sdc.baud = 420000ul; - //sdc.parity = SDC_SERIAL_PARITY_EVEN; - //sdc.stop_bits = SDC_SERIAL_STOP_BITS_2; - //sdc.inverted = true; - break; - default: - break; - } - } - else if(spc.functionMask & SERIAL_FUNCTION_BLACKBOX) - { - //sdc.baud = spc.blackboxBaud; - if(sdc.baud == 230400 || sdc.baud == 460800) - { - sdc.stop_bits = SDC_SERIAL_STOP_BITS_2; - } - } + int begin(); + int update(); - /*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; - - if(i == ESPFC_SERIAL_DEBUG_PORT) - { - initDebugStream(port); - } - - _model.logger.info().log(F("UART")).log(i).log(spc.id).log(spc.functionMask).log(sdc.baud).log(sdc.tx_pin).logln(sdc.rx_pin); - } - return 1; - } - - int update() - { - //D("serial", _current); - SerialPortState& ss = _model.state.serial[_current]; - const SerialPortConfig& sc = _model.config.serial[_current]; - Device::SerialDevice * stream = ss.stream; - - { - Stats::Measure measure(_model.state.stats, COUNTER_SERIAL); - if(!stream || sc.functionMask & SERIAL_FUNCTION_RX_SERIAL) { - next(); - return 0; - } - - size_t len = stream->available(); - if(len > 0) - { - uint8_t buff[64] = {0}; - len = std::min(len, sizeof(buff)); - stream->readMany(buff, len); - char * c = (char*)&buff[0]; - while(len--) - { - if(sc.functionMask & SERIAL_FUNCTION_MSP) - { - bool consumed = _msp.process(*c, ss.mspRequest, ss.mspResponse, *stream); - if(!consumed) - { - _cli.process(*c, ss.cliCmd, *stream); - } - } - c++; - } - } - } - if(sc.functionMask & SERIAL_FUNCTION_TELEMETRY_FRSKY && _model.state.telemetryTimer.check()) - { - _telemetry.process(*stream); - } - -#ifdef ESPFC_SERIAL_SOFT_0_WIFI - if(_current == SERIAL_SOFT_0) - { - _wireless.update(); - } -#endif - - next(); - - return 1; - } - - static Device::SerialDevice * getSerialPortById(SerialPort portId) - { - switch(portId) - { -#ifdef ESPFC_SERIAL_0 - case SERIAL_UART_0: return &_uart0; -#endif -#ifdef ESPFC_SERIAL_1 - case SERIAL_UART_1: return &_uart1; -#endif -#ifdef ESPFC_SERIAL_2 - case SERIAL_UART_2: return &_uart2; -#endif -#ifdef ESPFC_SERIAL_USB - case SERIAL_USB: return &_usb; -#endif -#ifdef ESPFC_SERIAL_SOFT_0_RX - case SERIAL_SOFT_0: return &_soft0; -#endif - default: return nullptr; - } - } + static Device::SerialDevice * getSerialPortById(SerialPort portId); private: void next() @@ -238,11 +31,11 @@ class SerialManager Model& _model; Msp::MspProcessor _msp; Cli _cli; +#ifdef ESPFC_SERIAL_SOFT_0_WIFI Wireless _wireless; +#endif Telemetry _telemetry; size_t _current; }; } - -#endif \ No newline at end of file diff --git a/lib/Espfc/src/Stats.h b/lib/Espfc/src/Stats.h index 9b5e3fcd..20725b15 100644 --- a/lib/Espfc/src/Stats.h +++ b/lib/Espfc/src/Stats.h @@ -18,16 +18,16 @@ enum StatCounter : int8_t { COUNTER_BARO, COUNTER_IMU_FUSION, COUNTER_IMU_FUSION2, - COUNTER_INPUT_READ, - COUNTER_INPUT_FILTER, - COUNTER_FAILSAFE, - COUNTER_ACTUATOR, COUNTER_OUTER_PID, COUNTER_INNER_PID, COUNTER_MIXER, COUNTER_MIXER_WRITE, COUNTER_MIXER_READ, COUNTER_BLACKBOX, + COUNTER_INPUT_READ, + COUNTER_INPUT_FILTER, + COUNTER_FAILSAFE, + COUNTER_ACTUATOR, COUNTER_TELEMETRY, COUNTER_SERIAL, COUNTER_WIFI, @@ -85,7 +85,7 @@ class Stats uint32_t now = micros(); uint32_t diff = now - _loop_last; //_loop_time = diff; - _loop_time += (((int32_t)diff - _loop_time + 8) >> 4); + _loop_time += (((int32_t)diff - _loop_time + 4) >> 3); _loop_last = now; } @@ -154,8 +154,12 @@ class Stats float getCpuLoad() const { - float load = std::max(getLoad(COUNTER_CPU_0), getLoad(COUNTER_CPU_1)); - return load; + float cpu0 = getLoad(COUNTER_CPU_0); + float cpu1 = getLoad(COUNTER_CPU_1); + float maxLoad = std::max(cpu0, cpu1); + float minLoad = std::min(cpu0, cpu1); + float alpha = maxLoad / (minLoad + maxLoad); + return alpha * maxLoad + (1.f - alpha) * minLoad; } float getCpuTime() const @@ -167,34 +171,34 @@ class Stats { switch(c) { - case COUNTER_GYRO_READ: return PSTR(" gyro_r"); - case COUNTER_GYRO_FILTER: return PSTR(" gyro_f"); - case COUNTER_GYRO_FFT: return PSTR(" gyro_a"); - case COUNTER_RPM_UPDATE: return PSTR(" rpm_u"); - case COUNTER_ACCEL_READ: return PSTR(" acc_r"); - case COUNTER_ACCEL_FILTER: return PSTR(" acc_f"); - 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_IMU_FUSION: return PSTR(" imu_p"); - case COUNTER_IMU_FUSION2: return PSTR(" imu_c"); - case COUNTER_INPUT_READ: return PSTR(" rx_r"); - case COUNTER_INPUT_FILTER: return PSTR(" rx_f"); - case COUNTER_FAILSAFE: return PSTR(" rx_s"); - case COUNTER_ACTUATOR: return PSTR(" rx_a"); - case COUNTER_OUTER_PID: return PSTR(" pid_o"); - case COUNTER_INNER_PID: return PSTR(" pid_i"); - case COUNTER_MIXER: return PSTR("mixer_p"); - case COUNTER_MIXER_WRITE: return PSTR("mixer_w"); - case COUNTER_MIXER_READ: return PSTR("mixer_r"); - case COUNTER_BLACKBOX: return PSTR(" bblog"); - case COUNTER_TELEMETRY: return PSTR(" tlm"); - case COUNTER_SERIAL: return PSTR(" serial"); - case COUNTER_WIFI: return PSTR(" wifi"); - case COUNTER_BATTERY: return PSTR(" bat"); - case COUNTER_CPU_0: return PSTR(" cpu_0"); - case COUNTER_CPU_1: return PSTR(" cpu_1"); - default: return PSTR("unknown"); + case COUNTER_GYRO_READ: return PSTR("gyro_r"); + case COUNTER_GYRO_FILTER: return PSTR("gyro_f"); + case COUNTER_GYRO_FFT: return PSTR("gyro_a"); + case COUNTER_RPM_UPDATE: return PSTR(" rpm_u"); + case COUNTER_ACCEL_READ: return PSTR(" acc_r"); + case COUNTER_ACCEL_FILTER: return PSTR(" acc_f"); + 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_IMU_FUSION: return PSTR(" imu_p"); + case COUNTER_IMU_FUSION2: return PSTR(" imu_c"); + case COUNTER_OUTER_PID: return PSTR(" pid_o"); + case COUNTER_INNER_PID: return PSTR(" pid_i"); + case COUNTER_MIXER: return PSTR(" mix_p"); + case COUNTER_MIXER_WRITE: return PSTR(" mix_w"); + case COUNTER_MIXER_READ: return PSTR(" mix_r"); + case COUNTER_BLACKBOX: return PSTR(" bblog"); + case COUNTER_INPUT_READ: return PSTR(" rx_r"); + case COUNTER_INPUT_FILTER: return PSTR(" rx_f"); + case COUNTER_FAILSAFE: return PSTR(" rx_s"); + case COUNTER_ACTUATOR: return PSTR(" rx_a"); + case COUNTER_SERIAL: return PSTR("serial"); + case COUNTER_WIFI: return PSTR(" wifi"); + case COUNTER_BATTERY: return PSTR(" bat"); + case COUNTER_TELEMETRY: return PSTR(" tlm"); + case COUNTER_CPU_0: return PSTR(" cpu_0"); + case COUNTER_CPU_1: return PSTR(" cpu_1"); + default: return PSTR("unknwn"); } } diff --git a/lib/Espfc/src/Target/Queue.cpp b/lib/Espfc/src/Target/Queue.cpp new file mode 100644 index 00000000..61d69502 --- /dev/null +++ b/lib/Espfc/src/Target/Queue.cpp @@ -0,0 +1,25 @@ +#include "Target.h" + +#if defined(UNIT_TEST) || !defined(ESPFC_MULTI_CORE) + +#include "Queue.h" + +namespace Espfc { + +namespace Target { + +void Queue::begin() {} + +void FAST_CODE_ATTR Queue::send(const Event& e) { (void)e; } + +Event FAST_CODE_ATTR Queue::receive() { return Event(); } + +bool FAST_CODE_ATTR Queue::isEmpty() const { return true; } + +bool FAST_CODE_ATTR Queue::isFull() const { return false; } + +} + +} + +#endif \ No newline at end of file diff --git a/lib/Espfc/src/Target/Queue.h b/lib/Espfc/src/Target/Queue.h index 6d42f873..39de5214 100644 --- a/lib/Espfc/src/Target/Queue.h +++ b/lib/Espfc/src/Target/Queue.h @@ -58,14 +58,6 @@ class Queue TargetQueueHandle _q; }; -#if defined(UNIT_TEST) || !defined(ESPFC_MULTI_CORE) -void Queue::begin() {} -void Queue::send(const Event& e) { (void)e; } -Event Queue::receive() { return Event(); } -bool Queue::isEmpty() const { return true; } -bool Queue::isFull() const { return false; } -#endif - } } diff --git a/lib/Espfc/src/Target/QueueAtomic.cpp b/lib/Espfc/src/Target/QueueAtomic.cpp index 111471c2..d4fa47a7 100644 --- a/lib/Espfc/src/Target/QueueAtomic.cpp +++ b/lib/Espfc/src/Target/QueueAtomic.cpp @@ -12,25 +12,25 @@ void Queue::begin() { } -void Queue::send(const Event& e) +void FAST_CODE_ATTR Queue::send(const Event& e) { if(isFull()) return; _q.push(e); } -Event Queue::receive() +Event FAST_CODE_ATTR Queue::receive() { Event e; _q.pop(e); return e; } -bool Queue::isEmpty() const +bool FAST_CODE_ATTR Queue::isEmpty() const { return _q.isEmpty(); } -bool Queue::isFull() const +bool FAST_CODE_ATTR Queue::isFull() const { return _q.isFull(); } diff --git a/lib/Espfc/src/Target/QueueAtomic.h b/lib/Espfc/src/Target/QueueAtomic.h index 2e326fda..5b663775 100644 --- a/lib/Espfc/src/Target/QueueAtomic.h +++ b/lib/Espfc/src/Target/QueueAtomic.h @@ -2,6 +2,7 @@ #if defined(ESPFC_ATOMIC_QUEUE) || defined(UNIT_TEST) +#include #include namespace Espfc { @@ -30,7 +31,7 @@ class QueueAtomic bool pop(Element& item) { - const auto current_head = _head.load(); + const auto current_head = _head.load(); if(current_head == _tail.load()) return false; // empty queue item = _array[current_head]; @@ -50,11 +51,11 @@ class QueueAtomic private: size_t increment(size_t idx) const { return (idx + 1) % Capacity; } - std::atomic _tail; - std::atomic _head; + std::atomic _tail; + std::atomic _head; Element _array[Capacity]; }; } -#endif \ No newline at end of file +#endif diff --git a/lib/Espfc/src/Target/Target.h b/lib/Espfc/src/Target/Target.h index 4b6343fe..b73e29e7 100644 --- a/lib/Espfc/src/Target/Target.h +++ b/lib/Espfc/src/Target/Target.h @@ -21,6 +21,7 @@ #endif #include "Queue.h" +#include "Utils/MemoryHelper.h" #if defined(ESPFC_I2C_0) #if defined(ESPFC_I2C_0_SOFT) diff --git a/lib/Espfc/src/Target/TargetESP32.h b/lib/Espfc/src/Target/TargetESP32.h index 97524e3f..47a4f131 100644 --- a/lib/Espfc/src/Target/TargetESP32.h +++ b/lib/Espfc/src/Target/TargetESP32.h @@ -1,8 +1,5 @@ #pragma once -#include "Esp.h" -#include "Debug_Espfc.h" - #define ESPFC_INPUT #define ESPFC_INPUT_PIN 35 // ppm @@ -78,10 +75,8 @@ #define ESPFC_FEATURE_MASK (FEATURE_RX_SERIAL | FEATURE_DYNAMIC_FILTER) -#define ESPFC_GUARD 0 - #define ESPFC_GYRO_I2C_RATE_MAX 2000 -#define ESPFC_GYRO_SPI_RATE_MAX 8000 +#define ESPFC_GYRO_SPI_RATE_MAX 4000 #define ESPFC_DSHOT_TELEMETRY @@ -93,9 +88,6 @@ //#define ESPFC_FREE_RTOS_QUEUE #define ESPFC_ATOMIC_QUEUE - #define ESPFC_DSP -#include "Device/SerialDevice.h" - #include "Target/TargetEsp32Common.h" diff --git a/lib/Espfc/src/Target/TargetESP32c3.h b/lib/Espfc/src/Target/TargetESP32c3.h index 3fe1d0fa..4ba66808 100644 --- a/lib/Espfc/src/Target/TargetESP32c3.h +++ b/lib/Espfc/src/Target/TargetESP32c3.h @@ -37,6 +37,10 @@ #define ESPFC_SERIAL_USB_DEV_T HWCDC #define ESPFC_SERIAL_USB_FN (SERIAL_FUNCTION_MSP) +#define ESPFC_SERIAL_SOFT_0 +#define ESPFC_SERIAL_SOFT_0_FN (SERIAL_FUNCTION_MSP) +#define ESPFC_SERIAL_SOFT_0_WIFI + #define ESPFC_SERIAL_REMAP_PINS #define ESPFC_SERIAL_DEBUG_PORT SERIAL_UART_0 #define SERIAL_TX_FIFO_SIZE 0xFF diff --git a/lib/Espfc/src/Target/TargetESP32s3.h b/lib/Espfc/src/Target/TargetESP32s3.h index 66e28f0a..30a5b9b1 100644 --- a/lib/Espfc/src/Target/TargetESP32s3.h +++ b/lib/Espfc/src/Target/TargetESP32s3.h @@ -3,6 +3,11 @@ #include "Esp.h" #include "Debug_Espfc.h" +// pins to avoid: +// strapping: 0, 3, 45, 46 +// flash/psram: 26-37 (reserved) +// usb/jtag: 19, 20 + #define ESPFC_INPUT #define ESPFC_INPUT_PIN 6 // ppm @@ -49,7 +54,7 @@ #define ESPFC_SERIAL_SOFT_0_WIFI #define ESPFC_SERIAL_REMAP_PINS -#define ESPFC_SERIAL_DEBUG_PORT SERIAL_UART_0 +#define ESPFC_SERIAL_DEBUG_PORT SERIAL_USB #define SERIAL_TX_FIFO_SIZE 0xFF #define ESPFC_SPI_0 @@ -82,7 +87,7 @@ #define ESPFC_GUARD 0 #define ESPFC_GYRO_I2C_RATE_MAX 2000 -#define ESPFC_GYRO_SPI_RATE_MAX 8000 +#define ESPFC_GYRO_SPI_RATE_MAX 4000 #define ESPFC_DSHOT_TELEMETRY @@ -94,7 +99,6 @@ //#define ESPFC_FREE_RTOS_QUEUE #define ESPFC_ATOMIC_QUEUE - #define ESPFC_DSP #include "Device/SerialDevice.h" diff --git a/lib/Espfc/src/Target/TargetESP8266.h b/lib/Espfc/src/Target/TargetESP8266.h index 4a7a11fc..ba25375a 100644 --- a/lib/Espfc/src/Target/TargetESP8266.h +++ b/lib/Espfc/src/Target/TargetESP8266.h @@ -30,8 +30,8 @@ #define ESPFC_SERIAL_1_BBAUD (SERIAL_SPEED_NONE) #define ESPFC_SERIAL_SOFT_0 -#define ESPFC_SERIAL_SOFT_0_FN (SERIAL_FUNCTION_NONE) -#define ESPFC_SERIAL_SOFT_0_RX +#define ESPFC_SERIAL_SOFT_0_FN (SERIAL_FUNCTION_MSP) +#define ESPFC_SERIAL_SOFT_0_WIFI #define ESPFC_SERIAL_DEBUG_PORT SERIAL_UART_0 #define SERIAL_TX_FIFO_SIZE UART_TX_FIFO_SIZE @@ -57,6 +57,7 @@ #define ESPFC_GYRO_SPI_RATE_MAX 1000 #define ESPFC_WIFI_ALT +#define ESPFC_ESPNOW #include "Device/SerialDevice.h" diff --git a/lib/Espfc/src/Target/TargetEsp32Common.h b/lib/Espfc/src/Target/TargetEsp32Common.h index 281fd9e3..008320d3 100644 --- a/lib/Espfc/src/Target/TargetEsp32Common.h +++ b/lib/Espfc/src/Target/TargetEsp32Common.h @@ -1,5 +1,9 @@ #pragma once +#include "Esp.h" +#include "Debug_Espfc.h" +#include "Device/SerialDevice.h" + #define SERIAL_UART_PARITY_NONE 0B00000000 #define SERIAL_UART_PARITY_EVEN 0B00000010 #define SERIAL_UART_PARITY_ODD 0B00000011 @@ -14,6 +18,9 @@ #define SERIAL_UART_NB_STOP_BIT_15 0B00100000 #define SERIAL_UART_NB_STOP_BIT_2 0B00110000 +#define ESPFC_WIFI +#define ESPFC_ESPNOW + namespace Espfc { template diff --git a/lib/Espfc/src/Target/TargetUnitTest.h b/lib/Espfc/src/Target/TargetUnitTest.h index 77a0584a..7103ccf5 100644 --- a/lib/Espfc/src/Target/TargetUnitTest.h +++ b/lib/Espfc/src/Target/TargetUnitTest.h @@ -1,5 +1,7 @@ #pragma once +#include + #define ESPFC_INPUT #define ESPFC_INPUT_PIN 0 @@ -16,3 +18,36 @@ #define ESPFC_GYRO_I2C_RATE_MAX 2000 #define ESPFC_GYRO_SPI_RATE_MAX 8000 + +#define SERIAL_TX_FIFO_SIZE 0xFF + +#define ESPFC_SERIAL_DEBUG_PORT 0 + +inline void targetReset() +{ +} + +inline uint32_t getBoardId0() +{ + return 0; +} + +inline uint32_t getBoardId1() +{ + return 0; +} + +inline uint32_t getBoardId2() +{ + return 0; +} + +inline uint32_t targetCpuFreq() +{ + return 1; +} + +inline uint32_t targetFreeHeap() +{ + return 1; +} \ No newline at end of file diff --git a/lib/Espfc/src/Timer.cpp b/lib/Espfc/src/Timer.cpp new file mode 100644 index 00000000..71696458 --- /dev/null +++ b/lib/Espfc/src/Timer.cpp @@ -0,0 +1,70 @@ +#include +#include "Timer.h" +#include "Utils/MemoryHelper.h" + +namespace Espfc { + +Timer::Timer(): interval(0), last(0), next(0), iteration(0), delta(0) +{ +} + +int Timer::setInterval(uint32_t interval) +{ + this->interval = interval; + this->rate = (1000000UL + interval / 2) / interval; + this->denom = 1; + this->delta = this->interval; + this->intervalf = this->interval * 0.000001f; + iteration = 0; + return 1; +} + +int Timer::setRate(uint32_t rate, uint32_t denom) +{ + this->rate = (rate + denom / 2) / denom; + this->interval = (1000000UL + this->rate / 2) / this->rate; + this->denom = denom; + this->delta = this->interval; + this->intervalf = this->interval * 0.000001f; + iteration = 0; + return 1; +} + +bool FAST_CODE_ATTR Timer::check() +{ + return check(micros()); +} + +int FAST_CODE_ATTR Timer::update() +{ + return update(micros()); +} + +bool FAST_CODE_ATTR Timer::check(uint32_t now) +{ + if(interval == 0) return false; + if(now < next) return false; + return update(now); +} + +int FAST_CODE_ATTR Timer::update(uint32_t now) +{ + next = now + interval; + delta = now - last; + last = now; + iteration++; + return 1; +} + +bool FAST_CODE_ATTR Timer::syncTo(const Timer& t, uint32_t slot) +{ + if(denom > 0) + { + if(slot > denom - 1) slot = denom - 1; + if(t.iteration % denom != slot) return false; + return update(micros()); + } + return check(micros()); +} + +} diff --git a/lib/Espfc/src/Timer.h b/lib/Espfc/src/Timer.h index 47b109c4..189587d4 100644 --- a/lib/Espfc/src/Timer.h +++ b/lib/Espfc/src/Timer.h @@ -1,7 +1,5 @@ -#ifndef _ESPFC_TIMER_H_ -#define _ESPFC_TIMER_H_ +#pragma once -#include #include namespace Espfc { @@ -9,61 +7,15 @@ namespace Espfc { class Timer { public: - Timer(): interval(0), last(0), next(0), iteration(0), delta(0) {} + Timer(); + int setInterval(uint32_t interval); + int setRate(uint32_t rate, uint32_t denom = 1u); - int setInterval(uint32_t interval) - { - this->interval = interval; - this->rate = 1000000UL / interval; - this->denom = 1; - this->delta = this->interval; - this->intervalf = this->interval * 0.000001f; - iteration = 0; - return 1; - } - - int setRate(uint32_t rate, uint32_t denom = 1) - { - this->rate = rate / denom; - this->interval = 1000000UL / this->rate; - this->denom = denom; - this->delta = this->interval; - this->intervalf = this->interval * 0.000001f; - iteration = 0; - return 1; - } - - bool check() - { - return check(micros()); - } - - int update() - { - return update(micros()); - } - - bool check(uint32_t now) - { - if(interval == 0) return false; - if(now < next) return false; - return update(now); - } - - int update(uint32_t now) - { - next = now + interval; - delta = now - last; - last = now; - iteration++; - return 1; - } - - bool syncTo(const Timer& t) - { - if(t.iteration % denom != 0) return false; - return update(); - } + bool check(); + int update(); + bool check(uint32_t now); + int update(uint32_t now); + bool syncTo(const Timer& t, uint32_t slot = 0u); uint32_t interval; uint32_t rate; @@ -71,11 +23,9 @@ class Timer uint32_t last; uint32_t next; - uint32_t iteration; + volatile uint32_t iteration; uint32_t delta; float intervalf; }; } - -#endif diff --git a/lib/Espfc/src/Utils/FilterHelper.cpp b/lib/Espfc/src/Utils/FilterHelper.cpp new file mode 100644 index 00000000..2b20fe8d --- /dev/null +++ b/lib/Espfc/src/Utils/FilterHelper.cpp @@ -0,0 +1,24 @@ +#include "FilterHelper.h" +#include "MemoryHelper.h" + +namespace Espfc { + +namespace Utils { + +float FAST_CODE_ATTR applyFilter(Filter& filter, float sample) +{ + return filter.update(sample); +} + +VectorFloat FAST_CODE_ATTR applyFilter(Filter filters[3], const VectorFloat& samples) +{ + VectorFloat result; + result.x = filters[0].update(samples.x); + result.y = filters[1].update(samples.y); + result.z = filters[2].update(samples.z); + return result; +} + +} + +} diff --git a/lib/Espfc/src/Utils/FilterHelper.h b/lib/Espfc/src/Utils/FilterHelper.h index 3f6edeec..76834c9a 100644 --- a/lib/Espfc/src/Utils/FilterHelper.h +++ b/lib/Espfc/src/Utils/FilterHelper.h @@ -1,25 +1,15 @@ #pragma once #include "Filter.h" -#include "helper_3dmath.h" +#include namespace Espfc { namespace Utils { -float applyFilter(Filter& filter, float sample) -{ - return filter.update(sample); -} +float applyFilter(Filter& filter, float sample); -VectorFloat applyFilter(Filter * filters, const VectorFloat& samples) -{ - VectorFloat result; - result.x = filters[0].update(samples.x); - result.y = filters[1].update(samples.y); - result.z = filters[2].update(samples.z); - return result; -} +VectorFloat applyFilter(Filter filters[3], const VectorFloat& samples); } diff --git a/lib/Espfc/src/Utils/MemoryHelper.h b/lib/Espfc/src/Utils/MemoryHelper.h new file mode 100644 index 00000000..542d69c7 --- /dev/null +++ b/lib/Espfc/src/Utils/MemoryHelper.h @@ -0,0 +1,8 @@ +#pragma once + +#ifdef ESP32 +#include +#define FAST_CODE_ATTR IRAM_ATTR +#else +#define FAST_CODE_ATTR +#endif diff --git a/lib/Espfc/src/Utils/RingBuf.h b/lib/Espfc/src/Utils/RingBuf.h new file mode 100644 index 00000000..5981983b --- /dev/null +++ b/lib/Espfc/src/Utils/RingBuf.h @@ -0,0 +1,92 @@ +#pragma once + +#include + +// https://gist.github.com/edwintcloud/d547a4f9ccaf7245b06f0e8782acefaa + +namespace Espfc { + +namespace Utils { + +template +class RingBuf +{ +public: + enum { Capacity = Size + 1 }; + + RingBuf(): _tail(0), _head(0) {} + ~RingBuf() {} + + bool push(const Element& item) + { + if(isFull()) return false; + + _array[_tail] = item; + _tail = _increment(_tail); + + return true; + } + + size_t push(const Element *items, size_t len) + { + len = std::min(available(), len); + for(size_t i = 0; i < len; i++) + { + _array[_tail] = items[i]; + _tail = _increment(_tail); + } + return len; + } + + bool pop(Element& item) + { + if(isEmpty()) return false; + + item = _array[_head]; + _head = _increment(_head); + + return true; + } + + size_t pop(Element * items, size_t len) + { + len = std::min(size(), len); + for(size_t i = 0; i < len; i++) + { + items[i] = _array[_head]; + _head = _increment(_head); + } + return len; + } + + bool isEmpty() const + { + return _head == _tail; + } + + bool isFull() const + { + return _head == _increment(_tail); + } + + size_t size() const + { + if (_tail >= _head) return _tail - _head; + return Capacity - (_head - _tail); + } + + size_t available() const + { + return Size - size(); + } + +private: + size_t _increment(size_t idx) const { return (idx + 1) % Capacity; } + size_t _tail; + size_t _head; + Element _array[Capacity]; +}; + +} + +} diff --git a/lib/Espfc/src/Wireless.h b/lib/Espfc/src/Wireless.h index b233c066..cf222afe 100644 --- a/lib/Espfc/src/Wireless.h +++ b/lib/Espfc/src/Wireless.h @@ -3,9 +3,11 @@ #include "Model.h" #include "Device/SerialDeviceAdapter.h" -#ifdef ESPFC_WIFI_ALT + +#ifdef ESPFC_SERIAL_SOFT_0_WIFI +#if defined(ESPFC_WIFI_ALT) #include -#else +#elif defined(ESPFC_WIFI) #include #endif @@ -13,124 +15,122 @@ namespace Espfc { class Wireless { + enum Status { + STOPPED, + STARTED, + }; public: - Wireless(Model& model): _model(model), -#ifdef ESPFC_SERIAL_SOFT_0_WIFI - _server(1111), - _adapter(_client), -#endif - _initialized(false) {} + Wireless(Model& model): _model(model), _status(STOPPED), _server(1111), _adapter(_client) {} int begin() { -#ifdef ESPFC_SERIAL_SOFT_0_WIFI - int status = -1; - if(_model.config.wireless.mode == WIRELESS_MODE_STA) + WiFi.persistent(false); +#ifdef ESPFC_ESPNOW + if(_model.isFeatureActive(FEATURE_RX_SPI)) { - using std::placeholders::_1; - using std::placeholders::_2; - WiFi.onEvent(std::function(std::bind(&Wireless::wifiEvent, this, _1, _2))); - WiFi.persistent(false); - WiFi.mode((WiFiMode_t)_model.config.wireless.mode); - status = WiFi.begin(_model.config.wireless.ssid, _model.config.wireless.pass); - _model.setArmingDisabled(ARMING_DISABLED_PARALYZE, true); + startAp(); } - else - { - WiFi.disconnect(); - } - const char * modeName = WirelessConfig::getModeName((WirelessMode)_model.config.wireless.mode); - _model.logger.info().log(F("WIFI")).log(FPSTR(modeName)).log(_model.config.wireless.ssid).log(_model.config.wireless.pass).logln(status); - return 1; -#elif defined(ESPFC_WIFI_ALT) - WiFi.disconnect(); - WiFi.mode(WIFI_OFF); - _model.logger.info().logln(F("WIFI OFF")); #endif + return 1; + } - return 0; + void startAp() + { + bool status = WiFi.softAP("ESP-FC"); + _model.logger.info().log(F("WIFI AP START")).logln(status); } -#ifdef ESPFC_SERIAL_SOFT_0_WIFI - void wifiEvent(WiFiEvent_t event, WiFiEventInfo_t info) + int connect() { - static const char * names[] = { - "SYSTEM_EVENT_WIFI_READY", /**< ESP32 WiFi ready */ - "SYSTEM_EVENT_SCAN_DONE", /**< ESP32 finish scanning AP */ - "SYSTEM_EVENT_STA_START", /**< ESP32 station start */ - "SYSTEM_EVENT_STA_STOP", /**< ESP32 station stop */ - "SYSTEM_EVENT_STA_CONNECTED", /**< ESP32 station connected to AP */ - "SYSTEM_EVENT_STA_DISCONNECTED", /**< ESP32 station disconnected from AP */ - "SYSTEM_EVENT_STA_AUTHMODE_CHANGE", /**< the auth mode of AP connected by ESP32 station changed */ - "SYSTEM_EVENT_STA_GOT_IP", /**< ESP32 station got IP from connected AP */ - "SYSTEM_EVENT_STA_LOST_IP", /**< ESP32 station lost IP and the IP is reset to 0 */ - "SYSTEM_EVENT_STA_WPS_ER_SUCCESS", /**< ESP32 station wps succeeds in enrollee mode */ - "SYSTEM_EVENT_STA_WPS_ER_FAILED", /**< ESP32 station wps fails in enrollee mode */ - "SYSTEM_EVENT_STA_WPS_ER_TIMEOUT", /**< ESP32 station wps timeout in enrollee mode */ - "SYSTEM_EVENT_STA_WPS_ER_PIN", /**< ESP32 station wps pin code in enrollee mode */ - "SYSTEM_EVENT_AP_START", /**< ESP32 soft-AP start */ - "SYSTEM_EVENT_AP_STOP", /**< ESP32 soft-AP stop */ - "SYSTEM_EVENT_AP_STACONNECTED", /**< a station connected to ESP32 soft-AP */ - "SYSTEM_EVENT_AP_STADISCONNECTED", /**< a station disconnected from ESP32 soft-AP */ - "SYSTEM_EVENT_AP_STAIPASSIGNED", /**< ESP32 soft-AP assign an IP to a connected station */ - "SYSTEM_EVENT_AP_PROBEREQRECVED", /**< Receive probe request packet in soft-AP interface */ - "SYSTEM_EVENT_GOT_IP6", /**< ESP32 station or ap or ethernet interface v6IP addr is preferred */ - "SYSTEM_EVENT_ETH_START", /**< ESP32 ethernet start */ - "SYSTEM_EVENT_ETH_STOP", /**< ESP32 ethernet stop */ - "SYSTEM_EVENT_ETH_CONNECTED", /**< ESP32 ethernet phy link up */ - "SYSTEM_EVENT_ETH_DISCONNECTED", /**< ESP32 ethernet phy link down */ - "SYSTEM_EVENT_ETH_GOT_IP" /**< ESP32 ethernet got IP from connected AP */ - }; - - switch(event) +#ifdef ESPFC_WIFI_ALT + // https://github.com/esp8266/Arduino/issues/2545#issuecomment-249222211 + _events[0] = WiFi.onStationModeConnected([this](const WiFiEventStationModeConnected& ev) { this->wifiEventConnected(ev.ssid, ev.channel); }); + _events[1] = WiFi.onStationModeGotIP([this](const WiFiEventStationModeGotIP& ev) { this->wifiEventGotIp(ev.ip); }); + _events[2] = WiFi.onStationModeDisconnected([this](const WiFiEventStationModeDisconnected& ev) { this->wifiEventDisconnected(); }); + _events[3] = WiFi.onSoftAPModeStationConnected([this](const WiFiEventSoftAPModeStationConnected& ev) { this->wifiEventApConnected(ev.mac); }); +#elif defined(ESPFC_WIFI) + WiFi.onEvent([this](WiFiEvent_t ev, WiFiEventInfo_t info) { + this->wifiEventConnected(String(info.wifi_sta_connected.ssid, info.wifi_sta_connected.ssid_len), info.wifi_sta_connected.channel); + }, ARDUINO_EVENT_WIFI_STA_CONNECTED); + WiFi.onEvent([this](WiFiEvent_t ev, WiFiEventInfo_t info) { this->wifiEventGotIp(IPAddress(info.got_ip.ip_info.ip.addr)); }, ARDUINO_EVENT_WIFI_STA_GOT_IP); + WiFi.onEvent([this](WiFiEvent_t ev, WiFiEventInfo_t info) { this->wifiEventDisconnected(); }, ARDUINO_EVENT_WIFI_STA_DISCONNECTED); +#endif + if(_model.config.wireless.ssid[0] != 0) { - case SYSTEM_EVENT_STA_GOT_IP: - _server.begin(_model.config.wireless.port); - _model.state.serial[SERIAL_SOFT_0].stream = &_adapter; - _model.state.localIp = WiFi.localIP(); - _initialized = true; - _model.logger.info().log(F("WIFI EV")).log(F("IP")).logln(WiFi.localIP()); - break; - case SYSTEM_EVENT_STA_DISCONNECTED: - _model.state.serial[SERIAL_SOFT_0].stream = nullptr; - _model.state.localIp = IPAddress(0,0,0,0); - _initialized = false; - _model.logger.info().log(F("WIFI EV")).logln(F("DISCONNECTED")); - break; - default: - _model.logger.info().log(F("WIFI EV")).logln(names[event]); - break; + WiFi.begin(_model.config.wireless.ssid, _model.config.wireless.pass); + _model.logger.info().log(F("WIFI STA")).log(_model.config.wireless.ssid).log(_model.config.wireless.pass).log(WiFi.getMode()).logln(WiFi.status()); } + if(!(WiFi.getMode() & WIFI_AP)) + { + startAp(); + } + _server.begin(_model.config.wireless.port); + _server.setNoDelay(true); + _model.state.serial[SERIAL_SOFT_0].stream = &_adapter; + _model.logger.info().log(F("WIFI SERVER PORT")).log(WiFi.status()).logln(_model.config.wireless.port); + return 1; + } + + void wifiEventConnected(const String& ssid, int channel) + { + _model.logger.info().log(F("WIFI STA CONN")).log(ssid).logln(channel); + } + + void wifiEventApConnected(const uint8_t* mac) + { + char buf[20]; + snprintf(buf, sizeof(buf), "%02x:%02x:%02x:%02x:%02x:%02x", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]); + _model.logger.info().log(F("WIFI AP CONNECT")).logln(buf); + } + + void wifiEventGotIp(const IPAddress& ip) + { + _model.logger.info().log(F("WIFI STA IP")).logln(ip.toString()); + } + + void wifiEventDisconnected() + { + _model.logger.info().logln(F("WIFI STA DISCONNECT")); } -#endif int update() { -#ifdef ESPFC_SERIAL_SOFT_0_WIFI - if(_model.config.wireless.mode == WIRELESS_MODE_NULL) return 0; - if(!_initialized) return 0; Stats::Measure measure(_model.state.stats, COUNTER_WIFI); - if(_server.hasClient()) + switch(_status) { - _client = _server.available(); + case STOPPED: + if(_model.state.rescueConfigMode == RESCUE_CONFIG_ACTIVE && _model.isFeatureActive(FEATURE_SOFTSERIAL)) + { + connect(); + _status = STARTED; + return 1; + } + break; + case STARTED: + if(_server.hasClient()) + { + _client = _server.accept(); + } + break; } return 1; -#endif - return 0; } private: Model& _model; -#ifdef ESPFC_SERIAL_SOFT_0_WIFI + Status _status; WiFiServer _server; WiFiClient _client; Device::SerialDeviceAdapter _adapter; +#ifdef ESPFC_WIFI_ALT + WiFiEventHandler _events[4]; #endif - bool _initialized; }; } +#endif + #endif \ No newline at end of file diff --git a/lib/Hal/library.json b/lib/Hal/library.json deleted file mode 100644 index 00f5d9e1..00000000 --- a/lib/Hal/library.json +++ /dev/null @@ -1,3 +0,0 @@ -{ - "name": "Hal" -} diff --git a/lib/betaflight/src/platform.c b/lib/betaflight/src/platform.c index 9440c7ff..9274a8a1 100644 --- a/lib/betaflight/src/platform.c +++ b/lib/betaflight/src/platform.c @@ -1,13 +1,5 @@ #include "platform.h" -#ifndef ESPFC_REVISION -#define ESPFC_REVISION 0000000 -#endif - -#ifndef ESPFC_VERSION -#define ESPFC_VERSION v0.0.0 -#endif - const char * const targetName = ESPFC_TARGET; const char * const targetVersion = STR(ESPFC_VERSION); const char * const shortGitRevision = STR(ESPFC_REVISION); @@ -48,7 +40,7 @@ const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 2500 pidProfile_t *currentPidProfile; boxBitmask_t rcModeActivationMask; -uint16_t flightModeFlags; +boxBitmask_t rcModeActivationPresent; uint8_t stateFlags; uint8_t armingFlags; uint8_t debugMode; @@ -73,12 +65,6 @@ const char* const lookupTableMixerType[] = { "LEGACY", "LINEAR", "DYNAMIC", "EZLANDING", }; -int gcd(int num, int denom) -{ - if (denom == 0) return num; - return gcd(denom, num % denom); -} - bool bitArrayGet(const void *array, unsigned bit) { return BITARRAY_BIT_OP((uint32_t*)array, bit, &); @@ -106,7 +92,7 @@ bool rxAreFlightChannelsValid(void) bool isModeActivationConditionPresent(boxId_e modeId) { - return false; + return bitArrayGet(&rcModeActivationPresent, modeId); } static uint32_t armingBeepTimeUs = 0; diff --git a/lib/betaflight/src/platform.h b/lib/betaflight/src/platform.h index 07591218..82fd7e76 100644 --- a/lib/betaflight/src/platform.h +++ b/lib/betaflight/src/platform.h @@ -16,6 +16,11 @@ #include #include +#if defined(ESP32) +#define USE_FLASHFS +#include "esp_partition.h" +#endif + #if defined(ESP8266) #define ESPFC_TARGET "ESP8266" #elif defined(ESP32S3) @@ -34,6 +39,14 @@ #error "Unsupported platform" #endif +#ifndef ESPFC_REVISION +#define ESPFC_REVISION 0000000 +#endif + +#ifndef ESPFC_VERSION +#define ESPFC_VERSION v0.0.0 +#endif + #define MAX_SUPPORTED_MOTORS 8 #define MAX_SUPPORTED_SERVOS 8 #define PID_PROCESS_DENOM_DEFAULT 1 @@ -47,8 +60,6 @@ #define STR(x) STR_HELPER(x) #define FC_VERSION_STRING STR(FC_VERSION_MAJOR) "." STR(FC_VERSION_MINOR) "." STR(FC_VERSION_PATCH_LEVEL) -#define MW_VERSION 1 - #ifdef __cplusplus extern "C" { #endif @@ -91,7 +102,7 @@ extern const char * boardIdentifier; #ifdef UNIT_TEST #define STATIC_UNIT_TESTED #else -#define STATIC_UNIT_TESTED static +#define STATIC_UNIT_TESTED #endif #define offsetof(TYPE, MEMBER) __builtin_offsetof (TYPE, MEMBER) @@ -104,8 +115,6 @@ void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count); uint32_t castFloatBytesToInt(float f); uint32_t zigzagEncode(int32_t value); -int gcd(int num, int denom); - #ifndef constrain #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) #endif @@ -495,7 +504,6 @@ typedef enum { WAS_ARMED_WITH_PREARM = (1 << 2) } armingFlag_e; -extern uint16_t flightModeFlags; extern uint8_t stateFlags; extern uint8_t armingFlags; @@ -567,6 +575,7 @@ PG_DECLARE(rcControlsConfig_t, rcControlsConfig); extern uint16_t rssi; extern boxBitmask_t rcModeActivationMask; +extern boxBitmask_t rcModeActivationPresent; bool isModeActivationConditionPresent(boxId_e modeId); uint32_t getArmingBeepTimeMicros(void); @@ -784,6 +793,7 @@ extern uint8_t activePidLoopDenom; /* SENSOR START */ bool sensors(uint32_t mask); +void sensorsSet(uint32_t mask); typedef enum { SENSOR_GYRO = 1 << 0, // always present @@ -1208,6 +1218,93 @@ void IOLo(IO_t pin); // end ESC 4-Way IF +typedef enum { + // IMPORTANT: the order of the elements should be preserved for backwards compatibility with the configurator. + BEEPER_SILENCE = 0, // Silence, see beeperSilence() + + BEEPER_GYRO_CALIBRATED, + BEEPER_RX_LOST, // Beeps when TX is turned off or signal lost (repeat until TX is okay) + BEEPER_RX_LOST_LANDING, // Beeps SOS when armed and TX is turned off or signal lost (autolanding/autodisarm) + BEEPER_DISARMING, // Beep when disarming the board + BEEPER_ARMING, // Beep when arming the board + BEEPER_ARMING_GPS_FIX, // Beep a special tone when arming the board and GPS has fix + BEEPER_BAT_CRIT_LOW, // Longer warning beeps when battery is critically low (repeats) + BEEPER_BAT_LOW, // Warning beeps when battery is getting low (repeats) + BEEPER_GPS_STATUS, // Use the number of beeps to indicate how many GPS satellites were found + BEEPER_RX_SET, // Beeps when aux channel is set for beep + BEEPER_ACC_CALIBRATION, // ACC inflight calibration completed confirmation + BEEPER_ACC_CALIBRATION_FAIL, // ACC inflight calibration failed + BEEPER_READY_BEEP, // Ring a tone when GPS is locked and ready + BEEPER_MULTI_BEEPS, // Internal value used by 'beeperConfirmationBeeps()'. + BEEPER_DISARM_REPEAT, // Beeps sounded while stick held in disarm position + BEEPER_ARMED, // Warning beeps when board is armed with motors off when idle (repeats until board is disarmed or throttle is increased) + BEEPER_SYSTEM_INIT, // Initialisation beeps when board is powered on + BEEPER_USB, // Some boards have beeper powered USB connected + BEEPER_BLACKBOX_ERASE, // Beep when blackbox erase completes + BEEPER_CRASH_FLIP_MODE, // Crash flip mode is active + BEEPER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated + BEEPER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop + BEEPER_RC_SMOOTHING_INIT_FAIL, // Warning beep pattern when armed and rc smoothing has not initialized filters + BEEPER_ARMING_GPS_NO_FIX, // Beep a special tone when arming the board and GPS has no fix + BEEPER_ALL, // Turn ON or OFF all beeper conditions + // BEEPER_ALL must remain at the bottom of this enum +} beeperMode_e; + +void beeper(int mode); + +// FLASHFS START +#ifdef USE_FLASHFS + +#define FLASHFS_WRITE_BUFFER_SIZE 128u +#define FLASHFS_FLUSH_BUFFER_SIZE 64u +#define FLASHFS_JOURNAL_ITEMS 32u + +typedef struct +{ + uint32_t logBegin; + uint32_t logEnd; +} __attribute__((packed)) FlashfsJournalItem; + +typedef struct +{ + const void* partition; + void* buffer; + uint32_t address; + FlashfsJournalItem journal[FLASHFS_JOURNAL_ITEMS]; + uint32_t journalIdx; +} FlashfsRuntime; + +#define FLASHFS_JOURNAL_SIZE (FLASHFS_JOURNAL_ITEMS * sizeof(FlashfsJournalItem)) + +const FlashfsRuntime * flashfsGetRuntime(); + +void flashfsJournalLoad(FlashfsJournalItem * data, size_t start, size_t num); + +int flashfsInit(void); +uint32_t flashfsGetSize(void); +uint32_t flashfsGetOffset(void); +uint32_t flashfsGetSectors(void); + +void flashfsWriteByte(uint8_t byte); +void flashfsWrite(const uint8_t *data, unsigned int len, bool sync); + +void flashfsWriteAbs(uint32_t address, const uint8_t *data, unsigned int len); +int flashfsReadAbs(uint32_t address, uint8_t *data, unsigned int len); + +bool flashfsFlushAsync(bool force); + +bool flashfsIsSupported(void); +bool flashfsIsReady(void); +bool flashfsIsEOF(void); + +void flashfsEraseCompletely(void); +void flashfsClose(void); + +uint32_t flashfsGetWriteBufferFreeSpace(void); +uint32_t flashfsGetWriteBufferSize(void); + +#endif + #ifdef __cplusplus } #endif diff --git a/lib/betaflight/src/platform_cpp.cpp b/lib/betaflight/src/platform_cpp.cpp index fd5ed641..7ab4ebe0 100644 --- a/lib/betaflight/src/platform_cpp.cpp +++ b/lib/betaflight/src/platform_cpp.cpp @@ -1,12 +1,13 @@ #include -#include "platform.h" +#include #include "Device/SerialDevice.h" #include "EscDriver.h" -#include "EspGpio.h" +#include "Hal/Gpio.h" +#include "Utils/MemoryHelper.h" int IORead(IO_t pin) { - return EspGpio::digitalRead(pin); + return Espfc::Hal::Gpio::digitalRead(pin); } void IOConfigGPIO(IO_t pin, uint8_t mode) @@ -24,12 +25,12 @@ void IOConfigGPIO(IO_t pin, uint8_t mode) void IOHi(IO_t pin) { - EspGpio::digitalWrite(pin, HIGH); + Espfc::Hal::Gpio::digitalWrite(pin, HIGH); } void IOLo(IO_t pin) { - EspGpio::digitalWrite(pin, LOW); + Espfc::Hal::Gpio::digitalWrite(pin, LOW); } static serialPort_t _sp[2] = {{ @@ -96,34 +97,34 @@ void serialEndWrite(serialPort_t * instance) { } -void serialWrite(serialPort_t * instance, uint8_t ch) +void FAST_CODE_ATTR serialWrite(serialPort_t * instance, uint8_t ch) { Espfc::Device::SerialDevice * dev = (Espfc::Device::SerialDevice *)instance->espfcDevice; if(dev) dev->write(ch); } -uint32_t serialRxBytesWaiting(serialPort_t * instance) +uint32_t FAST_CODE_ATTR serialRxBytesWaiting(serialPort_t * instance) { Espfc::Device::SerialDevice * dev = (Espfc::Device::SerialDevice *)instance->espfcDevice; if(!dev) return 0; return dev->available(); } -int serialRead(serialPort_t * instance) +int FAST_CODE_ATTR serialRead(serialPort_t * instance) { Espfc::Device::SerialDevice * dev = (Espfc::Device::SerialDevice *)instance->espfcDevice; if(dev) return dev->read(); return -1; } -uint32_t serialTxBytesFree(const serialPort_t * instance) +uint32_t FAST_CODE_ATTR serialTxBytesFree(const serialPort_t * instance) { Espfc::Device::SerialDevice * dev = (Espfc::Device::SerialDevice *)instance->espfcDevice; if(!dev) return 0; return dev->availableForWrite(); } -bool isSerialTransmitBufferEmpty(const serialPort_t * instance) +bool FAST_CODE_ATTR isSerialTransmitBufferEmpty(const serialPort_t * instance) { Espfc::Device::SerialDevice * dev = (Espfc::Device::SerialDevice *)instance->espfcDevice; if(!dev) return 0; @@ -169,3 +170,8 @@ uint8_t getMotorCount() { return _motorCount; } + +void beeper(int mode) +{ + +} diff --git a/partitions_4M_nota.csv b/partitions_4M_nota.csv new file mode 100644 index 00000000..71d9f5e7 --- /dev/null +++ b/partitions_4M_nota.csv @@ -0,0 +1,6 @@ +# Name, Type, SubType, Offset, Size, Flags +nvs, data, nvs, 0x9000, 0x5000, +otadata, data, ota, 0xe000, 0x2000, +app0, app, ota_0, 0x10000, 0x100000, +spiffs, data, spiffs, 0x110000,0x2E0000, +coredump, data, coredump,0x3F0000,0x10000, diff --git a/platformio.ini b/platformio.ini index d6c6ccd6..cf55a836 100644 --- a/platformio.ini +++ b/platformio.ini @@ -8,7 +8,7 @@ ; Please visit documentation for the other options and examples ; http://docs.platformio.org/page/projectconf.html [platformio] -default_envs = esp32 +default_envs = esp32, esp32s2, esp32s3, esp32c3, esp8266, rp2040 [env] build_flags = @@ -16,19 +16,26 @@ build_flags = -Wall -O2 -DESPFC_DEBUG_SERIAL -; -DDEBUG_RP2040_PORT=Serial -; -DDEBUG_RP2040_SPI ; -DESPFC_DEBUG_PIN=2 ; specify pin number (board specific) -; -DNO_GLOBAL_INSTANCES ; -DESPFC_DEV_PRESET_MODES ; -DESPFC_DEV_PRESET_BLACKBOX=1 ; specify port number (board specific) ; -DESPFC_DEV_PRESET_DSHOT ; -DESPFC_DEV_PRESET_SCALER +; -DNO_GLOBAL_INSTANCES +; -DDEBUG_ESP_PORT=Serial +; -DDEBUG_ESP_CORE +; -DDEBUG_ESP_WIFI +; -DDEBUG_RP2040_PORT=Serial +; -DDEBUG_RP2040_SPI monitor_speed = 115200 upload_speed = 921600 ; monitor_filters = esp8266_exception_decoder ; monitor_filters = esp32_exception_decoder +lib_deps = + yoursunny/WifiEspNow @ ^0.0.20230713 + https://github.com/rtlopez/espnow-rclink/archive/refs/tags/v0.1.1.zip +; EspNowRcLink=symlink://../espnow-rclink [env:esp32] board = lolin32 @@ -37,24 +44,30 @@ framework = arduino board_build.f_cpu = 240000000L board_build.f_flash = 80000000L board_build.flash_mode = qio +board_build.partitions = partitions_4M_nota.csv lib_deps = + ${env.lib_deps} build_flags = ${env.build_flags} extra_scripts = merge_firmware.py [env:esp32s3] -board = lolin_s3 +board = lolin_s3_mini platform = espressif32 framework = arduino board_build.f_cpu = 240000000L board_build.f_flash = 80000000L board_build.flash_mode = qio +board_build.partitions = partitions_4M_nota.csv lib_deps = + ${env.lib_deps} build_flags = ${env.build_flags} -DESP32S3 -DARDUINO_USB_MODE=1 -DARDUINO_USB_CDC_ON_BOOT=1 +build_unflags = + -DBOARD_HAS_PSRAM extra_scripts = merge_firmware.py board_upload.use_1200bps_touch = yes board_upload.wait_for_upload_port = yes @@ -68,6 +81,7 @@ board_build.f_cpu = 240000000L board_build.f_flash = 80000000L board_build.flash_mode = qio lib_deps = + ${env.lib_deps} build_flags = ${env.build_flags} -DESP32S2 @@ -87,6 +101,7 @@ board_build.f_cpu = 160000000L board_build.f_flash = 80000000L board_build.flash_mode = qio lib_deps = + ${env.lib_deps} build_flags = ${env.build_flags} -DESP32C3 @@ -105,6 +120,7 @@ board_build.ldscript = eagle.flash.4m3m.ld ;board_build.ldscript = eagle.flash.1m128.ld ; d1_mini_lite ;board_build.ldscript = eagle.flash.16m15m.ld ; d1_mini_pro lib_deps = + ${env.lib_deps} build_flags = ${env.build_flags} @@ -113,6 +129,7 @@ board = pico platform = https://github.com/maxgerhardt/platform-raspberrypi.git framework = arduino board_build.core = earlephilhower +lib_deps = build_flags = ${env.build_flags} -DARCH_RP2040 @@ -123,6 +140,7 @@ build_flags = platform = https://github.com/maxgerhardt/platform-nordicnrf52 board = xiaoblesense framework = arduino +lib_deps = build_flags = ${env.build_flags} -DARCH_NRF52840 @@ -130,7 +148,8 @@ build_flags = [env:native] platform = native -lib_deps = ArduinoFake +lib_deps = + ArduinoFake build_flags = -DIRAM_ATTR="" -DUNIT_TEST diff --git a/src/main.cpp b/src/main.cpp index c94ea18d..a2369228 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -8,20 +8,20 @@ #include #include #include -#include -#include #include #include -#include "Debug_Espfc.h" - +#if defined(ESPFC_ESPNOW) +#include +#endif #ifdef ESPFC_WIFI_ALT - #include +#include #else - #include +#include #endif +#include "Debug_Espfc.h" #ifdef ESP32 -void serialEventRun(void) {} +void IRAM_ATTR serialEventRun(void) {} #endif Espfc::Espfc espfc; @@ -30,51 +30,87 @@ Espfc::Espfc espfc; #if defined(ESPFC_FREE_RTOS) // ESP32 multicore - #include "freertos/FreeRTOS.h" - #include "freertos/task.h" - TaskHandle_t otherTaskHandle = NULL; - extern TaskHandle_t loopTaskHandle; + #include + #include + #include + + TaskHandle_t gyroTaskHandle = NULL; + TaskHandle_t pidTaskHandle = NULL; + static const timer_group_t TIMER_GROUP = TIMER_GROUP_0; + static const timer_idx_t TIMER_IDX = TIMER_0; + + bool IRAM_ATTR gyroTimerIsr(void* args) + { + //PIN_DEBUG(HIGH); + BaseType_t xHigherPriorityTaskWoken; + vTaskNotifyGiveFromISR(gyroTaskHandle, &xHigherPriorityTaskWoken); + //PIN_DEBUG(LOW); + return xHigherPriorityTaskWoken == pdTRUE; + } + + void gyroTimerInit(bool (*isrCb)(void* args), int interval) + { + timer_config_t config = { + .alarm_en = TIMER_ALARM_EN, + .counter_en = TIMER_PAUSE, + .intr_type = TIMER_INTR_LEVEL, + .counter_dir = TIMER_COUNT_UP, + .auto_reload = TIMER_AUTORELOAD_EN, + .divider = 80, + }; + timer_init(TIMER_GROUP, TIMER_IDX, &config); + timer_set_counter_value(TIMER_GROUP, TIMER_IDX, 0); + timer_set_alarm_value(TIMER_GROUP, TIMER_IDX, interval); + timer_isr_callback_add(TIMER_GROUP, TIMER_IDX, isrCb, nullptr, ESP_INTR_FLAG_IRAM); + timer_enable_intr(TIMER_GROUP, TIMER_IDX); + timer_start(TIMER_GROUP, TIMER_IDX); + } - void otherTask(void *pvParameters) + void gyroTask(void *pvParameters) + { + espfc.load(); + espfc.begin(); + gyroTimerInit(gyroTimerIsr, espfc.getGyroInterval()); + while(true) + { + ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // wait for timer notification + espfc.update(true); + } + } + + void pidTask(void *pvParameters) { - espfc.beginOther(); - xTaskNotifyGive(loopTaskHandle); while(true) { espfc.updateOther(); } } + void setup() { - espfc.load(); - espfc.begin(); disableCore0WDT(); - xTaskCreateUniversal(otherTask, "otherTask", 8192, NULL, 1, &otherTaskHandle, 0); // run on PRO(0) core, loopTask is on APP(1) - ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // wait for `otherTask` initialization + // internal task priorities + // PRO(0): hi-res timer(22), timer(1), event-loop(20), lwip(18/any), wifi(23), wpa(2/any), BT/vhci(23), NimBle(21), BT/other(19,20,22), Eth(15), Mqtt(5/any) + // APP(1): free + xTaskCreateUniversal(gyroTask, "gyroTask", 8192, NULL, 24, &gyroTaskHandle, 1); + xTaskCreateUniversal(pidTask, "pidTask", 8192, NULL, 1, &pidTaskHandle, 0); + vTaskDelete(NULL); // delete arduino loop task } + void loop() { - //const uint32_t timeout = millis() + 200; - //while(millis() < timeout) - //{ - espfc.update(); - //} } #elif defined(ESPFC_MULTI_CORE_RP2040) // RP2040 multicore - volatile static bool setup1Done = false; void setup1() { - espfc.beginOther(); - setup1Done = true; } void setup() { espfc.load(); espfc.begin(); - while(!setup1Done); //wait for setup1() } void loop() { @@ -96,7 +132,6 @@ Espfc::Espfc espfc; { espfc.load(); espfc.begin(); - espfc.beginOther(); } void loop() { diff --git a/test/test_esc/test_esc.cpp b/test/test_esc/test_esc.cpp index e437b7a4..e5b7d48d 100644 --- a/test/test_esc/test_esc.cpp +++ b/test/test_esc/test_esc.cpp @@ -1,6 +1,10 @@ #include #include -#include "EscDriver.h" +#include +#include +#include +#include "msp/msp_protocol.h" +#include void test_esc_dshot_encode() { diff --git a/test/test_input_crsf/test_input_crsf.cpp b/test/test_input_crsf/test_input_crsf.cpp index fbd97109..6103b5a4 100644 --- a/test/test_input_crsf/test_input_crsf.cpp +++ b/test/test_input_crsf/test_input_crsf.cpp @@ -1,7 +1,11 @@ #include #include #include "Device/InputCRSF.h" -#include +#include +#include +#include +#include "msp/msp_protocol.h" +#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 fe3931a0..e7e34fd5 100644 --- a/test/test_math/test_math.cpp +++ b/test/test_math/test_math.cpp @@ -1,11 +1,15 @@ #include -#include +#include +#include +#include +#include "msp/msp_protocol.h" #include "Math/Utils.h" #include "Math/Bits.h" -#include "helper_3dmath.h" #include "Filter.h" #include "Control/Pid.h" #include "Target/QueueAtomic.h" +#include "Utils/RingBuf.h" +#include // void setUp(void) { // // set stuff up here @@ -20,1087 +24,1287 @@ using namespace Espfc::Control; void test_math_map() { - TEST_ASSERT_FLOAT_WITHIN(1.f, 0.f, Math::map( 0.0f, -100.0f, 100.0f, -1000.0f, 1000.0f)); - TEST_ASSERT_FLOAT_WITHIN(1.f, 1000.f, Math::map( 100.0f, -100.0f, 100.0f, -1000.0f, 1000.0f)); - TEST_ASSERT_FLOAT_WITHIN(1.f, -1000.f, Math::map(-100.0f, -100.0f, 100.0f, -1000.0f, 1000.0f)); - TEST_ASSERT_FLOAT_WITHIN(1.f, 200.f, Math::map( 20.0f, -100.0f, 100.0f, -1000.0f, 1000.0f)); - - TEST_ASSERT_FLOAT_WITHIN(.001f, 0.f, Math::map( 0.0f, -100.0f, 100.0f, -1.0f, 1.0f)); - TEST_ASSERT_FLOAT_WITHIN(.001f, 1.f, Math::map( 100.0f, -100.0f, 100.0f, -1.0f, 1.0f)); - TEST_ASSERT_FLOAT_WITHIN(.001f, -1.f, Math::map(-100.0f, -100.0f, 100.0f, -1.0f, 1.0f)); + TEST_ASSERT_FLOAT_WITHIN(1.f, 0.f, Math::map( 0.0f, -100.0f, 100.0f, -1000.0f, 1000.0f)); + TEST_ASSERT_FLOAT_WITHIN(1.f, 1000.f, Math::map( 100.0f, -100.0f, 100.0f, -1000.0f, 1000.0f)); + TEST_ASSERT_FLOAT_WITHIN(1.f, -1000.f, Math::map(-100.0f, -100.0f, 100.0f, -1000.0f, 1000.0f)); + TEST_ASSERT_FLOAT_WITHIN(1.f, 200.f, Math::map( 20.0f, -100.0f, 100.0f, -1000.0f, 1000.0f)); + + TEST_ASSERT_FLOAT_WITHIN(.001f, 0.f, Math::map( 0.0f, -100.0f, 100.0f, -1.0f, 1.0f)); + TEST_ASSERT_FLOAT_WITHIN(.001f, 1.f, Math::map( 100.0f, -100.0f, 100.0f, -1.0f, 1.0f)); + TEST_ASSERT_FLOAT_WITHIN(.001f, -1.f, Math::map(-100.0f, -100.0f, 100.0f, -1.0f, 1.0f)); } void test_math_map3() { - TEST_ASSERT_FLOAT_WITHIN(1.f, 0.f, Math::map3( 0.0f, -100.0f, 0.0f, 100.0f, -1000.0f, 0.0f, 1000.0f)); - TEST_ASSERT_FLOAT_WITHIN(1.f, 500.f, Math::map3( 50.0f, -100.0f, 0.0f, 100.0f, -1000.0f, 0.0f, 1000.0f)); - TEST_ASSERT_FLOAT_WITHIN(1.f, -500.f, Math::map3(-50.0f, -100.0f, 0.0f, 100.0f, -1000.0f, 0.0f, 1000.0f)); + TEST_ASSERT_FLOAT_WITHIN(1.f, 0.f, Math::map3( 0.0f, -100.0f, 0.0f, 100.0f, -1000.0f, 0.0f, 1000.0f)); + TEST_ASSERT_FLOAT_WITHIN(1.f, 500.f, Math::map3( 50.0f, -100.0f, 0.0f, 100.0f, -1000.0f, 0.0f, 1000.0f)); + TEST_ASSERT_FLOAT_WITHIN(1.f, -500.f, Math::map3(-50.0f, -100.0f, 0.0f, 100.0f, -1000.0f, 0.0f, 1000.0f)); } void test_math_baro_altitude() { - TEST_ASSERT_FLOAT_WITHIN(0.1f, 0.0f, Math::toAltitude(101325.f)); // sea level - TEST_ASSERT_FLOAT_WITHIN(0.1f, 27.0f, Math::toAltitude(101000.f)); - TEST_ASSERT_FLOAT_WITHIN(0.1f, 110.9f, Math::toAltitude(100000.f)); + TEST_ASSERT_FLOAT_WITHIN(0.1f, 0.0f, Math::toAltitude(101325.f)); // sea level + TEST_ASSERT_FLOAT_WITHIN(0.1f, 27.0f, Math::toAltitude(101000.f)); + TEST_ASSERT_FLOAT_WITHIN(0.1f, 110.9f, Math::toAltitude(100000.f)); } void test_math_deadband() { - TEST_ASSERT_EQUAL_INT32( 0, Math::deadband( 0, 10)); - TEST_ASSERT_EQUAL_INT32( 0, Math::deadband( 10, 10)); - TEST_ASSERT_EQUAL_INT32( 1, Math::deadband( 11, 10)); - TEST_ASSERT_EQUAL_INT32(-1, Math::deadband(-11, 10)); - TEST_ASSERT_EQUAL_INT32( 0, Math::deadband( -5, 10)); - TEST_ASSERT_EQUAL_INT32(10, Math::deadband( 20, 10)); + TEST_ASSERT_EQUAL_INT32( 0, Math::deadband( 0, 10)); + TEST_ASSERT_EQUAL_INT32( 0, Math::deadband( 10, 10)); + TEST_ASSERT_EQUAL_INT32( 1, Math::deadband( 11, 10)); + TEST_ASSERT_EQUAL_INT32(-1, Math::deadband(-11, 10)); + TEST_ASSERT_EQUAL_INT32( 0, Math::deadband( -5, 10)); + TEST_ASSERT_EQUAL_INT32(10, Math::deadband( 20, 10)); } void test_math_bit() { - TEST_ASSERT_EQUAL(0, Math::getBit(0, 0)); - TEST_ASSERT_EQUAL(1, Math::getBit(1, 0)); - TEST_ASSERT_EQUAL(0, Math::getBit(1, 1)); - TEST_ASSERT_EQUAL(1, Math::getBit(3, 1)); - - TEST_ASSERT_EQUAL_UINT8(0, Math::setBit(0, 0, 0)); - TEST_ASSERT_EQUAL_UINT8(1, Math::setBit(0, 0, 1)); - TEST_ASSERT_EQUAL_UINT8(2, Math::setBit(0, 1, 1)); - TEST_ASSERT_EQUAL_UINT8(3, Math::setBit(2, 0, 1)); + TEST_ASSERT_EQUAL(0, Math::getBit(0, 0)); + TEST_ASSERT_EQUAL(1, Math::getBit(1, 0)); + TEST_ASSERT_EQUAL(0, Math::getBit(1, 1)); + TEST_ASSERT_EQUAL(1, Math::getBit(3, 1)); + + TEST_ASSERT_EQUAL_UINT8(0, Math::setBit(0, 0, 0)); + TEST_ASSERT_EQUAL_UINT8(1, Math::setBit(0, 0, 1)); + TEST_ASSERT_EQUAL_UINT8(2, Math::setBit(0, 1, 1)); + TEST_ASSERT_EQUAL_UINT8(3, Math::setBit(2, 0, 1)); } void test_math_bitmask() { - TEST_ASSERT_EQUAL_UINT8( 0, Math::setMasked(0, 1, 0)); - TEST_ASSERT_EQUAL_UINT8( 1, Math::setMasked(0, 1, 1)); - TEST_ASSERT_EQUAL_UINT8(0x38, Math::setMasked(0x00, 0x38, 0xff)); - TEST_ASSERT_EQUAL_UINT8(0xc7, Math::setMasked(0xff, 0x38, 0x00)); + TEST_ASSERT_EQUAL_UINT8( 0, Math::setMasked(0, 1, 0)); + TEST_ASSERT_EQUAL_UINT8( 1, Math::setMasked(0, 1, 1)); + TEST_ASSERT_EQUAL_UINT8(0x38, Math::setMasked(0x00, 0x38, 0xff)); + TEST_ASSERT_EQUAL_UINT8(0xc7, Math::setMasked(0xff, 0x38, 0x00)); } void test_math_bitmask_lsb() { - TEST_ASSERT_EQUAL_UINT8( 1, Math::getMaskLsb(0, 1)); - TEST_ASSERT_EQUAL_UINT8( 2, Math::getMaskLsb(1, 1)); - TEST_ASSERT_EQUAL_UINT8( 4, Math::getMaskLsb(2, 1)); - TEST_ASSERT_EQUAL_UINT8(12, Math::getMaskLsb(2, 2)); - TEST_ASSERT_EQUAL_UINT8(56, Math::getMaskLsb(3, 3)); + TEST_ASSERT_EQUAL_UINT8( 1, Math::getMaskLsb(0, 1)); + TEST_ASSERT_EQUAL_UINT8( 2, Math::getMaskLsb(1, 1)); + TEST_ASSERT_EQUAL_UINT8( 4, Math::getMaskLsb(2, 1)); + TEST_ASSERT_EQUAL_UINT8(12, Math::getMaskLsb(2, 2)); + TEST_ASSERT_EQUAL_UINT8(56, Math::getMaskLsb(3, 3)); } void test_math_bitmask_msb() { - TEST_ASSERT_EQUAL_UINT8( 1, Math::getMaskMsb(0, 1)); - TEST_ASSERT_EQUAL_UINT8( 2, Math::getMaskMsb(1, 1)); - TEST_ASSERT_EQUAL_UINT8( 4, Math::getMaskMsb(2, 1)); - TEST_ASSERT_EQUAL_UINT8( 6, Math::getMaskMsb(2, 2)); - TEST_ASSERT_EQUAL_UINT8(14, Math::getMaskMsb(3, 3)); - TEST_ASSERT_EQUAL_UINT8(30, Math::getMaskMsb(4, 4)); + TEST_ASSERT_EQUAL_UINT8( 1, Math::getMaskMsb(0, 1)); + TEST_ASSERT_EQUAL_UINT8( 2, Math::getMaskMsb(1, 1)); + TEST_ASSERT_EQUAL_UINT8( 4, Math::getMaskMsb(2, 1)); + TEST_ASSERT_EQUAL_UINT8( 6, Math::getMaskMsb(2, 2)); + TEST_ASSERT_EQUAL_UINT8(14, Math::getMaskMsb(3, 3)); + TEST_ASSERT_EQUAL_UINT8(30, Math::getMaskMsb(4, 4)); } void test_math_bits_lsb() { - TEST_ASSERT_EQUAL_UINT8(0, Math::getBitsLsb(0x00, 1, 1)); - TEST_ASSERT_EQUAL_UINT8(0, Math::getBitsLsb(0x55, 1, 1)); - TEST_ASSERT_EQUAL_UINT8(1, Math::getBitsLsb(0x55, 2, 2)); - TEST_ASSERT_EQUAL_UINT8(1, Math::getBitsLsb(0x55, 4, 2)); - TEST_ASSERT_EQUAL_UINT8(5, Math::getBitsLsb(0x55, 2, 4)); - - TEST_ASSERT_EQUAL_UINT8( 8, Math::setBitsLsb(0x00, 3, 4, 1)); - TEST_ASSERT_EQUAL_UINT8(80, Math::setBitsLsb(0x00, 3, 4, 10)); - TEST_ASSERT_EQUAL_UINT8(16, Math::setBitsLsb(0x00, 4, 2, 1)); - TEST_ASSERT_EQUAL_UINT8(160, Math::setBitsLsb(0x00, 4, 4, 10)); + TEST_ASSERT_EQUAL_UINT8(0, Math::getBitsLsb(0x00, 1, 1)); + TEST_ASSERT_EQUAL_UINT8(0, Math::getBitsLsb(0x55, 1, 1)); + TEST_ASSERT_EQUAL_UINT8(1, Math::getBitsLsb(0x55, 2, 2)); + TEST_ASSERT_EQUAL_UINT8(1, Math::getBitsLsb(0x55, 4, 2)); + TEST_ASSERT_EQUAL_UINT8(5, Math::getBitsLsb(0x55, 2, 4)); + + TEST_ASSERT_EQUAL_UINT8( 8, Math::setBitsLsb(0x00, 3, 4, 1)); + TEST_ASSERT_EQUAL_UINT8(80, Math::setBitsLsb(0x00, 3, 4, 10)); + TEST_ASSERT_EQUAL_UINT8(16, Math::setBitsLsb(0x00, 4, 2, 1)); + TEST_ASSERT_EQUAL_UINT8(160, Math::setBitsLsb(0x00, 4, 4, 10)); } void test_math_bits_msb() { - TEST_ASSERT_EQUAL_UINT8( 0, Math::getBitsMsb(0x00, 1, 1)); - TEST_ASSERT_EQUAL_UINT8( 0, Math::getBitsMsb(0x55, 1, 1)); - TEST_ASSERT_EQUAL_UINT8( 2, Math::getBitsMsb(0x55, 2, 2)); - TEST_ASSERT_EQUAL_UINT8( 2, Math::getBitsMsb(0x55, 4, 2)); - TEST_ASSERT_EQUAL_UINT8(10, Math::getBitsMsb(0x55, 4, 4)); - - TEST_ASSERT_EQUAL_UINT8( 1, Math::setBitsMsb(0x00, 3, 4, 1)); - TEST_ASSERT_EQUAL_UINT8(10, Math::setBitsMsb(0x00, 3, 4, 10)); - TEST_ASSERT_EQUAL_UINT8( 8, Math::setBitsMsb(0x00, 6, 4, 1)); - TEST_ASSERT_EQUAL_UINT8(80, Math::setBitsMsb(0x00, 6, 4, 10)); + TEST_ASSERT_EQUAL_UINT8( 0, Math::getBitsMsb(0x00, 1, 1)); + TEST_ASSERT_EQUAL_UINT8( 0, Math::getBitsMsb(0x55, 1, 1)); + TEST_ASSERT_EQUAL_UINT8( 2, Math::getBitsMsb(0x55, 2, 2)); + TEST_ASSERT_EQUAL_UINT8( 2, Math::getBitsMsb(0x55, 4, 2)); + TEST_ASSERT_EQUAL_UINT8(10, Math::getBitsMsb(0x55, 4, 4)); + + TEST_ASSERT_EQUAL_UINT8( 1, Math::setBitsMsb(0x00, 3, 4, 1)); + TEST_ASSERT_EQUAL_UINT8(10, Math::setBitsMsb(0x00, 3, 4, 10)); + TEST_ASSERT_EQUAL_UINT8( 8, Math::setBitsMsb(0x00, 6, 4, 1)); + TEST_ASSERT_EQUAL_UINT8(80, Math::setBitsMsb(0x00, 6, 4, 10)); +} + +void test_math_clock_align() +{ + TEST_ASSERT_EQUAL_INT( 250, Math::alignToClock(1000, 332)); + TEST_ASSERT_EQUAL_INT( 333, Math::alignToClock(1000, 333)); + TEST_ASSERT_EQUAL_INT( 333, Math::alignToClock(1000, 334)); + TEST_ASSERT_EQUAL_INT( 333, Math::alignToClock(1000, 400)); + TEST_ASSERT_EQUAL_INT( 500, Math::alignToClock(1000, 500)); + TEST_ASSERT_EQUAL_INT( 500, Math::alignToClock(1000, 800)); + TEST_ASSERT_EQUAL_INT(1000, Math::alignToClock(1000, 2000)); + TEST_ASSERT_EQUAL_INT( 500, Math::alignToClock(8000, 500)); + TEST_ASSERT_EQUAL_INT( 476, Math::alignToClock(6667, 500)); } void test_math_peak_detect_full() { - using Math::Peak; + using Math::Peak; - float samples[32] = { 0, 20, 0, 0, 4, 0, 2, 4, 5, 3, 1, 4, 0, 6, 0, 0 }; - Peak peaks[8] = { Peak(), Peak(), Peak(), Peak() }; + float samples[32] = { 0, 20, 0, 0, 4, 0, 2, 4, 5, 3, 1, 4, 0, 6, 0, 0 }; + Peak peaks[8] = { Peak(), Peak(), Peak(), Peak() }; - Math::peakDetect(samples, 1, 14, 1, peaks, 4); + Math::peakDetect(samples, 1, 14, 1, peaks, 4); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 1.f, peaks[0].freq); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 20.f, peaks[0].value); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 1.f, peaks[0].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 20.f, peaks[0].value); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 13.f, peaks[1].freq); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 6.f, peaks[1].value); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 13.f, peaks[1].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 6.f, peaks[1].value); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 7.92f, peaks[2].freq); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 5.f, peaks[2].value); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 7.92f, peaks[2].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 5.f, peaks[2].value); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 4.f, peaks[3].freq); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 4.f, peaks[3].value); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 4.f, peaks[3].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 4.f, peaks[3].value); } void test_math_peak_detect_partial() { - using Math::Peak; + using Math::Peak; - float samples[32] = { 0, 20, 0, 0, 4, 0, 2, 4, 5, 3, 1, 4, 0, 6, 0, 0 }; - Peak peaks[8] = { Peak(), Peak(), Peak(), Peak() }; + float samples[32] = { 0, 20, 0, 0, 4, 0, 2, 4, 5, 3, 1, 4, 0, 6, 0, 0 }; + Peak peaks[8] = { Peak(), Peak(), Peak(), Peak() }; - Math::peakDetect(samples, 3, 12, 1, peaks, 3); + Math::peakDetect(samples, 3, 12, 1, peaks, 3); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 7.92f, peaks[0].freq); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 5.f, peaks[0].value); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 7.92f, peaks[0].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 5.f, peaks[0].value); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 4.f, peaks[1].freq); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 4.f, peaks[1].value); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 4.f, peaks[1].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 4.f, peaks[1].value); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 10.8f, peaks[2].freq); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 4.f, peaks[2].value); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 10.8f, peaks[2].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 4.f, peaks[2].value); } void test_math_peak_sort() { - using Math::Peak; + using Math::Peak; - Peak peaks[8] = { Peak(20, 10), Peak(10, 10), Peak(0, 10), Peak(5, 5) }; + Peak peaks[8] = { Peak(20, 10), Peak(10, 10), Peak(0, 10), Peak(5, 5) }; - Math::peakSort(peaks, 4); + Math::peakSort(peaks, 4); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 5.f, peaks[0].freq); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 10.f, peaks[1].freq); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 20.f, peaks[2].freq); - TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.f, peaks[3].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 5.f, peaks[0].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 10.f, peaks[1].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 20.f, peaks[2].freq); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.f, peaks[3].freq); } void test_vector_int16_access() { - VectorInt16 v; + VectorInt16 v; - TEST_ASSERT_EQUAL_INT16(0, v.x); - TEST_ASSERT_EQUAL_INT16(0, v.y); - TEST_ASSERT_EQUAL_INT16(0, v.z); + TEST_ASSERT_EQUAL_INT16(0, v.x); + TEST_ASSERT_EQUAL_INT16(0, v.y); + TEST_ASSERT_EQUAL_INT16(0, v.z); - TEST_ASSERT_EQUAL_INT16(0, v[0]); - TEST_ASSERT_EQUAL_INT16(0, v[1]); - TEST_ASSERT_EQUAL_INT16(0, v[2]); + TEST_ASSERT_EQUAL_INT16(0, v[0]); + TEST_ASSERT_EQUAL_INT16(0, v[1]); + TEST_ASSERT_EQUAL_INT16(0, v[2]); - TEST_ASSERT_EQUAL_INT16(0, v.get(0)); - TEST_ASSERT_EQUAL_INT16(0, v.get(1)); - TEST_ASSERT_EQUAL_INT16(0, v.get(2)); + TEST_ASSERT_EQUAL_INT16(0, v.get(0)); + TEST_ASSERT_EQUAL_INT16(0, v.get(1)); + TEST_ASSERT_EQUAL_INT16(0, v.get(2)); - v.set(0, 1); - v.set(1, 2); - v.set(2, 3); + v.set(0, 1); + v.set(1, 2); + v.set(2, 3); - TEST_ASSERT_EQUAL_INT16(1, v.x); - TEST_ASSERT_EQUAL_INT16(2, v.y); - TEST_ASSERT_EQUAL_INT16(3, v.z); + TEST_ASSERT_EQUAL_INT16(1, v.x); + TEST_ASSERT_EQUAL_INT16(2, v.y); + TEST_ASSERT_EQUAL_INT16(3, v.z); - TEST_ASSERT_EQUAL_INT16(1, v[0]); - TEST_ASSERT_EQUAL_INT16(2, v[1]); - TEST_ASSERT_EQUAL_INT16(3, v[2]); + TEST_ASSERT_EQUAL_INT16(1, v[0]); + TEST_ASSERT_EQUAL_INT16(2, v[1]); + TEST_ASSERT_EQUAL_INT16(3, v[2]); - TEST_ASSERT_EQUAL_INT16(1, v.get(0)); - TEST_ASSERT_EQUAL_INT16(2, v.get(1)); - TEST_ASSERT_EQUAL_INT16(3, v.get(2)); + TEST_ASSERT_EQUAL_INT16(1, v.get(0)); + TEST_ASSERT_EQUAL_INT16(2, v.get(1)); + TEST_ASSERT_EQUAL_INT16(3, v.get(2)); } void test_vector_int16_math() { - const VectorInt16 v0(0, -1, 2); - - VectorInt16 v = v0; - TEST_ASSERT_EQUAL_INT16(0, v.x); - TEST_ASSERT_EQUAL_INT16(-1, v.y); - TEST_ASSERT_EQUAL_INT16(2, v.z); - - v += v0; - TEST_ASSERT_EQUAL_INT16(0, v.x); - TEST_ASSERT_EQUAL_INT16(-2, v.y); - TEST_ASSERT_EQUAL_INT16(4, v.z); - - v *= 2; - TEST_ASSERT_EQUAL_INT16(0, v.x); - TEST_ASSERT_EQUAL_INT16(-4, v.y); - TEST_ASSERT_EQUAL_INT16(8, v.z); - - v /= 4; - TEST_ASSERT_EQUAL_INT16(0, v.x); - TEST_ASSERT_EQUAL_INT16(-1, v.y); - TEST_ASSERT_EQUAL_INT16(2, v.z); - - v -= v0; - TEST_ASSERT_EQUAL_INT16(0, v.x); - TEST_ASSERT_EQUAL_INT16(0, v.y); - TEST_ASSERT_EQUAL_INT16(0, v.z); + const VectorInt16 v0(0, -1, 2); + + VectorInt16 v = v0; + TEST_ASSERT_EQUAL_INT16(0, v.x); + TEST_ASSERT_EQUAL_INT16(-1, v.y); + TEST_ASSERT_EQUAL_INT16(2, v.z); + + v += v0; + TEST_ASSERT_EQUAL_INT16(0, v.x); + TEST_ASSERT_EQUAL_INT16(-2, v.y); + TEST_ASSERT_EQUAL_INT16(4, v.z); + + v *= 2; + TEST_ASSERT_EQUAL_INT16(0, v.x); + TEST_ASSERT_EQUAL_INT16(-4, v.y); + TEST_ASSERT_EQUAL_INT16(8, v.z); + + v /= 4; + TEST_ASSERT_EQUAL_INT16(0, v.x); + TEST_ASSERT_EQUAL_INT16(-1, v.y); + TEST_ASSERT_EQUAL_INT16(2, v.z); + + v -= v0; + TEST_ASSERT_EQUAL_INT16(0, v.x); + TEST_ASSERT_EQUAL_INT16(0, v.y); + TEST_ASSERT_EQUAL_INT16(0, v.z); } void test_vector_float_math3d() { - const VectorFloat v0(0.0f, 1.0f, 0.0f); - - const VectorFloat v1(0.0f, 0.0f, 1.0f); - const VectorFloat r1 = v0.cross(v1); - float d1 = v0.dot(v1); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.0f, r1.x); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, r1.y); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, r1.z); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, d1); - - const VectorFloat v2(0.0f, 2.0f, 0.0f); - const VectorFloat r2 = v0.cross(v2); - float d2 = v0.dot(v2); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, r2.x); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, r2.y); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, r2.z); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 2.0f, d2); - - const VectorFloat v3(2.0f, 2.0f, 0.0f); - const VectorFloat r3 = v0.cross(v3); - float d3 = v0.dot(v3); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, r3.x); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, r3.y); - TEST_ASSERT_FLOAT_WITHIN(0.001f, -2.0f, r3.z); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 2.0f, d3); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.0f, v0.getMagnitude()); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 2.0f, r3.getMagnitude()); - - const VectorFloat n = r3.getNormalized(); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, n.x); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, n.y); - TEST_ASSERT_FLOAT_WITHIN(0.001f, -1.0f, n.z); + const VectorFloat v0(0.0f, 1.0f, 0.0f); + + const VectorFloat v1(0.0f, 0.0f, 1.0f); + const VectorFloat r1 = v0.cross(v1); + float d1 = v0.dot(v1); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.0f, r1.x); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, r1.y); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, r1.z); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, d1); + + const VectorFloat v2(0.0f, 2.0f, 0.0f); + const VectorFloat r2 = v0.cross(v2); + float d2 = v0.dot(v2); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, r2.x); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, r2.y); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, r2.z); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 2.0f, d2); + + const VectorFloat v3(2.0f, 2.0f, 0.0f); + const VectorFloat r3 = v0.cross(v3); + float d3 = v0.dot(v3); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, r3.x); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, r3.y); + TEST_ASSERT_FLOAT_WITHIN(0.001f, -2.0f, r3.z); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 2.0f, d3); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.0f, v0.getMagnitude()); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 2.0f, r3.getMagnitude()); + + const VectorFloat n = r3.getNormalized(); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, n.x); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, n.y); + TEST_ASSERT_FLOAT_WITHIN(0.001f, -1.0f, n.z); } void test_filter_sanitize_none() { - FilterConfig conf(FILTER_NONE, 0); - TEST_ASSERT_EQUAL(FILTER_NONE, conf.type); - TEST_ASSERT_EQUAL_INT16(0, conf.freq); - TEST_ASSERT_EQUAL_INT16(0, conf.cutoff); - - FilterConfig conf2 = conf.sanitize(100); - TEST_ASSERT_EQUAL(FILTER_NONE, conf2.type); - TEST_ASSERT_EQUAL_INT16(0, conf2.freq); - TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); + FilterConfig conf(FILTER_NONE, 0); + TEST_ASSERT_EQUAL(FILTER_NONE, conf.type); + TEST_ASSERT_EQUAL_INT16(0, conf.freq); + TEST_ASSERT_EQUAL_INT16(0, conf.cutoff); + + FilterConfig conf2 = conf.sanitize(100); + TEST_ASSERT_EQUAL(FILTER_NONE, conf2.type); + TEST_ASSERT_EQUAL_INT16(0, conf2.freq); + TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); } void test_filter_sanitize_pt1_off() { - FilterConfig conf(FILTER_PT1, 0); - TEST_ASSERT_EQUAL(FILTER_PT1, conf.type); - TEST_ASSERT_EQUAL_INT16(0, conf.freq); - TEST_ASSERT_EQUAL_INT16(0, conf.cutoff); - - FilterConfig conf2 = conf.sanitize(100); - TEST_ASSERT_EQUAL(FILTER_NONE, conf2.type); - TEST_ASSERT_EQUAL_INT16(0, conf2.freq); - TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); + FilterConfig conf(FILTER_PT1, 0); + TEST_ASSERT_EQUAL(FILTER_PT1, conf.type); + TEST_ASSERT_EQUAL_INT16(0, conf.freq); + TEST_ASSERT_EQUAL_INT16(0, conf.cutoff); + + FilterConfig conf2 = conf.sanitize(100); + TEST_ASSERT_EQUAL(FILTER_NONE, conf2.type); + TEST_ASSERT_EQUAL_INT16(0, conf2.freq); + TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); } void test_filter_sanitize_pt1_nyquist() { - FilterConfig conf(FILTER_PT1, 80); - TEST_ASSERT_EQUAL(FILTER_PT1, conf.type); - TEST_ASSERT_EQUAL_INT16(80, conf.freq); - TEST_ASSERT_EQUAL_INT16(0, conf.cutoff); - - FilterConfig conf2 = conf.sanitize(100); - TEST_ASSERT_EQUAL(FILTER_PT1, conf2.type); - TEST_ASSERT_EQUAL_INT16(49, conf2.freq); - TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); + FilterConfig conf(FILTER_PT1, 80); + TEST_ASSERT_EQUAL(FILTER_PT1, conf.type); + TEST_ASSERT_EQUAL_INT16(80, conf.freq); + TEST_ASSERT_EQUAL_INT16(0, conf.cutoff); + + FilterConfig conf2 = conf.sanitize(100); + TEST_ASSERT_EQUAL(FILTER_PT1, conf2.type); + TEST_ASSERT_EQUAL_INT16(49, conf2.freq); + TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); } void test_filter_sanitize_pt1() { - FilterConfig conf(FILTER_PT1, 20); - TEST_ASSERT_EQUAL(FILTER_PT1, conf.type); - TEST_ASSERT_EQUAL_INT16(20, conf.freq); - TEST_ASSERT_EQUAL_INT16(0, conf.cutoff); - - FilterConfig conf2 = conf.sanitize(100); - TEST_ASSERT_EQUAL(FILTER_PT1, conf2.type); - TEST_ASSERT_EQUAL_INT16(20, conf2.freq); - TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); + FilterConfig conf(FILTER_PT1, 20); + TEST_ASSERT_EQUAL(FILTER_PT1, conf.type); + TEST_ASSERT_EQUAL_INT16(20, conf.freq); + TEST_ASSERT_EQUAL_INT16(0, conf.cutoff); + + FilterConfig conf2 = conf.sanitize(100); + TEST_ASSERT_EQUAL(FILTER_PT1, conf2.type); + TEST_ASSERT_EQUAL_INT16(20, conf2.freq); + TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); } void test_filter_sanitize_biquad_lpf() { - FilterConfig conf(FILTER_BIQUAD, 20); - TEST_ASSERT_EQUAL(FILTER_BIQUAD, conf.type); - TEST_ASSERT_EQUAL_INT16(20, conf.freq); - TEST_ASSERT_EQUAL_INT16(0, conf.cutoff); - - FilterConfig conf2 = conf.sanitize(100); - TEST_ASSERT_EQUAL(FILTER_BIQUAD, conf2.type); - TEST_ASSERT_EQUAL_INT16(20, conf2.freq); - TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); + FilterConfig conf(FILTER_BIQUAD, 20); + TEST_ASSERT_EQUAL(FILTER_BIQUAD, conf.type); + TEST_ASSERT_EQUAL_INT16(20, conf.freq); + TEST_ASSERT_EQUAL_INT16(0, conf.cutoff); + + FilterConfig conf2 = conf.sanitize(100); + TEST_ASSERT_EQUAL(FILTER_BIQUAD, conf2.type); + TEST_ASSERT_EQUAL_INT16(20, conf2.freq); + TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); } void test_filter_sanitize_biquad_lpf_off() { - FilterConfig conf(FILTER_BIQUAD, 0); - TEST_ASSERT_EQUAL(FILTER_BIQUAD, conf.type); - TEST_ASSERT_EQUAL_INT16(0, conf.freq); - TEST_ASSERT_EQUAL_INT16(0, conf.cutoff); - - FilterConfig conf2 = conf.sanitize(100); - TEST_ASSERT_EQUAL(FILTER_NONE, conf2.type); - TEST_ASSERT_EQUAL_INT16(0, conf2.freq); - TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); + FilterConfig conf(FILTER_BIQUAD, 0); + TEST_ASSERT_EQUAL(FILTER_BIQUAD, conf.type); + TEST_ASSERT_EQUAL_INT16(0, conf.freq); + TEST_ASSERT_EQUAL_INT16(0, conf.cutoff); + + FilterConfig conf2 = conf.sanitize(100); + TEST_ASSERT_EQUAL(FILTER_NONE, conf2.type); + TEST_ASSERT_EQUAL_INT16(0, conf2.freq); + TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); } void test_filter_sanitize_biquad_lpf_nyqist() { - FilterConfig conf(FILTER_BIQUAD, 80); - TEST_ASSERT_EQUAL(FILTER_BIQUAD, conf.type); - TEST_ASSERT_EQUAL_INT16(80, conf.freq); - TEST_ASSERT_EQUAL_INT16(0, conf.cutoff); - - FilterConfig conf2 = conf.sanitize(100); - TEST_ASSERT_EQUAL(FILTER_BIQUAD, conf2.type); - TEST_ASSERT_EQUAL_INT16(49, conf2.freq); - TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); + FilterConfig conf(FILTER_BIQUAD, 80); + TEST_ASSERT_EQUAL(FILTER_BIQUAD, conf.type); + TEST_ASSERT_EQUAL_INT16(80, conf.freq); + TEST_ASSERT_EQUAL_INT16(0, conf.cutoff); + + FilterConfig conf2 = conf.sanitize(100); + TEST_ASSERT_EQUAL(FILTER_BIQUAD, conf2.type); + TEST_ASSERT_EQUAL_INT16(49, conf2.freq); + TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); } void test_filter_sanitize_biquad_notch() { - FilterConfig conf(FILTER_NOTCH, 40, 30); - TEST_ASSERT_EQUAL(FILTER_NOTCH, conf.type); - TEST_ASSERT_EQUAL_INT16(40, conf.freq); - TEST_ASSERT_EQUAL_INT16(30, conf.cutoff); - - FilterConfig conf2 = conf.sanitize(100); - TEST_ASSERT_EQUAL(FILTER_NOTCH, conf2.type); - TEST_ASSERT_EQUAL_INT16(40, conf2.freq); - TEST_ASSERT_EQUAL_INT16(30, conf2.cutoff); + FilterConfig conf(FILTER_NOTCH, 40, 30); + TEST_ASSERT_EQUAL(FILTER_NOTCH, conf.type); + TEST_ASSERT_EQUAL_INT16(40, conf.freq); + TEST_ASSERT_EQUAL_INT16(30, conf.cutoff); + + FilterConfig conf2 = conf.sanitize(100); + TEST_ASSERT_EQUAL(FILTER_NOTCH, conf2.type); + TEST_ASSERT_EQUAL_INT16(40, conf2.freq); + TEST_ASSERT_EQUAL_INT16(30, conf2.cutoff); } void test_filter_sanitize_biquad_notch_off_freq() { - FilterConfig conf(FILTER_NOTCH, 0, 30); - TEST_ASSERT_EQUAL(FILTER_NOTCH, conf.type); - TEST_ASSERT_EQUAL_INT16(0, conf.freq); - TEST_ASSERT_EQUAL_INT16(30, conf.cutoff); - - FilterConfig conf2 = conf.sanitize(100); - TEST_ASSERT_EQUAL(FILTER_NONE, conf2.type); - TEST_ASSERT_EQUAL_INT16(0, conf2.freq); - TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); + FilterConfig conf(FILTER_NOTCH, 0, 30); + TEST_ASSERT_EQUAL(FILTER_NOTCH, conf.type); + TEST_ASSERT_EQUAL_INT16(0, conf.freq); + TEST_ASSERT_EQUAL_INT16(30, conf.cutoff); + + FilterConfig conf2 = conf.sanitize(100); + TEST_ASSERT_EQUAL(FILTER_NONE, conf2.type); + TEST_ASSERT_EQUAL_INT16(0, conf2.freq); + TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); } void test_filter_sanitize_biquad_notch_off_cut() { - FilterConfig conf(FILTER_NOTCH, 40, 0); - TEST_ASSERT_EQUAL(FILTER_NOTCH, conf.type); - TEST_ASSERT_EQUAL_INT16(40, conf.freq); - TEST_ASSERT_EQUAL_INT16(0, conf.cutoff); - - FilterConfig conf2 = conf.sanitize(100); - TEST_ASSERT_EQUAL(FILTER_NONE, conf2.type); - TEST_ASSERT_EQUAL_INT16(40, conf2.freq); - TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); + FilterConfig conf(FILTER_NOTCH, 40, 0); + TEST_ASSERT_EQUAL(FILTER_NOTCH, conf.type); + TEST_ASSERT_EQUAL_INT16(40, conf.freq); + TEST_ASSERT_EQUAL_INT16(0, conf.cutoff); + + FilterConfig conf2 = conf.sanitize(100); + TEST_ASSERT_EQUAL(FILTER_NONE, conf2.type); + TEST_ASSERT_EQUAL_INT16(40, conf2.freq); + TEST_ASSERT_EQUAL_INT16(0, conf2.cutoff); } void test_filter_sanitize_biquad_notch_nyquist() { - FilterConfig conf(FILTER_NOTCH, 80, 70); - TEST_ASSERT_EQUAL(FILTER_NOTCH, conf.type); - TEST_ASSERT_EQUAL_INT16(80, conf.freq); - TEST_ASSERT_EQUAL_INT16(70, conf.cutoff); - - FilterConfig conf2 = conf.sanitize(100); - TEST_ASSERT_EQUAL(FILTER_NOTCH, conf2.type); - TEST_ASSERT_EQUAL_INT16(49, conf2.freq); - TEST_ASSERT_EQUAL_INT16(48, conf2.cutoff); + FilterConfig conf(FILTER_NOTCH, 80, 70); + TEST_ASSERT_EQUAL(FILTER_NOTCH, conf.type); + TEST_ASSERT_EQUAL_INT16(80, conf.freq); + TEST_ASSERT_EQUAL_INT16(70, conf.cutoff); + + FilterConfig conf2 = conf.sanitize(100); + TEST_ASSERT_EQUAL(FILTER_NOTCH, conf2.type); + TEST_ASSERT_EQUAL_INT16(49, conf2.freq); + TEST_ASSERT_EQUAL_INT16(48, conf2.cutoff); } void assert_filter_off(Filter& filter) { - TEST_ASSERT_EQUAL(FILTER_NONE, filter._conf.type); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, filter.update(0.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, filter.update(0.1f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, filter.update(0.5f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.5f, filter.update(1.5f)); + TEST_ASSERT_EQUAL(FILTER_NONE, filter._conf.type); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, filter.update(0.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, filter.update(0.1f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, filter.update(0.5f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.5f, filter.update(1.5f)); } void test_filter_default() { - Filter filter; - assert_filter_off(filter); + Filter filter; + assert_filter_off(filter); } void test_filter_none() { - Filter filter; - const FilterConfig config(FILTER_NONE, 0); - filter.begin(config, 100); - assert_filter_off(filter); + Filter filter; + const FilterConfig config(FILTER_NONE, 0); + filter.begin(config, 100); + assert_filter_off(filter); } void test_filter_pt1_off() { - Filter filter; - const FilterConfig config(FILTER_PT1, 0); - filter.begin(config, 100); - assert_filter_off(filter); + Filter filter; + const FilterConfig config(FILTER_PT1, 0); + filter.begin(config, 100); + assert_filter_off(filter); } void test_filter_pt1_50_100() { - Filter filter; - const FilterConfig config(FILTER_PT1, 50); - filter.begin(config, 100); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.076f, filter.update(0.1f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.395f, filter.update(0.5f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.229f, filter.update(1.5f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.434f, filter.update(1.5f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.484f, filter.update(1.5f)); + Filter filter; + const FilterConfig config(FILTER_PT1, 50); + filter.begin(config, 100); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.076f, filter.update(0.1f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.395f, filter.update(0.5f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.229f, filter.update(1.5f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.434f, filter.update(1.5f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.484f, filter.update(1.5f)); } void test_filter_pt1_10_100_step() { - Filter filter; - const FilterConfig config(FILTER_PT1, 10); - filter.begin(config, 100); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.386f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.623f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.768f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.858f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.913f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.946f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.967f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.980f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.988f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.992f, filter.update(1.0f)); + Filter filter; + const FilterConfig config(FILTER_PT1, 10); + filter.begin(config, 100); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.386f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.623f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.768f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.858f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.913f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.946f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.967f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.980f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.988f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.992f, filter.update(1.0f)); } void test_filter_pt1_above_nyquist() { - Filter filter; - const FilterConfig config(FILTER_NOTCH, 200); - filter.begin(config, 100); - TEST_ASSERT_EQUAL_INT(100, filter._rate); - TEST_ASSERT_LESS_OR_EQUAL_INT(50, filter._conf.freq); - TEST_ASSERT_GREATER_THAN(10, filter._conf.freq); + Filter filter; + const FilterConfig config(FILTER_NOTCH, 200); + filter.begin(config, 100); + TEST_ASSERT_EQUAL_INT(100, filter._rate); + TEST_ASSERT_LESS_OR_EQUAL_INT(50, filter._conf.freq); + TEST_ASSERT_GREATER_THAN(10, filter._conf.freq); } void test_filter_pt2_10_100_step() { - Filter filter; - const FilterConfig config(FILTER_PT2, 10); - filter.begin(config, 100); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.244f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.490f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.678f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.804f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.885f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.933f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.962f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.978f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.988f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.993f, filter.update(1.0f)); + Filter filter; + const FilterConfig config(FILTER_PT2, 10); + filter.begin(config, 100); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.244f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.490f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.678f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.804f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.885f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.933f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.962f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.978f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.988f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.993f, filter.update(1.0f)); } void test_filter_pt3_10_100_step() { - Filter filter; - const FilterConfig config(FILTER_PT3, 10); - filter.begin(config, 100); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.168f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.394f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.596f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.748f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.850f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.913f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.952f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.973f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.986f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.992f, filter.update(1.0f)); + Filter filter; + const FilterConfig config(FILTER_PT3, 10); + filter.begin(config, 100); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.168f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.394f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.596f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.748f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.850f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.913f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.952f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.973f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.986f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.992f, filter.update(1.0f)); } void test_filter_biquad_off() { - Filter filter; - const FilterConfig config(FILTER_BIQUAD, 0); - filter.begin(config, 100); - assert_filter_off(filter); + Filter filter; + const FilterConfig config(FILTER_BIQUAD, 0); + filter.begin(config, 100); + assert_filter_off(filter); } void test_filter_biquad_20_100() { - Filter filter; - const FilterConfig config(FILTER_BIQUAD, 20); - filter.begin(config, 100); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.020f, filter.update(0.1f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.152f, filter.update(0.5f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.589f, filter.update(1.5f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.220f, filter.update(1.5f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.575f, filter.update(1.5f)); + Filter filter; + const FilterConfig config(FILTER_BIQUAD, 20); + filter.begin(config, 100); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.020f, filter.update(0.1f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.152f, filter.update(0.5f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.589f, filter.update(1.5f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.220f, filter.update(1.5f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.575f, filter.update(1.5f)); } void test_filter_biquad_10_100_step() { - Filter filter; - const FilterConfig config(FILTER_BIQUAD, 10); - filter.begin(config, 100); - - TEST_ASSERT_EQUAL(FILTER_BIQUAD, filter._conf.type); - TEST_ASSERT_EQUAL(100, filter._rate); - TEST_ASSERT_EQUAL(10, filter._conf.freq); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.067f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.279f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.561f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.796f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.948f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.025f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.050f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.047f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.033f, filter.update(1.0f)); + Filter filter; + const FilterConfig config(FILTER_BIQUAD, 10); + filter.begin(config, 100); + + TEST_ASSERT_EQUAL(FILTER_BIQUAD, filter._conf.type); + TEST_ASSERT_EQUAL(100, filter._rate); + TEST_ASSERT_EQUAL(10, filter._conf.freq); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.067f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.279f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.561f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.796f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.948f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.025f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.050f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.047f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.033f, filter.update(1.0f)); } void test_filter_notch_df1_150_200_1000() { - Filter filter; - const FilterConfig config(FILTER_NOTCH_DF1, 200, 150); - filter.begin(config, 1000); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.783f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.678f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.967f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.166f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.099f, filter.update(1.0f)); + Filter filter; + const FilterConfig config(FILTER_NOTCH_DF1, 200, 150); + filter.begin(config, 1000); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.783f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.678f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.967f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.166f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.099f, filter.update(1.0f)); } void test_filter_notch_df1_150_200_1000_reconf() { - Filter filter; - const FilterConfig config(FILTER_NOTCH_DF1, 200, 150); - filter.begin(config, 1000); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.783f, filter.update(1.0f)); - filter.reconfigure(200, 150); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.678f, filter.update(1.0f)); - filter.reconfigure(200, 150); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.967f, filter.update(1.0f)); - filter.reconfigure(200, 150); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.166f, filter.update(1.0f)); - filter.reconfigure(200, 150); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.099f, filter.update(1.0f)); + Filter filter; + const FilterConfig config(FILTER_NOTCH_DF1, 200, 150); + filter.begin(config, 1000); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.783f, filter.update(1.0f)); + filter.reconfigure(200, 150); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.678f, filter.update(1.0f)); + filter.reconfigure(200, 150); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.967f, filter.update(1.0f)); + filter.reconfigure(200, 150); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.166f, filter.update(1.0f)); + filter.reconfigure(200, 150); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.099f, filter.update(1.0f)); } void test_filter_notch_q() { - Filter filter; + Filter filter; - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.667f, filter.getNotchQ(200, 100)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.667f, filter.getNotchQApprox(200, 100)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.667f, filter.getNotchQ(200, 100)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.667f, filter.getNotchQApprox(200, 100)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.007f, filter.getNotchQ(200, 124)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.007f, filter.getNotchQApprox(200, 124)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.007f, filter.getNotchQ(200, 124)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.007f, filter.getNotchQApprox(200, 124)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.714f, filter.getNotchQ(200, 150)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.714f, filter.getNotchQApprox(200, 150)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.714f, filter.getNotchQ(200, 150)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.714f, filter.getNotchQApprox(200, 150)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 9.744f, filter.getNotchQ(200, 190)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 9.744f, filter.getNotchQApprox(200, 190)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 9.744f, filter.getNotchQ(200, 190)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 9.744f, filter.getNotchQApprox(200, 190)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 4.737f, filter.getNotchQ(100, 90)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 4.737f, filter.getNotchQApprox(100, 90)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 4.737f, filter.getNotchQ(100, 90)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 4.737f, filter.getNotchQApprox(100, 90)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 2.222f, filter.getNotchQ(100, 80)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 2.222f, filter.getNotchQApprox(100, 80)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 2.222f, filter.getNotchQ(100, 80)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 2.222f, filter.getNotchQApprox(100, 80)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.216f, filter.getNotchQ(100, 67)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.216f, filter.getNotchQApprox(100, 67)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.216f, filter.getNotchQ(100, 67)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.216f, filter.getNotchQApprox(100, 67)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.667f, filter.getNotchQ(300, 150)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.667f, filter.getNotchQApprox(300, 150)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.667f, filter.getNotchQ(300, 150)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.667f, filter.getNotchQApprox(300, 150)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.884f, filter.getNotchQ(300, 175)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.884f, filter.getNotchQApprox(300, 175)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.884f, filter.getNotchQ(300, 175)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.884f, filter.getNotchQApprox(300, 175)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.200f, filter.getNotchQ(300, 200)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.200f, filter.getNotchQApprox(300, 200)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.200f, filter.getNotchQ(300, 200)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.200f, filter.getNotchQApprox(300, 200)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 3.733f, filter.getNotchQ(400, 350)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 3.733f, filter.getNotchQApprox(400, 350)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 3.733f, filter.getNotchQ(400, 350)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 3.733f, filter.getNotchQApprox(400, 350)); } void test_filter_notch_no_cutoff() { - Filter filter; - const FilterConfig config(FILTER_NOTCH, 50, 0); - filter.begin(config, 100); - assert_filter_off(filter); + Filter filter; + const FilterConfig config(FILTER_NOTCH, 50, 0); + filter.begin(config, 100); + assert_filter_off(filter); } void test_filter_notch_above_nyquist() { - Filter filter; - const FilterConfig config(FILTER_NOTCH, 200, 150); - filter.begin(config, 100); - TEST_ASSERT_EQUAL_INT(100, filter._rate); - TEST_ASSERT_LESS_OR_EQUAL_INT(100, filter._conf.freq); - TEST_ASSERT_LESS_THAN(filter._conf.freq, filter._conf.cutoff); + Filter filter; + const FilterConfig config(FILTER_NOTCH, 200, 150); + filter.begin(config, 100); + TEST_ASSERT_EQUAL_INT(100, filter._rate); + TEST_ASSERT_LESS_OR_EQUAL_INT(100, filter._conf.freq); + TEST_ASSERT_LESS_THAN(filter._conf.freq, filter._conf.cutoff); } void test_filter_fir2_off() { - Filter filter; - const FilterConfig config(FILTER_FIR2, 0); - filter.begin(config, 100); - TEST_ASSERT_EQUAL_INT(100, filter._rate); - TEST_ASSERT_LESS_OR_EQUAL_INT(0, filter._conf.freq); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.000f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.000f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.000f, filter.update(1.0f)); + Filter filter; + const FilterConfig config(FILTER_FIR2, 0); + filter.begin(config, 100); + TEST_ASSERT_EQUAL_INT(100, filter._rate); + TEST_ASSERT_LESS_OR_EQUAL_INT(0, filter._conf.freq); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.000f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.000f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.000f, filter.update(1.0f)); } void test_filter_fir2_on() { - Filter filter; - const FilterConfig config(FILTER_FIR2, 1); - filter.begin(config, 100); - TEST_ASSERT_EQUAL_INT(100, filter._rate); - TEST_ASSERT_LESS_OR_EQUAL_INT(1, filter._conf.freq); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.500f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.000f, filter.update(1.0f)); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.000f, filter.update(1.0f)); + Filter filter; + const FilterConfig config(FILTER_FIR2, 1); + filter.begin(config, 100); + TEST_ASSERT_EQUAL_INT(100, filter._rate); + TEST_ASSERT_LESS_OR_EQUAL_INT(1, filter._conf.freq); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, filter.update(0.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.500f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.000f, filter.update(1.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.000f, filter.update(1.0f)); } void test_pid_init() { - Pid pid; - pid.rate = 100; - pid.begin(); + Pid pid; + pid.rate = 100; + pid.begin(); - TEST_ASSERT_FLOAT_WITHIN(0.1f, 100.0f, pid.rate); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.01f, pid.dt); + TEST_ASSERT_FLOAT_WITHIN(0.1f, 100.0f, pid.rate); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.01f, pid.dt); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1f, pid.Kp); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.Ki); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.Kd); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.Kf); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1f, pid.Kp); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.Ki); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.Kd); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.Kf); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.3f, pid.iLimit); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.0f, pid.oLimit); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.3f, pid.iLimit); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.0f, pid.oLimit); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.0f, pid.pScale); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.0f, pid.iScale); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.0f, pid.dScale); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.0f, pid.fScale); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.0f, pid.pScale); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.0f, pid.iScale); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.0f, pid.dScale); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.0f, pid.fScale); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.error); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.iTermError); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.error); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.iTermError); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.pTerm); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.iTerm); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.fTerm); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.pTerm); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.iTerm); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.dTerm); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.fTerm); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.prevMeasure); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.prevError); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.prevSetpoint); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.prevMeasurement); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.prevError); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.prevSetpoint); - TEST_ASSERT_FALSE(pid.outputSaturated); + TEST_ASSERT_FALSE(pid.outputSaturated); - TEST_ASSERT_EQUAL(0, pid.itermRelax); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.itermRelaxBase); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.0f, pid.itermRelaxFactor); + TEST_ASSERT_EQUAL(0, pid.itermRelax); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0f, pid.itermRelaxBase); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.0f, pid.itermRelaxFactor); } void ensure(Pid& pid, float rate = 100.0f) { - pid.rate = rate; + pid.rate = rate; } void gain(Pid& pid, float p, float i, float d, float f) { - pid.Kp = p; - pid.Ki = i; - pid.Kd = d; - pid.Kf = f; + pid.Kp = p; + pid.Ki = i; + pid.Kd = d; + pid.Kf = f; } void test_pid_update_p() { - Pid pid; - ensure(pid); - gain(pid, 1, 0, 0, 0); - pid.begin(); - - float result = pid.update(0.1f, 0.f); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, result); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, pid.prevSetpoint); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, pid.error); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, pid.pTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.iTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); + Pid pid; + ensure(pid); + gain(pid, 1, 0, 0, 0); + pid.begin(); + + float result = pid.update(0.1f, 0.f); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, result); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, pid.prevSetpoint); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, pid.error); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, pid.pTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.iTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); } void test_pid_update_i() { - Pid pid; - ensure(pid); - gain(pid, 0, 10, 0, 0); - pid.begin(); - - float result = pid.update(0.2f, 0.f); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.02f, result); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.2f, pid.error); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.2f, pid.prevSetpoint); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.2f, pid.prevError); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.prevMeasure); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.02f, pid.iTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); - - float result2 = pid.update(0.2f, 0.f); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.04f, result2); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.2f, pid.error); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.2f, pid.prevSetpoint); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.2f, pid.prevError); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.prevMeasure); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.04f, pid.iTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); + Pid pid; + ensure(pid); + gain(pid, 0, 10, 0, 0); + pid.begin(); + + float result = pid.update(0.2f, 0.f); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.02f, result); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.2f, pid.error); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.2f, pid.prevSetpoint); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.2f, pid.prevError); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.prevMeasurement); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.02f, pid.iTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); + + float result2 = pid.update(0.2f, 0.f); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.04f, result2); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.2f, pid.error); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.2f, pid.prevSetpoint); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.2f, pid.prevError); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.prevMeasurement); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.04f, pid.iTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); } void test_pid_update_i_limit() { - Pid pid; - ensure(pid); - gain(pid, 0, 10, 0, 0); - pid.iLimit = 0.2f; - pid.begin(); - - float result1 = pid.update(0.8f, 0.f); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.8f, pid.error); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.08f, pid.iTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.08f, result1); - - float result2 = pid.update(0.8f, 0.f); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.8f, pid.error); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.16f, pid.iTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.16f, result2); - - float result3 = pid.update(0.8f, 0.f); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.8f, pid.error); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.20f, pid.iTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.20f, result3); + Pid pid; + ensure(pid); + gain(pid, 0, 10, 0, 0); + pid.iLimit = 0.2f; + pid.begin(); + + float result1 = pid.update(0.8f, 0.f); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.8f, pid.error); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.08f, pid.iTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.08f, result1); + + float result2 = pid.update(0.8f, 0.f); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.8f, pid.error); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.16f, pid.iTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.16f, result2); + + float result3 = pid.update(0.8f, 0.f); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.8f, pid.error); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.20f, pid.iTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.20f, result3); } void test_pid_update_i_relax() { - Pid pid; - ensure(pid); - gain(pid, 0, 10, 0, 0); - pid.itermRelax = ITERM_RELAX_RP; - pid.itermRelaxFilter.begin(FilterConfig(FILTER_PT1, 15), pid.rate); - pid.begin(); - - float result0 = pid.update(0.0f, 0.f); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, result0); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.error); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.iTermError); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.itermRelaxBase); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.0f, pid.itermRelaxFactor); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.iTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); - - float result1 = pid.update(1.f, 0.f); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.026f, result1); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.000f, pid.error); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.262f, pid.iTermError); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.515f, pid.itermRelaxBase); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.262f, pid.itermRelaxFactor); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.026f, pid.iTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, pid.pTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, pid.fTerm); - - float result2 = pid.update(3.f, 0.f); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.026f, result2); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 3.000f, pid.error); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, pid.iTermError); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.294f, pid.itermRelaxBase); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, pid.itermRelaxFactor); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.026f, pid.iTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.00f, pid.pTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.00f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.00f, pid.fTerm); + Pid pid; + ensure(pid); + gain(pid, 0, 10, 0, 0); + pid.itermRelax = ITERM_RELAX_RP; + pid.itermRelaxFilter.begin(FilterConfig(FILTER_PT1, 15), pid.rate); + pid.begin(); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.000f, pid.iScale); + + float result0 = pid.update(0.0f, 0.f); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, result0); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.error); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.iTermError); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.itermRelaxBase); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.0f, pid.itermRelaxFactor); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.iTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); + + float result1 = pid.update(1.f, 0.f); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.000f, pid.error); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.262f, pid.iTermError); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.515f, pid.itermRelaxBase); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.262f, pid.itermRelaxFactor); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.026f, result1); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.026f, pid.iTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, pid.pTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, pid.dTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, pid.fTerm); + + float result2 = pid.update(3.f, 0.f); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.026f, result2); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 3.000f, pid.error); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, pid.iTermError); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.294f, pid.itermRelaxBase); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.000f, pid.itermRelaxFactor); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.026f, pid.iTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.00f, pid.pTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.00f, pid.dTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.00f, pid.fTerm); } void test_pid_update_d() { - Pid pid; - ensure(pid); - gain(pid, 0, 0, 0.1f, 0); - pid.begin(); - - float result1 = pid.update(0.0f, -0.05f); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.05f, pid.error); - TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.05f, pid.prevMeasure); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.iTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, result1); - - float result2 = pid.update(0.0f, 0.0f); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.error); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.prevMeasure); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.iTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.5f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.5f, result2); + Pid pid; + ensure(pid); + gain(pid, 0, 0, 0.1f, 0); + pid.begin(); + + float result1 = pid.update(0.0f, -0.05f); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.05f, pid.error); + TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.05f, pid.prevMeasurement); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.iTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, pid.dTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, result1); + + float result2 = pid.update(0.0f, 0.0f); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.error); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.prevMeasurement); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.iTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.5f, pid.dTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.fTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.5f, result2); } void test_pid_update_f() { - Pid pid; - ensure(pid); - gain(pid, 0, 0, 0, 0.1f); - pid.begin(); - - float result1 = pid.update(-0.05f, 0.0f); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.05f, pid.error); - TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.05f, pid.prevSetpoint); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.iTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.5f, pid.fTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.5f, result1); - - float result2 = pid.update(0.0f, 0.0f); - - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.error); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.prevMeasure); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.iTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, pid.fTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, result2); + Pid pid; + ensure(pid); + gain(pid, 0, 0, 0, 0.1f); + pid.begin(); + + float result1 = pid.update(-0.05f, 0.0f); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.05f, pid.error); + TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.05f, pid.prevSetpoint); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.iTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.5f, pid.fTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, -0.5f, result1); + + float result2 = pid.update(0.0f, 0.0f); + + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.error); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.prevMeasurement); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.pTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.iTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, pid.fTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, result2); } void test_pid_update_sum() { - Pid pid; - ensure(pid); - gain(pid, 1, 100, 0.1f, 0.01f); - pid.begin(); + Pid pid; + ensure(pid); + gain(pid, 1, 100, 0.1f, 0.01f); + pid.begin(); - float result = pid.update(0.1f, 0.f); + float result = pid.update(0.1f, 0.f); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, pid.error); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, pid.error); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, pid.pTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, pid.iTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, pid.fTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.3f, result); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, pid.pTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, pid.iTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.1f, pid.fTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.3f, result); } void test_pid_update_sum_limit() { - Pid pid; - ensure(pid); - gain(pid, 1, 100, 0.01f, 0.01f); - pid.begin(); + Pid pid; + ensure(pid); + gain(pid, 1, 100, 0.01f, 0.01f); + pid.begin(); - float result = pid.update(0.5f, 0.f); + float result = pid.update(0.5f, 0.f); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, pid.error); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, pid.error); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, pid.pTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.3f, pid.iTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, pid.fTerm); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.0f, result); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, pid.pTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.3f, pid.iTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.0f, pid.dTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.5f, pid.fTerm); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.0f, result); } void test_queue_atomic() { - QueueAtomic q; - int e1 = 1, e2 = 2, e3 = 3, e4 = 4; - int r1 = 91, r2 = 92, r3 = 93, r4 = 94; + QueueAtomic q; + int e1 = 1, e2 = 2, e3 = 3, e4 = 4; + int r1 = 91, r2 = 92, r3 = 93, r4 = 94; + + // empty + TEST_ASSERT_TRUE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); + + TEST_ASSERT_FALSE(q.pop(r1)); + TEST_ASSERT_EQUAL(91, r1); + + TEST_ASSERT_TRUE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); - // empty - TEST_ASSERT_TRUE(q.isEmpty()); - TEST_ASSERT_FALSE(q.isFull()); + // push first element + TEST_ASSERT_TRUE(q.push(e1)); - TEST_ASSERT_FALSE(q.pop(r1)); - TEST_ASSERT_EQUAL(91, r1); + TEST_ASSERT_FALSE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); - TEST_ASSERT_TRUE(q.isEmpty()); - TEST_ASSERT_FALSE(q.isFull()); + // pop last element + TEST_ASSERT_TRUE(q.pop(r1)); + TEST_ASSERT_EQUAL(1, r1); - // push first element - TEST_ASSERT_TRUE(q.push(e1)); + TEST_ASSERT_TRUE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); - TEST_ASSERT_FALSE(q.isEmpty()); - TEST_ASSERT_FALSE(q.isFull()); + // pop element again + TEST_ASSERT_FALSE(q.pop(r1)); + TEST_ASSERT_EQUAL(1, r1); - // pop last element - TEST_ASSERT_TRUE(q.pop(r1)); - TEST_ASSERT_EQUAL(1, r1); + TEST_ASSERT_TRUE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); - TEST_ASSERT_TRUE(q.isEmpty()); - TEST_ASSERT_FALSE(q.isFull()); + // make full + TEST_ASSERT_TRUE(q.push(e1)); + TEST_ASSERT_TRUE(q.push(e2)); + TEST_ASSERT_TRUE(q.push(e3)); + TEST_ASSERT_FALSE(q.push(e4)); - // pop element again - TEST_ASSERT_FALSE(q.pop(r1)); - TEST_ASSERT_EQUAL(1, r1); + TEST_ASSERT_FALSE(q.isEmpty()); + TEST_ASSERT_TRUE(q.isFull()); - TEST_ASSERT_TRUE(q.isEmpty()); - TEST_ASSERT_FALSE(q.isFull()); + // make empty + TEST_ASSERT_TRUE(q.pop(r1)); + TEST_ASSERT_EQUAL(1, r1); - // make full - TEST_ASSERT_TRUE(q.push(e1)); - TEST_ASSERT_TRUE(q.push(e2)); - TEST_ASSERT_TRUE(q.push(e3)); - TEST_ASSERT_FALSE(q.push(e4)); + TEST_ASSERT_TRUE(q.pop(r2)); + TEST_ASSERT_EQUAL(2, r2); - TEST_ASSERT_FALSE(q.isEmpty()); - TEST_ASSERT_TRUE(q.isFull()); + TEST_ASSERT_TRUE(q.pop(r3)); + TEST_ASSERT_EQUAL(3, r3); - // make empty - TEST_ASSERT_TRUE(q.pop(r1)); - TEST_ASSERT_EQUAL(1, r1); + TEST_ASSERT_FALSE(q.pop(r4)); + TEST_ASSERT_EQUAL(94, r4); - TEST_ASSERT_TRUE(q.pop(r2)); - TEST_ASSERT_EQUAL(2, r2); + TEST_ASSERT_TRUE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); +} - TEST_ASSERT_TRUE(q.pop(r3)); - TEST_ASSERT_EQUAL(3, r3); +void test_ring_buf() +{ + Utils::RingBuf q; + uint8_t e1 = 1, e2 = 2, e3 = 3, e4 = 4; + uint8_t r1 = 91, r2 = 92, r3 = 93, r4 = 94; + + // empty + TEST_ASSERT_TRUE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); + TEST_ASSERT_EQUAL(0, q.size()); + TEST_ASSERT_EQUAL(3, q.available()); + + TEST_ASSERT_FALSE(q.pop(r1)); + TEST_ASSERT_EQUAL(91, r1); + + TEST_ASSERT_TRUE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); + TEST_ASSERT_EQUAL(0, q.size()); + TEST_ASSERT_EQUAL(3, q.available()); + + // push first element + TEST_ASSERT_TRUE(q.push(e1)); + + TEST_ASSERT_FALSE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); + TEST_ASSERT_EQUAL(1, q.size()); + TEST_ASSERT_EQUAL(2, q.available()); + + // pop last element + TEST_ASSERT_TRUE(q.pop(r1)); + TEST_ASSERT_EQUAL(1, r1); + + TEST_ASSERT_TRUE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); + TEST_ASSERT_EQUAL(0, q.size()); + TEST_ASSERT_EQUAL(3, q.available()); + + // pop element again + TEST_ASSERT_FALSE(q.pop(r1)); + TEST_ASSERT_EQUAL(1, r1); + + TEST_ASSERT_TRUE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); + TEST_ASSERT_EQUAL(0, q.size()); + TEST_ASSERT_EQUAL(3, q.available()); + + // make full + TEST_ASSERT_TRUE(q.push(e1)); + TEST_ASSERT_TRUE(q.push(e2)); + TEST_ASSERT_TRUE(q.push(e3)); + TEST_ASSERT_FALSE(q.push(e4)); + + TEST_ASSERT_FALSE(q.isEmpty()); + TEST_ASSERT_TRUE(q.isFull()); + TEST_ASSERT_EQUAL(3, q.size()); + TEST_ASSERT_EQUAL(0, q.available()); + + // make empty + TEST_ASSERT_TRUE(q.pop(r1)); + + TEST_ASSERT_EQUAL(1, r1); + TEST_ASSERT_FALSE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); + TEST_ASSERT_EQUAL(2, q.size()); + TEST_ASSERT_EQUAL(1, q.available()); + + TEST_ASSERT_TRUE(q.pop(r2)); + + TEST_ASSERT_EQUAL(2, r2); + TEST_ASSERT_FALSE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); + TEST_ASSERT_EQUAL(1, q.size()); + TEST_ASSERT_EQUAL(2, q.available()); + + TEST_ASSERT_TRUE(q.pop(r3)); + + TEST_ASSERT_EQUAL(3, r3); + TEST_ASSERT_TRUE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); + TEST_ASSERT_EQUAL(0, q.size()); + TEST_ASSERT_EQUAL(3, q.available()); + + TEST_ASSERT_FALSE(q.pop(r4)); + + TEST_ASSERT_EQUAL(94, r4); + TEST_ASSERT_TRUE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); + TEST_ASSERT_EQUAL_UINT32(0, q.size()); + TEST_ASSERT_EQUAL_UINT32(3, q.available()); + + // push 2 elements at once + uint8_t a1[3] = {11, 22, 33}; + uint8_t a2[3] = {10, 20, 30}; + TEST_ASSERT_EQUAL_UINT32(2, q.push(a1, 2)); + + TEST_ASSERT_FALSE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); + TEST_ASSERT_EQUAL_UINT32(2, q.size()); + TEST_ASSERT_EQUAL_UINT32(1, q.available()); + + // try to pop 3 elements at once + TEST_ASSERT_EQUAL_UINT32(2, q.pop(a2, 3)); + TEST_ASSERT_EQUAL_UINT8(11, a2[0]); + TEST_ASSERT_EQUAL_UINT8(22, a2[1]); + TEST_ASSERT_EQUAL_UINT8(30, a2[2]); + + TEST_ASSERT_TRUE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); + TEST_ASSERT_EQUAL_UINT32(0, q.size()); + TEST_ASSERT_EQUAL_UINT32(3, q.available()); +} - TEST_ASSERT_FALSE(q.pop(r4)); - TEST_ASSERT_EQUAL(94, r4); +void test_ring_buf2() +{ + Utils::RingBuf q; + int x[] = { + 100, 101, 102, 103, 104, 105, 106, 107, + 108, 109, 110, 111, 112, 113, 114, 115, + }; + int y[] = { + 99, 99, 99, 99, 99, 99, 99, 99, + 99, 99, 99, 99, 99, 99, 99, 99, + }; + + TEST_ASSERT_TRUE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); + TEST_ASSERT_EQUAL_UINT32(0, q.size()); + TEST_ASSERT_EQUAL_UINT32(8, q.available()); + + // push 6 + TEST_ASSERT_EQUAL_UINT32(6, q.push(&x[0], 6)); + + TEST_ASSERT_FALSE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); + TEST_ASSERT_EQUAL_UINT32(6, q.size()); + TEST_ASSERT_EQUAL_UINT32(2, q.available()); + + // pop 4 + TEST_ASSERT_EQUAL_UINT32(4, q.pop(&y[0], 4)); + + TEST_ASSERT_FALSE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); + TEST_ASSERT_EQUAL_UINT32(2, q.size()); + TEST_ASSERT_EQUAL_UINT32(6, q.available()); + + TEST_ASSERT_EQUAL_INT32(100, y[0]); + TEST_ASSERT_EQUAL_INT32(101, y[1]); + TEST_ASSERT_EQUAL_INT32(102, y[2]); + TEST_ASSERT_EQUAL_INT32(103, y[3]); + + // push 6 + TEST_ASSERT_EQUAL_UINT32(6, q.push(&x[6], 6)); + + TEST_ASSERT_FALSE(q.isEmpty()); + TEST_ASSERT_TRUE(q.isFull()); + TEST_ASSERT_EQUAL_UINT32(8, q.size()); + TEST_ASSERT_EQUAL_UINT32(0, q.available()); + + // pop 4 + TEST_ASSERT_EQUAL_UINT32(4, q.pop(&y[0], 4)); + + TEST_ASSERT_FALSE(q.isEmpty()); + TEST_ASSERT_FALSE(q.isFull()); + TEST_ASSERT_EQUAL_UINT32(4, q.size()); + TEST_ASSERT_EQUAL_UINT32(4, q.available()); + + TEST_ASSERT_EQUAL_INT32(104, y[0]); + TEST_ASSERT_EQUAL_INT32(105, y[1]); + TEST_ASSERT_EQUAL_INT32(106, y[2]); + TEST_ASSERT_EQUAL_INT32(107, y[3]); +} - TEST_ASSERT_TRUE(q.isEmpty()); - TEST_ASSERT_FALSE(q.isFull()); +void test_align_addr_to_write() +{ + TEST_ASSERT_EQUAL_UINT32( 0, Math::alignAddressToWrite( 0, 8, 16)); + TEST_ASSERT_EQUAL_UINT32( 16, Math::alignAddressToWrite( 0, 16, 16)); + TEST_ASSERT_EQUAL_UINT32( 16, Math::alignAddressToWrite( 0, 24, 16)); + TEST_ASSERT_EQUAL_UINT32(144, Math::alignAddressToWrite(128, 16, 16)); + TEST_ASSERT_EQUAL_UINT32( 32, Math::alignAddressToWrite( 0, 32, 16)); + TEST_ASSERT_EQUAL_UINT32(128, Math::alignAddressToWrite(100, 32, 16)); } int main(int argc, char **argv) { - UNITY_BEGIN(); - - RUN_TEST(test_math_map); - RUN_TEST(test_math_map3); - RUN_TEST(test_math_deadband); - - RUN_TEST(test_math_bit); - RUN_TEST(test_math_bitmask); - RUN_TEST(test_math_bitmask_lsb); - RUN_TEST(test_math_bitmask_msb); - RUN_TEST(test_math_bits_lsb); - RUN_TEST(test_math_bits_msb); - - RUN_TEST(test_math_baro_altitude); - RUN_TEST(test_math_peak_detect_full); - RUN_TEST(test_math_peak_detect_partial); - RUN_TEST(test_math_peak_sort); - - RUN_TEST(test_vector_int16_access); - RUN_TEST(test_vector_int16_math); - RUN_TEST(test_vector_float_math3d); - - RUN_TEST(test_filter_sanitize_none); - RUN_TEST(test_filter_sanitize_pt1); - RUN_TEST(test_filter_sanitize_pt1_off); - RUN_TEST(test_filter_sanitize_pt1_nyquist); - RUN_TEST(test_filter_sanitize_biquad_lpf); - RUN_TEST(test_filter_sanitize_biquad_lpf_off); - RUN_TEST(test_filter_sanitize_biquad_lpf_nyqist); - RUN_TEST(test_filter_sanitize_biquad_notch); - RUN_TEST(test_filter_sanitize_biquad_notch_off_freq); - RUN_TEST(test_filter_sanitize_biquad_notch_off_cut); - RUN_TEST(test_filter_sanitize_biquad_notch_nyquist); - - RUN_TEST(test_filter_default); - RUN_TEST(test_filter_none); - RUN_TEST(test_filter_pt1_off); - RUN_TEST(test_filter_pt1_50_100); - RUN_TEST(test_filter_pt1_10_100_step); - RUN_TEST(test_filter_pt1_above_nyquist); - RUN_TEST(test_filter_pt2_10_100_step); - RUN_TEST(test_filter_pt3_10_100_step); - RUN_TEST(test_filter_biquad_off); - RUN_TEST(test_filter_biquad_20_100); - RUN_TEST(test_filter_biquad_10_100_step); - RUN_TEST(test_filter_notch_df1_150_200_1000); - RUN_TEST(test_filter_notch_df1_150_200_1000_reconf); - RUN_TEST(test_filter_notch_q); - RUN_TEST(test_filter_notch_no_cutoff); - RUN_TEST(test_filter_notch_above_nyquist); - RUN_TEST(test_filter_fir2_off); - RUN_TEST(test_filter_fir2_on); - - RUN_TEST(test_pid_init); - RUN_TEST(test_pid_update_p); - RUN_TEST(test_pid_update_i); - RUN_TEST(test_pid_update_i_limit); - RUN_TEST(test_pid_update_i_relax); - RUN_TEST(test_pid_update_d); - RUN_TEST(test_pid_update_f); - RUN_TEST(test_pid_update_sum); - RUN_TEST(test_pid_update_sum_limit); - - RUN_TEST(test_queue_atomic); - - UNITY_END(); - - return 0; + UNITY_BEGIN(); + + RUN_TEST(test_math_map); + RUN_TEST(test_math_map3); + RUN_TEST(test_math_deadband); + + RUN_TEST(test_math_bit); + RUN_TEST(test_math_bitmask); + RUN_TEST(test_math_bitmask_lsb); + RUN_TEST(test_math_bitmask_msb); + RUN_TEST(test_math_bits_lsb); + RUN_TEST(test_math_bits_msb); + + RUN_TEST(test_math_clock_align); + + RUN_TEST(test_math_baro_altitude); + RUN_TEST(test_math_peak_detect_full); + RUN_TEST(test_math_peak_detect_partial); + RUN_TEST(test_math_peak_sort); + + RUN_TEST(test_vector_int16_access); + RUN_TEST(test_vector_int16_math); + RUN_TEST(test_vector_float_math3d); + + RUN_TEST(test_filter_sanitize_none); + RUN_TEST(test_filter_sanitize_pt1); + RUN_TEST(test_filter_sanitize_pt1_off); + RUN_TEST(test_filter_sanitize_pt1_nyquist); + RUN_TEST(test_filter_sanitize_biquad_lpf); + RUN_TEST(test_filter_sanitize_biquad_lpf_off); + RUN_TEST(test_filter_sanitize_biquad_lpf_nyqist); + RUN_TEST(test_filter_sanitize_biquad_notch); + RUN_TEST(test_filter_sanitize_biquad_notch_off_freq); + RUN_TEST(test_filter_sanitize_biquad_notch_off_cut); + RUN_TEST(test_filter_sanitize_biquad_notch_nyquist); + + RUN_TEST(test_filter_default); + RUN_TEST(test_filter_none); + RUN_TEST(test_filter_pt1_off); + RUN_TEST(test_filter_pt1_50_100); + RUN_TEST(test_filter_pt1_10_100_step); + RUN_TEST(test_filter_pt1_above_nyquist); + RUN_TEST(test_filter_pt2_10_100_step); + RUN_TEST(test_filter_pt3_10_100_step); + RUN_TEST(test_filter_biquad_off); + RUN_TEST(test_filter_biquad_20_100); + RUN_TEST(test_filter_biquad_10_100_step); + RUN_TEST(test_filter_notch_df1_150_200_1000); + RUN_TEST(test_filter_notch_df1_150_200_1000_reconf); + RUN_TEST(test_filter_notch_q); + RUN_TEST(test_filter_notch_no_cutoff); + RUN_TEST(test_filter_notch_above_nyquist); + RUN_TEST(test_filter_fir2_off); + RUN_TEST(test_filter_fir2_on); + + RUN_TEST(test_pid_init); + RUN_TEST(test_pid_update_p); + RUN_TEST(test_pid_update_i); + RUN_TEST(test_pid_update_i_limit); + RUN_TEST(test_pid_update_i_relax); + RUN_TEST(test_pid_update_d); + RUN_TEST(test_pid_update_f); + RUN_TEST(test_pid_update_sum); + RUN_TEST(test_pid_update_sum_limit); + + RUN_TEST(test_queue_atomic); + RUN_TEST(test_ring_buf); + RUN_TEST(test_ring_buf2); + RUN_TEST(test_align_addr_to_write); + + return UNITY_END(); } \ No newline at end of file diff --git a/test/test_msp/test_msp.cpp b/test/test_msp/test_msp.cpp index a2d49df9..6eb9eff5 100644 --- a/test/test_msp/test_msp.cpp +++ b/test/test_msp/test_msp.cpp @@ -1,6 +1,8 @@ #include #include -#include +#include +#include +#include #include #include #include "Msp/Msp.h"