From a7390e10adb2769dac162a4247a37c726c1976c7 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 8 Aug 2024 11:34:12 +0200 Subject: [PATCH 01/68] blackbox log fields selection --- lib/Espfc/src/Blackbox/Blackbox.cpp | 2 +- lib/Espfc/src/Cli.h | 32 ++++++++++++++++++++++++++++- lib/Espfc/src/ModelConfig.h | 22 ++++++++++++++++++-- lib/Espfc/src/Msp/MspProcessor.h | 4 ++-- 4 files changed, 54 insertions(+), 6 deletions(-) diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index f87306a6..70ed645a 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -165,7 +165,7 @@ int Blackbox::begin() blackboxConfigMutable()->sample_rate = blackboxCalculateSampleRate(_model.config.blackboxPdenom); } blackboxConfigMutable()->device = _model.config.blackboxDev; - blackboxConfigMutable()->fields_disabled_mask = _model.config.blackboxFieldsDisabledMask; + blackboxConfigMutable()->fields_disabled_mask = ~_model.config.blackboxFieldsMask; blackboxConfigMutable()->mode = _model.config.blackboxMode; featureConfigMutable()->enabledFeatures = _model.config.featureMask; diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 9b8a4cf0..e10d3d98 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -45,6 +45,7 @@ class Cli PARAM_MIXER, // mixer config PARAM_SERIAL, // mixer config PARAM_STRING, // string + PARAM_BITMASK, // set or clear bit }; class Param @@ -62,6 +63,8 @@ class Cli Param(const char * n, int32_t * a): Param(n, PARAM_INT, reinterpret_cast(a), NULL) {} Param(const char * n, int8_t * a, const char ** c): Param(n, PARAM_BYTE, reinterpret_cast(a), c) {} + Param(const char * n, int32_t * a, uint8_t b): Param(n, PARAM_BITMASK, reinterpret_cast(a), NULL, b) {} + Param(const char * n, InputChannelConfig * a): Param(n, PARAM_INPUT_CHANNEL, reinterpret_cast(a), NULL) {} Param(const char * n, OutputChannelConfig * a): Param(n, PARAM_OUTPUT_CHANNEL, reinterpret_cast(a), NULL) {} Param(const char * n, ScalerConfig * a): Param(n, PARAM_SCALER, reinterpret_cast(a), NULL) {} @@ -102,6 +105,9 @@ class Cli case PARAM_STRING: stream.print(addr); break; + case PARAM_BITMASK: + stream.print((*reinterpret_cast(addr) & (1ul << maxLen)) ? 1 : 0); + break; case PARAM_INPUT_CHANNEL: print(stream, *reinterpret_cast(addr)); break; @@ -245,6 +251,17 @@ class Cli case PARAM_STRING: write(String(v ? v : "")); break; + case PARAM_BITMASK: + if(!v) return; + if(*v == '0') + { + *reinterpret_cast(addr) &= ~(1ul << maxLen); + } + if(*v == '1') + { + *reinterpret_cast(addr) |= (1ul << maxLen); + } + break; case PARAM_OUTPUT_CHANNEL: if(!v) return; write(*reinterpret_cast(addr), args); @@ -711,7 +728,20 @@ class Cli Param(PSTR("blackbox_dev"), &c.blackboxDev), Param(PSTR("blackbox_mode"), &c.blackboxMode, blackboxModeChoices), Param(PSTR("blackbox_rate"), &c.blackboxPdenom), - Param(PSTR("blackbox_mask"), &c.blackboxFieldsDisabledMask), + Param(PSTR("blackbox_log_acc"), &c.blackboxFieldsMask, BLACKBOX_FIELD_ACC), + Param(PSTR("blackbox_log_alt"), &c.blackboxFieldsMask, BLACKBOX_FIELD_ALTITUDE), + Param(PSTR("blackbox_log_bat"), &c.blackboxFieldsMask, BLACKBOX_FIELD_BATTERY), + Param(PSTR("blackbox_log_debug"), &c.blackboxFieldsMask, BLACKBOX_FIELD_DEBUG_LOG), + Param(PSTR("blackbox_log_gps"), &c.blackboxFieldsMask, BLACKBOX_FIELD_GPS), + Param(PSTR("blackbox_log_gyro"), &c.blackboxFieldsMask, BLACKBOX_FIELD_GYRO), + Param(PSTR("blackbox_log_gyro_raw"), &c.blackboxFieldsMask, BLACKBOX_FIELD_GYROUNFILT), + Param(PSTR("blackbox_log_mag"), &c.blackboxFieldsMask, BLACKBOX_FIELD_MAG), + Param(PSTR("blackbox_log_motor"), &c.blackboxFieldsMask, BLACKBOX_FIELD_MOTOR), + Param(PSTR("blackbox_log_pid"), &c.blackboxFieldsMask, BLACKBOX_FIELD_PID), + Param(PSTR("blackbox_log_rc"), &c.blackboxFieldsMask, BLACKBOX_FIELD_RC_COMMANDS), + Param(PSTR("blackbox_log_rpm"), &c.blackboxFieldsMask, BLACKBOX_FIELD_RPM), + Param(PSTR("blackbox_log_rssi"), &c.blackboxFieldsMask, BLACKBOX_FIELD_RSSI), + Param(PSTR("blackbox_log_sp"), &c.blackboxFieldsMask, BLACKBOX_FIELD_SETPOINT), #ifdef ESPFC_SERIAL_SOFT_0_WIFI Param(PSTR("wifi_ssid"), PARAM_STRING, &c.wireless.ssid[0], NULL, 32), diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index cf99a171..ed44eed4 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -369,6 +369,24 @@ enum PidIndex { FC_PID_ITEM_COUNT }; +enum BlacboxLogField { // no more than 32, sync with FlightLogFieldSelect_e + BLACKBOX_FIELD_PID = 0, + BLACKBOX_FIELD_RC_COMMANDS, + BLACKBOX_FIELD_SETPOINT, + BLACKBOX_FIELD_BATTERY, + BLACKBOX_FIELD_MAG, + BLACKBOX_FIELD_ALTITUDE, + BLACKBOX_FIELD_RSSI, + BLACKBOX_FIELD_GYRO, + BLACKBOX_FIELD_ACC, + BLACKBOX_FIELD_DEBUG_LOG, + BLACKBOX_FIELD_MOTOR, + BLACKBOX_FIELD_GPS, + BLACKBOX_FIELD_RPM, + BLACKBOX_FIELD_GYROUNFILT, + BLACKBOX_FIELD_COUNT +}; + class PidConfig { public: @@ -585,7 +603,7 @@ class ModelConfig int8_t blackboxDev; int16_t blackboxPdenom; - int32_t blackboxFieldsDisabledMask; + int32_t blackboxFieldsMask; int8_t blackboxMode; SerialPortConfig serial[SERIAL_UART_COUNT]; @@ -952,7 +970,7 @@ class ModelConfig debugAxis = 1; blackboxDev = 0; blackboxPdenom = 32; // 1kHz - blackboxFieldsDisabledMask = 0; + blackboxFieldsMask = 0xffff; blackboxMode = 0; // development settings diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 0d402a6a..211e172f 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -663,7 +663,7 @@ class MspProcessor r.writeU8(1); // blackboxGetRateDenom()); r.writeU16(_model.config.blackboxPdenom);//blackboxGetPRatio()); // p_denom //r.writeU8(_model.config.blackboxPdenom); // sample_rate - //r.writeU32(_model.config.blackboxFieldsDisabledMask); + //r.writeU32(~_model.config.blackboxFieldsMask); break; case MSP_SET_BLACKBOX_CONFIG: @@ -689,7 +689,7 @@ class MspProcessor //_model.config.blackboxPdenom = pRatio; } if (m.remain() >= 4) { - _model.config.blackboxFieldsDisabledMask = m.readU32(); + _model.config.blackboxFieldsMask = ~m.readU32(); }*/ } break; From 0cce2b07fd381f1ac58547046d23872520888f21 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Fri, 9 Aug 2024 00:03:33 +0200 Subject: [PATCH 02/68] features selection --- lib/Espfc/src/Cli.h | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index e10d3d98..f4808e0c 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -420,7 +420,14 @@ class Cli size_t i = 0; static const Param params[] = { - Param(PSTR("features"), &c.featureMask), + Param(PSTR("feature_dyn_notch"), &c.featureMask, 29), + Param(PSTR("feature_motor_stop"), &c.featureMask, 4), + Param(PSTR("feature_rx_ppm"), &c.featureMask, 0), + Param(PSTR("feature_rx_serial"), &c.featureMask, 3), + Param(PSTR("feature_rx_spi"), &c.featureMask, 25), + Param(PSTR("feature_soft_serial"), &c.featureMask, 6), + Param(PSTR("feature_telemetry"), &c.featureMask, 10), + Param(PSTR("debug_mode"), &c.debugMode, debugModeChoices), Param(PSTR("debug_axis"), &c.debugAxis), From 2c42ea0d15d8b191398343ed34022982a88fba58 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Wed, 31 Jul 2024 16:00:46 +0200 Subject: [PATCH 03/68] refactor telemetry and msp processing flow --- lib/Espfc/src/Device/InputCRSF.cpp | 31 ++++++-- lib/Espfc/src/Device/InputCRSF.h | 11 ++- lib/Espfc/src/Device/SerialDevice.h | 24 +----- lib/Espfc/src/Espfc.cpp | 4 +- lib/Espfc/src/Espfc.h | 2 + lib/Espfc/src/Input.cpp | 4 +- lib/Espfc/src/Input.h | 4 +- lib/Espfc/src/Msp/Msp.h | 79 +++++++++++++++++++ lib/Espfc/src/Msp/MspProcessor.h | 76 ++---------------- lib/Espfc/src/SerialManager.cpp | 57 +++++++------ lib/Espfc/src/SerialManager.h | 10 +-- lib/Espfc/src/Target/Target.h | 23 ++++++ lib/Espfc/src/Telemetry/TelemetryCRSF.h | 30 +++++++ .../TelemetryText.h} | 17 ++-- lib/Espfc/src/TelemetryManager.h | 73 +++++++++++++++++ lib/betaflight/src/blackbox/blackbox.c | 7 +- test/test_input_crsf/test_input_crsf.cpp | 9 ++- 17 files changed, 308 insertions(+), 153 deletions(-) create mode 100644 lib/Espfc/src/Telemetry/TelemetryCRSF.h rename lib/Espfc/src/{Telemetry.h => Telemetry/TelemetryText.h} (76%) create mode 100644 lib/Espfc/src/TelemetryManager.h diff --git a/lib/Espfc/src/Device/InputCRSF.cpp b/lib/Espfc/src/Device/InputCRSF.cpp index 809c70d7..da483245 100644 --- a/lib/Espfc/src/Device/InputCRSF.cpp +++ b/lib/Espfc/src/Device/InputCRSF.cpp @@ -1,4 +1,3 @@ - #include "InputCRSF.h" #include "Utils/MemoryHelper.h" @@ -8,11 +7,13 @@ namespace Device { using namespace Espfc::Rc; -InputCRSF::InputCRSF(): _serial(NULL), _state(CRSF_ADDR), _idx(0), _new_data(false) {} +InputCRSF::InputCRSF(): _serial(NULL), _telemetry(NULL), _state(CRSF_ADDR), _idx(0), _new_data(false) {} -int InputCRSF::begin(Device::SerialDevice * serial) +int InputCRSF::begin(Device::SerialDevice * serial, TelemetryManager * telemetry) { _serial = serial; + _telemetry = telemetry; + _telemetry_next = micros() + TELEMETRY_INTERVAL; for(size_t i = 0; i < CRSF_FRAME_SIZE_MAX; i++) { _frame.data[i] = 0; @@ -38,6 +39,12 @@ InputStatus FAST_CODE_ATTR InputCRSF::update() } } + if(_telemetry && micros() > _telemetry_next) + { + _telemetry_next = micros() + TELEMETRY_INTERVAL; + _telemetry->process(*_serial, TELEMETRY_PROTOCOL_CRSF); + } + if(_new_data) { _new_data = false; @@ -65,7 +72,6 @@ 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); @@ -132,19 +138,23 @@ void FAST_CODE_ATTR InputCRSF::apply(const CrsfFrame& frame) applyLinkStats(frame); break; + case CRSF_FRAMETYPE_MSP_REQ: + applyMspReq(frame); + break; + default: break; } } -void FAST_CODE_ATTR InputCRSF::applyLinkStats(const CrsfFrame f) +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) +void FAST_CODE_ATTR InputCRSF::applyChannels(const CrsfFrame& f) { const CrsfData* frame = reinterpret_cast(f.message.payload); Crsf::decodeRcDataShift8(_channels, frame); @@ -152,6 +162,15 @@ void FAST_CODE_ATTR InputCRSF::applyChannels(const CrsfFrame f) _new_data = true; } +void FAST_CODE_ATTR InputCRSF::applyMspReq(const CrsfFrame& f) +{ + if(_telemetry) + { + _telemetry_next = micros() + TELEMETRY_INTERVAL; + _telemetry->processMsp(*_serial, TELEMETRY_PROTOCOL_CRSF, f.message.payload, CRSF_PAYLOAD_SIZE_MAX); + } +} + } } diff --git a/lib/Espfc/src/Device/InputCRSF.h b/lib/Espfc/src/Device/InputCRSF.h index b9fb0eb3..289040dc 100644 --- a/lib/Espfc/src/Device/InputCRSF.h +++ b/lib/Espfc/src/Device/InputCRSF.h @@ -3,6 +3,7 @@ #include "Device/SerialDevice.h" #include "Device/InputDevice.h" #include "Rc/Crsf.h" +#include "TelemetryManager.h" // https://github.com/CapnBry/CRServoF/blob/master/lib/CrsfSerial/crsf_protocol.h // https://github.com/AlessioMorale/crsf_parser/tree/master @@ -25,7 +26,7 @@ class InputCRSF: public InputDevice InputCRSF(); - int begin(Device::SerialDevice * serial); + int begin(Device::SerialDevice * serial, TelemetryManager * telemetry); virtual InputStatus update() override; virtual uint16_t get(uint8_t i) const override; virtual void get(uint16_t * data, size_t len) const override; @@ -38,17 +39,21 @@ class InputCRSF: public InputDevice private: void reset(); void apply(const Rc::CrsfFrame& frame); - void applyLinkStats(const Rc::CrsfFrame f); - void applyChannels(const Rc::CrsfFrame f); + void applyLinkStats(const Rc::CrsfFrame& f); + void applyChannels(const Rc::CrsfFrame& f); + void applyMspReq(const Rc::CrsfFrame& f); static const size_t CHANNELS = 16; + static const size_t TELEMETRY_INTERVAL = 20000; Device::SerialDevice * _serial; + TelemetryManager * _telemetry; CrsfState _state; uint8_t _idx; bool _new_data; Rc::CrsfFrame _frame; uint16_t _channels[CHANNELS]; + uint32_t _telemetry_next; }; } diff --git a/lib/Espfc/src/Device/SerialDevice.h b/lib/Espfc/src/Device/SerialDevice.h index d5c9ce84..2eadc696 100644 --- a/lib/Espfc/src/Device/SerialDevice.h +++ b/lib/Espfc/src/Device/SerialDevice.h @@ -1,5 +1,4 @@ -#ifndef _ESPFC_DEVICE_SERIAL_DEVICE_H_ -#define _ESPFC_DEVICE_SERIAL_DEVICE_H_ +#pragma once #include #include @@ -26,25 +25,6 @@ enum SerialSpeed { SERIAL_SPEED_2470000 = 2470000, }; -enum SerialPort { -#ifdef ESPFC_SERIAL_USB - SERIAL_USB, -#endif -#ifdef ESPFC_SERIAL_0 - SERIAL_UART_0, -#endif -#ifdef ESPFC_SERIAL_1 - SERIAL_UART_1, -#endif -#ifdef ESPFC_SERIAL_2 - SERIAL_UART_2, -#endif -#ifdef ESPFC_SERIAL_SOFT_0 - SERIAL_SOFT_0, -#endif - SERIAL_UART_COUNT -}; - enum SerialPortId { SERIAL_ID_NONE = -1, SERIAL_ID_UART_1 = 0, @@ -148,5 +128,3 @@ class SerialDevice: public Stream } } - -#endif // _ESPFC_SERIAL_DEVICE_H_ diff --git a/lib/Espfc/src/Espfc.cpp b/lib/Espfc/src/Espfc.cpp index a1d7014f..a9203996 100644 --- a/lib/Espfc/src/Espfc.cpp +++ b/lib/Espfc/src/Espfc.cpp @@ -4,12 +4,12 @@ namespace Espfc { Espfc::Espfc(): - _hardware(_model), _controller(_model), _input(_model), _actuator(_model), _sensor(_model), + _hardware(_model), _controller(_model), _telemetry(_model), _input(_model, _telemetry), _actuator(_model), _sensor(_model), _mixer(_model), _blackbox(_model) #ifdef ESPFC_BUZER , _buzzer(_model) #endif - , _serial(_model) + , _serial(_model, _telemetry) {} int Espfc::load() diff --git a/lib/Espfc/src/Espfc.h b/lib/Espfc/src/Espfc.h index 06f0ea64..a6a45f8e 100644 --- a/lib/Espfc/src/Espfc.h +++ b/lib/Espfc/src/Espfc.h @@ -6,6 +6,7 @@ #include "Input.h" #include "Actuator.h" #include "SensorManager.h" +#include "TelemetryManager.h" #include "SerialManager.h" #include "Output/Mixer.h" #include "Blackbox/Blackbox.h" @@ -34,6 +35,7 @@ class Espfc Model _model; Hardware _hardware; Controller _controller; + TelemetryManager _telemetry; Input _input; Actuator _actuator; SensorManager _sensor; diff --git a/lib/Espfc/src/Input.cpp b/lib/Espfc/src/Input.cpp index a1744894..fd6511ea 100644 --- a/lib/Espfc/src/Input.cpp +++ b/lib/Espfc/src/Input.cpp @@ -5,7 +5,7 @@ namespace Espfc { -Input::Input(Model& model): _model(model) {} +Input::Input(Model& model, TelemetryManager& telemetry): _model(model), _telemetry(telemetry) {} int Input::begin() { @@ -348,7 +348,7 @@ Device::InputDevice * Input::getInputDevice() } if(serial && _model.isFeatureActive(FEATURE_RX_SERIAL) && _model.config.input.serialRxProvider == SERIALRX_CRSF) { - _crsf.begin(serial); + _crsf.begin(serial, &_telemetry); _model.logger.info().logln(F("RX CRSF")); return &_crsf; } diff --git a/lib/Espfc/src/Input.h b/lib/Espfc/src/Input.h index 0a10d58c..68730a1b 100644 --- a/lib/Espfc/src/Input.h +++ b/lib/Espfc/src/Input.h @@ -6,6 +6,7 @@ #include "Device/InputPPM.h" #include "Device/InputSBUS.h" #include "Device/InputCRSF.h" +#include "TelemetryManager.h" #if defined(ESPFC_ESPNOW) #include "Device/InputEspNow.h" #endif @@ -28,7 +29,7 @@ enum InputPwmRange { class Input { public: - Input(Model& model); + Input(Model& model, TelemetryManager& telemetry); int begin(); int update(); @@ -55,6 +56,7 @@ class Input } Model& _model; + TelemetryManager& _telemetry; Device::InputDevice * _device; Filter _filter[INPUT_CHANNELS]; float _step; diff --git a/lib/Espfc/src/Msp/Msp.h b/lib/Espfc/src/Msp/Msp.h index 0fd0b5ca..c7ffe1de 100644 --- a/lib/Espfc/src/Msp/Msp.h +++ b/lib/Espfc/src/Msp/Msp.h @@ -3,6 +3,7 @@ #include #include "Hal/Pgm.h" +#include "Math/Crc.h" extern "C" { #include "msp/msp_protocol.h" @@ -71,6 +72,21 @@ class MspMessage uint8_t checksum2; uint8_t buffer[MSP_BUF_SIZE]; + bool isReady() const + { + return state == MSP_STATE_RECEIVED; + } + + bool isCmd() const + { + return dir == MSP_TYPE_CMD; + } + + bool isIdle() const + { + return state == MSP_STATE_IDLE; + } + int remain() const { return received - read; @@ -165,6 +181,69 @@ class MspResponse writeU8(v >> 16); writeU8(v >> 24); } + + size_t serialize(uint8_t * buff, size_t len_max) const + { + switch(version) + { + case MSP_V1: + return serializeV1(buff, len_max); + break; + case MSP_V2: + return serializeV2(buff, len_max); + break; + } + return 0; + } + + size_t serializeV1(uint8_t * buff, size_t len_max) const + { + // not enough space in target buffer + if(len + 5 > len_max) return 0; + + buff[0] = '$'; + buff[1] = 'M'; + buff[2] = result == -1 ? '!' : '>'; + buff[3] = len; + buff[4] = cmd; + + uint8_t checksum = Math::crc8_xor(0, &buff[3], 2); + size_t i = 5; + for(size_t j = 0; j < len; j++) + { + checksum = Math::crc8_xor(checksum, data[j]); + buff[i++] = data[j]; + } + buff[i] = checksum; + + return i + 1; + } + + size_t serializeV2(uint8_t * buff, size_t len_max) const + { + // not enough space in target buffer + if(len + 8 > len_max) return 0; + + buff[0] = '$'; + buff[1] = 'X'; + buff[2] = result == -1 ? '!' : '>'; + buff[3] = 0; + buff[4] = cmd & 0xff; + buff[5] = (cmd >> 8) & 0xff; + buff[6] = len & 0xff; + buff[7] = (len >> 8) & 0xff; + + uint8_t checksum = Math::crc8_dvb_s2(0, &buff[3], 5); + size_t i = 8; + for(size_t j = 0; j < len; j++) + { + checksum = Math::crc8_dvb_s2(checksum, data[j]); + buff[i++] = data[j]; + } + buff[i] = checksum; + + return i + 1; + } }; } diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 211e172f..975c0865 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -162,29 +162,13 @@ class MspProcessor public: MspProcessor(Model& model): _model(model) {} - bool process(char c, MspMessage& msg, MspResponse& res, Device::SerialDevice& s) + bool parse(char c, MspMessage& msg) { _parser.parse(c, msg); - if(msg.state == MSP_STATE_RECEIVED) - { - debugMessage(msg); - switch(msg.dir) - { - case MSP_TYPE_CMD: - processCommand(msg, res, s); - sendResponse(res, s); - msg = MspMessage(); - res = MspResponse(); - break; - case MSP_TYPE_REPLY: - //processCommand(msg, s); - break; - } - return true; - } + if(msg.isReady()) debugMessage(msg); - return msg.state != MSP_STATE_IDLE; + return !msg.isIdle(); } void processCommand(MspMessage& m, MspResponse& r, Device::SerialDevice& s) @@ -1543,57 +1527,8 @@ class MspProcessor void sendResponse(MspResponse& r, Device::SerialDevice& s) { debugResponse(r); - switch(r.version) - { - case MSP_V1: - sendResponseV1(r, s); - break; - case MSP_V2: - sendResponseV2(r, s); - break; - } - //postCommand(); - } - - void sendResponseV1(MspResponse& r, Device::SerialDevice& s) - { - // TODO: optimize to write at once - uint8_t hdr[5] = { '$', 'M', '>' }; - if(r.result == -1) - { - hdr[2] = '!'; // error ?? - } - hdr[3] = r.len; - hdr[4] = r.cmd; - uint8_t checksum = Math::crc8_xor(0, &hdr[3], 2); - s.write(hdr, 5); - if(r.len > 0) - { - s.write(r.data, r.len); - checksum = Math::crc8_xor(checksum, r.data, r.len); - } - s.write(checksum); - } - - void sendResponseV2(MspResponse& r, Device::SerialDevice& s) - { - uint8_t hdr[8] = { '$', 'X', '>', 0 }; - if(r.result == -1) - { - hdr[2] = '!'; // error ?? - } - hdr[4] = r.cmd & 0xff; - hdr[5] = (r.cmd >> 8) & 0xff; - hdr[6] = r.len & 0xff; - hdr[7] = (r.len >> 8) & 0xff; - uint8_t checksum = Math::crc8_dvb_s2(0, &hdr[3], 5); - s.write(hdr, 8); - if(r.len > 0) - { - s.write(r.data, r.len); - checksum = Math::crc8_dvb_s2(checksum, r.data, r.len); - } - s.write(checksum); + size_t len = r.serialize(_buff, 256); + s.write(_buff, len); } void postCommand() @@ -1660,6 +1595,7 @@ class MspProcessor Model& _model; MspParser _parser; std::function _postCommand; + uint8_t _buff[256]; }; } diff --git a/lib/Espfc/src/SerialManager.cpp b/lib/Espfc/src/SerialManager.cpp index 744b9c8f..80704906 100644 --- a/lib/Espfc/src/SerialManager.cpp +++ b/lib/Espfc/src/SerialManager.cpp @@ -20,11 +20,11 @@ namespace Espfc { -SerialManager::SerialManager(Model& model): _model(model), _msp(model), _cli(model), +SerialManager::SerialManager(Model& model, TelemetryManager& telemetry): _model(model), _msp(model), _cli(model), #ifdef ESPFC_SERIAL_SOFT_0_WIFI _wireless(model), #endif -_telemetry(model), _current(0) {} +_telemetry(telemetry), _current(0) {} int SerialManager::begin() { @@ -138,47 +138,54 @@ int SerialManager::begin() 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) + if(stream) { - size_t len = stream->available(); - if(len > 0) + if(!serialRx) { - uint8_t buff[64] = {0}; - len = std::min(len, (size_t)sizeof(buff)); - stream->readMany(buff, len); - char * c = (char*)&buff[0]; - while(len--) + Stats::Measure measure(_model.state.stats, COUNTER_SERIAL); + size_t len = stream->available(); + if(len > 0) { - if(sc.functionMask & SERIAL_FUNCTION_MSP) + uint8_t buff[64] = {0}; + len = std::min(len, (size_t)sizeof(buff)); + stream->readMany(buff, len); + char * c = (char*)&buff[0]; + while(len--) { - bool consumed = _msp.process(*c, ss.mspRequest, ss.mspResponse, *stream); - if(!consumed) + if(sc.functionMask & SERIAL_FUNCTION_MSP) { - _cli.process(*c, ss.cliCmd, *stream); + bool consumed = _msp.parse(*c, ss.mspRequest); + if(consumed) + { + if(ss.mspRequest.isReady() && ss.mspRequest.isCmd()) + { + _msp.processCommand(ss.mspRequest, ss.mspResponse, *stream); + _msp.sendResponse(ss.mspResponse, *stream); + _msp.postCommand(); + ss.mspRequest = Msp::MspMessage(); + ss.mspResponse = Msp::MspResponse(); + } + } + else + { + _cli.process(*c, ss.cliCmd, *stream); + } } + c++; } - c++; } } - if(!stream->available()) + if(sc.functionMask & SERIAL_FUNCTION_TELEMETRY_FRSKY && _model.state.telemetryTimer.check()) { - _msp.postCommand(); + _telemetry.process(*stream, TELEMETRY_PROTOCOL_TEXT); } } - 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) { diff --git a/lib/Espfc/src/SerialManager.h b/lib/Espfc/src/SerialManager.h index d0e0721a..b025f8c9 100644 --- a/lib/Espfc/src/SerialManager.h +++ b/lib/Espfc/src/SerialManager.h @@ -2,19 +2,19 @@ #include "Model.h" #include "Device/SerialDevice.h" +#include "Msp/MspProcessor.h" +#include "Cli.h" +#include "TelemetryManager.h" #ifdef ESPFC_SERIAL_SOFT_0_WIFI #include "Wireless.h" #endif -#include "Msp/MspProcessor.h" -#include "Cli.h" -#include "Telemetry.h" namespace Espfc { class SerialManager { public: - SerialManager(Model& model); + SerialManager(Model& model, TelemetryManager& telemetry); int begin(); int update(); @@ -34,7 +34,7 @@ class SerialManager #ifdef ESPFC_SERIAL_SOFT_0_WIFI Wireless _wireless; #endif - Telemetry _telemetry; + TelemetryManager& _telemetry; size_t _current; }; diff --git a/lib/Espfc/src/Target/Target.h b/lib/Espfc/src/Target/Target.h index b73e29e7..35ebbdd5 100644 --- a/lib/Espfc/src/Target/Target.h +++ b/lib/Espfc/src/Target/Target.h @@ -53,3 +53,26 @@ #endif #endif #endif + +namespace Espfc { + +enum SerialPort { +#ifdef ESPFC_SERIAL_USB + SERIAL_USB, +#endif +#ifdef ESPFC_SERIAL_0 + SERIAL_UART_0, +#endif +#ifdef ESPFC_SERIAL_1 + SERIAL_UART_1, +#endif +#ifdef ESPFC_SERIAL_2 + SERIAL_UART_2, +#endif +#ifdef ESPFC_SERIAL_SOFT_0 + SERIAL_SOFT_0, +#endif + SERIAL_UART_COUNT +}; + +} diff --git a/lib/Espfc/src/Telemetry/TelemetryCRSF.h b/lib/Espfc/src/Telemetry/TelemetryCRSF.h new file mode 100644 index 00000000..a799cca0 --- /dev/null +++ b/lib/Espfc/src/Telemetry/TelemetryCRSF.h @@ -0,0 +1,30 @@ +#pragma once + +namespace Espfc { + +namespace Telemetry { + +class TelemetryCRSF +{ +public: + TelemetryCRSF(Model& model): _model(model) {} + + int process(Device::SerialDevice& s) const + { + + return 1; + } + + int sendMsp(Device::SerialDevice& s, Msp::MspResponse r) const + { + + return 1; + } + +private: + Model& _model; +}; + +} + +} diff --git a/lib/Espfc/src/Telemetry.h b/lib/Espfc/src/Telemetry/TelemetryText.h similarity index 76% rename from lib/Espfc/src/Telemetry.h rename to lib/Espfc/src/Telemetry/TelemetryText.h index fe8d8f2d..bd7976d2 100644 --- a/lib/Espfc/src/Telemetry.h +++ b/lib/Espfc/src/Telemetry/TelemetryText.h @@ -1,21 +1,18 @@ -#ifndef _ESPFC_TELEMETRY_H_ -#define _ESPFC_TELEMETRY_H_ +#pragma once #include "Model.h" -#include "Hardware.h" -#include "Rc/Crsf.h" namespace Espfc { -class Telemetry +namespace Telemetry { + +class TelemetryText { public: - Telemetry(Model& model): _model(model), _value(172), _up(1) {} + TelemetryText(Model& model): _model(model) {} int process(Stream& s) const { - Stats::Measure measure(_model.state.stats, COUNTER_TELEMETRY); - //print(s, _model.state.gyro.x, 3); //println(s); @@ -70,10 +67,8 @@ class Telemetry } Model& _model; - mutable int _value; - mutable bool _up; }; } -#endif +} diff --git a/lib/Espfc/src/TelemetryManager.h b/lib/Espfc/src/TelemetryManager.h new file mode 100644 index 00000000..e87d3dd5 --- /dev/null +++ b/lib/Espfc/src/TelemetryManager.h @@ -0,0 +1,73 @@ +#pragma once + +#include "Model.h" +#include "Telemetry/TelemetryText.h" +#include "Telemetry/TelemetryCRSF.h" +#include "Msp/Msp.h" +#include "Msp/MspProcessor.h" + +namespace Espfc { + +enum TelemetryProtocol { + TELEMETRY_PROTOCOL_TEXT, + TELEMETRY_PROTOCOL_CRSF, +}; + +class TelemetryManager +{ +public: + TelemetryManager(Model& model): _model(model), _msp(model), _text(model), _crsf(model) {} + + int process(Device::SerialDevice& s, TelemetryProtocol protocol) const + { + Stats::Measure measure(_model.state.stats, COUNTER_TELEMETRY); + + switch(protocol) + { + case TELEMETRY_PROTOCOL_TEXT: + _text.process(s); + break; + case TELEMETRY_PROTOCOL_CRSF: + _crsf.process(s); + break; + } + + return 1; + } + + int processMsp(Device::SerialDevice& s, TelemetryProtocol protocol, const uint8_t * payload, size_t max_len) + { + Msp::MspMessage m; + Msp::MspResponse r; + for(size_t i = 0; i < max_len; i++) + { + if(!_msp.parse(payload[i], m)) break; + } + + // not valid msp message, stop processing + if(!m.isReady() || !m.isCmd()) return 0; + + _msp.processCommand(m, r, s); + + switch(protocol) + { + case TELEMETRY_PROTOCOL_CRSF: + _crsf.sendMsp(s, r); + break; + default: + break; + } + + _msp.postCommand(); + + return 1; + } + +private: + Model& _model; + Msp::MspProcessor _msp; + Telemetry::TelemetryText _text; + Telemetry::TelemetryCRSF _crsf; +}; + +} diff --git a/lib/betaflight/src/blackbox/blackbox.c b/lib/betaflight/src/blackbox/blackbox.c index 0ef8467c..d77bb11b 100644 --- a/lib/betaflight/src/blackbox/blackbox.c +++ b/lib/betaflight/src/blackbox/blackbox.c @@ -1319,11 +1319,12 @@ STATIC_UNIT_TESTED char *blackboxGetStartDateTime(char *buf) // when time is not known. rtcGetDateTime(&dt); dateTimeFormatLocal(buf, &dt); + + return buf; #else - buf = "0000-01-01T00:00:00.000"; + static char b[] = "0000-01-01T00:00:00.000"; + return b; #endif - - return buf; } #ifndef BLACKBOX_PRINT_HEADER_LINE diff --git a/test/test_input_crsf/test_input_crsf.cpp b/test/test_input_crsf/test_input_crsf.cpp index 6103b5a4..20132206 100644 --- a/test/test_input_crsf/test_input_crsf.cpp +++ b/test/test_input_crsf/test_input_crsf.cpp @@ -10,6 +10,7 @@ using namespace Espfc; using namespace Espfc::Device; using namespace Espfc::Rc; +using namespace fakeit; void test_input_crsf_rc_valid() { @@ -17,7 +18,9 @@ void test_input_crsf_rc_valid() CrsfFrame frame; memset(&frame, 0, sizeof(frame)); - input.begin(nullptr); + When(Method(ArduinoFake(), micros)).Return(0); + + input.begin(nullptr, nullptr); const uint8_t data[] = { 0xC8, 0x18, 0x16, 0xE0, 0x03, 0xDF, 0xD9, 0xC0, 0xF7, 0x8B, 0x5F, 0x94, 0xAF, @@ -52,7 +55,9 @@ void test_input_crsf_rc_prefix() CrsfFrame frame; memset(&frame, 0, sizeof(frame)); - input.begin(nullptr); + When(Method(ArduinoFake(), micros)).Return(0); + + input.begin(nullptr, nullptr); // prefix with few random bytes const uint8_t data[] = { From f82159710eca935b0f78f80a0841a7a42100d085 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 1 Aug 2024 23:18:44 +0200 Subject: [PATCH 04/68] basic crsf telemetry --- lib/Espfc/src/Input.cpp | 2 +- lib/Espfc/src/Math/Utils.h | 27 ++++- lib/Espfc/src/Msp/Msp.h | 6 +- lib/Espfc/src/Rc/Crsf.h | 34 +++++- lib/Espfc/src/Telemetry/TelemetryCRSF.h | 145 ++++++++++++++++++++++++ 5 files changed, 206 insertions(+), 8 deletions(-) diff --git a/lib/Espfc/src/Input.cpp b/lib/Espfc/src/Input.cpp index fd6511ea..95654fbd 100644 --- a/lib/Espfc/src/Input.cpp +++ b/lib/Espfc/src/Input.cpp @@ -348,7 +348,7 @@ Device::InputDevice * Input::getInputDevice() } if(serial && _model.isFeatureActive(FEATURE_RX_SERIAL) && _model.config.input.serialRxProvider == SERIALRX_CRSF) { - _crsf.begin(serial, &_telemetry); + _crsf.begin(serial, _model.isFeatureActive(FEATURE_TELEMETRY) ? &_telemetry : nullptr); _model.logger.info().logln(F("RX CRSF")); return &_crsf; } diff --git a/lib/Espfc/src/Math/Utils.h b/lib/Espfc/src/Math/Utils.h index ce9673cb..2c2f4640 100644 --- a/lib/Espfc/src/Math/Utils.h +++ b/lib/Espfc/src/Math/Utils.h @@ -74,6 +74,11 @@ class Peak return 3.14159265358979f; } + constexpr float twoPi() + { + return 3.14159265358979f * 2.0f; + } + constexpr float invPi() { return 1.0f / pi(); @@ -84,16 +89,34 @@ class Peak return 1.0f / 180.0f; } - inline float toRad(float deg) + constexpr float toRad(float deg) { return deg * (pi() * inv180()); } - inline float toDeg(float rad) + constexpr float toDeg(float rad) { return rad * (180.0f * invPi()); } + constexpr uint16_t toBigEndian16(uint16_t v) + { +#if BYTE_ORDER == BIG_ENDIAN + return v; +#else + return __builtin_bswap16(v); +#endif + } + + constexpr uint32_t toBigEndian32(uint32_t v) + { +#if BYTE_ORDER == BIG_ENDIAN + return v; +#else + return __builtin_bswap32(v); +#endif + } + inline float toAltitude(float pressure, float seaLevelPressure = 101325.f) { return 44330.f * (1.f - std::pow(pressure / seaLevelPressure, 0.1903f)); diff --git a/lib/Espfc/src/Msp/Msp.h b/lib/Espfc/src/Msp/Msp.h index c7ffe1de..4d94e1a4 100644 --- a/lib/Espfc/src/Msp/Msp.h +++ b/lib/Espfc/src/Msp/Msp.h @@ -188,10 +188,8 @@ class MspResponse { case MSP_V1: return serializeV1(buff, len_max); - break; case MSP_V2: return serializeV2(buff, len_max); - break; } return 0; } @@ -199,7 +197,7 @@ class MspResponse size_t serializeV1(uint8_t * buff, size_t len_max) const { // not enough space in target buffer - if(len + 5 > len_max) return 0; + if(len + 6 > len_max) return 0; buff[0] = '$'; buff[1] = 'M'; @@ -222,7 +220,7 @@ class MspResponse size_t serializeV2(uint8_t * buff, size_t len_max) const { // not enough space in target buffer - if(len + 8 > len_max) return 0; + if(len + 9 > len_max) return 0; buff[0] = '$'; buff[1] = 'X'; diff --git a/lib/Espfc/src/Rc/Crsf.h b/lib/Espfc/src/Rc/Crsf.h index 39784613..77eb3086 100644 --- a/lib/Espfc/src/Rc/Crsf.h +++ b/lib/Espfc/src/Rc/Crsf.h @@ -28,6 +28,7 @@ enum { enum { CRSF_FRAMETYPE_GPS = 0x02, + CRSF_FRAMETYPE_VARIO_SENSOR = 0x07, CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, CRSF_FRAMETYPE_HEARTBEAT = 0x0B, CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, @@ -104,8 +105,9 @@ struct CrsfData * Every frame has the structure: * * Device address: (uint8_t) - * Frame length: length in bytes including Type (uint8_t) + * Frame length: length in bytes including Type and CRC (uint8_t) * Type: (uint8_t) + * Payload: (....) * CRC: (uint8_t) */ struct CrsfMessage @@ -114,6 +116,36 @@ struct CrsfMessage uint8_t size; // counts size after this byte, so it must be the payload size + 2 (type and crc) uint8_t type; // CrsfFrameType uint8_t payload[CRSF_PAYLOAD_SIZE_MAX + 1]; + + void writeU8(uint8_t v) + { + payload[size++] = v; + } + + void writeU16(uint16_t v) + { + writeU8(v >> 0); + writeU8(v >> 8); + } + + void writeU32(uint32_t v) + { + writeU8(v >> 0); + writeU8(v >> 8); + writeU8(v >> 16); + writeU8(v >> 24); + } + + void writeString(const char * v, bool terminate = false) + { + while(*v) writeU8(*v++); + if(terminate) writeU8(0); + } + + void writeCRC(uint8_t v) + { + payload[size - 2] = v; + } } __attribute__ ((__packed__)); union CrsfFrame diff --git a/lib/Espfc/src/Telemetry/TelemetryCRSF.h b/lib/Espfc/src/Telemetry/TelemetryCRSF.h index a799cca0..0b4561d6 100644 --- a/lib/Espfc/src/Telemetry/TelemetryCRSF.h +++ b/lib/Espfc/src/Telemetry/TelemetryCRSF.h @@ -1,16 +1,69 @@ #pragma once +#include "Model.h" +#include "Device/SerialDevice.h" +#include "Rc/Crsf.h" +#include "Math/Utils.h" + namespace Espfc { namespace Telemetry { +enum TelemetryState { + CRSF_TELEMETRY_STATE_ATTI, + CRSF_TELEMETRY_STATE_BAT, + CRSF_TELEMETRY_STATE_FM, + CRSF_TELEMETRY_STATE_GPS, + CRSF_TELEMETRY_STATE_VARIO, + CRSF_TELEMETRY_STATE_HB, +}; + class TelemetryCRSF { public: TelemetryCRSF(Model& model): _model(model) {} + int begin() + { + return 1; + } + int process(Device::SerialDevice& s) const { + Rc::CrsfFrame f; + switch(_current) + { + case CRSF_TELEMETRY_STATE_ATTI: + attitude(f); + send(f, s); + _current = CRSF_TELEMETRY_STATE_BAT; + break; + case CRSF_TELEMETRY_STATE_BAT: + battery(f); + send(f, s); + _current = CRSF_TELEMETRY_STATE_FM; + break; + case CRSF_TELEMETRY_STATE_FM: + flightMode(f); + send(f, s); + _current = CRSF_TELEMETRY_STATE_GPS; + break; + case CRSF_TELEMETRY_STATE_GPS: + //gps(f); + //send(f, s); + _current = CRSF_TELEMETRY_STATE_VARIO; + break; + case CRSF_TELEMETRY_STATE_VARIO: + vario(f); + send(f, s); + _current = CRSF_TELEMETRY_STATE_HB; + break; + case CRSF_TELEMETRY_STATE_HB: + heartbeat(f); + send(f, s); + _current = CRSF_TELEMETRY_STATE_ATTI; + break; + } return 1; } @@ -21,8 +74,100 @@ class TelemetryCRSF return 1; } + void send(const Rc::CrsfFrame& f, Device::SerialDevice& s) const + { + s.write(f.data, f.message.size + 2); + } + + int16_t toAngle(float angle) const + { + if(angle < -Math::pi()) angle += Math::twoPi(); + if(angle > Math::pi()) angle -= Math::twoPi(); + return lrintf(angle * 1000); + } + + void attitude(Rc::CrsfFrame& f) const + { + prepare(f, Rc::CRSF_FRAMETYPE_ATTITUDE); + + int16_t r = toAngle(_model.state.angle.x); + int16_t p = toAngle(_model.state.angle.y); + int16_t y = toAngle(_model.state.angle.z); + + f.message.writeU16(Math::toBigEndian16(r)); + f.message.writeU16(Math::toBigEndian16(p)); + f.message.writeU16(Math::toBigEndian16(y)); + + finalize(f); + } + + void battery(Rc::CrsfFrame& f) const + { + prepare(f, Rc::CRSF_FRAMETYPE_BATTERY_SENSOR); + + uint16_t voltage = Math::clamp(lrintf(_model.state.battery.voltage * 10.0f), 0l, 32000l); + uint16_t current = Math::clamp(lrintf(_model.state.battery.current * 10.0f), 0l, 32000l); + uint32_t mahDrawn = 0; + uint8_t remainPerc = 100; + + f.message.writeU16(Math::toBigEndian16(voltage)); + f.message.writeU16(Math::toBigEndian16(current)); + f.message.writeU8(mahDrawn >> 16); + f.message.writeU8(mahDrawn >> 8); + f.message.writeU8(mahDrawn); + f.message.writeU8(remainPerc); + + finalize(f); + } + + void flightMode(Rc::CrsfFrame& f) const + { + prepare(f, Rc::CRSF_FRAMETYPE_FLIGHT_MODE); + + if(_model.armingDisabled()) f.message.writeString("!DIS"); + if(_model.isModeActive(MODE_FAILSAFE)) f.message.writeString("!FS,"); + if(_model.isModeActive(MODE_ARMED)) f.message.writeString("ARM,"); + if(_model.isModeActive(MODE_AIRMODE)) f.message.writeString("AIR,"); + if(_model.isModeActive(MODE_ANGLE)) f.message.writeString("STAB,"); + f.message.writeU8(0); + + finalize(f); + } + + void vario(Rc::CrsfFrame& f) const + { + prepare(f, Rc::CRSF_FRAMETYPE_VARIO_SENSOR); + + f.message.writeU16(Math::toBigEndian16(0)); + + finalize(f); + } + + void heartbeat(Rc::CrsfFrame& f) const + { + prepare(f, Rc::CRSF_FRAMETYPE_HEARTBEAT); + + f.message.writeU16(Math::toBigEndian16(Rc::CRSF_ADDRESS_FLIGHT_CONTROLLER)); + + finalize(f); + } + + void prepare(Rc::CrsfFrame& f, uint8_t type) const + { + f.message.addr = Rc::CRSF_SYNC_BYTE; + f.message.type = type; + f.message.size = 0; + } + + void finalize(Rc::CrsfFrame& f) const + { + f.message.size += 2; + f.message.writeCRC(Rc::Crsf::crc(f)); + } + private: Model& _model; + mutable TelemetryState _current; }; } From 338605d1daeb54370497e8c8ebb5216665389cda Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 5 Aug 2024 00:11:41 +0200 Subject: [PATCH 05/68] msp over crsf --- lib/Espfc/src/Device/InputCRSF.cpp | 69 ++++---- lib/Espfc/src/Device/InputCRSF.h | 12 +- lib/Espfc/src/Rc/Crsf.cpp | 95 ++++++++-- lib/Espfc/src/Rc/Crsf.h | 39 ++++- lib/Espfc/src/Telemetry/TelemetryCRSF.h | 97 +++++------ lib/Espfc/src/TelemetryManager.h | 9 +- test/test_input_crsf/test_input_crsf.cpp | 212 ++++++++++++++++++----- 7 files changed, 373 insertions(+), 160 deletions(-) diff --git a/lib/Espfc/src/Device/InputCRSF.cpp b/lib/Espfc/src/Device/InputCRSF.cpp index da483245..ff355474 100644 --- a/lib/Espfc/src/Device/InputCRSF.cpp +++ b/lib/Espfc/src/Device/InputCRSF.cpp @@ -1,3 +1,4 @@ +#include #include "InputCRSF.h" #include "Utils/MemoryHelper.h" @@ -14,11 +15,8 @@ int InputCRSF::begin(Device::SerialDevice * serial, TelemetryManager * telemetry _serial = serial; _telemetry = telemetry; _telemetry_next = micros() + TELEMETRY_INTERVAL; - for(size_t i = 0; i < CRSF_FRAME_SIZE_MAX; i++) - { - _frame.data[i] = 0; - if(i < CHANNELS) _channels[i] = 0; - } + std::fill_n((uint8_t*)&_frame, sizeof(_frame), 0); + std::fill_n(_channels, CHANNELS, 0); return 1; } @@ -72,49 +70,50 @@ size_t InputCRSF::getChannelCount() const { return CHANNELS; } bool InputCRSF::needAverage() const { return false; } -void FAST_CODE_ATTR InputCRSF::parse(CrsfFrame& frame, int d) +void FAST_CODE_ATTR InputCRSF::parse(CrsfMessage& msg, int d) { + uint8_t *data = reinterpret_cast(&msg); uint8_t c = (uint8_t)(d & 0xff); switch(_state) { case CRSF_ADDR: - if(c == CRSF_ADDRESS_FLIGHT_CONTROLLER) + if(c == CRSF_SYNC_BYTE) { - frame.data[_idx++] = c; + data[_idx++] = c; _state = CRSF_SIZE; } break; case CRSF_SIZE: if(c > 3 && c <= CRSF_PAYLOAD_SIZE_MAX) { - frame.data[_idx++] = c; + data[_idx++] = c; _state = CRSF_TYPE; } else { reset(); } break; case CRSF_TYPE: - if(c == CRSF_FRAMETYPE_RC_CHANNELS_PACKED || c == CRSF_FRAMETYPE_LINK_STATISTICS) + if(c == CRSF_FRAMETYPE_RC_CHANNELS_PACKED || c == CRSF_FRAMETYPE_LINK_STATISTICS || c == CRSF_FRAMETYPE_MSP_REQ) { - frame.data[_idx++] = c; + 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 + data[_idx++] = c; + if(_idx > msg.size) // _idx is incremented here and operator > accounts as size - 2 { _state = CRSF_CRC; } break; case CRSF_CRC: - frame.data[_idx++] = c; + data[_idx++] = c; reset(); - uint8_t crc = Crsf::crc(frame); + uint8_t crc = msg.crc(); if(c == crc) { - apply(frame); + apply(msg); } break; } @@ -126,20 +125,20 @@ void FAST_CODE_ATTR InputCRSF::reset() _idx = 0; } -void FAST_CODE_ATTR InputCRSF::apply(const CrsfFrame& frame) +void FAST_CODE_ATTR InputCRSF::apply(const CrsfMessage& msg) { - switch (frame.message.type) + switch (msg.type) { case CRSF_FRAMETYPE_RC_CHANNELS_PACKED: - applyChannels(frame); + applyChannels(msg); break; case CRSF_FRAMETYPE_LINK_STATISTICS: - applyLinkStats(frame); + applyLinkStats(msg); break; case CRSF_FRAMETYPE_MSP_REQ: - applyMspReq(frame); + applyMspReq(msg); break; default: @@ -147,28 +146,36 @@ void FAST_CODE_ATTR InputCRSF::apply(const CrsfFrame& frame) } } -void FAST_CODE_ATTR InputCRSF::applyLinkStats(const CrsfFrame& f) +void FAST_CODE_ATTR InputCRSF::applyLinkStats(const CrsfMessage& msg) { - const CrsfLinkStats* frame = reinterpret_cast(f.message.payload); - (void)frame; + const CrsfLinkStats* stats = reinterpret_cast(msg.payload); + (void)stats; // TODO: } -void FAST_CODE_ATTR InputCRSF::applyChannels(const CrsfFrame& f) +void FAST_CODE_ATTR InputCRSF::applyChannels(const CrsfMessage& msg) { - const CrsfData* frame = reinterpret_cast(f.message.payload); - Crsf::decodeRcDataShift8(_channels, frame); + const CrsfData* data = reinterpret_cast(msg.payload); + Crsf::decodeRcDataShift8(_channels, data); //Crsf::decodeRcData(_channels, frame); _new_data = true; } -void FAST_CODE_ATTR InputCRSF::applyMspReq(const CrsfFrame& f) +void FAST_CODE_ATTR InputCRSF::applyMspReq(const CrsfMessage& msg) { - if(_telemetry) + if(!_telemetry) return; + + uint8_t origin; + Msp::MspMessage m; + + Crsf::decodeMsp(msg, m, origin); + + if(m.isCmd() && m.isReady()) { - _telemetry_next = micros() + TELEMETRY_INTERVAL; - _telemetry->processMsp(*_serial, TELEMETRY_PROTOCOL_CRSF, f.message.payload, CRSF_PAYLOAD_SIZE_MAX); + _telemetry->processMsp(*_serial, TELEMETRY_PROTOCOL_CRSF, m, origin); } + + _telemetry_next = micros() + TELEMETRY_INTERVAL; } } diff --git a/lib/Espfc/src/Device/InputCRSF.h b/lib/Espfc/src/Device/InputCRSF.h index 289040dc..1d145ffc 100644 --- a/lib/Espfc/src/Device/InputCRSF.h +++ b/lib/Espfc/src/Device/InputCRSF.h @@ -34,14 +34,14 @@ class InputCRSF: public InputDevice virtual bool needAverage() const override; void print(char c) const; - void parse(Rc::CrsfFrame& frame, int d); + void parse(Rc::CrsfMessage& frame, int d); private: void reset(); - void apply(const Rc::CrsfFrame& frame); - void applyLinkStats(const Rc::CrsfFrame& f); - void applyChannels(const Rc::CrsfFrame& f); - void applyMspReq(const Rc::CrsfFrame& f); + void apply(const Rc::CrsfMessage& msg); + void applyLinkStats(const Rc::CrsfMessage& msg); + void applyChannels(const Rc::CrsfMessage& msg); + void applyMspReq(const Rc::CrsfMessage& msg); static const size_t CHANNELS = 16; static const size_t TELEMETRY_INTERVAL = 20000; @@ -51,7 +51,7 @@ class InputCRSF: public InputDevice CrsfState _state; uint8_t _idx; bool _new_data; - Rc::CrsfFrame _frame; + Rc::CrsfMessage _frame; uint16_t _channels[CHANNELS]; uint32_t _telemetry_next; }; diff --git a/lib/Espfc/src/Rc/Crsf.cpp b/lib/Espfc/src/Rc/Crsf.cpp index 54189e45..e93e041f 100644 --- a/lib/Espfc/src/Rc/Crsf.cpp +++ b/lib/Espfc/src/Rc/Crsf.cpp @@ -1,4 +1,4 @@ - +#include #include "Crsf.h" #include "Math/Utils.h" #include "Math/Crc.h" @@ -83,13 +83,86 @@ void FAST_CODE_ATTR Crsf::decodeRcDataShift8(uint16_t* channels, const CrsfData* channels[15] = convert((crsfData[5] >> 5) & 0x07FF); }*/ -void Crsf::encodeRcData(CrsfFrame& frame, const CrsfData& data) +void Crsf::encodeRcData(CrsfMessage& msg, const CrsfData& data) +{ + msg.addr = CRSF_ADDRESS_FLIGHT_CONTROLLER; + msg.type = CRSF_FRAMETYPE_RC_CHANNELS_PACKED; + msg.size = sizeof(data) + 2; + std::memcpy(msg.payload, (void*)&data, sizeof(data)); + msg.payload[sizeof(data)] = crc(msg); +} + +int Crsf::encodeMsp(CrsfMessage& msg, const Msp::MspResponse& resp, uint8_t origin) +{ + uint8_t buff[CRSF_PAYLOAD_SIZE_MAX]; + size_t size = resp.serialize(buff, CRSF_PAYLOAD_SIZE_MAX); + + if(size < 4) return 0; // unable to serialize + + uint8_t status = 0; + status |= (1 << 4); // start bit + status |= ((resp.version == Msp::MSP_V1 ? 1 : 2) << 5); + + msg.prepare(Rc::CRSF_FRAMETYPE_MSP_RESP); + msg.writeU8(origin); + msg.writeU8(Rc::CRSF_ADDRESS_FLIGHT_CONTROLLER); + msg.writeU8(status); + msg.write(buff + 3, size - 4); // skip sync bytes and crc + msg.finalize(); + + return msg.size; +} + +int Crsf::decodeMsp(const CrsfMessage& msg, Msp::MspMessage& m, uint8_t& origin) { - 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); + //uint8_t dst = msg.payload[0]; + origin = msg.payload[1]; + uint8_t status = msg.payload[2]; + + //uint8_t sequence = (status & 0x0f); // 00001111 + uint8_t start = (status & 0x10) >> 4; // 00010000 + uint8_t version = (status & 0x60) >> 5; // 01100000 + //uint8_t error = (status & 0x80) >> 7; // 10000000 + + if(start) + { + if(version == 1) + { + const Msp::MspHeaderV1 * hdr = reinterpret_cast(msg.payload + 3); + size_t framePayloadSize = msg.size - 5 - sizeof(Msp::MspHeaderV1); + if(framePayloadSize >= hdr->size) + { + m.expected = hdr->size; + m.received = hdr->size; + m.cmd = hdr->cmd; + m.state = Msp::MSP_STATE_RECEIVED; + m.dir = Msp::MSP_TYPE_CMD; + m.version = Msp::MSP_V1; + std::copy_n(msg.payload + 3 + sizeof(Msp::MspHeaderV1), m.received, m.buffer); + } + } + else if(version == 2) + { + const Msp::MspHeaderV2 * hdr = reinterpret_cast(msg.payload + 3); + size_t framePayloadSize = msg.size - 5 - sizeof(Msp::MspHeaderV2); + if(framePayloadSize >= hdr->size) + { + m.expected = hdr->size; + m.received = hdr->size; + m.cmd = hdr->cmd; + m.state = Msp::MSP_STATE_RECEIVED; + m.dir = Msp::MSP_TYPE_CMD; + m.version = Msp::MSP_V1; + std::copy_n(msg.payload + 3 + sizeof(Msp::MspHeaderV2), m.received, m.buffer); + } + } + } + else + { + // next chunk + } + + return 0; } uint16_t Crsf::convert(int v) @@ -109,12 +182,12 @@ uint16_t Crsf::convert(int v) //return Math::mapi(v, 172, 1811, 988, 2012); } -uint8_t Crsf::crc(const CrsfFrame& frame) +uint8_t Crsf::crc(const CrsfMessage& msg) { // 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]); + uint8_t crc = Math::crc8_dvb_s2(0, msg.type); + for (int i = 0; i < msg.size - 2; i++) { // size includes type and crc + crc = Math::crc8_dvb_s2(crc, msg.payload[i]); } return crc; } diff --git a/lib/Espfc/src/Rc/Crsf.h b/lib/Espfc/src/Rc/Crsf.h index 77eb3086..78d7ed13 100644 --- a/lib/Espfc/src/Rc/Crsf.h +++ b/lib/Espfc/src/Rc/Crsf.h @@ -1,6 +1,9 @@ #pragma once #include +#include +#include "Math/Crc.h" +#include "Msp/Msp.h" namespace Espfc { @@ -117,6 +120,19 @@ struct CrsfMessage uint8_t type; // CrsfFrameType uint8_t payload[CRSF_PAYLOAD_SIZE_MAX + 1]; + void prepare(uint8_t t) + { + addr = Rc::CRSF_SYNC_BYTE; + type = t; + size = 0; + } + + void finalize() + { + size += 2; + writeCRC(crc()); + } + void writeU8(uint8_t v) { payload[size++] = v; @@ -136,6 +152,11 @@ struct CrsfMessage writeU8(v >> 24); } + void write(const uint8_t * v, size_t len) + { + while(len--) writeU8(*v++); + } + void writeString(const char * v, bool terminate = false) { while(*v) writeU8(*v++); @@ -146,13 +167,13 @@ struct CrsfMessage { payload[size - 2] = v; } -} __attribute__ ((__packed__)); -union CrsfFrame -{ - CrsfMessage message; - uint8_t data[CRSF_FRAME_SIZE_MAX]; -}; + uint8_t crc() const + { + uint8_t crc = Math::crc8_dvb_s2(0, type); + return Math::crc8_dvb_s2(crc, payload, size - 2); // size includes type and crc + } +} __attribute__ ((__packed__)); class Crsf { @@ -160,9 +181,11 @@ class Crsf 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 void encodeRcData(CrsfMessage& frame, const CrsfData& data); + static int encodeMsp(CrsfMessage& msg, const Msp::MspResponse& res, uint8_t origin); + static int decodeMsp(const CrsfMessage& msg, Msp::MspMessage& m, uint8_t& origin); static uint16_t convert(int v); - static uint8_t crc(const CrsfFrame& frame); + static uint8_t crc(const CrsfMessage& frame); }; } diff --git a/lib/Espfc/src/Telemetry/TelemetryCRSF.h b/lib/Espfc/src/Telemetry/TelemetryCRSF.h index 0b4561d6..b5dfc97c 100644 --- a/lib/Espfc/src/Telemetry/TelemetryCRSF.h +++ b/lib/Espfc/src/Telemetry/TelemetryCRSF.h @@ -5,6 +5,10 @@ #include "Rc/Crsf.h" #include "Math/Utils.h" +// https://github.com/crsf-wg/crsf/wiki/CRSF_FRAMETYPE_MSP_REQ - not correct +// https://github.com/betaflight/betaflight/blob/2525be9a3369fa666d8ce1485ec5ad344326b085/src/main/telemetry/crsf.c#L664 +// https://github.com/betaflight/betaflight/blob/2525be9a3369fa666d8ce1485ec5ad344326b085/src/main/telemetry/msp_shared.c#L46 + namespace Espfc { namespace Telemetry { @@ -30,7 +34,7 @@ class TelemetryCRSF int process(Device::SerialDevice& s) const { - Rc::CrsfFrame f; + Rc::CrsfMessage f; switch(_current) { case CRSF_TELEMETRY_STATE_ATTI: @@ -46,7 +50,8 @@ class TelemetryCRSF case CRSF_TELEMETRY_STATE_FM: flightMode(f); send(f, s); - _current = CRSF_TELEMETRY_STATE_GPS; + //_current = CRSF_TELEMETRY_STATE_GPS; + _current = CRSF_TELEMETRY_STATE_VARIO; break; case CRSF_TELEMETRY_STATE_GPS: //gps(f); @@ -68,15 +73,20 @@ class TelemetryCRSF return 1; } - int sendMsp(Device::SerialDevice& s, Msp::MspResponse r) const + int sendMsp(Device::SerialDevice& s, Msp::MspResponse r, uint8_t origin) const { + Rc::CrsfMessage msg; + + Rc::Crsf::encodeMsp(msg, r, origin); + + send(msg, s); return 1; } - void send(const Rc::CrsfFrame& f, Device::SerialDevice& s) const + void send(const Rc::CrsfMessage& msg, Device::SerialDevice& s) const { - s.write(f.data, f.message.size + 2); + s.write((uint8_t*)&msg, msg.size + 2); } int16_t toAngle(float angle) const @@ -86,83 +96,70 @@ class TelemetryCRSF return lrintf(angle * 1000); } - void attitude(Rc::CrsfFrame& f) const + void attitude(Rc::CrsfMessage& msg) const { - prepare(f, Rc::CRSF_FRAMETYPE_ATTITUDE); + msg.prepare(Rc::CRSF_FRAMETYPE_ATTITUDE); int16_t r = toAngle(_model.state.angle.x); int16_t p = toAngle(_model.state.angle.y); int16_t y = toAngle(_model.state.angle.z); - f.message.writeU16(Math::toBigEndian16(r)); - f.message.writeU16(Math::toBigEndian16(p)); - f.message.writeU16(Math::toBigEndian16(y)); + msg.writeU16(Math::toBigEndian16(r)); + msg.writeU16(Math::toBigEndian16(p)); + msg.writeU16(Math::toBigEndian16(y)); - finalize(f); + msg.finalize(); } - void battery(Rc::CrsfFrame& f) const + void battery(Rc::CrsfMessage& msg) const { - prepare(f, Rc::CRSF_FRAMETYPE_BATTERY_SENSOR); + msg.prepare(Rc::CRSF_FRAMETYPE_BATTERY_SENSOR); uint16_t voltage = Math::clamp(lrintf(_model.state.battery.voltage * 10.0f), 0l, 32000l); uint16_t current = Math::clamp(lrintf(_model.state.battery.current * 10.0f), 0l, 32000l); uint32_t mahDrawn = 0; uint8_t remainPerc = 100; - f.message.writeU16(Math::toBigEndian16(voltage)); - f.message.writeU16(Math::toBigEndian16(current)); - f.message.writeU8(mahDrawn >> 16); - f.message.writeU8(mahDrawn >> 8); - f.message.writeU8(mahDrawn); - f.message.writeU8(remainPerc); + msg.writeU16(Math::toBigEndian16(voltage)); + msg.writeU16(Math::toBigEndian16(current)); + msg.writeU8(mahDrawn >> 16); + msg.writeU8(mahDrawn >> 8); + msg.writeU8(mahDrawn); + msg.writeU8(remainPerc); - finalize(f); + msg.finalize(); } - void flightMode(Rc::CrsfFrame& f) const + void flightMode(Rc::CrsfMessage& msg) const { - prepare(f, Rc::CRSF_FRAMETYPE_FLIGHT_MODE); + msg.prepare(Rc::CRSF_FRAMETYPE_FLIGHT_MODE); - if(_model.armingDisabled()) f.message.writeString("!DIS"); - if(_model.isModeActive(MODE_FAILSAFE)) f.message.writeString("!FS,"); - if(_model.isModeActive(MODE_ARMED)) f.message.writeString("ARM,"); - if(_model.isModeActive(MODE_AIRMODE)) f.message.writeString("AIR,"); - if(_model.isModeActive(MODE_ANGLE)) f.message.writeString("STAB,"); - f.message.writeU8(0); + if(_model.armingDisabled()) msg.writeString("!DIS"); + if(_model.isModeActive(MODE_FAILSAFE)) msg.writeString("!FS,"); + if(_model.isModeActive(MODE_ARMED)) msg.writeString("ARM,"); + if(_model.isModeActive(MODE_AIRMODE)) msg.writeString("AIR,"); + if(_model.isModeActive(MODE_ANGLE)) msg.writeString("STAB,"); + msg.writeU8(0); - finalize(f); + msg.finalize(); } - void vario(Rc::CrsfFrame& f) const + void vario(Rc::CrsfMessage& msg) const { - prepare(f, Rc::CRSF_FRAMETYPE_VARIO_SENSOR); + msg.prepare(Rc::CRSF_FRAMETYPE_VARIO_SENSOR); - f.message.writeU16(Math::toBigEndian16(0)); + msg.writeU16(Math::toBigEndian16(0)); - finalize(f); + msg.finalize(); } - void heartbeat(Rc::CrsfFrame& f) const + void heartbeat(Rc::CrsfMessage& msg) const { - prepare(f, Rc::CRSF_FRAMETYPE_HEARTBEAT); - - f.message.writeU16(Math::toBigEndian16(Rc::CRSF_ADDRESS_FLIGHT_CONTROLLER)); + msg.prepare(Rc::CRSF_FRAMETYPE_HEARTBEAT); - finalize(f); - } + msg.writeU16(Math::toBigEndian16(Rc::CRSF_ADDRESS_FLIGHT_CONTROLLER)); - void prepare(Rc::CrsfFrame& f, uint8_t type) const - { - f.message.addr = Rc::CRSF_SYNC_BYTE; - f.message.type = type; - f.message.size = 0; - } - - void finalize(Rc::CrsfFrame& f) const - { - f.message.size += 2; - f.message.writeCRC(Rc::Crsf::crc(f)); + msg.finalize(); } private: diff --git a/lib/Espfc/src/TelemetryManager.h b/lib/Espfc/src/TelemetryManager.h index e87d3dd5..84fe58c5 100644 --- a/lib/Espfc/src/TelemetryManager.h +++ b/lib/Espfc/src/TelemetryManager.h @@ -35,14 +35,9 @@ class TelemetryManager return 1; } - int processMsp(Device::SerialDevice& s, TelemetryProtocol protocol, const uint8_t * payload, size_t max_len) + int processMsp(Device::SerialDevice& s, TelemetryProtocol protocol, Msp::MspMessage m, uint8_t origin) { - Msp::MspMessage m; Msp::MspResponse r; - for(size_t i = 0; i < max_len; i++) - { - if(!_msp.parse(payload[i], m)) break; - } // not valid msp message, stop processing if(!m.isReady() || !m.isCmd()) return 0; @@ -52,7 +47,7 @@ class TelemetryManager switch(protocol) { case TELEMETRY_PROTOCOL_CRSF: - _crsf.sendMsp(s, r); + _crsf.sendMsp(s, r, origin); break; default: break; diff --git a/test/test_input_crsf/test_input_crsf.cpp b/test/test_input_crsf/test_input_crsf.cpp index 20132206..b50e4ec9 100644 --- a/test/test_input_crsf/test_input_crsf.cpp +++ b/test/test_input_crsf/test_input_crsf.cpp @@ -15,8 +15,9 @@ using namespace fakeit; void test_input_crsf_rc_valid() { InputCRSF input; - CrsfFrame frame; + CrsfMessage frame; memset(&frame, 0, sizeof(frame)); + uint8_t * frame_data = reinterpret_cast(&frame); When(Method(ArduinoFake(), micros)).Return(0); @@ -31,15 +32,16 @@ void test_input_crsf_rc_valid() } for (size_t i; i < sizeof(data); i++) { - TEST_ASSERT_EQUAL_UINT8(data[i], frame.data[i]); + TEST_ASSERT_EQUAL_UINT8(data[i], frame_data[i]); } const uint8_t crc = Crsf::crc(frame); TEST_ASSERT_EQUAL_UINT8(0x23, crc); + TEST_ASSERT_EQUAL_UINT8(0x23, frame.crc()); - TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.message.addr); - TEST_ASSERT_EQUAL_UINT8(0x18, frame.message.size); - TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, frame.message.type); + TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.addr); + TEST_ASSERT_EQUAL_UINT8(0x18, frame.size); + TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, frame.type); TEST_ASSERT_EQUAL_UINT16(1500u, input.get(0)); TEST_ASSERT_EQUAL_UINT16(1500u, input.get(1)); @@ -52,7 +54,7 @@ void test_input_crsf_rc_valid() void test_input_crsf_rc_prefix() { InputCRSF input; - CrsfFrame frame; + CrsfMessage frame; memset(&frame, 0, sizeof(frame)); When(Method(ArduinoFake(), micros)).Return(0); @@ -72,9 +74,9 @@ void test_input_crsf_rc_prefix() const uint8_t crc = Crsf::crc(frame); TEST_ASSERT_EQUAL_UINT8(0x23, crc); - TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.message.addr); - TEST_ASSERT_EQUAL_UINT8(0x18, frame.message.size); - TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, frame.message.type); + TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.addr); + TEST_ASSERT_EQUAL_UINT8(0x18, frame.size); + TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, frame.type); TEST_ASSERT_EQUAL_UINT16(1500, input.get(0)); TEST_ASSERT_EQUAL_UINT16(1500, input.get(1)); @@ -82,7 +84,7 @@ void test_input_crsf_rc_prefix() void test_crsf_encode_rc() { - CrsfFrame frame; + CrsfMessage frame; memset(&frame, 0, sizeof(frame)); CrsfData data; @@ -110,44 +112,46 @@ void test_crsf_encode_rc() 0x7C, 0xE0, 0x03, 0x1F, 0xF8, 0xC0, 0x07, 0x3E, 0xF0, 0x81, 0x0F, 0x7C, 0xDB }; - TEST_ASSERT_EQUAL_UINT8(expected[0], frame.data[0]); // addr - TEST_ASSERT_EQUAL_UINT8(expected[1], frame.data[1]); // size - TEST_ASSERT_EQUAL_UINT8(expected[2], frame.data[2]); // type - TEST_ASSERT_EQUAL_UINT8(expected[3], frame.data[3]); - TEST_ASSERT_EQUAL_UINT8(expected[4], frame.data[4]); - TEST_ASSERT_EQUAL_UINT8(expected[5], frame.data[5]); - TEST_ASSERT_EQUAL_UINT8(expected[6], frame.data[6]); - TEST_ASSERT_EQUAL_UINT8(expected[7], frame.data[7]); - TEST_ASSERT_EQUAL_UINT8(expected[8], frame.data[8]); - TEST_ASSERT_EQUAL_UINT8(expected[9], frame.data[9]); - TEST_ASSERT_EQUAL_UINT8(expected[10], frame.data[10]); - TEST_ASSERT_EQUAL_UINT8(expected[11], frame.data[11]); - TEST_ASSERT_EQUAL_UINT8(expected[12], frame.data[12]); - TEST_ASSERT_EQUAL_UINT8(expected[13], frame.data[13]); - TEST_ASSERT_EQUAL_UINT8(expected[14], frame.data[14]); - TEST_ASSERT_EQUAL_UINT8(expected[15], frame.data[15]); - TEST_ASSERT_EQUAL_UINT8(expected[16], frame.data[16]); - TEST_ASSERT_EQUAL_UINT8(expected[17], frame.data[17]); - TEST_ASSERT_EQUAL_UINT8(expected[18], frame.data[18]); - TEST_ASSERT_EQUAL_UINT8(expected[19], frame.data[19]); - TEST_ASSERT_EQUAL_UINT8(expected[20], frame.data[20]); - TEST_ASSERT_EQUAL_UINT8(expected[21], frame.data[21]); - TEST_ASSERT_EQUAL_UINT8(expected[22], frame.data[22]); - TEST_ASSERT_EQUAL_UINT8(expected[23], frame.data[23]); - TEST_ASSERT_EQUAL_UINT8(expected[24], frame.data[24]); - TEST_ASSERT_EQUAL_UINT8(expected[25], frame.data[25]); // crc + uint8_t * frame_data = reinterpret_cast(&frame); + + TEST_ASSERT_EQUAL_UINT8(expected[0], frame_data[0]); // addr + TEST_ASSERT_EQUAL_UINT8(expected[1], frame_data[1]); // size + TEST_ASSERT_EQUAL_UINT8(expected[2], frame_data[2]); // type + TEST_ASSERT_EQUAL_UINT8(expected[3], frame_data[3]); + TEST_ASSERT_EQUAL_UINT8(expected[4], frame_data[4]); + TEST_ASSERT_EQUAL_UINT8(expected[5], frame_data[5]); + TEST_ASSERT_EQUAL_UINT8(expected[6], frame_data[6]); + TEST_ASSERT_EQUAL_UINT8(expected[7], frame_data[7]); + TEST_ASSERT_EQUAL_UINT8(expected[8], frame_data[8]); + TEST_ASSERT_EQUAL_UINT8(expected[9], frame_data[9]); + TEST_ASSERT_EQUAL_UINT8(expected[10], frame_data[10]); + TEST_ASSERT_EQUAL_UINT8(expected[11], frame_data[11]); + TEST_ASSERT_EQUAL_UINT8(expected[12], frame_data[12]); + TEST_ASSERT_EQUAL_UINT8(expected[13], frame_data[13]); + TEST_ASSERT_EQUAL_UINT8(expected[14], frame_data[14]); + TEST_ASSERT_EQUAL_UINT8(expected[15], frame_data[15]); + TEST_ASSERT_EQUAL_UINT8(expected[16], frame_data[16]); + TEST_ASSERT_EQUAL_UINT8(expected[17], frame_data[17]); + TEST_ASSERT_EQUAL_UINT8(expected[18], frame_data[18]); + TEST_ASSERT_EQUAL_UINT8(expected[19], frame_data[19]); + TEST_ASSERT_EQUAL_UINT8(expected[20], frame_data[20]); + TEST_ASSERT_EQUAL_UINT8(expected[21], frame_data[21]); + TEST_ASSERT_EQUAL_UINT8(expected[22], frame_data[22]); + TEST_ASSERT_EQUAL_UINT8(expected[23], frame_data[23]); + TEST_ASSERT_EQUAL_UINT8(expected[24], frame_data[24]); + TEST_ASSERT_EQUAL_UINT8(expected[25], frame_data[25]); // crc const uint8_t crc = Crsf::crc(frame); TEST_ASSERT_EQUAL_UINT8(0xdb, crc); - TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.message.addr); - TEST_ASSERT_EQUAL_UINT8(0x18, frame.message.size); - TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, frame.message.type); + TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.addr); + TEST_ASSERT_EQUAL_UINT8(0x18, frame.size); + TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, frame.type); } void test_crsf_decode_rc_struct() { - CrsfFrame frame; + CrsfMessage frame; memset(&frame, 0, sizeof(frame)); CrsfData data; @@ -172,7 +176,7 @@ void test_crsf_decode_rc_struct() uint16_t channels[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - Crsf::decodeRcData(channels, (const CrsfData*)frame.message.payload); + Crsf::decodeRcData(channels, (const CrsfData*)frame.payload); TEST_ASSERT_EQUAL_UINT16(1500, channels[0]); TEST_ASSERT_EQUAL_UINT16(1500, channels[1]); @@ -197,7 +201,7 @@ void test_crsf_decode_rc_struct() void test_crsf_decode_rc_shift8() { - CrsfFrame frame; + CrsfMessage frame; memset(&frame, 0, sizeof(frame)); CrsfData data; @@ -222,7 +226,7 @@ void test_crsf_decode_rc_shift8() uint16_t channels[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - Crsf::decodeRcDataShift8(channels, (const CrsfData*)frame.message.payload); + Crsf::decodeRcDataShift8(channels, (const CrsfData*)frame.payload); TEST_ASSERT_EQUAL_UINT16(1500, channels[0]); TEST_ASSERT_EQUAL_UINT16(1500, channels[1]); @@ -247,7 +251,7 @@ void test_crsf_decode_rc_shift8() /*void test_crsf_decode_rc_shift32() { - CrsfFrame frame; + CrsfMessage frame; memset(&frame, 0, sizeof(frame)); CrsfData data; @@ -295,6 +299,119 @@ void test_crsf_decode_rc_shift8() TEST_ASSERT_EQUAL_UINT16(1500, channels[15]); }*/ +void test_crsf_encode_msp_v1() +{ + CrsfMessage frame; + memset(&frame, 0, sizeof(frame)); + + Msp::MspResponse resp; + resp.version = Msp::MSP_V1; + resp.cmd = MSP_API_VERSION; + resp.result = 0; + resp.writeU8(1); + resp.writeU8(2); + resp.writeU8(3); + + Crsf::encodeMsp(frame, resp, CRSF_ADDRESS_RADIO_TRANSMITTER); + + // crsf headers + TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.addr); + TEST_ASSERT_EQUAL_UINT8(10, frame.size); + TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_MSP_RESP, frame.type); + + // crsf ext headers + TEST_ASSERT_EQUAL_UINT8(0xEA, frame.payload[0]); // radio-transmitter + TEST_ASSERT_EQUAL_UINT8(0xC8, frame.payload[1]); // FC + TEST_ASSERT_EQUAL_UINT8(0x30, frame.payload[2]); // status + + // ext msp v1 header + TEST_ASSERT_EQUAL_UINT8(3, frame.payload[3]); // size + TEST_ASSERT_EQUAL_UINT8(1, frame.payload[4]); // type // api_version(1) + + // ext msp payload + TEST_ASSERT_EQUAL_UINT8(1, frame.payload[5]); // param1 + TEST_ASSERT_EQUAL_UINT8(2, frame.payload[6]); // param2 + TEST_ASSERT_EQUAL_UINT8(3, frame.payload[7]); // param3 + + // crsf crc + TEST_ASSERT_EQUAL_UINT8(0x6D, frame.crc()); +} + +void test_crsf_encode_msp_v2() +{ + CrsfMessage frame; + memset(&frame, 0, sizeof(frame)); + + Msp::MspResponse resp; + resp.version = Msp::MSP_V2; + resp.cmd = MSP_API_VERSION; + resp.result = 0; + resp.writeU8(1); + resp.writeU8(2); + resp.writeU8(3); + + Crsf::encodeMsp(frame, resp, CRSF_ADDRESS_RADIO_TRANSMITTER); + + // crsf headers + TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.addr); + TEST_ASSERT_EQUAL_UINT8(13, frame.size); + TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_MSP_RESP, frame.type); + + // crsf ext headers + TEST_ASSERT_EQUAL_UINT8(0xEA, frame.payload[0]); // radio-transmitter addr + TEST_ASSERT_EQUAL_UINT8(0xC8, frame.payload[1]); // FC addr + TEST_ASSERT_EQUAL_UINT8(0x50, frame.payload[2]); // status flags + + // ext msp v2 header + TEST_ASSERT_EQUAL_UINT8(0, frame.payload[3]); // flags + TEST_ASSERT_EQUAL_UINT8(1, frame.payload[4]); // type: api_version(1) (lo) + TEST_ASSERT_EQUAL_UINT8(0, frame.payload[5]); // type: api_version(1) (hi) + TEST_ASSERT_EQUAL_UINT8(3, frame.payload[6]); // size (lo) + TEST_ASSERT_EQUAL_UINT8(0, frame.payload[7]); // size (hi) + + // ext msp payload + TEST_ASSERT_EQUAL_UINT8(1, frame.payload[8]); // param1 + TEST_ASSERT_EQUAL_UINT8(2, frame.payload[9]); // param2 + TEST_ASSERT_EQUAL_UINT8(3, frame.payload[10]); // param3 + + // crsf crc + TEST_ASSERT_EQUAL_UINT8(0xF8, frame.crc()); +} + +void test_crsf_decode_msp_v1() +{ + const uint8_t data[] = { + 0xc8, 0x08, 0x7a, 0xc8, 0xea, 0x32, 0x00, 0x70, 0x70, 0x4b + }; + CrsfMessage frame; + std::copy_n(data, sizeof(data), (uint8_t*)&frame); + + Msp::MspMessage m; + uint8_t origin = 0; + + Crsf::decodeMsp(frame, m, origin); + + // crsf headers + TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.addr); + TEST_ASSERT_EQUAL_UINT8(0x08, frame.size); + TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_MSP_REQ, frame.type); + + // crsf ext headers + TEST_ASSERT_EQUAL_UINT8(0xC8, frame.payload[0]); // FC addr + TEST_ASSERT_EQUAL_UINT8(0xEA, frame.payload[1]); // radio-transmitter addr (origin) + TEST_ASSERT_EQUAL_UINT8(0x32, frame.payload[2]); // status flags + + // ext msp v2 header + TEST_ASSERT_EQUAL_UINT8(0x00, frame.payload[3]); // size + TEST_ASSERT_EQUAL_UINT8(0x70, frame.payload[4]); // type: msp_pid(0x70) + + // crsf crc + TEST_ASSERT_EQUAL_UINT8(0x4B, frame.crc()); + + // origin + TEST_ASSERT_EQUAL_UINT8(0xEA, origin); +} + int main(int argc, char **argv) { UNITY_BEGIN(); @@ -304,7 +421,8 @@ int main(int argc, char **argv) RUN_TEST(test_crsf_decode_rc_struct); RUN_TEST(test_crsf_decode_rc_shift8); //RUN_TEST(test_crsf_decode_rc_shift32); - UNITY_END(); - - return 0; + RUN_TEST(test_crsf_encode_msp_v1); + RUN_TEST(test_crsf_encode_msp_v2); + RUN_TEST(test_crsf_decode_msp_v1); + return UNITY_END(); } \ No newline at end of file From 158ea618e5ad95a590861f7cf18ca1f7d3a7a81d Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 12 Sep 2024 21:51:56 +0200 Subject: [PATCH 06/68] telemetry percentage + lib versions --- lib/AHRS/library.json | 3 ++- lib/EscDriver/library.json | 3 ++- lib/EspWire/library.json | 3 ++- lib/Espfc/library.json | 3 ++- lib/Espfc/src/ModelState.h | 1 + lib/Espfc/src/Msp/Msp.h | 4 ++-- lib/Espfc/src/Sensor/VoltageSensor.h | 2 ++ lib/Espfc/src/Telemetry/TelemetryCRSF.h | 2 +- lib/Kalman/library.json | 4 ++++ lib/betaflight/library.json | 3 ++- lib/printf/library.json | 4 ++++ src/main.cpp | 2 +- 12 files changed, 25 insertions(+), 9 deletions(-) create mode 100644 lib/Kalman/library.json create mode 100644 lib/printf/library.json diff --git a/lib/AHRS/library.json b/lib/AHRS/library.json index a95b6ca4..30b02bdc 100644 --- a/lib/AHRS/library.json +++ b/lib/AHRS/library.json @@ -1,3 +1,4 @@ { - "name": "AHRS" + "name": "AHRS", + "version": "1.0.0" } diff --git a/lib/EscDriver/library.json b/lib/EscDriver/library.json index ccbc2805..d7005038 100644 --- a/lib/EscDriver/library.json +++ b/lib/EscDriver/library.json @@ -1,3 +1,4 @@ { - "name": "EscDriver" + "name": "EscDriver", + "version": "1.0.0" } diff --git a/lib/EspWire/library.json b/lib/EspWire/library.json index f8b80216..36fdf9a9 100644 --- a/lib/EspWire/library.json +++ b/lib/EspWire/library.json @@ -1,3 +1,4 @@ { - "name": "EspWire" + "name": "EspWire", + "version": "1.0.0" } diff --git a/lib/Espfc/library.json b/lib/Espfc/library.json index 16845b32..05d54da1 100644 --- a/lib/Espfc/library.json +++ b/lib/Espfc/library.json @@ -1,3 +1,4 @@ { - "name": "Espfc" + "name": "Espfc", + "version": "0.2.0" } diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 08f7b801..ec465867 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -100,6 +100,7 @@ class BatteryState float current; float currentUnfiltered; float cellVoltage; + float percentage; int8_t cells; int8_t samples; Timer timer; diff --git a/lib/Espfc/src/Msp/Msp.h b/lib/Espfc/src/Msp/Msp.h index 4d94e1a4..7328a4e7 100644 --- a/lib/Espfc/src/Msp/Msp.h +++ b/lib/Espfc/src/Msp/Msp.h @@ -197,7 +197,7 @@ class MspResponse size_t serializeV1(uint8_t * buff, size_t len_max) const { // not enough space in target buffer - if(len + 6 > len_max) return 0; + if(len + 6ul > len_max) return 0; buff[0] = '$'; buff[1] = 'M'; @@ -220,7 +220,7 @@ class MspResponse size_t serializeV2(uint8_t * buff, size_t len_max) const { // not enough space in target buffer - if(len + 9 > len_max) return 0; + if(len + 9ul > len_max) return 0; buff[0] = '$'; buff[1] = 'X'; diff --git a/lib/Espfc/src/Sensor/VoltageSensor.h b/lib/Espfc/src/Sensor/VoltageSensor.h index 847aeaff..aff06666 100644 --- a/lib/Espfc/src/Sensor/VoltageSensor.h +++ b/lib/Espfc/src/Sensor/VoltageSensor.h @@ -76,7 +76,9 @@ class VoltageSensor: public BaseSensor _model.state.battery.cells = std::ceil(_model.state.battery.voltage / 4.2f); _model.state.battery.samples--; } + _model.state.battery.cellVoltage = _model.state.battery.voltage / constrain(_model.state.battery.cells, 1, 6); + _model.state.battery.percentage = Math::clamp(Math::map(_model.state.battery.cellVoltage, 3.4f, 4.2f, 0.0f, 100.0f), 0.0f, 100.0f); if(_model.config.debugMode == DEBUG_BATTERY) { diff --git a/lib/Espfc/src/Telemetry/TelemetryCRSF.h b/lib/Espfc/src/Telemetry/TelemetryCRSF.h index b5dfc97c..ff85d3bc 100644 --- a/lib/Espfc/src/Telemetry/TelemetryCRSF.h +++ b/lib/Espfc/src/Telemetry/TelemetryCRSF.h @@ -118,7 +118,7 @@ class TelemetryCRSF uint16_t voltage = Math::clamp(lrintf(_model.state.battery.voltage * 10.0f), 0l, 32000l); uint16_t current = Math::clamp(lrintf(_model.state.battery.current * 10.0f), 0l, 32000l); uint32_t mahDrawn = 0; - uint8_t remainPerc = 100; + uint8_t remainPerc = lrintf(_model.state.battery.percentage); msg.writeU16(Math::toBigEndian16(voltage)); msg.writeU16(Math::toBigEndian16(current)); diff --git a/lib/Kalman/library.json b/lib/Kalman/library.json new file mode 100644 index 00000000..9c3100dc --- /dev/null +++ b/lib/Kalman/library.json @@ -0,0 +1,4 @@ +{ + "name": "Kalman", + "version": "2.0.0" +} diff --git a/lib/betaflight/library.json b/lib/betaflight/library.json index f370b449..d53311df 100644 --- a/lib/betaflight/library.json +++ b/lib/betaflight/library.json @@ -1,3 +1,4 @@ { - "name": "betaflight" + "name": "betaflight", + "version": "4.5.0" } diff --git a/lib/printf/library.json b/lib/printf/library.json new file mode 100644 index 00000000..a13b886e --- /dev/null +++ b/lib/printf/library.json @@ -0,0 +1,4 @@ +{ + "name": "printf", + "version": "1.0.0" +} diff --git a/src/main.cpp b/src/main.cpp index f887fc95..0f9421fc 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -15,7 +15,7 @@ #endif #ifdef ESPFC_WIFI_ALT #include -#else +#elif defined(ESPFC_WIFI) #include #endif #include "Debug_Espfc.h" From 8a4594b59edbb4cac93be48103e5c2e4e310f66d Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 12 Sep 2024 22:52:51 +0200 Subject: [PATCH 07/68] rp2350 single core --- lib/Espfc/src/Blackbox/BlackboxSerialBuffer.h | 2 +- lib/Espfc/src/Device/SerialDeviceAdapter.h | 2 +- lib/Espfc/src/Espfc.cpp | 2 +- lib/Espfc/src/Model.h | 11 +++++++---- lib/Espfc/src/Storage.h | 8 ++++++-- lib/Espfc/src/Target/QueueRP2040.cpp | 2 +- lib/Espfc/src/Target/TargetRP2040.h | 6 +++++- lib/betaflight/src/platform.h | 2 ++ platformio.ini | 15 +++++++++++++++ 9 files changed, 39 insertions(+), 11 deletions(-) diff --git a/lib/Espfc/src/Blackbox/BlackboxSerialBuffer.h b/lib/Espfc/src/Blackbox/BlackboxSerialBuffer.h index b41c2cbf..0cd95e75 100644 --- a/lib/Espfc/src/Blackbox/BlackboxSerialBuffer.h +++ b/lib/Espfc/src/Blackbox/BlackboxSerialBuffer.h @@ -50,7 +50,7 @@ class BlackboxSerialBuffer: public Device::SerialDevice 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 +#if defined(ARCH_RP2040) size_t count = std::min(l, (size_t)available()); for(size_t i = 0; i < count; i++) { diff --git a/lib/Espfc/src/Device/SerialDeviceAdapter.h b/lib/Espfc/src/Device/SerialDeviceAdapter.h index 9ea854fd..3da031be 100644 --- a/lib/Espfc/src/Device/SerialDeviceAdapter.h +++ b/lib/Espfc/src/Device/SerialDeviceAdapter.h @@ -19,7 +19,7 @@ class SerialDeviceAdapter: public SerialDevice 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 +#if defined(ARCH_RP2040) size_t count = std::min(l, (size_t)available()); for(size_t i = 0; i < count; i++) { diff --git a/lib/Espfc/src/Espfc.cpp b/lib/Espfc/src/Espfc.cpp index a9203996..ca807af8 100644 --- a/lib/Espfc/src/Espfc.cpp +++ b/lib/Espfc/src/Espfc.cpp @@ -23,7 +23,7 @@ int Espfc::load() int Espfc::begin() { _serial.begin(); // requires _model.load() - _model.logStorageResult(); + //_model.logStorageResult(); _hardware.begin(); // requires _model.load() _model.begin(); // requires _hardware.begin() _mixer.begin(); diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 4dde78a7..8dc47af3 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -267,7 +267,9 @@ class Model logger.begin(); #ifndef UNIT_TEST _storage.begin(); + logger.info().log(F("F_CPU")).logln(F_CPU); _storageResult = _storage.load(config); + logStorageResult(); #endif postLoad(); return 1; @@ -278,6 +280,7 @@ class Model preSave(); #ifndef UNIT_TEST _storageResult = _storage.write(config); + logStorageResult(); #endif } @@ -595,17 +598,17 @@ class Model void logStorageResult() { #ifndef UNIT_TEST - logger.info().log(F("F_CPU")).logln(F_CPU); switch(_storageResult) { - case STORAGE_LOAD_SUCCESS: logger.info().logln(F("EEPROM loaded")); break; - case STORAGE_SAVE_SUCCESS: logger.info().logln(F("EEPROM saved")); break; + case STORAGE_LOAD_SUCCESS: logger.info().logln(F("EEPROM load ok")); break; + case STORAGE_SAVE_SUCCESS: logger.info().logln(F("EEPROM save ok")); break; + case STORAGE_SAVE_ERROR: logger.err().logln(F("EEPROM save failed")); break; case STORAGE_ERR_BAD_MAGIC: logger.err().logln(F("EEPROM wrong magic")); break; case STORAGE_ERR_BAD_VERSION: logger.err().logln(F("EEPROM wrong version")); break; case STORAGE_ERR_BAD_SIZE: logger.err().logln(F("EEPROM wrong size")); break; case STORAGE_NONE: default: - logger.err().logln(F("EEPROM unknown")); break; + logger.err().logln(F("EEPROM unknown result")); break; } #endif } diff --git a/lib/Espfc/src/Storage.h b/lib/Espfc/src/Storage.h index 55ca4184..f7a279ca 100644 --- a/lib/Espfc/src/Storage.h +++ b/lib/Espfc/src/Storage.h @@ -8,6 +8,7 @@ enum StorageResult STORAGE_NONE, STORAGE_LOAD_SUCCESS, STORAGE_SAVE_SUCCESS, + STORAGE_SAVE_ERROR, STORAGE_ERR_BAD_MAGIC, STORAGE_ERR_BAD_VERSION, STORAGE_ERR_BAD_SIZE, @@ -82,8 +83,11 @@ class Storage { EEPROM.write(addr++, *it); } - EEPROM.commit(); - return STORAGE_SAVE_SUCCESS; + if(EEPROM.commit()) + { + return STORAGE_SAVE_SUCCESS; + } + return STORAGE_SAVE_ERROR; } private: diff --git a/lib/Espfc/src/Target/QueueRP2040.cpp b/lib/Espfc/src/Target/QueueRP2040.cpp index 5b9b7b7c..ecb78f95 100644 --- a/lib/Espfc/src/Target/QueueRP2040.cpp +++ b/lib/Espfc/src/Target/QueueRP2040.cpp @@ -1,6 +1,6 @@ #include "Target.h" -#ifdef ARCH_RP2040 +#if defined(ARCH_RP2040) #include "Queue.h" diff --git a/lib/Espfc/src/Target/TargetRP2040.h b/lib/Espfc/src/Target/TargetRP2040.h index 8e3a7c97..4f8e15c1 100644 --- a/lib/Espfc/src/Target/TargetRP2040.h +++ b/lib/Espfc/src/Target/TargetRP2040.h @@ -69,11 +69,15 @@ #define ESPFC_GUARD 0 +#if defined(ARCH_RP2350) +#define ESPFC_GYRO_I2C_RATE_MAX 1000 +#define ESPFC_GYRO_SPI_RATE_MAX 4000 +#else #define ESPFC_GYRO_I2C_RATE_MAX 1000 #define ESPFC_GYRO_SPI_RATE_MAX 1000 - #define ESPFC_MULTI_CORE #define ESPFC_MULTI_CORE_RP2040 +#endif #include "Device/SerialDevice.h" #include "Debug_Espfc.h" diff --git a/lib/betaflight/src/platform.h b/lib/betaflight/src/platform.h index 82fd7e76..f5e31b18 100644 --- a/lib/betaflight/src/platform.h +++ b/lib/betaflight/src/platform.h @@ -31,6 +31,8 @@ #define ESPFC_TARGET "ESP32C3" #elif defined(ESP32) #define ESPFC_TARGET "ESP32" +#elif defined(ARCH_RP2350) +#define ESPFC_TARGET "RP2350" #elif defined(ARCH_RP2040) #define ESPFC_TARGET "RP2040" #elif defined(UNIT_TEST) diff --git a/platformio.ini b/platformio.ini index 03e1d8be..6a7be98c 100644 --- a/platformio.ini +++ b/platformio.ini @@ -124,6 +124,7 @@ lib_deps = build_flags = ${env.build_flags} +# https://github.com/platformio/platform-raspberrypi/pull/36 [env:rp2040] board = pico platform = https://github.com/maxgerhardt/platform-raspberrypi.git @@ -136,6 +137,20 @@ build_flags = -DIRAM_ATTR="" ; -DIRAM_ATTR='__attribute__ ((section(".time_critical.iram_attr")))' +[env:rp2350] +board = rpipico2 +;platform = https://github.com/maxgerhardt/platform-raspberrypi.git#rp2350_picosdk +platform = https://github.com/maxgerhardt/platform-raspberrypi.git +framework = arduino +board_build.core = earlephilhower +lib_deps = +build_flags = + ${env.build_flags} + -DARCH_RP2040 + -DARCH_RP2350 + -DIRAM_ATTR="" +; -DIRAM_ATTR='__attribute__ ((section(".time_critical.iram_attr")))' + [env:nrf52840] platform = https://github.com/maxgerhardt/platform-nordicnrf52 board = xiaoblesense From 22734ee174e5f7156ed6bd6bd9d743fcfc3b9188 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 6 Oct 2024 01:13:15 +0200 Subject: [PATCH 08/68] board alignment --- lib/AHRS/src/helper_3dmath.h | 48 ++++++++++++++++++++ lib/Espfc/src/Cli.h | 14 +++--- lib/Espfc/src/Model.h | 2 + lib/Espfc/src/ModelConfig.h | 2 + lib/Espfc/src/ModelState.h | 2 + lib/Espfc/src/Msp/MspProcessor.h | 14 ++++-- lib/Espfc/src/Sensor/AccelSensor.h | 6 ++- lib/Espfc/src/Sensor/BaseSensor.h | 8 ++-- lib/Espfc/src/Sensor/GyroSensor.cpp | 5 ++- lib/Espfc/src/Sensor/MagSensor.h | 4 +- lib/Espfc/src/Target/TargetRP2040.h | 4 +- test/test_math/test_math.cpp | 68 +++++++++++++++++++++++++++++ 12 files changed, 157 insertions(+), 20 deletions(-) diff --git a/lib/AHRS/src/helper_3dmath.h b/lib/AHRS/src/helper_3dmath.h index 7838dd8e..34f50c2c 100644 --- a/lib/AHRS/src/helper_3dmath.h +++ b/lib/AHRS/src/helper_3dmath.h @@ -435,7 +435,55 @@ class VectorBase { } }; +template +class RotationMatrix +{ +public: + RotationMatrix() = default; + + void init(const VectorBase& v) + { + const T cosx = cosf(v.x); + const T sinx = sinf(v.x); + const T cosy = cosf(v.y); + const T siny = sinf(v.y); + const T cosz = cosf(v.z); + const T sinz = sinf(v.z); + + const T coszcosx = cosz * cosx; + const T sinzcosx = sinz * cosx; + const T coszsinx = sinx * cosz; + const T sinzsinx = sinx * sinz; + + _m[0][0] = cosz * cosy; + _m[0][1] = -cosy * sinz; + _m[0][2] = siny; + _m[1][0] = sinzcosx + (coszsinx * siny); + _m[1][1] = coszcosx - (sinzsinx * siny); + _m[1][2] = -sinx * cosy; + _m[2][0] = (sinzsinx) - (coszcosx * siny); + _m[2][1] = (coszsinx) + (sinzcosx * siny); + _m[2][2] = cosy * cosx; + } + + VectorBase apply(const VectorBase& v) + { + const T x = _m[0][0] * v.x + _m[1][0] * v.y + _m[2][0] * v.z; + const T y = _m[0][1] * v.x + _m[1][1] * v.y + _m[2][1] * v.z; + const T z = _m[0][2] * v.x + _m[1][2] * v.y + _m[2][2] * v.z; + return VectorBase{x, y, z}; + } + +private: + T _m[3][3] = { + { T{1}, T{0}, T{0} }, + { T{0}, T{1}, T{0} }, + { T{0}, T{0}, T{1} }, + }; +}; + typedef VectorBase VectorFloat; typedef VectorBase VectorInt16; +typedef RotationMatrix RotationMatrixFloat; #endif /* _HELPER_3DMATH_H_ */ diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index f4808e0c..306ed886 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -489,6 +489,10 @@ class Cli Param(PSTR("baro_lpf_type"), &c.baroFilter.type, filterTypeChoices), Param(PSTR("baro_lpf_freq"), &c.baroFilter.freq), + Param(PSTR("board_align_roll"), &c.boardAlignment[0]), + Param(PSTR("board_align_pitch"), &c.boardAlignment[1]), + Param(PSTR("board_align_yaw"), &c.boardAlignment[2]), + Param(PSTR("vbat_source"), &c.vbatSource, voltageSourceChoices), Param(PSTR("vbat_scale"), &c.vbatScale), Param(PSTR("vbat_mul"), &c.vbatResMult), @@ -1039,9 +1043,9 @@ class Cli s.print(_model.config.gyroBias[0]); s.print(' '); s.print(_model.config.gyroBias[1]); s.print(' '); s.print(_model.config.gyroBias[2]); s.print(F(" [")); - s.print(_model.state.gyroBias[0]); s.print(' '); - s.print(_model.state.gyroBias[1]); s.print(' '); - s.print(_model.state.gyroBias[2]); s.println(F("]")); + s.print(Math::toDeg(_model.state.gyroBias[0])); s.print(' '); + s.print(Math::toDeg(_model.state.gyroBias[1])); s.print(' '); + s.print(Math::toDeg(_model.state.gyroBias[2])); s.println(F("]")); s.print(F("accel offset: ")); s.print(_model.config.accelBias[0]); s.print(' '); @@ -1051,7 +1055,7 @@ class Cli s.print(_model.state.accelBias[1]); s.print(' '); s.print(_model.state.accelBias[2]); s.println(F("]")); - s.print(F(" mag offset: ")); + s.print(F(" mag offset: ")); s.print(_model.config.magCalibrationOffset[0]); s.print(' '); s.print(_model.config.magCalibrationOffset[1]); s.print(' '); s.print(_model.config.magCalibrationOffset[2]); s.print(F(" [")); @@ -1059,7 +1063,7 @@ class Cli s.print(_model.state.magCalibrationOffset[1]); s.print(' '); s.print(_model.state.magCalibrationOffset[2]); s.println(F("]")); - s.print(F(" mag scale: ")); + s.print(F(" mag scale: ")); s.print(_model.config.magCalibrationScale[0]); s.print(' '); s.print(_model.config.magCalibrationScale[1]); s.print(' '); s.print(_model.config.magCalibrationScale[2]); s.print(F(" [")); diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 8dc47af3..b80844d2 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -444,6 +444,8 @@ class Model state.magTimer.setRate(state.magRate); } + state.boardAlignment.init(VectorFloat(Math::toRad(config.boardAlignment[0]), Math::toRad(config.boardAlignment[1]), Math::toRad(config.boardAlignment[2]))); + const uint32_t gyroPreFilterRate = state.gyroTimer.rate; const uint32_t gyroFilterRate = state.loopTimer.rate; const uint32_t inputFilterRate = state.inputTimer.rate; diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index ed44eed4..5fce34e6 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -653,6 +653,8 @@ class ModelConfig uint8_t rescueConfigDelay = 30; + int16_t boardAlignment[3] = {0, 0, 0}; + ModelConfig() { #ifdef ESPFC_INPUT diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index ec465867..a56f0c31 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -176,6 +176,8 @@ struct ModelState VectorFloat angle; Quaternion angleQ; + RotationMatrixFloat boardAlignment; + Filter gyroFilter[3]; Filter gyroFilter2[3]; Filter gyroFilter3[3]; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 975c0865..c9275db3 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -700,9 +700,15 @@ class MspProcessor break; case MSP_BOARD_ALIGNMENT_CONFIG: - r.writeU16(0); // roll - r.writeU16(0); // pitch - r.writeU16(0); // yaw + r.writeU16(_model.config.boardAlignment[0]); // roll + r.writeU16(_model.config.boardAlignment[1]); // pitch + r.writeU16(_model.config.boardAlignment[2]); // yaw + break; + + case MSP_SET_BOARD_ALIGNMENT_CONFIG: + _model.config.boardAlignment[0] = m.readU16(); + _model.config.boardAlignment[1] = m.readU16(); + _model.config.boardAlignment[2] = m.readU16(); break; case MSP_RX_MAP: @@ -1273,7 +1279,7 @@ class MspProcessor case MSP_RAW_IMU: for (int i = 0; i < 3; i++) { - r.writeU16(lrintf(_model.state.accel[i] * ACCEL_G_INV * 512.f)); + r.writeU16(lrintf(_model.state.accel[i] * ACCEL_G_INV * 2048.f)); } for (int i = 0; i < 3; i++) { diff --git a/lib/Espfc/src/Sensor/AccelSensor.h b/lib/Espfc/src/Sensor/AccelSensor.h index 8b7db252..91221e9e 100644 --- a/lib/Espfc/src/Sensor/AccelSensor.h +++ b/lib/Espfc/src/Sensor/AccelSensor.h @@ -62,9 +62,11 @@ class AccelSensor: public BaseSensor Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_FILTER); - align(_model.state.accelRaw, _model.config.gyroAlign); - _model.state.accel = (VectorFloat)_model.state.accelRaw * _model.state.accelScale; + + align(_model.state.accel, _model.config.gyroAlign); + _model.state.accel = _model.state.boardAlignment.apply(_model.state.accel); + for(size_t i = 0; i < 3; i++) { if(_model.config.debugMode == DEBUG_ACCELEROMETER) diff --git a/lib/Espfc/src/Sensor/BaseSensor.h b/lib/Espfc/src/Sensor/BaseSensor.h index 863b30de..3f3442b1 100644 --- a/lib/Espfc/src/Sensor/BaseSensor.h +++ b/lib/Espfc/src/Sensor/BaseSensor.h @@ -12,11 +12,11 @@ namespace Sensor { class BaseSensor { public: - void FAST_CODE_ATTR align(VectorInt16& dest, uint8_t rotation) + void FAST_CODE_ATTR align(VectorFloat& dest, uint8_t rotation) { - const int16_t x = dest.x; - const int16_t y = dest.y; - const int16_t z = dest.z; + const float x = dest.x; + const float y = dest.y; + const float z = dest.z; switch(rotation) { diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index 67815a36..a1e9d0f6 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -67,10 +67,11 @@ int FAST_CODE_ATTR GyroSensor::read() _gyro->readGyro(_model.state.gyroRaw); - align(_model.state.gyroRaw, _model.config.gyroAlign); - VectorFloat input = static_cast(_model.state.gyroRaw) * _model.state.gyroScale; + align(input, _model.config.gyroAlign); + input = _model.state.boardAlignment.apply(input); + if (_model.config.gyroFilter3.freq) { _model.state.gyroSampled = Utils::applyFilter(_model.state.gyroFilter3, input); diff --git a/lib/Espfc/src/Sensor/MagSensor.h b/lib/Espfc/src/Sensor/MagSensor.h index d183e54d..99853d0c 100644 --- a/lib/Espfc/src/Sensor/MagSensor.h +++ b/lib/Espfc/src/Sensor/MagSensor.h @@ -58,9 +58,11 @@ class MagSensor: public BaseSensor Stats::Measure measure(_model.state.stats, COUNTER_MAG_FILTER); - align(_model.state.magRaw, _model.config.magAlign); _model.state.mag = _mag->convert(_model.state.magRaw); + align(_model.state.mag, _model.config.magAlign); + _model.state.mag = _model.state.boardAlignment.apply(_model.state.mag); + for(size_t i = 0; i < 3; i++) { _model.state.mag.set(i, _model.state.magFilter[i].update(_model.state.mag[i])); diff --git a/lib/Espfc/src/Target/TargetRP2040.h b/lib/Espfc/src/Target/TargetRP2040.h index 4f8e15c1..fb8c90d4 100644 --- a/lib/Espfc/src/Target/TargetRP2040.h +++ b/lib/Espfc/src/Target/TargetRP2040.h @@ -51,8 +51,8 @@ #define ESPFC_SPI_CS_BARO 11 #define ESPFC_I2C_0 -#define ESPFC_I2C_0_SDA -1 //12 //8 -#define ESPFC_I2C_0_SCL -1 //13 //9 +#define ESPFC_I2C_0_SDA 16 +#define ESPFC_I2C_0_SCL 17 #define ESPFC_BUZZER #define ESPFC_BUZZER_PIN -1 diff --git a/test/test_math/test_math.cpp b/test/test_math/test_math.cpp index e7e34fd5..23c150c0 100644 --- a/test/test_math/test_math.cpp +++ b/test/test_math/test_math.cpp @@ -1234,6 +1234,69 @@ void test_align_addr_to_write() TEST_ASSERT_EQUAL_UINT32(128, Math::alignAddressToWrite(100, 32, 16)); } +void test_rotation_matrix_no_rotation() +{ + VectorFloat v{1.f, 2.f, 3.f}; + RotationMatrixFloat rm; + + VectorFloat r = rm.apply(v); + + TEST_ASSERT_FLOAT_WITHIN(0.01f, 1.f, r.x); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 2.f, r.y); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 3.f, r.z); +} + +void test_rotation_matrix_90_roll() +{ + VectorFloat v{0.f, 0.f, 1.f}; + RotationMatrixFloat rm; + rm.init(VectorFloat{ + Math::toRad(90), + Math::toRad(0), + Math::toRad(0), + }); + + VectorFloat r = rm.apply(v); + + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.f, r.x); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 1.f, r.y); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.f, r.z); +} + +void test_rotation_matrix_90_pitch() +{ + VectorFloat v{0.f, 0.f, 1.f}; + RotationMatrixFloat rm; + rm.init(VectorFloat{ + Math::toRad(0), + Math::toRad(90), + Math::toRad(0), + }); + + VectorFloat r = rm.apply(v); + + TEST_ASSERT_FLOAT_WITHIN(0.01f, -1.f, r.x); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.f, r.y); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.f, r.z); +} + +void test_rotation_matrix_90_yaw() +{ + VectorFloat v{1.f, 2.f, 3.f}; + RotationMatrixFloat rm; + rm.init(VectorFloat{ + Math::toRad(0), + Math::toRad(0), + Math::toRad(90), + }); + + VectorFloat r = rm.apply(v); + + TEST_ASSERT_FLOAT_WITHIN(0.01f, 2.f, r.x); + TEST_ASSERT_FLOAT_WITHIN(0.01f, -1.f, r.y); + TEST_ASSERT_FLOAT_WITHIN(0.01f, 3.f, r.z); +} + int main(int argc, char **argv) { UNITY_BEGIN(); @@ -1306,5 +1369,10 @@ int main(int argc, char **argv) RUN_TEST(test_ring_buf2); RUN_TEST(test_align_addr_to_write); + RUN_TEST(test_rotation_matrix_no_rotation); + RUN_TEST(test_rotation_matrix_90_roll); + RUN_TEST(test_rotation_matrix_90_pitch); + RUN_TEST(test_rotation_matrix_90_yaw); + return UNITY_END(); } \ No newline at end of file From bb3c7db17703a6c6ad2a454c5d819d9d8e83f355 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 6 Oct 2024 01:42:37 +0200 Subject: [PATCH 09/68] ci fix - extract BaseSensor .h to .cpp --- .github/workflows/platformio.yml | 2 +- lib/Espfc/src/Sensor/BaseSensor.cpp | 70 ++++++++++++++++++++++++++++ lib/Espfc/src/Sensor/BaseSensor.h | 72 +++-------------------------- lib/Espfc/src/Sensor/GyroSensor.cpp | 3 +- lib/Espfc/src/Sensor/GyroSensor.h | 4 +- platformio.ini | 2 +- 6 files changed, 82 insertions(+), 71 deletions(-) create mode 100644 lib/Espfc/src/Sensor/BaseSensor.cpp diff --git a/.github/workflows/platformio.yml b/.github/workflows/platformio.yml index 23a1d63d..11370d6c 100644 --- a/.github/workflows/platformio.yml +++ b/.github/workflows/platformio.yml @@ -40,7 +40,7 @@ jobs: runs-on: ubuntu-22.04 strategy: matrix: - target: ['esp32', 'esp32s2', 'esp32s3', 'esp32c3', 'esp8266'] + target: ['esp32', 'esp32s2', 'esp32s3', 'esp32c3', 'esp8266', 'rp2040', 'rp2350'] steps: - uses: actions/checkout@v4 - name: Extract Version diff --git a/lib/Espfc/src/Sensor/BaseSensor.cpp b/lib/Espfc/src/Sensor/BaseSensor.cpp new file mode 100644 index 00000000..479893db --- /dev/null +++ b/lib/Espfc/src/Sensor/BaseSensor.cpp @@ -0,0 +1,70 @@ +#include "BaseSensor.h" +#include "ModelConfig.h" +#include "Utils/MemoryHelper.h" + +namespace Espfc { + +namespace Sensor { + +void FAST_CODE_ATTR BaseSensor::align(VectorFloat& dest, uint8_t rotation) +{ + const float x = dest.x; + const float y = dest.y; + const float z = dest.z; + + switch(rotation) + { + default: + case ALIGN_CW0_DEG: + dest.x = x; + dest.y = y; + dest.z = z; + break; + case ALIGN_CW90_DEG: + dest.x = y; + dest.y = -x; + dest.z = z; + break; + case ALIGN_CW180_DEG: + dest.x = -x; + dest.y = -y; + dest.z = z; + break; + case ALIGN_CW270_DEG: + dest.x = -y; + dest.y = x; + dest.z = z; + break; + case ALIGN_CW0_DEG_FLIP: + dest.x = -x; + dest.y = y; + dest.z = -z; + break; + case ALIGN_CW90_DEG_FLIP: + dest.x = y; + dest.y = x; + dest.z = -z; + break; + case ALIGN_CW180_DEG_FLIP: + dest.x = x; + dest.y = -y; + dest.z = -z; + break; + case ALIGN_CW270_DEG_FLIP: + dest.x = -y; + dest.y = -x; + dest.z = -z; + break; + } +} + +void FAST_CODE_ATTR BaseSensor::toVector(VectorInt16& v, uint8_t * buf) +{ + v.x = (int16_t)(((uint16_t)buf[0] << 8) | (uint16_t)buf[1]); + v.y = (int16_t)(((uint16_t)buf[2] << 8) | (uint16_t)buf[3]); + v.z = (int16_t)(((uint16_t)buf[4] << 8) | (uint16_t)buf[5]); +} + +} + +} diff --git a/lib/Espfc/src/Sensor/BaseSensor.h b/lib/Espfc/src/Sensor/BaseSensor.h index 3f3442b1..fe98aa87 100644 --- a/lib/Espfc/src/Sensor/BaseSensor.h +++ b/lib/Espfc/src/Sensor/BaseSensor.h @@ -1,9 +1,7 @@ -#ifndef _ESPFC_SENSOR_BASE_SENSOR_H_ -#define _ESPFC_SENSOR_BASE_SENSOR_H_ +#pragma once -#include -#include "Model.h" -#include "Hardware.h" +#include +#include namespace Espfc { @@ -11,69 +9,11 @@ namespace Sensor { class BaseSensor { - public: - void FAST_CODE_ATTR align(VectorFloat& dest, uint8_t rotation) - { - const float x = dest.x; - const float y = dest.y; - const float z = dest.z; - - switch(rotation) - { - default: - case ALIGN_CW0_DEG: - dest.x = x; - dest.y = y; - dest.z = z; - break; - case ALIGN_CW90_DEG: - dest.x = y; - dest.y = -x; - dest.z = z; - break; - case ALIGN_CW180_DEG: - dest.x = -x; - dest.y = -y; - dest.z = z; - break; - case ALIGN_CW270_DEG: - dest.x = -y; - dest.y = x; - dest.z = z; - break; - case ALIGN_CW0_DEG_FLIP: - dest.x = -x; - dest.y = y; - dest.z = -z; - break; - case ALIGN_CW90_DEG_FLIP: - dest.x = y; - dest.y = x; - dest.z = -z; - break; - case ALIGN_CW180_DEG_FLIP: - dest.x = x; - dest.y = -y; - dest.z = -z; - break; - case ALIGN_CW270_DEG_FLIP: - dest.x = -y; - dest.y = -x; - dest.z = -z; - break; - } - } - - void toVector(VectorInt16& v, uint8_t * buf) - { - v.x = (int16_t)(((uint16_t)buf[0] << 8) | (uint16_t)buf[1]); - v.y = (int16_t)(((uint16_t)buf[2] << 8) | (uint16_t)buf[3]); - v.z = (int16_t)(((uint16_t)buf[4] << 8) | (uint16_t)buf[5]); - } +public: + void align(VectorFloat& dest, uint8_t rotation); + void toVector(VectorInt16& v, uint8_t * buf); }; } } - -#endif \ No newline at end of file diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index a1e9d0f6..b99f046b 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -1,6 +1,5 @@ #include "GyroSensor.h" -#include "Model.h" #include "Utils/FilterHelper.h" #define ESPFC_FUZZY_ACCEL_ZERO 0.05 @@ -22,7 +21,7 @@ int GyroSensor::begin() _gyro->setDLPFMode(_model.config.gyroDlpf); _gyro->setRate(_gyro->getRate()); - _model.state.gyroScale = radians(2000.f) / 32768.f; + _model.state.gyroScale = Math::toRad(2000.f) / 32768.f; _model.state.gyroCalibrationState = CALIBRATION_START; // calibrate gyro on start _model.state.gyroCalibrationRate = _model.state.loopTimer.rate; diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index b5b1d4aa..43bdcfcc 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -1,11 +1,13 @@ #pragma once #include "BaseSensor.h" +#include "Model.h" #include "Device/GyroDevice.h" #include "Math/Sma.h" -#include "Math/FreqAnalyzer.h" #ifdef ESPFC_DSP #include "Math/FFTAnalyzer.h" +#else +#include "Math/FreqAnalyzer.h" #endif #define ESPFC_FUZZY_ACCEL_ZERO 0.05 diff --git a/platformio.ini b/platformio.ini index 6a7be98c..391a003a 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, esp32s2, esp32s3, esp32c3, esp8266, rp2040 +default_envs = esp32, esp32s2, esp32s3, esp32c3, esp8266, rp2040, rp2350 [env] build_flags = From 283ce2a190c46f1eaa94ceed51f0a7846acc6d6b Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 6 Oct 2024 02:09:57 +0200 Subject: [PATCH 10/68] avoid compiler warning template-id not allowed for constructor in C++20 + formatting --- lib/AHRS/src/helper_3dmath.h | 785 ++++++++++++++++++----------------- 1 file changed, 406 insertions(+), 379 deletions(-) diff --git a/lib/AHRS/src/helper_3dmath.h b/lib/AHRS/src/helper_3dmath.h index 34f50c2c..e1815ac5 100644 --- a/lib/AHRS/src/helper_3dmath.h +++ b/lib/AHRS/src/helper_3dmath.h @@ -1,5 +1,4 @@ -#ifndef _HELPER_3DMATH_H_ -#define _HELPER_3DMATH_H_ +#pragma once #include #include @@ -8,431 +7,461 @@ // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root inline float invSqrt(float x) { - //return 1.f / sqrt(x); - #pragma GCC diagnostic push - #pragma GCC diagnostic ignored "-Wstrict-aliasing" - #pragma GCC diagnostic ignored "-Wuninitialized" +// return 1.f / sqrt(x); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wstrict-aliasing" +#pragma GCC diagnostic ignored "-Wuninitialized" float halfx = 0.5f * x; float y = x; - long i = *(long*)&y; - i = 0x5f3759df - (i>>1); - y = *(float*)&i; + long i = *(long *)&y; + i = 0x5f3759df - (i >> 1); + y = *(float *)&i; y = y * (1.5f - (halfx * y * y)); y = y * (1.5f - (halfx * y * y)); - #pragma GCC diagnostic pop +#pragma GCC diagnostic pop return y; } template class VectorBase; -class Quaternion { - public: - float w; - float x; - float y; - float z; - - Quaternion(): w(1.f), x(0.f), y(0.f), z(0.f) {} - - Quaternion(float nw, float nx, float ny, float nz): w(nw), x(nx), y(ny), z(nz) {} - - Quaternion getProduct(const Quaternion& q) const { - // Quaternion multiplication is defined by: - // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2) - // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2) - // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2) - // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2) - return Quaternion( - w*q.w - x*q.x - y*q.y - z*q.z, // new w - w*q.x + x*q.w + y*q.z - z*q.y, // new x - w*q.y - x*q.z + y*q.w + z*q.x, // new y - w*q.z + x*q.y - y*q.x + z*q.w); // new z - } - - Quaternion operator*(const Quaternion& q) const { - return getProduct(q); - } - - Quaternion& operator*=(const Quaternion& q) { - *this = getProduct(q); - return *this; - } - - Quaternion getConjugate() const { - return Quaternion(w, -x, -y, -z); - } - - float getMagnitude() const { - return sqrt(w * w + x * x + y * y + z * z); - } - - void normalize() { - float m = invSqrt(w * w + x * x + y * y + z * z); - (*this) *= m; - } - - Quaternion getNormalized() const { - Quaternion r(w, x, y, z); - r.normalize(); - return r; - } - - float get(size_t i) const { - return i == 0 ? w : - (i == 1 ? x : - (i == 2 ? y : - (i == 3 ? z : - 0 - ))); - } - - Quaternion operator*(float v) const { - return Quaternion(w * v, x * v, y * v, z * v); - } - - Quaternion& operator*=(float v) { - w *= v; - x *= v; - y *= v; - z *= v; - return *this; - } - - Quaternion operator/(float v) const { - return Quaternion(w / v, x / v, y / v, z / v); - } - - Quaternion operator+(const Quaternion& q) const { - return Quaternion(w + q.w, x + q.x, y + q.y, z + q.z); - } - - float static dot(const Quaternion& q1, const Quaternion& q2) { - return q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z; - } - - /** - * Linear interpolation - * actually it is nlerp (normalised lerp) - */ - Quaternion static lerp(const Quaternion &q1, const Quaternion &q2, float t) - { - return (q1 * (1.f - t) + q2 * t).getNormalized(); - } - - /** - * Spherical linear interpolation, references: - * http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/ - * https://keithmaggio.wordpress.com/2011/02/15/math-magician-lerp-slerp-and-nlerp/ - * http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/ - * https://en.wikipedia.org/wiki/Slerp - */ - Quaternion static slerp(const Quaternion& q1, const Quaternion& q2, float pc) - { - Quaternion qa = q1.getNormalized(); - Quaternion qb = q2.getNormalized(); - - // If the dot product is negative, the quaternions - // have opposite handed-ness and slerp won't take - // the shorter path. Fix by reversing one quaternion. - float cosHalfTheta = dot(qa, qb); - if(cosHalfTheta < 0) - { - qb = qb * -1.f; - cosHalfTheta = -cosHalfTheta; - } - - // if qa = qb or qa =- qb then theta = 0 and we can return qa - if(std::fabs(cosHalfTheta) >= 0.995f) - { - return lerp(qa, qb, pc); - } - - // Calculate temporary values. - float halfTheta = acosf(cosHalfTheta); - float sinHalfTheta = sqrtf(1.0 - cosHalfTheta * cosHalfTheta); - - // if theta = 180 degrees then result is not fully defined - // we could rotate around any axis normal to q1 or q2 - if(std::fabs(sinHalfTheta) < 0.001) - { - return (qa + qb) / 2.f; - } - - // calculate result - float ra = sinf((1.f - pc) * halfTheta) / sinHalfTheta; - float rb = sinf(pc * halfTheta) / sinHalfTheta; - - return qa * ra + qb * rb; - } - - template - void toAngleVector(float& angle, VectorBase& v) const - { - float halfTheta = acosf(w); - float sinHalfTheta = sinf(halfTheta); - angle = 2.0f * halfTheta; - if (sinHalfTheta == 0) { - v.x = 1.0; - v.y = 0; - v.z = 0; - } else { - v.x = x / sinHalfTheta; - v.y = y / sinHalfTheta; - v.z = z / sinHalfTheta; - } - } - - template - void fromAngleVector(float angle, const VectorBase& v) - { - float halfAngle = angle * 0.5f; - float sinHalfTheta = sinf(halfAngle); - w = cosf(halfAngle); - x = v.x * sinHalfTheta; - y = v.y * sinHalfTheta; - z = v.z * sinHalfTheta; - } - - template - void fromAngularVelocity(const VectorBase& v, float dt) - { - float theta = v.getMagnitude() * dt; - fromAngleVector(theta, v.getNormalized()); - } -}; +class Quaternion +{ +public: + float w; + float x; + float y; + float z; -template -class VectorBase { - public: - T x; - T y; - T z; - - VectorBase(): x(), y(), z() {} - VectorBase(T nx, T ny, T nz): x(nx), y(ny), z(nz) {} - VectorBase(const VectorBase& o): x(o.x), y(o.y), z(o.z) {} - - VectorBase& operator =(const VectorBase& o) { - if(this == &o) return *this; - x = o.x; - y = o.y; - z = o.z; - return *this; - } + Quaternion() : w(1.f), x(0.f), y(0.f), z(0.f) {} - T get(size_t i) const { return i == 0 ? x : (i == 1 ? y : (i == 2 ? z : T())); } - T operator[](size_t i) const { return get(i); } + Quaternion(float nw, float nx, float ny, float nz) : w(nw), x(nx), y(ny), z(nz) {} - void set(size_t i, T v) { i == 0 ? x = v : (i == 1 ? y = v : (i == 2 ? z = v : false)); } + Quaternion getProduct(const Quaternion& q) const + { + // Quaternion multiplication is defined by: + // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2) + // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2) + // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2) + // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2) + return Quaternion( + w * q.w - x * q.x - y * q.y - z * q.z, // new w + w * q.x + x * q.w + y * q.z - z * q.y, // new x + w * q.y - x * q.z + y * q.w + z * q.x, // new y + w * q.z + x * q.y - y * q.x + z * q.w); // new z + } - operator VectorBase() const { - return VectorBase(x, y, z); - } + Quaternion operator*(const Quaternion& q) const + { + return getProduct(q); + } - float getMagnitude() const { - return sqrtf(x * x + y * y + z * z); - } + Quaternion& operator*=(const Quaternion& q) + { + *this = getProduct(q); + return *this; + } - VectorBase& normalize() { - float m = invSqrt(x * x + y * y + z * z); - (*this) *= m; - return *this; - } + Quaternion getConjugate() const + { + return Quaternion(w, -x, -y, -z); + } - VectorBase getNormalized() const { - VectorBase r(x, y, z); - r.normalize(); - return r; - } + float getMagnitude() const + { + return sqrt(w * w + x * x + y * y + z * z); + } - void rotate(const Quaternion& q) { - // http://www.cprogramming.com/tutorial/3d/quaternions.html - // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm - // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation - // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1 + void normalize() + { + float m = invSqrt(w * w + x * x + y * y + z * z); + (*this) *= m; + } - // P_out = q * P_in * conj(q) - // - P_out is the output vector - // - q is the orientation quaternion - // - P_in is the input vector (a*aReal) - // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z]) - Quaternion p(0, x, y, z); + Quaternion getNormalized() const + { + Quaternion r(w, x, y, z); + r.normalize(); + return r; + } - // quaternion multiplication: q * p, stored back in p - //p = q.getProduct(p); + float get(size_t i) const + { + return i == 0 ? w : (i == 1 ? x : (i == 2 ? y : (i == 3 ? z : 0))); + } - // quaternion multiplication: p * conj(q), stored back in p - //p = p.getProduct(q.getConjugate()); + Quaternion operator*(float v) const + { + return Quaternion(w * v, x * v, y * v, z * v); + } - p = q * p * q.getConjugate(); + Quaternion& operator*=(float v) + { + w *= v; + x *= v; + y *= v; + z *= v; + return *this; + } - // p quaternion is now [0, x', y', z'] - x = p.x; - y = p.y; - z = p.z; - } + Quaternion operator/(float v) const + { + return Quaternion(w / v, x / v, y / v, z / v); + } - VectorBase getRotated(const Quaternion& q) const { - VectorBase r(x, y, z); - r.rotate(q); - return r; - } + Quaternion operator+(const Quaternion& q) const + { + return Quaternion(w + q.w, x + q.x, y + q.y, z + q.z); + } - float static dotProduct(const VectorBase& a, const VectorBase& b) - { - return a.x * b.x + a.y * b.y + a.z * b.z; - } + float static dot(const Quaternion& q1, const Quaternion& q2) + { + return q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z; + } - VectorBase static crossProduct(const VectorBase& a, const VectorBase& b) - { - VectorBase r; - r.x = a.y * b.z - a.z * b.y; - r.y = a.z * b.x - a.x * b.z; - r.z = a.x * b.y - a.y * b.x; - return r; - } + /** + * Linear interpolation + * actually it is nlerp (normalised lerp) + */ + Quaternion static lerp(const Quaternion& q1, const Quaternion& q2, float t) + { + return (q1 * (1.f - t) + q2 * t).getNormalized(); + } - float dot(const VectorBase& v) const + /** + * Spherical linear interpolation, references: + * http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/ + * https://keithmaggio.wordpress.com/2011/02/15/math-magician-lerp-slerp-and-nlerp/ + * http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/ + * https://en.wikipedia.org/wiki/Slerp + */ + Quaternion static slerp(const Quaternion& q1, const Quaternion& q2, float pc) + { + Quaternion qa = q1.getNormalized(); + Quaternion qb = q2.getNormalized(); + + // If the dot product is negative, the quaternions + // have opposite handed-ness and slerp won't take + // the shorter path. Fix by reversing one quaternion. + float cosHalfTheta = dot(qa, qb); + if (cosHalfTheta < 0) { - return dotProduct(*this, v); + qb = qb * -1.f; + cosHalfTheta = -cosHalfTheta; } - VectorBase cross(const VectorBase& v) const + // if qa = qb or qa =- qb then theta = 0 and we can return qa + if (std::fabs(cosHalfTheta) >= 0.995f) { - return crossProduct(*this, v); + return lerp(qa, qb, pc); } - VectorBase accelToEuler() const - { - VectorBase rpy; // roll pitch yaw - VectorBase na = getNormalized(); - rpy.x = atan2f(na.y, na.z); - rpy.y = -atan2f(na.x, sqrt(na.y * na.y + na.z * na.z)); - rpy.z = 0.f; - return rpy; - } + // Calculate temporary values. + float halfTheta = acosf(cosHalfTheta); + float sinHalfTheta = sqrtf(1.0 - cosHalfTheta * cosHalfTheta); - Quaternion accelToQuaternion() const + // if theta = 180 degrees then result is not fully defined + // we could rotate around any axis normal to q1 or q2 + if (std::fabs(sinHalfTheta) < 0.001) { - /*VectorBase ref(0, 0, 1); - VectorBase na = getNormalized(); - float angle = acosf(ref.dot(na)); - VectorBase v = na.cross(ref).getNormalized(); - Quaternion q; - q.fromAngleVector(angle, v); - q.normalize(); - return q;*/ - return diffVectors(VectorBase(T(0), T(0), T(1)), *this, 1.0f); + return (qa + qb) / 2.f; } - static Quaternion diffVectors(const VectorBase& src, const VectorBase& dst, float gain = 1.f) - { - VectorBase src_n = src.getNormalized(); - VectorBase dst_n = dst.getNormalized(); - Quaternion q; - - float angle = acosf(dst_n.dot(src_n)); - VectorBase v = dst_n.cross(src_n).getNormalized(); - q.fromAngleVector(angle * gain, v); - return q.getNormalized(); - } + // calculate result + float ra = sinf((1.f - pc) * halfTheta) / sinHalfTheta; + float rb = sinf(pc * halfTheta) / sinHalfTheta; + + return qa * ra + qb * rb; + } - Quaternion eulerToQuaternion() const + template + void toAngleVector(float &angle, VectorBase& v) const + { + float halfTheta = acosf(w); + float sinHalfTheta = sinf(halfTheta); + angle = 2.0f * halfTheta; + if (sinHalfTheta == 0) { - Quaternion q; - float cosX2 = cosf(x / 2.0f); - float sinX2 = sinf(x / 2.0f); - float cosY2 = cosf(y / 2.0f); - float sinY2 = sinf(y / 2.0f); - float cosZ2 = cosf(z / 2.0f); - float sinZ2 = sinf(z / 2.0f); - - q.w = cosX2 * cosY2 * cosZ2 + sinX2 * sinY2 * sinZ2; - q.x = sinX2 * cosY2 * cosZ2 - cosX2 * sinY2 * sinZ2; - q.y = cosX2 * sinY2 * cosZ2 + sinX2 * cosY2 * sinZ2; - q.z = cosX2 * cosY2 * sinZ2 - sinX2 * sinY2 * cosZ2; - q.normalize(); - return q; + v.x = 1.0; + v.y = 0; + v.z = 0; } - - VectorBase eulerFromQuaternion(const Quaternion& q) + else { - //x = atan2f(2.0f * (q.y * q.z + q.w * q.x), 1.f - 2.0f * (q.x * q.x + q.y * q.y)); - //y = asinf(2.0f * (q.w * q.y - q.x * q.z)); - //z = atan2f(2.0f * (q.x * q.y + q.w * q.z), 1.f - 2.0f * (q.y * q.y + q.z * q.z)); - x = atan2f(q.w * q.x + q.y * q.z, 0.5f - q.x * q.x - q.y * q.y); - y = asinf(-2.0f * (q.x * q.z - q.w * q.y)); - z = atan2f(q.x * q.y + q.w * q.z, 0.5f - q.y * q.y - q.z * q.z); - return *this; + v.x = x / sinHalfTheta; + v.y = y / sinHalfTheta; + v.z = z / sinHalfTheta; } + } - // vector arithmetics - VectorBase& operator+=(const VectorBase& o) { - x += o.x; - y += o.y; - z += o.z; - return *this; - } + template + void fromAngleVector(float angle, const VectorBase& v) + { + float halfAngle = angle * 0.5f; + float sinHalfTheta = sinf(halfAngle); + w = cosf(halfAngle); + x = v.x * sinHalfTheta; + y = v.y * sinHalfTheta; + z = v.z * sinHalfTheta; + } - VectorBase operator+(const VectorBase& o) { - VectorBase r(*this); - r += o; - return r; - } + template + void fromAngularVelocity(const VectorBase& v, float dt) + { + float theta = v.getMagnitude() * dt; + fromAngleVector(theta, v.getNormalized()); + } +}; - VectorBase& operator-=(const VectorBase& o) { - x -= o.x; - y -= o.y; - z -= o.z; - return *this; - } +template +class VectorBase +{ +public: + T x; + T y; + T z; - VectorBase operator-(const VectorBase& o) { - VectorBase r(*this); - r -= o; - return r; - } + VectorBase() = default; + VectorBase(const VectorBase& o) = default; + VectorBase(T nx, T ny, T nz) : x(nx), y(ny), z(nz) {} - VectorBase& operator*=(const VectorBase& o) { - x *= o.x; - y *= o.y; - z *= o.z; + VectorBase& operator=(const VectorBase& o) + { + if (this == &o) return *this; - } + x = o.x; + y = o.y; + z = o.z; + return *this; + } - VectorBase operator*(const VectorBase& o) { - VectorBase r(*this); - r *= o; - return r; - } + T get(size_t i) const { return i == 0 ? x : (i == 1 ? y : (i == 2 ? z : T())); } + T operator[](size_t i) const { return get(i); } - // scalar operations - VectorBase& operator*=(T o) { - x *= o; - y *= o; - z *= o; - return *this; - } + void set(size_t i, T v) { i == 0 ? x = v : (i == 1 ? y = v : (i == 2 ? z = v : false)); } - VectorBase operator*(T o) { - VectorBase r(*this); - r *= o; - return r; - } + operator VectorBase() const + { + return VectorBase(x, y, z); + } - VectorBase& operator/=(T o) { - x /= o; - y /= o; - z /= o; - return *this; - } + float getMagnitude() const + { + return sqrtf(x * x + y * y + z * z); + } - VectorBase operator/(T o) { - VectorBase r(*this); - r /= o; - return r; - } + VectorBase& normalize() + { + float m = invSqrt(x * x + y * y + z * z); + (*this) *= m; + return *this; + } + + VectorBase getNormalized() const + { + VectorBase r(x, y, z); + r.normalize(); + return r; + } + + void rotate(const Quaternion& q) + { + // http://www.cprogramming.com/tutorial/3d/quaternions.html + // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm + // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation + // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1 + + // P_out = q * P_in * conj(q) + // - P_out is the output vector + // - q is the orientation quaternion + // - P_in is the input vector (a*aReal) + // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z]) + Quaternion p(0, x, y, z); + + // quaternion multiplication: q * p, stored back in p + // p = q.getProduct(p); + + // quaternion multiplication: p * conj(q), stored back in p + // p = p.getProduct(q.getConjugate()); + + p = q * p * q.getConjugate(); + + // p quaternion is now [0, x', y', z'] + x = p.x; + y = p.y; + z = p.z; + } + + VectorBase getRotated(const Quaternion& q) const + { + VectorBase r(x, y, z); + r.rotate(q); + return r; + } + + float static dotProduct(const VectorBase& a, const VectorBase& b) + { + return a.x * b.x + a.y * b.y + a.z * b.z; + } + + VectorBase static crossProduct(const VectorBase& a, const VectorBase& b) + { + VectorBase r; + r.x = a.y * b.z - a.z * b.y; + r.y = a.z * b.x - a.x * b.z; + r.z = a.x * b.y - a.y * b.x; + return r; + } + + float dot(const VectorBase& v) const + { + return dotProduct(*this, v); + } + + VectorBase cross(const VectorBase& v) const + { + return crossProduct(*this, v); + } + + VectorBase accelToEuler() const + { + VectorBase na = getNormalized(); + T x = atan2f(na.y, na.z); + T y = -atan2f(na.x, sqrt(na.y * na.y + na.z * na.z)); + T z = 0.f; + return VectorBase(x, y, z); + } + + Quaternion accelToQuaternion() const + { + /*VectorBase ref(0, 0, 1); + VectorBase na = getNormalized(); + float angle = acosf(ref.dot(na)); + VectorBase v = na.cross(ref).getNormalized(); + Quaternion q; + q.fromAngleVector(angle, v); + q.normalize(); + return q;*/ + return diffVectors(VectorBase(T(0), T(0), T(1)), *this, 1.0f); + } + + static Quaternion diffVectors(const VectorBase& src, const VectorBase& dst, float gain = 1.f) + { + VectorBase src_n = src.getNormalized(); + VectorBase dst_n = dst.getNormalized(); + Quaternion q; + + float angle = acosf(dst_n.dot(src_n)); + VectorBase v = dst_n.cross(src_n).getNormalized(); + q.fromAngleVector(angle * gain, v); + return q.getNormalized(); + } + + Quaternion eulerToQuaternion() const + { + Quaternion q; + float cosX2 = cosf(x / 2.0f); + float sinX2 = sinf(x / 2.0f); + float cosY2 = cosf(y / 2.0f); + float sinY2 = sinf(y / 2.0f); + float cosZ2 = cosf(z / 2.0f); + float sinZ2 = sinf(z / 2.0f); + + q.w = cosX2 * cosY2 * cosZ2 + sinX2 * sinY2 * sinZ2; + q.x = sinX2 * cosY2 * cosZ2 - cosX2 * sinY2 * sinZ2; + q.y = cosX2 * sinY2 * cosZ2 + sinX2 * cosY2 * sinZ2; + q.z = cosX2 * cosY2 * sinZ2 - sinX2 * sinY2 * cosZ2; + q.normalize(); + return q; + } + + VectorBase eulerFromQuaternion(const Quaternion& q) + { + // x = atan2f(2.0f * (q.y * q.z + q.w * q.x), 1.f - 2.0f * (q.x * q.x + q.y * q.y)); + // y = asinf(2.0f * (q.w * q.y - q.x * q.z)); + // z = atan2f(2.0f * (q.x * q.y + q.w * q.z), 1.f - 2.0f * (q.y * q.y + q.z * q.z)); + x = atan2f(q.w * q.x + q.y * q.z, 0.5f - q.x * q.x - q.y * q.y); + y = asinf(-2.0f * (q.x * q.z - q.w * q.y)); + z = atan2f(q.x * q.y + q.w * q.z, 0.5f - q.y * q.y - q.z * q.z); + return *this; + } + + // vector arithmetics + VectorBase& operator+=(const VectorBase& o) + { + x += o.x; + y += o.y; + z += o.z; + return *this; + } + + VectorBase operator+(const VectorBase& o) + { + VectorBase r(*this); + r += o; + return r; + } + + VectorBase& operator-=(const VectorBase& o) + { + x -= o.x; + y -= o.y; + z -= o.z; + return *this; + } + + VectorBase operator-(const VectorBase& o) + { + VectorBase r(*this); + r -= o; + return r; + } + + VectorBase& operator*=(const VectorBase& o) + { + x *= o.x; + y *= o.y; + z *= o.z; + return *this; + } + + VectorBase operator*(const VectorBase& o) + { + VectorBase r(*this); + r *= o; + return r; + } + + // scalar operations + VectorBase& operator*=(T o) + { + x *= o; + y *= o; + z *= o; + return *this; + } + + VectorBase operator*(T o) + { + VectorBase r(*this); + r *= o; + return r; + } + + VectorBase& operator/=(T o) + { + x /= o; + y /= o; + z /= o; + return *this; + } + + VectorBase operator/(T o) + { + VectorBase r(*this); + r /= o; + return r; + } }; template @@ -476,14 +505,12 @@ class RotationMatrix private: T _m[3][3] = { - { T{1}, T{0}, T{0} }, - { T{0}, T{1}, T{0} }, - { T{0}, T{0}, T{1} }, + {T{1}, T{0}, T{0}}, + {T{0}, T{1}, T{0}}, + {T{0}, T{0}, T{1}}, }; }; typedef VectorBase VectorFloat; typedef VectorBase VectorInt16; typedef RotationMatrix RotationMatrixFloat; - -#endif /* _HELPER_3DMATH_H_ */ From dcbe63c6f60f72c434c688013ac43dfc3a4ee01b Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 6 Oct 2024 12:45:00 +0200 Subject: [PATCH 11/68] helper 3dmath fix --- lib/AHRS/src/helper_3dmath.h | 18 +++++++++--------- test/test_esc/test_esc.cpp | 3 +-- test/test_fc/test_fc.cpp | 3 +-- test/test_input_crsf/test_input_crsf.cpp | 1 + test/test_msp/test_msp.cpp | 3 +-- 5 files changed, 13 insertions(+), 15 deletions(-) diff --git a/lib/AHRS/src/helper_3dmath.h b/lib/AHRS/src/helper_3dmath.h index e1815ac5..829a71a9 100644 --- a/lib/AHRS/src/helper_3dmath.h +++ b/lib/AHRS/src/helper_3dmath.h @@ -28,12 +28,12 @@ class VectorBase; class Quaternion { public: - float w; - float x; - float y; - float z; + float w = 1.f; + float x = 0.f; + float y = 0.f; + float z = 0.f; - Quaternion() : w(1.f), x(0.f), y(0.f), z(0.f) {} + Quaternion() = default; Quaternion(float nw, float nx, float ny, float nz) : w(nw), x(nx), y(ny), z(nz) {} @@ -217,13 +217,13 @@ template class VectorBase { public: - T x; - T y; - T z; + T x = T{}; + T y = T{}; + T z = T{}; VectorBase() = default; VectorBase(const VectorBase& o) = default; - VectorBase(T nx, T ny, T nz) : x(nx), y(ny), z(nz) {} + VectorBase(T nx, T ny, T nz) : x{nx}, y{ny}, z{nz} {} VectorBase& operator=(const VectorBase& o) { diff --git a/test/test_esc/test_esc.cpp b/test/test_esc/test_esc.cpp index e5b7d48d..547237c3 100644 --- a/test/test_esc/test_esc.cpp +++ b/test/test_esc/test_esc.cpp @@ -263,7 +263,6 @@ int main(int argc, char **argv) RUN_TEST(test_esc_extract_telemetry_dshot300_sample); RUN_TEST(test_esc_extract_telemetry_dshot300_running); RUN_TEST(test_esc_extract_telemetry_dshot300_idle); - UNITY_END(); - return 0; + return UNITY_END(); } diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index ccde43c3..b0797108 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -648,7 +648,6 @@ int main(int argc, char **argv) RUN_TEST(test_mixer_throttle_limit_clip); RUN_TEST(test_mixer_output_limit_motor); RUN_TEST(test_mixer_output_limit_servo); - UNITY_END(); - return 0; + return UNITY_END(); } diff --git a/test/test_input_crsf/test_input_crsf.cpp b/test/test_input_crsf/test_input_crsf.cpp index b50e4ec9..501001ff 100644 --- a/test/test_input_crsf/test_input_crsf.cpp +++ b/test/test_input_crsf/test_input_crsf.cpp @@ -424,5 +424,6 @@ int main(int argc, char **argv) RUN_TEST(test_crsf_encode_msp_v1); RUN_TEST(test_crsf_encode_msp_v2); RUN_TEST(test_crsf_decode_msp_v1); + 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 6eb9eff5..aee123b3 100644 --- a/test/test_msp/test_msp.cpp +++ b/test/test_msp/test_msp.cpp @@ -127,7 +127,6 @@ int main(int argc, char **argv) RUN_TEST(test_msp_v2_parse_header); RUN_TEST(test_msp_v2_parse_no_payload); RUN_TEST(test_msp_v2_parse_payload); - UNITY_END(); - return 0; + return UNITY_END(); } \ No newline at end of file From c487cc4cc2aec6820f5f76b8ae13c55d915ad987 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 6 Oct 2024 23:25:27 +0200 Subject: [PATCH 12/68] pico multicore fix --- lib/Espfc/src/Target/TargetESP32.h | 2 +- lib/Espfc/src/Target/TargetESP32c3.h | 2 -- lib/Espfc/src/Target/TargetESP32s2.h | 2 -- lib/Espfc/src/Target/TargetESP32s3.h | 2 -- lib/Espfc/src/Target/TargetESP8266.h | 2 -- lib/Espfc/src/Target/TargetRP2040.h | 7 ++----- lib/Espfc/src/Target/TargetUnitTest.h | 2 -- src/main.cpp | 12 +++++++++--- 8 files changed, 12 insertions(+), 19 deletions(-) diff --git a/lib/Espfc/src/Target/TargetESP32.h b/lib/Espfc/src/Target/TargetESP32.h index 47a4f131..fbaed1a5 100644 --- a/lib/Espfc/src/Target/TargetESP32.h +++ b/lib/Espfc/src/Target/TargetESP32.h @@ -90,4 +90,4 @@ #define ESPFC_DSP -#include "Target/TargetEsp32Common.h" +#include "./TargetEsp32Common.h" diff --git a/lib/Espfc/src/Target/TargetESP32c3.h b/lib/Espfc/src/Target/TargetESP32c3.h index 4ba66808..1b78c69e 100644 --- a/lib/Espfc/src/Target/TargetESP32c3.h +++ b/lib/Espfc/src/Target/TargetESP32c3.h @@ -72,8 +72,6 @@ #define ESPFC_FEATURE_MASK (FEATURE_RX_SERIAL | FEATURE_DYNAMIC_FILTER) -#define ESPFC_GUARD 0 - #define ESPFC_GYRO_I2C_RATE_MAX 1000 #define ESPFC_GYRO_SPI_RATE_MAX 2000 diff --git a/lib/Espfc/src/Target/TargetESP32s2.h b/lib/Espfc/src/Target/TargetESP32s2.h index d379dc91..614933ba 100644 --- a/lib/Espfc/src/Target/TargetESP32s2.h +++ b/lib/Espfc/src/Target/TargetESP32s2.h @@ -69,8 +69,6 @@ #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 2000 diff --git a/lib/Espfc/src/Target/TargetESP32s3.h b/lib/Espfc/src/Target/TargetESP32s3.h index bf6366fd..88650a2e 100644 --- a/lib/Espfc/src/Target/TargetESP32s3.h +++ b/lib/Espfc/src/Target/TargetESP32s3.h @@ -84,8 +84,6 @@ #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 4000 diff --git a/lib/Espfc/src/Target/TargetESP8266.h b/lib/Espfc/src/Target/TargetESP8266.h index ba25375a..2fe6ad41 100644 --- a/lib/Espfc/src/Target/TargetESP8266.h +++ b/lib/Espfc/src/Target/TargetESP8266.h @@ -51,8 +51,6 @@ #define ESPFC_FEATURE_MASK (FEATURE_RX_PPM | FEATURE_DYNAMIC_FILTER) -#define ESPFC_GUARD 1 - #define ESPFC_GYRO_I2C_RATE_MAX 1000 #define ESPFC_GYRO_SPI_RATE_MAX 1000 diff --git a/lib/Espfc/src/Target/TargetRP2040.h b/lib/Espfc/src/Target/TargetRP2040.h index fb8c90d4..ff4d851a 100644 --- a/lib/Espfc/src/Target/TargetRP2040.h +++ b/lib/Espfc/src/Target/TargetRP2040.h @@ -67,17 +67,14 @@ #define ESPFC_FEATURE_MASK (FEATURE_RX_SERIAL | FEATURE_DYNAMIC_FILTER) -#define ESPFC_GUARD 0 - #if defined(ARCH_RP2350) -#define ESPFC_GYRO_I2C_RATE_MAX 1000 #define ESPFC_GYRO_SPI_RATE_MAX 4000 #else -#define ESPFC_GYRO_I2C_RATE_MAX 1000 #define ESPFC_GYRO_SPI_RATE_MAX 1000 +#endif +#define ESPFC_GYRO_I2C_RATE_MAX 1000 #define ESPFC_MULTI_CORE #define ESPFC_MULTI_CORE_RP2040 -#endif #include "Device/SerialDevice.h" #include "Debug_Espfc.h" diff --git a/lib/Espfc/src/Target/TargetUnitTest.h b/lib/Espfc/src/Target/TargetUnitTest.h index 7103ccf5..91461b4a 100644 --- a/lib/Espfc/src/Target/TargetUnitTest.h +++ b/lib/Espfc/src/Target/TargetUnitTest.h @@ -14,8 +14,6 @@ #define ESPFC_OUTPUT_PROTOCOL ESC_PROTOCOL_DISABLED #define ESPFC_FEATURE_MASK (0) -#define ESPFC_GUARD 1 - #define ESPFC_GYRO_I2C_RATE_MAX 2000 #define ESPFC_GYRO_SPI_RATE_MAX 8000 diff --git a/src/main.cpp b/src/main.cpp index 0f9421fc..45f14bce 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -101,19 +101,25 @@ Espfc::Espfc espfc; #elif defined(ESPFC_MULTI_CORE_RP2040) + bool core1_separate_stack = true; + volatile bool setup_done = false; + // RP2040 multicore - void setup1() - { - } + // TODO: https://emalliab.wordpress.com/2021/04/18/raspberry-pi-pico-arduino-core-and-timers/ void setup() { espfc.load(); espfc.begin(); + setup_done = true; } void loop() { espfc.update(); } + void setup1() + { + while(!setup_done); + } void loop1() { espfc.updateOther(); From 1fbbf7fd99a148cdad3b04398a9ae21ab59e293f Mon Sep 17 00:00:00 2001 From: rtlopez Date: Wed, 16 Oct 2024 00:13:32 +0200 Subject: [PATCH 13/68] model config refactor --- lib/Espfc/src/ModelConfig.h | 609 ++++++++++++------------------------ 1 file changed, 199 insertions(+), 410 deletions(-) diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 5fce34e6..2d2ebb97 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -57,27 +57,26 @@ enum FusionMode { FUSION_MAX, }; -class FusionConfig +struct FusionConfig { - public: - int8_t mode; - uint8_t gain; - uint8_t gainI; - - static const char * getModeName(FusionMode mode) - { - if(mode >= FUSION_MAX) return PSTR("?"); - return getModeNames()[mode]; - } - - static const char ** getModeNames() - { - static const char* modeChoices[] = { - PSTR("NONE"), PSTR("MADGWICK"), PSTR("MAHONY"), PSTR("COMPLEMENTARY"), PSTR("KALMAN"), - PSTR("RTQF"), PSTR("LERP"), PSTR("SIMPLE"), PSTR("EXPERIMENTAL"), - NULL }; - return modeChoices; - } + int8_t mode = FUSION_MAHONY; + uint8_t gain = 50; + uint8_t gainI = 5; + + static const char * getModeName(FusionMode mode) + { + if(mode >= FUSION_MAX) return PSTR("?"); + return getModeNames()[mode]; + } + + static const char ** getModeNames() + { + static const char* modeChoices[] = { + PSTR("NONE"), PSTR("MADGWICK"), PSTR("MAHONY"), PSTR("COMPLEMENTARY"), PSTR("KALMAN"), + PSTR("RTQF"), PSTR("LERP"), PSTR("SIMPLE"), PSTR("EXPERIMENTAL"), + NULL }; + return modeChoices; + } }; enum FlightMode { @@ -109,12 +108,11 @@ enum ScalerDimension { const size_t SCALER_COUNT = 3; -class ScalerConfig { - public: - ScalerDimension dimension; - int16_t minScale; - int16_t maxScale; - int8_t channel; +struct ScalerConfig { + uint32_t dimension = 0; + int16_t minScale = 20; + int16_t maxScale = 400; + int8_t channel = 0; }; enum DebugMode { @@ -297,24 +295,22 @@ enum PinFunction { #define ACTUATOR_CONDITIONS 8 -class ActuatorCondition +struct ActuatorCondition { - public: - uint8_t id; - uint8_t ch; - int16_t min; - int16_t max; - uint8_t logicMode; - uint8_t linkId; + uint8_t id = 0; + uint8_t ch = AXIS_AUX_1; + int16_t min = 900; + int16_t max = 900; + uint8_t logicMode = 0; + uint8_t linkId = 0; }; -class SerialPortConfig +struct SerialPortConfig { - public: - int8_t id; - int32_t functionMask; - int32_t baud; - int32_t blackboxBaud; + int8_t id; + int32_t functionMask; + int32_t baud; + int32_t blackboxBaud; }; #define BUZZER_MAX_EVENTS 8 @@ -348,11 +344,10 @@ enum BuzzerEvent { // BUZZER_ALL and BUZZER_PREFERENCE must remain at the bottom of this enum }; -class BuzzerConfig +struct BuzzerConfig { - public: - int8_t inverted; - int32_t beeperMask; + int8_t inverted = true; + int32_t beeperMask; }; enum PidIndex { @@ -387,88 +382,83 @@ enum BlacboxLogField { // no more than 32, sync with FlightLogFieldSelect_e BLACKBOX_FIELD_COUNT }; -class PidConfig +struct PidConfig { - public: - uint8_t P; - uint8_t I; - uint8_t D; - int16_t F; + uint8_t P; + uint8_t I; + uint8_t D; + int16_t F; }; -class InputChannelConfig +struct InputChannelConfig { - public: - int16_t min; - int16_t neutral; - int16_t max; - int8_t map; - int8_t fsMode; - int16_t fsValue; + int16_t min = 1000; + int16_t neutral = 1500; + int16_t max = 2000; + int8_t map = 0; + int8_t fsMode = 0; + int16_t fsValue = 1500; }; -class InputConfig +struct InputConfig { - public: - int8_t ppmMode; - uint8_t serialRxProvider; + int8_t ppmMode = PPM_MODE_NORMAL; + uint8_t serialRxProvider = SERIALRX_SBUS; - int16_t maxCheck; - int16_t minCheck; - int16_t minRc; - int16_t midRc; - int16_t maxRc; + int16_t maxCheck = 1050; + int16_t minCheck = 1900; + int16_t minRc = 885; + int16_t midRc = 1500; + int16_t maxRc = 2115; - int8_t interpolationMode; - int8_t interpolationInterval; - int8_t deadband; + int8_t interpolationMode = INPUT_INTERPOLATION_AUTO; + int8_t interpolationInterval = 26; + int8_t deadband = 3; - int8_t filterType; - int8_t filterAutoFactor; - FilterConfig filter; - FilterConfig filterDerivative; + int8_t filterType = INPUT_FILTER; + int8_t filterAutoFactor = 50; + FilterConfig filter{FILTER_PT3, 0}; + FilterConfig filterDerivative{FILTER_PT3, 0}; - uint8_t expo[3]; - uint8_t rate[3]; - uint8_t superRate[3]; - int16_t rateLimit[3]; - int8_t rateType; + uint8_t expo[3] = { 0, 0, 0 }; + uint8_t rate[3] = { 20, 20, 30 }; + uint8_t superRate[3] = { 40, 40, 36 }; + int16_t rateLimit[3] = { 1998, 1998, 1998 }; + int8_t rateType = 3; - uint8_t rssiChannel; + uint8_t rssiChannel = 0; - InputChannelConfig channel[INPUT_CHANNELS]; + InputChannelConfig channel[INPUT_CHANNELS]; }; -class OutputChannelConfig +struct OutputChannelConfig { - public: - int16_t min; - int16_t neutral; - int16_t max; - bool reverse; - bool servo; + int16_t min = 1000; + int16_t neutral = 1500; + int16_t max = 2000; + bool reverse = false; + bool servo = false; }; -class OutputConfig +struct OutputConfig { - public: - int8_t protocol; - int8_t async; - int8_t dshotTelemetry; - int8_t motorPoles; - int16_t rate; - int16_t servoRate; - - int16_t minCommand; - int16_t minThrottle; - int16_t maxThrottle; - int16_t dshotIdle; - - int8_t throttleLimitType = 0; - int8_t throttleLimitPercent = 100; - int8_t motorLimit = 100; - - OutputChannelConfig channel[ESPFC_OUTPUT_COUNT]; + int8_t protocol = ESC_PROTOCOL_DISABLED; + int8_t async = false; + int8_t dshotTelemetry = false; + int8_t motorPoles = 14; + int16_t rate = 480; + int16_t servoRate = 0; + + int16_t minCommand = 1000; + int16_t minThrottle = 1070; + int16_t maxThrottle = 2000; + int16_t dshotIdle = 550; + + int8_t throttleLimitType = 0; + int8_t throttleLimitPercent = 100; + int8_t motorLimit = 100; + + OutputChannelConfig channel[ESPFC_OUTPUT_COUNT]; }; enum DisarmReason { @@ -515,50 +505,49 @@ enum ArmingDisabledFlags { static const size_t ARMING_DISABLED_FLAGS_COUNT = 25; -class WirelessConfig +struct WirelessConfig { - public: - static const size_t MAX_LEN = 32; - int16_t port; - char ssid[MAX_LEN + 1]; - char pass[MAX_LEN + 1]; + static const size_t MAX_LEN = 32; + int16_t port = 1111; + char ssid[MAX_LEN + 1]; + char pass[MAX_LEN + 1]; }; -class FailsafeConfig +struct FailsafeConfig { - public: - uint8_t delay; - uint8_t killSwitch; + uint8_t delay = 4; + uint8_t killSwitch = 0; }; // persistent data class ModelConfig { public: - int8_t gyroBus; - int8_t gyroDev; - int8_t gyroDlpf; - int8_t gyroAlign; - FilterConfig gyroFilter; - FilterConfig gyroFilter2; - FilterConfig gyroFilter3; - FilterConfig gyroNotch1Filter; - FilterConfig gyroNotch2Filter; - FilterConfig gyroDynLpfFilter; - - int8_t accelBus; - int8_t accelDev; - int8_t accelAlign; - FilterConfig accelFilter; - - int8_t magBus; - int8_t magDev; - int8_t magAlign; - FilterConfig magFilter; - - int8_t baroBus; - int8_t baroDev; - FilterConfig baroFilter; + int8_t gyroBus = BUS_AUTO; + int8_t gyroDev = GYRO_AUTO; + int8_t gyroDlpf = GYRO_DLPF_256; + int8_t gyroAlign = ALIGN_DEFAULT; + + FilterConfig gyroFilter{FILTER_PT1, 100}; + FilterConfig gyroFilter2{FILTER_PT1, 213}; + FilterConfig gyroFilter3{FILTER_FO, 150}; + FilterConfig gyroNotch1Filter{FILTER_NOTCH, 0, 0}; + FilterConfig gyroNotch2Filter{FILTER_NOTCH, 0, 0}; + FilterConfig gyroDynLpfFilter{FILTER_PT1, 425, 170}; + + int8_t accelBus = BUS_AUTO; + int8_t accelDev = GYRO_AUTO; + int8_t accelAlign = ALIGN_DEFAULT; + FilterConfig accelFilter{FILTER_BIQUAD, 15}; + + int8_t magBus = BUS_AUTO; + int8_t magDev = MAG_NONE; + int8_t magAlign = ALIGN_DEFAULT; + FilterConfig magFilter{FILTER_BIQUAD, 10}; + + int8_t baroBus = BUS_AUTO; + int8_t baroDev = BARO_NONE; + FilterConfig baroFilter{FILTER_BIQUAD, 10}; InputConfig input; FailsafeConfig failsafe; @@ -569,87 +558,98 @@ class ModelConfig OutputConfig output; - int8_t mixerType; - bool yawReverse; + int8_t mixerType = FC_MIXER_QUADX; + bool yawReverse = 0; - PidConfig pid[FC_PID_ITEM_COUNT]; + PidConfig pid[FC_PID_ITEM_COUNT] = { + [FC_PID_ROLL] = { .P = 42, .I = 85, .D = 24, .F = 72 }, // ROLL + [FC_PID_PITCH] = { .P = 46, .I = 90, .D = 26, .F = 76 }, // PITCH + [FC_PID_YAW] = { .P = 45, .I = 90, .D = 0, .F = 72 }, // YAW + [FC_PID_ALT] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // ALT + [FC_PID_POS] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // POSHOLD_P * 100, POSHOLD_I * 100, + [FC_PID_POSR] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // POSHOLD_RATE_P * 10, POSHOLD_RATE_I * 100, POSHOLD_RATE_D * 1000, + [FC_PID_NAVR] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // NAV_P * 10, NAV_I * 100, NAV_D * 1000 + [FC_PID_LEVEL] = { .P = 55, .I = 0, .D = 0, .F = 0 }, // LEVEL + [FC_PID_MAG] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // MAG + [FC_PID_VEL] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // VEL + }; - FilterConfig yawFilter; + FilterConfig yawFilter{FILTER_PT1, 90}; - FilterConfig dtermFilter; - FilterConfig dtermFilter2; - FilterConfig dtermNotchFilter; - FilterConfig dtermDynLpfFilter; - FilterConfig levelPtermFilter; + FilterConfig dtermFilter{FILTER_PT1, 128}; + FilterConfig dtermFilter2{FILTER_PT1, 128}; + FilterConfig dtermNotchFilter{FILTER_NOTCH, 0, 0}; + FilterConfig dtermDynLpfFilter{FILTER_PT1, 145, 60}; + FilterConfig levelPtermFilter{FILTER_PT1, 90}; - int16_t dtermSetpointWeight; - int8_t itermLimit; - int8_t itermRelax; - int8_t itermRelaxCutoff; + int16_t dtermSetpointWeight = 30; + int8_t itermLimit = 30; + int8_t itermRelax = ITERM_RELAX_RP; + int8_t itermRelaxCutoff = 15; - int8_t angleLimit; - int16_t angleRateLimit; + int8_t angleLimit = 55; + int16_t angleRateLimit = 300; - int8_t loopSync; - int8_t mixerSync; + int8_t loopSync = 8; // MPU 1000Hz + int8_t mixerSync = 1; - int32_t featureMask; + int32_t featureMask = ESPFC_FEATURE_MASK; - bool lowThrottleZeroIterm; + bool lowThrottleZeroIterm = true; - bool telemetry; - int32_t telemetryInterval; - int8_t telemetryPort; + bool telemetry = 0; + int32_t telemetryInterval = 1000; + int8_t telemetryPort; // unused - int8_t blackboxDev; - int16_t blackboxPdenom; - int32_t blackboxFieldsMask; - int8_t blackboxMode; + int8_t blackboxDev = 0; + int16_t blackboxPdenom = 32; // 1k + int32_t blackboxFieldsMask = 0xffff; + int8_t blackboxMode = 0; SerialPortConfig serial[SERIAL_UART_COUNT]; FusionConfig fusion; - int16_t gyroBias[3]; - int16_t accelBias[3]; - int16_t magCalibrationScale[3]; - int16_t magCalibrationOffset[3]; + int16_t gyroBias[3] = { 0, 0, 0 }; + int16_t accelBias[3] = { 0, 0, 0 }; + int16_t magCalibrationScale[3] = { 0, 0, 0 }; + int16_t magCalibrationOffset[3] = { 1000, 1000, 1000 }; char modelName[MODEL_NAME_LEN + 1]; - int16_t vbatCellWarning; - uint8_t vbatScale; - uint8_t vbatResDiv; - uint8_t vbatResMult; - int8_t vbatSource; + int16_t vbatCellWarning = 350; + uint8_t vbatScale = 100; + uint8_t vbatResDiv = 10; + uint8_t vbatResMult = 1; + int8_t vbatSource = 0; - int8_t ibatSource; - int16_t ibatScale; - int16_t ibatOffset; + int8_t ibatSource = 0; + int16_t ibatScale = 100; + int16_t ibatOffset = 0; - int8_t debugMode; - uint8_t debugAxis; + int8_t debugMode = DEBUG_NONE; + uint8_t debugAxis = 1; BuzzerConfig buzzer; int8_t pin[PIN_COUNT]; - int16_t i2cSpeed; - int8_t tpaScale; - int16_t tpaBreakpoint; + int16_t i2cSpeed = 800; + int8_t tpaScale = 10; + int16_t tpaBreakpoint = 1650; - int8_t customMixerCount; + int8_t customMixerCount = 0; MixerEntry customMixes[MIXER_RULE_MAX]; WirelessConfig wireless; - DynamicFilterConfig dynamicFilter; + DynamicFilterConfig dynamicFilter{4, 300, 80, 400}; - uint8_t rpmFilterHarmonics; - uint8_t rpmFilterMinFreq; - int16_t rpmFilterQ; - uint8_t rpmFilterFreqLpf; - uint8_t rpmFilterWeights[RPM_FILTER_HARMONICS_MAX]; - uint8_t rpmFilterFade; + uint8_t rpmFilterHarmonics = 3; + uint8_t rpmFilterMinFreq = 100; + int16_t rpmFilterQ = 500; + uint8_t rpmFilterFreqLpf = 150; + uint8_t rpmFilterWeights[RPM_FILTER_HARMONICS_MAX] = {100, 100, 100}; + uint8_t rpmFilterFade = 30; uint8_t rescueConfigDelay = 30; @@ -711,63 +711,6 @@ class ModelConfig pin[PIN_SPI_CS1] = ESPFC_SPI_CS_BARO; pin[PIN_SPI_CS2] = -1; #endif - i2cSpeed = 800; - - gyroBus = BUS_AUTO; - gyroDev = GYRO_AUTO; - gyroAlign = ALIGN_DEFAULT; - gyroDlpf = GYRO_DLPF_256; - - loopSync = 8; // MPU 1000Hz - mixerSync = 1; - - accelBus = BUS_AUTO; - accelDev = GYRO_AUTO; - accelAlign = ALIGN_DEFAULT; - - magBus = BUS_AUTO; - magDev = MAG_NONE; - magAlign = ALIGN_DEFAULT; - - baroBus = BUS_AUTO; - baroDev = BARO_NONE; - - fusion.mode = FUSION_MAHONY; - fusion.gain = 50; - fusion.gainI = 5; - - // BF x 0.85 - gyroDynLpfFilter = FilterConfig(FILTER_PT1, 425, 170); - gyroFilter = FilterConfig(FILTER_PT1, 100); - gyroFilter2 = FilterConfig(FILTER_PT1, 213); - dynamicFilter = DynamicFilterConfig(4, 300, 80, 400); // 8%. q:3.0, 80-400 Hz - - dtermDynLpfFilter = FilterConfig(FILTER_PT1, 145, 60); - dtermFilter = FilterConfig(FILTER_PT1, 128); - dtermFilter2 = FilterConfig(FILTER_PT1, 128); - - rpmFilterHarmonics = 3; - rpmFilterMinFreq = 100; - rpmFilterQ = 500; - rpmFilterFade = 30; - rpmFilterWeights[0] = 100; - rpmFilterWeights[1] = 100; - rpmFilterWeights[2] = 100; - rpmFilterFreqLpf = 150; - - gyroFilter3 = FilterConfig(FILTER_FO, 150); - gyroNotch1Filter = FilterConfig(FILTER_NOTCH, 0, 0); // off - gyroNotch2Filter = FilterConfig(FILTER_NOTCH, 0, 0); // off - dtermNotchFilter = FilterConfig(FILTER_NOTCH, 0, 0); // off - - accelFilter = FilterConfig(FILTER_BIQUAD, 15); - magFilter = FilterConfig(FILTER_BIQUAD, 10); - yawFilter = FilterConfig(FILTER_PT1, 90); - levelPtermFilter = FilterConfig(FILTER_PT1, 90); - baroFilter = FilterConfig(FILTER_BIQUAD, 10); - - telemetry = 0; - telemetryInterval = 1000; #ifdef ESPFC_SERIAL_0 serial[SERIAL_UART_0].id = SERIAL_ID_UART_1; @@ -800,46 +743,10 @@ class ModelConfig serial[SERIAL_SOFT_0].blackboxBaud = SERIAL_SPEED_NONE; #endif - // output config - output.minCommand = 1000; - output.minThrottle = 1070; - output.maxThrottle = 2000; - output.dshotIdle = 550; - for(size_t i = 0; i < OUTPUT_CHANNELS; i++) - { - output.channel[i].servo = false; - output.channel[i].reverse = false; - output.channel[i].min = 1000; - output.channel[i].max = 2000; - output.channel[i].neutral = 1500; - } - - mixerType = FC_MIXER_QUADX; - yawReverse = 0; - - output.protocol = ESC_PROTOCOL_DISABLED; - //output.rate = 2000; // max 500 for PWM, 2000 for Oneshot125 - output.rate = 480; // max 500 for PWM, 2000 for Oneshot125 - //output.async = true; - output.async = false; - output.servoRate = 0; // default 50, 0 to disable - output.dshotTelemetry = false; - output.motorPoles = 14; - - // input config - input.ppmMode = PPM_MODE_NORMAL; - input.minCheck = 1050; - input.maxCheck = 1900; - input.minRc = 885; - input.midRc = 1500; - input.maxRc = 2115; - input.rssiChannel = 0; for(size_t i = 0; i < INPUT_CHANNELS; i++) { input.channel[i].map = i; - input.channel[i].min = 1000; input.channel[i].neutral = input.midRc; - input.channel[i].max = 2000; input.channel[i].fsMode = i <= AXIS_THRUST ? 0 : 1; // auto, hold, set input.channel[i].fsValue = i != AXIS_THRUST ? input.midRc : 1000; } @@ -847,134 +754,16 @@ 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 = 3; // actual - - input.rate[AXIS_ROLL] = 20; - input.expo[AXIS_ROLL] = 0; - input.superRate[AXIS_ROLL] = 40; - input.rateLimit[AXIS_ROLL] = 1998; - - input.rate[AXIS_PITCH] = 20; - input.expo[AXIS_PITCH] = 0; - input.superRate[AXIS_PITCH] = 40; - input.rateLimit[AXIS_PITCH] = 1998; - - input.rate[AXIS_YAW] = 30; - input.expo[AXIS_YAW] = 0; - input.superRate[AXIS_YAW] = 36; - input.rateLimit[AXIS_YAW] = 1998; - - input.filterType = INPUT_FILTER; - input.filterAutoFactor = 50; - input.filter = FilterConfig(FILTER_PT3, 0); // 0: auto - input.filterDerivative = FilterConfig(FILTER_PT3, 0); // 0: auto - - input.interpolationMode = INPUT_INTERPOLATION_AUTO; // mode - input.interpolationInterval = 26; - input.deadband = 3; // us - - failsafe.delay = 4; - failsafe.killSwitch = 0; - // PID controller config (BF default) //pid[FC_PID_ROLL] = { .P = 42, .I = 85, .D = 30, .F = 90 }; //pid[FC_PID_PITCH] = { .P = 46, .I = 90, .D = 32, .F = 95 }; //pid[FC_PID_YAW] = { .P = 45, .I = 90, .D = 0, .F = 90 }; //pid[FC_PID_LEVEL] = { .P = 55, .I = 0, .D = 0, .F = 0 }; - // PID controller config (ESPFC default) - pid[FC_PID_ROLL] = { .P = 42, .I = 85, .D = 24, .F = 72 }; - pid[FC_PID_PITCH] = { .P = 46, .I = 90, .D = 26, .F = 76 }; - pid[FC_PID_YAW] = { .P = 45, .I = 90, .D = 0, .F = 72 }; - pid[FC_PID_LEVEL] = { .P = 55, .I = 0, .D = 0, .F = 0 }; - - pid[FC_PID_ALT] = { .P = 0, .I = 0, .D = 0, .F = 0 }; - pid[FC_PID_POS] = { .P = 0, .I = 0, .D = 0, .F = 0 }; // POSHOLD_P * 100, POSHOLD_I * 100, - pid[FC_PID_POSR] = { .P = 0, .I = 0, .D = 0, .F = 0 }; // POSHOLD_RATE_P * 10, POSHOLD_RATE_I * 100, POSHOLD_RATE_D * 1000, - pid[FC_PID_NAVR] = { .P = 0, .I = 0, .D = 0, .F = 0 }; // NAV_P * 10, NAV_I * 100, NAV_D * 1000 - pid[FC_PID_MAG] = { .P = 0, .I = 0, .D = 0, .F = 0 }; - pid[FC_PID_VEL] = { .P = 0, .I = 0, .D = 0, .F = 0 }; - - itermLimit = 30; - itermRelax = ITERM_RELAX_RP; - itermRelaxCutoff = 15; - dtermSetpointWeight = 30; - - angleLimit = 55; // deg - angleRateLimit = 300; // deg - - featureMask = ESPFC_FEATURE_MASK; - - input.serialRxProvider = SERIALRX_SBUS; - - lowThrottleZeroIterm = true; - - tpaScale = 10; - tpaBreakpoint = 1650; - - for(size_t i = 0; i < ACTUATOR_CONDITIONS; i++) - { - conditions[i].id = 0; - conditions[i].ch = AXIS_AUX_1 + 0; - conditions[i].min = 900; - conditions[i].max = 900; - } - - // actuator config - pid scaling - scaler[0].dimension = (ScalerDimension)(0); - scaler[0].channel = 0; - scaler[0].minScale = 20; //% - scaler[0].maxScale = 400; - - scaler[1].dimension = (ScalerDimension)(0); - scaler[1].channel = 0; - scaler[1].minScale = 20; //% - scaler[1].maxScale = 400; - - scaler[2].dimension = (ScalerDimension)(0); - scaler[2].channel = 0; - scaler[2].minScale = 20; //% - scaler[2].maxScale = 200; - - // default callibration values - gyroBias[0] = 0; - gyroBias[1] = 0; - gyroBias[2] = 0; - accelBias[0] = 0; - accelBias[1] = 0; - accelBias[2] = 0; - magCalibrationOffset[0] = 0; - magCalibrationOffset[1] = 0; - magCalibrationOffset[2] = 0; - magCalibrationScale[0] = 1000; - magCalibrationScale[1] = 1000; - magCalibrationScale[2] = 1000; - - vbatSource = 0; - vbatScale = 100; - vbatResDiv = 10; - vbatResMult = 1; - vbatCellWarning = 350; - - ibatSource = 0; - ibatScale = 100; - ibatOffset = 0; - - buzzer.inverted = true; - wireless.ssid[0] = 0; wireless.pass[0] = 0; - wireless.port = 1111; - modelName[0] = 0; - debugMode = DEBUG_NONE; - debugAxis = 1; - blackboxDev = 0; - blackboxPdenom = 32; // 1kHz - blackboxFieldsMask = 0xffff; - blackboxMode = 0; - // development settings #if !defined(ESPFC_REVISION) devPreset(); @@ -1015,11 +804,11 @@ class ModelConfig #endif #ifdef ESPFC_DEV_PRESET_SCALER - scaler[0].dimension = (ScalerDimension)(ACT_INNER_P | ACT_AXIS_ROLL | ACT_AXIS_PITCH); + scaler[0].dimension = (ACT_INNER_P | ACT_AXIS_ROLL | ACT_AXIS_PITCH); scaler[0].channel = AXIS_AUX_1 + 1; - scaler[1].dimension = (ScalerDimension)(ACT_INNER_I | ACT_AXIS_ROLL | ACT_AXIS_PITCH); + scaler[1].dimension = (ACT_INNER_I | ACT_AXIS_ROLL | ACT_AXIS_PITCH); scaler[1].channel = AXIS_AUX_1 + 2; - scaler[2].dimension = (ScalerDimension)(ACT_INNER_D | ACT_AXIS_ROLL | ACT_AXIS_PITCH); + scaler[2].dimension = (ACT_INNER_D | ACT_AXIS_ROLL | ACT_AXIS_PITCH); scaler[2].channel = AXIS_AUX_1 + 3; #endif @@ -1054,10 +843,10 @@ class ModelConfig output.channel[1].servo = true; // ROBOT output.channel[0].reverse = true; // ROBOT - scaler[0].dimension = (ScalerDimension)(ACT_INNER_P | ACT_AXIS_PITCH); // ROBOT - //scaler[1].dimension = (ScalerDimension)(ACT_INNER_P | ACT_AXIS_YAW); // ROBOT - scaler[1].dimension = (ScalerDimension)(ACT_INNER_I | ACT_AXIS_PITCH); // ROBOT - scaler[2].dimension = (ScalerDimension)(ACT_INNER_D | ACT_AXIS_PITCH); // ROBOT + scaler[0].dimension = (ACT_INNER_P | ACT_AXIS_PITCH); // ROBOT + //scaler[1].dimension = (ACT_INNER_P | ACT_AXIS_YAW); // ROBOT + scaler[1].dimension = (ACT_INNER_I | ACT_AXIS_PITCH); // ROBOT + scaler[2].dimension = (ACT_INNER_D | ACT_AXIS_PITCH); // ROBOT } }; From 29901cb57c55930560bb3e754dc0888da5b72e13 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Fri, 18 Oct 2024 23:25:56 +0200 Subject: [PATCH 14/68] target cleanup + buzzer fix --- lib/Espfc/src/Cli.h | 4 ---- lib/Espfc/src/Espfc.cpp | 8 -------- lib/Espfc/src/Espfc.h | 4 ---- lib/Espfc/src/ModelConfig.h | 4 ---- lib/Espfc/src/Target/Target.h | 2 -- lib/Espfc/src/Target/TargetESP32.h | 1 - lib/Espfc/src/Target/TargetESP32c3.h | 1 - lib/Espfc/src/Target/TargetESP32s2.h | 1 - lib/Espfc/src/Target/TargetESP32s3.h | 1 - lib/Espfc/src/Target/TargetESP8266.h | 1 - lib/Espfc/src/Target/TargetNRF52840.h | 1 - lib/Espfc/src/Target/TargetRP2040.h | 1 - lib/Espfc/src/Target/TargetUnitTest.h | 1 + platformio.ini | 11 +---------- 14 files changed, 2 insertions(+), 39 deletions(-) delete mode 100644 lib/Espfc/src/Target/TargetNRF52840.h diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 306ed886..a29c4239 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -691,9 +691,7 @@ class Cli #if ESPFC_OUTPUT_COUNT > 7 Param(PSTR("pin_output_7"), &c.pin[PIN_OUTPUT_7]), #endif -#ifdef ESPFC_BUZZER Param(PSTR("pin_buzzer"), &c.pin[PIN_BUZZER]), -#endif #if defined(ESPFC_SERIAL_0) && defined(ESPFC_SERIAL_REMAP_PINS) Param(PSTR("pin_serial_0_tx"), &c.pin[PIN_SERIAL_0_TX]), Param(PSTR("pin_serial_0_rx"), &c.pin[PIN_SERIAL_0_RX]), @@ -724,9 +722,7 @@ class Cli Param(PSTR("pin_spi_cs_1"), &c.pin[PIN_SPI_CS1]), Param(PSTR("pin_spi_cs_2"), &c.pin[PIN_SPI_CS2]), #endif -#ifdef ESPFC_BUZZER Param(PSTR("pin_buzzer_invert"), &c.buzzer.inverted), -#endif #ifdef ESPFC_I2C_0 Param(PSTR("i2c_speed"), &c.i2cSpeed), diff --git a/lib/Espfc/src/Espfc.cpp b/lib/Espfc/src/Espfc.cpp index ca807af8..b2e2e729 100644 --- a/lib/Espfc/src/Espfc.cpp +++ b/lib/Espfc/src/Espfc.cpp @@ -6,9 +6,7 @@ namespace Espfc { Espfc::Espfc(): _hardware(_model), _controller(_model), _telemetry(_model), _input(_model, _telemetry), _actuator(_model), _sensor(_model), _mixer(_model), _blackbox(_model) -#ifdef ESPFC_BUZER , _buzzer(_model) -#endif , _serial(_model, _telemetry) {} @@ -32,9 +30,7 @@ int Espfc::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; @@ -65,9 +61,7 @@ int FAST_CODE_ATTR Espfc::update(bool externalTrigger) } _serial.update(); -#ifdef ESPFC_BUZER _buzzer.update(); -#endif _model.state.stats.update(); #else @@ -93,9 +87,7 @@ int FAST_CODE_ATTR Espfc::update(bool externalTrigger) _sensor.updateDelayed(); _serial.update(); -#ifdef ESPFC_BUZER _buzzer.update(); -#endif _model.state.stats.update(); #endif diff --git a/lib/Espfc/src/Espfc.h b/lib/Espfc/src/Espfc.h index a6a45f8e..e25f1dec 100644 --- a/lib/Espfc/src/Espfc.h +++ b/lib/Espfc/src/Espfc.h @@ -10,9 +10,7 @@ #include "SerialManager.h" #include "Output/Mixer.h" #include "Blackbox/Blackbox.h" -#ifdef ESPFC_BUZER #include "Buzzer.h" -#endif namespace Espfc { @@ -41,9 +39,7 @@ class Espfc SensorManager _sensor; Output::Mixer _mixer; Blackbox::Blackbox _blackbox; -#ifdef ESPFC_BUZER Buzzer _buzzer; -#endif SerialManager _serial; uint32_t _loop_next; }; diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 2d2ebb97..9a9ab905 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -255,9 +255,7 @@ enum PinFunction { #if ESPFC_OUTPUT_COUNT > 7 PIN_OUTPUT_7, #endif -#ifdef ESPFC_BUZZER PIN_BUZZER, -#endif #ifdef ESPFC_SERIAL_0 PIN_SERIAL_0_TX, PIN_SERIAL_0_RX, @@ -676,9 +674,7 @@ class ModelConfig #if ESPFC_OUTPUT_COUNT > 7 pin[PIN_OUTPUT_7] = ESPFC_OUTPUT_7; #endif -#ifdef ESPFC_BUZZER pin[PIN_BUZZER] = ESPFC_BUZZER_PIN; -#endif #ifdef ESPFC_SERIAL_0 pin[PIN_SERIAL_0_TX] = ESPFC_SERIAL_0_TX; pin[PIN_SERIAL_0_RX] = ESPFC_SERIAL_0_RX; diff --git a/lib/Espfc/src/Target/Target.h b/lib/Espfc/src/Target/Target.h index 35ebbdd5..a7654ec7 100644 --- a/lib/Espfc/src/Target/Target.h +++ b/lib/Espfc/src/Target/Target.h @@ -12,8 +12,6 @@ #include "TargetESP8266.h" #elif defined(ARCH_RP2040) #include "TargetRP2040.h" -#elif defined(ARCH_NRF52840) - #include "TargetNRF52840.h" #elif defined(UNIT_TEST) #include "TargetUnitTest.h" #else diff --git a/lib/Espfc/src/Target/TargetESP32.h b/lib/Espfc/src/Target/TargetESP32.h index fbaed1a5..faa270bf 100644 --- a/lib/Espfc/src/Target/TargetESP32.h +++ b/lib/Espfc/src/Target/TargetESP32.h @@ -62,7 +62,6 @@ #define ESPFC_I2C_0_SDA 21 #define ESPFC_I2C_0_SOFT -#define ESPFC_BUZZER #define ESPFC_BUZZER_PIN 0 #define ESPFC_ADC_0 diff --git a/lib/Espfc/src/Target/TargetESP32c3.h b/lib/Espfc/src/Target/TargetESP32c3.h index 1b78c69e..555e2348 100644 --- a/lib/Espfc/src/Target/TargetESP32c3.h +++ b/lib/Espfc/src/Target/TargetESP32c3.h @@ -59,7 +59,6 @@ #define ESPFC_I2C_0_SDA 8 #define ESPFC_I2C_0_SOFT -#define ESPFC_BUZZER #define ESPFC_BUZZER_PIN -1 #define ESPFC_ADC_0 diff --git a/lib/Espfc/src/Target/TargetESP32s2.h b/lib/Espfc/src/Target/TargetESP32s2.h index 614933ba..1d302662 100644 --- a/lib/Espfc/src/Target/TargetESP32s2.h +++ b/lib/Espfc/src/Target/TargetESP32s2.h @@ -56,7 +56,6 @@ #define ESPFC_I2C_0_SDA 9 #define ESPFC_I2C_0_SOFT -#define ESPFC_BUZZER #define ESPFC_BUZZER_PIN 5 #define ESPFC_ADC_0 diff --git a/lib/Espfc/src/Target/TargetESP32s3.h b/lib/Espfc/src/Target/TargetESP32s3.h index 88650a2e..15efedc4 100644 --- a/lib/Espfc/src/Target/TargetESP32s3.h +++ b/lib/Espfc/src/Target/TargetESP32s3.h @@ -71,7 +71,6 @@ #define ESPFC_I2C_0_SDA 9 #define ESPFC_I2C_0_SOFT -#define ESPFC_BUZZER #define ESPFC_BUZZER_PIN 5 #define ESPFC_ADC_0 diff --git a/lib/Espfc/src/Target/TargetESP8266.h b/lib/Espfc/src/Target/TargetESP8266.h index 2fe6ad41..3e3e841b 100644 --- a/lib/Espfc/src/Target/TargetESP8266.h +++ b/lib/Espfc/src/Target/TargetESP8266.h @@ -41,7 +41,6 @@ #define ESPFC_I2C_0_SDA 4 // D2 #define ESPFC_I2C_0_SOFT -#define ESPFC_BUZZER #define ESPFC_BUZZER_PIN 16 // D0 #define ESPFC_ADC_0 diff --git a/lib/Espfc/src/Target/TargetNRF52840.h b/lib/Espfc/src/Target/TargetNRF52840.h deleted file mode 100644 index 7b9637ef..00000000 --- a/lib/Espfc/src/Target/TargetNRF52840.h +++ /dev/null @@ -1 +0,0 @@ -#pragma once \ No newline at end of file diff --git a/lib/Espfc/src/Target/TargetRP2040.h b/lib/Espfc/src/Target/TargetRP2040.h index ff4d851a..c73de2a1 100644 --- a/lib/Espfc/src/Target/TargetRP2040.h +++ b/lib/Espfc/src/Target/TargetRP2040.h @@ -54,7 +54,6 @@ #define ESPFC_I2C_0_SDA 16 #define ESPFC_I2C_0_SCL 17 -#define ESPFC_BUZZER #define ESPFC_BUZZER_PIN -1 #define ESPFC_ADC_0 diff --git a/lib/Espfc/src/Target/TargetUnitTest.h b/lib/Espfc/src/Target/TargetUnitTest.h index 91461b4a..f4793aaa 100644 --- a/lib/Espfc/src/Target/TargetUnitTest.h +++ b/lib/Espfc/src/Target/TargetUnitTest.h @@ -20,6 +20,7 @@ #define SERIAL_TX_FIFO_SIZE 0xFF #define ESPFC_SERIAL_DEBUG_PORT 0 +#define ESPFC_BUZZER_PIN -1 inline void targetReset() { diff --git a/platformio.ini b/platformio.ini index 391a003a..91cd381d 100644 --- a/platformio.ini +++ b/platformio.ini @@ -151,16 +151,7 @@ build_flags = -DIRAM_ATTR="" ; -DIRAM_ATTR='__attribute__ ((section(".time_critical.iram_attr")))' -[env:nrf52840] -platform = https://github.com/maxgerhardt/platform-nordicnrf52 -board = xiaoblesense -framework = arduino -lib_deps = -build_flags = - ${env.build_flags} - -DARCH_NRF52840 - -DIRAM_ATTR="" - +; target for unit tests [env:native] platform = native lib_deps = From bb9d641b88671f63aab8b40127d75a5348c738d1 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 26 Oct 2024 21:43:44 +0200 Subject: [PATCH 15/68] code tidy --- lib/Espfc/src/Espfc.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/lib/Espfc/src/Espfc.cpp b/lib/Espfc/src/Espfc.cpp index b2e2e729..d8c3e77e 100644 --- a/lib/Espfc/src/Espfc.cpp +++ b/lib/Espfc/src/Espfc.cpp @@ -4,10 +4,8 @@ namespace Espfc { Espfc::Espfc(): - _hardware(_model), _controller(_model), _telemetry(_model), _input(_model, _telemetry), _actuator(_model), _sensor(_model), - _mixer(_model), _blackbox(_model) - , _buzzer(_model) - , _serial(_model, _telemetry) + _hardware{_model}, _controller{_model}, _telemetry{_model}, _input{_model, _telemetry}, _actuator{_model}, _sensor{_model}, + _mixer{_model}, _blackbox{_model}, _buzzer{_model}, _serial{_model, _telemetry} {} int Espfc::load() From 36358c466bf2b9e7cee64bc5c7c5f0f277627e18 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 26 Oct 2024 22:47:35 +0200 Subject: [PATCH 16/68] blackbox config refactor --- lib/Espfc/src/Blackbox/Blackbox.cpp | 18 ++++----- lib/Espfc/src/Cli.h | 35 ++++++++--------- lib/Espfc/src/Model.h | 2 +- lib/Espfc/src/ModelConfig.h | 58 +++++++++++++++++------------ lib/Espfc/src/Msp/MspProcessor.h | 20 +++++----- 5 files changed, 73 insertions(+), 60 deletions(-) diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index 70ed645a..cf4a7431 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -27,7 +27,7 @@ int Blackbox::begin() if(!_model.blackboxEnabled()) return 0; - if(_model.config.blackboxDev == 3) + if(_model.config.blackbox.dev == BLACKBOX_DEV_SERIAL) { _serial = _model.getSerialStream(SERIAL_FUNCTION_BLACKBOX); if(!_serial) return 0; @@ -156,17 +156,17 @@ int Blackbox::begin() targetPidLooptime = _model.state.loopTimer.interval; activePidLoopDenom = _model.config.loopSync; - if(_model.config.blackboxPdenom >= 0 && _model.config.blackboxPdenom <= 4) + if(_model.config.blackbox.pDenom >= 0 && _model.config.blackbox.pDenom <= 4) { - blackboxConfigMutable()->sample_rate = _model.config.blackboxPdenom; + blackboxConfigMutable()->sample_rate = _model.config.blackbox.pDenom; } else { - blackboxConfigMutable()->sample_rate = blackboxCalculateSampleRate(_model.config.blackboxPdenom); + blackboxConfigMutable()->sample_rate = blackboxCalculateSampleRate(_model.config.blackbox.pDenom); } - blackboxConfigMutable()->device = _model.config.blackboxDev; - blackboxConfigMutable()->fields_disabled_mask = ~_model.config.blackboxFieldsMask; - blackboxConfigMutable()->mode = _model.config.blackboxMode; + blackboxConfigMutable()->device = _model.config.blackbox.dev; + blackboxConfigMutable()->fields_disabled_mask = ~_model.config.blackbox.fieldsMask; + blackboxConfigMutable()->mode = _model.config.blackbox.mode; featureConfigMutable()->enabledFeatures = _model.config.featureMask; @@ -206,7 +206,7 @@ int Blackbox::begin() int FAST_CODE_ATTR Blackbox::update() { if(!_model.blackboxEnabled()) return 0; - if(_model.config.blackboxDev == 3 && !_serial) return 0; + if(_model.config.blackbox.dev == BLACKBOX_DEV_SERIAL && !_serial) return 0; Stats::Measure measure(_model.state.stats, COUNTER_BLACKBOX); @@ -219,7 +219,7 @@ int FAST_CODE_ATTR Blackbox::update() } //PIN_DEBUG(HIGH); blackboxUpdate(_model.state.loopTimer.last); - if(_model.config.blackboxDev == 3) + if(_model.config.blackbox.dev == BLACKBOX_DEV_SERIAL) { _buffer.flush(); } diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index a29c4239..57268b3c 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -415,6 +415,7 @@ class Cli static const char* voltageSourceChoices[] = { PSTR("NONE"), PSTR("ADC"), NULL }; static const char* currentSourceChoices[] = { PSTR("NONE"), PSTR("ADC"), NULL }; + static const char* blackboxDevChoices[] = { PSTR("NONE"), PSTR("FLASH"), PSTR("SD_CARD"), PSTR("SERIAL"), NULL }; static const char* blackboxModeChoices[] = { PSTR("NORMAL"), PSTR("TEST"), PSTR("ALWAYS"), NULL }; size_t i = 0; @@ -732,23 +733,23 @@ class Cli //Param(PSTR("telemetry"), &c.telemetry), Param(PSTR("telemetry_interval"), &c.telemetryInterval), - Param(PSTR("blackbox_dev"), &c.blackboxDev), - Param(PSTR("blackbox_mode"), &c.blackboxMode, blackboxModeChoices), - Param(PSTR("blackbox_rate"), &c.blackboxPdenom), - Param(PSTR("blackbox_log_acc"), &c.blackboxFieldsMask, BLACKBOX_FIELD_ACC), - Param(PSTR("blackbox_log_alt"), &c.blackboxFieldsMask, BLACKBOX_FIELD_ALTITUDE), - Param(PSTR("blackbox_log_bat"), &c.blackboxFieldsMask, BLACKBOX_FIELD_BATTERY), - Param(PSTR("blackbox_log_debug"), &c.blackboxFieldsMask, BLACKBOX_FIELD_DEBUG_LOG), - Param(PSTR("blackbox_log_gps"), &c.blackboxFieldsMask, BLACKBOX_FIELD_GPS), - Param(PSTR("blackbox_log_gyro"), &c.blackboxFieldsMask, BLACKBOX_FIELD_GYRO), - Param(PSTR("blackbox_log_gyro_raw"), &c.blackboxFieldsMask, BLACKBOX_FIELD_GYROUNFILT), - Param(PSTR("blackbox_log_mag"), &c.blackboxFieldsMask, BLACKBOX_FIELD_MAG), - Param(PSTR("blackbox_log_motor"), &c.blackboxFieldsMask, BLACKBOX_FIELD_MOTOR), - Param(PSTR("blackbox_log_pid"), &c.blackboxFieldsMask, BLACKBOX_FIELD_PID), - Param(PSTR("blackbox_log_rc"), &c.blackboxFieldsMask, BLACKBOX_FIELD_RC_COMMANDS), - Param(PSTR("blackbox_log_rpm"), &c.blackboxFieldsMask, BLACKBOX_FIELD_RPM), - Param(PSTR("blackbox_log_rssi"), &c.blackboxFieldsMask, BLACKBOX_FIELD_RSSI), - Param(PSTR("blackbox_log_sp"), &c.blackboxFieldsMask, BLACKBOX_FIELD_SETPOINT), + Param(PSTR("blackbox_dev"), &c.blackbox.dev, blackboxDevChoices), + Param(PSTR("blackbox_mode"), &c.blackbox.mode, blackboxModeChoices), + Param(PSTR("blackbox_rate"), &c.blackbox.pDenom), + Param(PSTR("blackbox_log_acc"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_ACC), + Param(PSTR("blackbox_log_alt"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_ALTITUDE), + Param(PSTR("blackbox_log_bat"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_BATTERY), + Param(PSTR("blackbox_log_debug"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_DEBUG_LOG), + Param(PSTR("blackbox_log_gps"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_GPS), + Param(PSTR("blackbox_log_gyro"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_GYRO), + Param(PSTR("blackbox_log_gyro_raw"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_GYROUNFILT), + Param(PSTR("blackbox_log_mag"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_MAG), + Param(PSTR("blackbox_log_motor"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_MOTOR), + Param(PSTR("blackbox_log_pid"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_PID), + Param(PSTR("blackbox_log_rc"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_RC_COMMANDS), + Param(PSTR("blackbox_log_rpm"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_RPM), + Param(PSTR("blackbox_log_rssi"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_RSSI), + Param(PSTR("blackbox_log_sp"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_SETPOINT), #ifdef ESPFC_SERIAL_SOFT_0_WIFI Param(PSTR("wifi_ssid"), PARAM_STRING, &c.wireless.ssid[0], NULL, 32), diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index b80844d2..8243eb53 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -104,7 +104,7 @@ class Model bool blackboxEnabled() const { // serial or flash - return (config.blackboxDev == 3 || config.blackboxDev == 1) && config.blackboxPdenom > 0; + return (config.blackbox.dev == BLACKBOX_DEV_SERIAL || config.blackbox.dev == BLACKBOX_DEV_FLASH) && config.blackbox.pDenom > 0; } bool gyroActive() const /* IRAM_ATTR */ diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 9a9ab905..8e0a7581 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -87,7 +87,7 @@ enum FlightMode { MODE_FAILSAFE, MODE_BLACKBOX, MODE_BLACKBOX_ERASE, - MODE_COUNT + MODE_COUNT, }; enum ScalerDimension { @@ -103,10 +103,10 @@ enum ScalerDimension { ACT_AXIS_PITCH = 1 << 9, // 512 ACT_AXIS_YAW = 1 << 10, // 1024 ACT_AXIS_THRUST = 1 << 11, // 2048 - ACT_GYRO_THRUST = 1 << 12 // 4096 + ACT_GYRO_THRUST = 1 << 12, // 4096 }; -const size_t SCALER_COUNT = 3; +constexpr size_t SCALER_COUNT = 3; struct ScalerConfig { uint32_t dimension = 0; @@ -179,7 +179,7 @@ enum DebugMode { DEBUG_BLACKBOX_OUTPUT, DEBUG_GYRO_SAMPLE, DEBUG_RX_TIMING, - DEBUG_COUNT + DEBUG_COUNT, }; enum Axis { @@ -199,7 +199,7 @@ enum Axis { AXIS_AUX_10, AXIS_AUX_11, AXIS_AUX_12, - AXIS_COUNT + AXIS_COUNT, }; enum Feature { @@ -222,18 +222,18 @@ enum InputInterpolation { enum InputFilterType : uint8_t { INPUT_INTERPOLATION, - INPUT_FILTER + INPUT_FILTER, }; -const size_t MODEL_NAME_LEN = 16; -const size_t AXES = 4; -const size_t AXES_RPY = 3; -const size_t INPUT_CHANNELS = AXIS_COUNT; -const size_t OUTPUT_CHANNELS = ESC_CHANNEL_COUNT; +constexpr size_t MODEL_NAME_LEN = 16; +constexpr size_t AXES = 4; +constexpr size_t AXES_RPY = 3; +constexpr size_t INPUT_CHANNELS = AXIS_COUNT; +constexpr size_t OUTPUT_CHANNELS = ESC_CHANNEL_COUNT; static_assert(ESC_CHANNEL_COUNT == ESPFC_OUTPUT_COUNT, "ESC_CHANNEL_COUNT and ESPFC_OUTPUT_COUNT must be equal"); -const size_t RPM_FILTER_MOTOR_MAX = 4; -const size_t RPM_FILTER_HARMONICS_MAX = 3; +constexpr size_t RPM_FILTER_MOTOR_MAX = 4; +constexpr size_t RPM_FILTER_HARMONICS_MAX = 3; enum PinFunction { #ifdef ESPFC_INPUT @@ -288,10 +288,10 @@ enum PinFunction { PIN_SPI_CS1, PIN_SPI_CS2, #endif - PIN_COUNT + PIN_COUNT, }; -#define ACTUATOR_CONDITIONS 8 +constexpr size_t ACTUATOR_CONDITIONS = 8; struct ActuatorCondition { @@ -311,7 +311,7 @@ struct SerialPortConfig int32_t blackboxBaud; }; -#define BUZZER_MAX_EVENTS 8 +constexpr size_t BUZZER_MAX_EVENTS = 8; enum BuzzerEvent { BUZZER_SILENCE = 0, // Silence, see beeperSilence() @@ -359,7 +359,7 @@ enum PidIndex { FC_PID_LEVEL, FC_PID_MAG, FC_PID_VEL, - FC_PID_ITEM_COUNT + FC_PID_ITEM_COUNT, }; enum BlacboxLogField { // no more than 32, sync with FlightLogFieldSelect_e @@ -377,7 +377,14 @@ enum BlacboxLogField { // no more than 32, sync with FlightLogFieldSelect_e BLACKBOX_FIELD_GPS, BLACKBOX_FIELD_RPM, BLACKBOX_FIELD_GYROUNFILT, - BLACKBOX_FIELD_COUNT + BLACKBOX_FIELD_COUNT, +}; + +enum BlackboxLogDevice { + BLACKBOX_DEV_NONE = 0, + BLACKBOX_DEV_FLASH = 1, + BLACKBOX_DEV_SDCARD = 2, + BLACKBOX_DEV_SERIAL = 3, }; struct PidConfig @@ -517,6 +524,14 @@ struct FailsafeConfig uint8_t killSwitch = 0; }; +struct BlackboxConfig +{ + int8_t dev = 0; + int16_t pDenom = 32; // 1k + int32_t fieldsMask = 0xffff; + int8_t mode = 0; +}; + // persistent data class ModelConfig { @@ -599,10 +614,7 @@ class ModelConfig int32_t telemetryInterval = 1000; int8_t telemetryPort; // unused - int8_t blackboxDev = 0; - int16_t blackboxPdenom = 32; // 1k - int32_t blackboxFieldsMask = 0xffff; - int8_t blackboxMode = 0; + BlackboxConfig blackbox; SerialPortConfig serial[SERIAL_UART_COUNT]; @@ -769,7 +781,7 @@ class ModelConfig void devPreset() { #ifdef ESPFC_DEV_PRESET_BLACKBOX - blackboxDev = 3; // serial + blackbox.dev = BLACKBOX_DEV_SERIAL; // serial debugMode = DEBUG_GYRO_SCALED; serial[ESPFC_DEV_PRESET_BLACKBOX].functionMask |= SERIAL_FUNCTION_BLACKBOX; serial[ESPFC_DEV_PRESET_BLACKBOX].blackboxBaud = SERIAL_SPEED_250000; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index c9275db3..dd4b5f76 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -642,18 +642,18 @@ class MspProcessor case MSP_BLACKBOX_CONFIG: r.writeU8(1); // Blackbox supported - r.writeU8(_model.config.blackboxDev); // device serial or none + r.writeU8(_model.config.blackbox.dev); // device serial or none r.writeU8(1); // blackboxGetRateNum()); // unused r.writeU8(1); // blackboxGetRateDenom()); - r.writeU16(_model.config.blackboxPdenom);//blackboxGetPRatio()); // p_denom - //r.writeU8(_model.config.blackboxPdenom); // sample_rate - //r.writeU32(~_model.config.blackboxFieldsMask); + r.writeU16(_model.config.blackbox.pDenom);//blackboxGetPRatio()); // p_denom + //r.writeU8(_model.config.blackbox.pDenom); // sample_rate + //r.writeU32(~_model.config.blackbox.fieldsMask); break; case MSP_SET_BLACKBOX_CONFIG: // Don't allow config to be updated while Blackbox is logging if (true) { - _model.config.blackboxDev = m.readU8(); + _model.config.blackbox.dev = m.readU8(); const int rateNum = m.readU8(); // was rate_num const int rateDenom = m.readU8(); // was rate_denom uint16_t pRatio = 0; @@ -664,16 +664,16 @@ class MspProcessor //pRatio = blackboxCalculatePDenom(rateNum, rateDenom); (void)(rateNum + rateDenom); } - _model.config.blackboxPdenom = pRatio; + _model.config.blackbox.pDenom = pRatio; /*if (m.remain() >= 1) { - _model.config.blackboxPdenom = m.readU8(); + _model.config.blackbox.pDenom = m.readU8(); } else if(pRatio > 0) { - _model.config.blackboxPdenom = blackboxCalculateSampleRate(pRatio); - //_model.config.blackboxPdenom = pRatio; + _model.config.blackbox.pDenom = blackboxCalculateSampleRate(pRatio); + //_model.config.blackbox.pDenom = pRatio; } if (m.remain() >= 4) { - _model.config.blackboxFieldsMask = ~m.readU32(); + _model.config.blackbox.fieldsMask = ~m.readU32(); }*/ } break; From 9c9e6923900e7cf7cf38689202f49eef20f90799 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 26 Oct 2024 22:57:29 +0200 Subject: [PATCH 17/68] update CI --- .github/workflows/platformio.yml | 46 +++++++++++--------------------- 1 file changed, 16 insertions(+), 30 deletions(-) diff --git a/.github/workflows/platformio.yml b/.github/workflows/platformio.yml index 11370d6c..249e104b 100644 --- a/.github/workflows/platformio.yml +++ b/.github/workflows/platformio.yml @@ -9,35 +9,28 @@ on: jobs: test: - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - uses: actions/checkout@v4 - - name: Cache pip + - name: Cache Pio uses: actions/cache@v4 with: - path: ~/.cache/pip - key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }} - restore-keys: | - ${{ runner.os }}-pip- - - name: Cache PlatformIO - uses: actions/cache@v4 - with: - path: ~/.platformio - key: ${{ runner.os }}-${{ hashFiles('**/lockfiles') }} + path: | + ~/.cache/pip + ~/.platformio/.cache + key: ${{ runner.os }}-pio - name: Set up Python uses: actions/setup-python@v5 with: - python-version: '3.10' + python-version: '3.11' - name: Install Dependencies - run: | - python -m pip install --upgrade pip - pip install platformio + run: pip install --upgrade platformio - name: Run PlatformIO Test run: platformio test -e native build: needs: test - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 strategy: matrix: target: ['esp32', 'esp32s2', 'esp32s3', 'esp32c3', 'esp8266', 'rp2040', 'rp2350'] @@ -61,26 +54,19 @@ jobs: echo NAME: ${{ env.build_name }} echo DEVEL: ${{ env.build_file_devel }} echo RELEASE: ${{ env.build_file_release }} - - name: Cache pip + - name: Cache Pio uses: actions/cache@v4 with: - path: ~/.cache/pip - key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }} - restore-keys: | - ${{ runner.os }}-pip- - - name: Cache PlatformIO - uses: actions/cache@v4 - with: - path: ~/.platformio - key: ${{ runner.os }}-${{ hashFiles('**/lockfiles') }} + path: | + ~/.cache/pip + ~/.platformio/.cache + key: ${{ runner.os }}-pio - name: Set up Python uses: actions/setup-python@v5 with: - python-version: '3.10' + python-version: '3.11' - name: Install Dependencies - run: | - python -m pip install --upgrade pip - pip install platformio + run: pip install --upgrade platformio - name: Build Development Target From fa1dffd0d8ae36966aae482b445a15d0d8481aaa Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 26 Oct 2024 23:13:27 +0200 Subject: [PATCH 18/68] rpm filter config refactor --- lib/Espfc/src/Blackbox/Blackbox.cpp | 16 ++++++++-------- lib/Espfc/src/Cli.h | 16 ++++++++-------- lib/Espfc/src/Model.h | 6 +++--- lib/Espfc/src/ModelConfig.h | 17 +++++++++++------ lib/Espfc/src/Msp/MspProcessor.h | 8 ++++---- lib/Espfc/src/Sensor/GyroSensor.cpp | 16 ++++++++-------- 6 files changed, 42 insertions(+), 37 deletions(-) diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index cf4a7431..8e6a9d67 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -182,14 +182,14 @@ int Blackbox::begin() 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]; + rpmFilterConfigMutable()->rpm_filter_harmonics = _model.config.rpmFilter.harmonics; + rpmFilterConfigMutable()->rpm_filter_q = _model.config.rpmFilter.q; + rpmFilterConfigMutable()->rpm_filter_min_hz = _model.config.rpmFilter.minFreq; + rpmFilterConfigMutable()->rpm_filter_fade_range_hz = _model.config.rpmFilter.fade; + rpmFilterConfigMutable()->rpm_filter_lpf_hz = _model.config.rpmFilter.freqLpf; + rpmFilterConfigMutable()->rpm_filter_weights[0] = _model.config.rpmFilter.weights[0]; + rpmFilterConfigMutable()->rpm_filter_weights[1] = _model.config.rpmFilter.weights[1]; + rpmFilterConfigMutable()->rpm_filter_weights[2] = _model.config.rpmFilter.weights[2]; updateModeFlag(&rcModeActivationPresent, BOXARM, _model.state.modeMaskPresent & 1 << MODE_ARMED); updateModeFlag(&rcModeActivationPresent, BOXANGLE, _model.state.modeMaskPresent & 1 << MODE_ANGLE); diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 57268b3c..a9cef0df 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -452,14 +452,14 @@ class Cli Param(PSTR("gyro_dyn_notch_count"), &c.dynamicFilter.width), Param(PSTR("gyro_dyn_notch_min"), &c.dynamicFilter.min_freq), Param(PSTR("gyro_dyn_notch_max"), &c.dynamicFilter.max_freq), - Param(PSTR("gyro_rpm_harmonics"), &c.rpmFilterHarmonics), - Param(PSTR("gyro_rpm_q"), &c.rpmFilterQ), - Param(PSTR("gyro_rpm_min_freq"), &c.rpmFilterMinFreq), - Param(PSTR("gyro_rpm_fade"), &c.rpmFilterFade), - Param(PSTR("gyro_rpm_weight_1"), &c.rpmFilterWeights[0]), - Param(PSTR("gyro_rpm_weight_2"), &c.rpmFilterWeights[1]), - Param(PSTR("gyro_rpm_weight_3"), &c.rpmFilterWeights[2]), - Param(PSTR("gyro_rpm_tlm_lpf_freq"), &c.rpmFilterFreqLpf), + Param(PSTR("gyro_rpm_harmonics"), &c.rpmFilter.harmonics), + Param(PSTR("gyro_rpm_q"), &c.rpmFilter.q), + Param(PSTR("gyro_rpm_min_freq"), &c.rpmFilter.minFreq), + Param(PSTR("gyro_rpm_fade"), &c.rpmFilter.fade), + Param(PSTR("gyro_rpm_weight_1"), &c.rpmFilter.weights[0]), + Param(PSTR("gyro_rpm_weight_2"), &c.rpmFilter.weights[1]), + Param(PSTR("gyro_rpm_weight_3"), &c.rpmFilter.weights[2]), + Param(PSTR("gyro_rpm_tlm_lpf_freq"), &c.rpmFilter.freqLpf), Param(PSTR("gyro_offset_x"), &c.gyroBias[0]), Param(PSTR("gyro_offset_y"), &c.gyroBias[1]), Param(PSTR("gyro_offset_z"), &c.gyroBias[2]), diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 8243eb53..c41df055 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -477,10 +477,10 @@ class Model state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accelTimer.rate / 3), gyroFilterRate); for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) { - state.rpmFreqFilter[m].begin(FilterConfig(FILTER_PT1, config.rpmFilterFreqLpf), gyroFilterRate); - for(size_t n = 0; n < config.rpmFilterHarmonics; n++) + state.rpmFreqFilter[m].begin(FilterConfig(FILTER_PT1, config.rpmFilter.freqLpf), gyroFilterRate); + for(size_t n = 0; n < config.rpmFilter.harmonics; n++) { - int center = Math::mapi(m * RPM_FILTER_HARMONICS_MAX + n, 0, RPM_FILTER_MOTOR_MAX * config.rpmFilterHarmonics, config.rpmFilterMinFreq, gyroFilterRate / 2); + int center = Math::mapi(m * RPM_FILTER_HARMONICS_MAX + n, 0, RPM_FILTER_MOTOR_MAX * config.rpmFilter.harmonics, config.rpmFilter.minFreq, gyroFilterRate / 2); state.rpmFilter[m][n][i].begin(FilterConfig(FILTER_NOTCH_DF1, center, center * 0.98f), gyroFilterRate); } } diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 8e0a7581..74332b65 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -532,6 +532,16 @@ struct BlackboxConfig int8_t mode = 0; }; +struct RpmFilterConfig +{ + uint8_t harmonics = 3; + uint8_t minFreq = 100; + int16_t q = 500; + uint8_t freqLpf = 150; + uint8_t weights[RPM_FILTER_HARMONICS_MAX] = {100, 100, 100}; + uint8_t fade = 30; +}; + // persistent data class ModelConfig { @@ -654,12 +664,7 @@ class ModelConfig DynamicFilterConfig dynamicFilter{4, 300, 80, 400}; - uint8_t rpmFilterHarmonics = 3; - uint8_t rpmFilterMinFreq = 100; - int16_t rpmFilterQ = 500; - uint8_t rpmFilterFreqLpf = 150; - uint8_t rpmFilterWeights[RPM_FILTER_HARMONICS_MAX] = {100, 100, 100}; - uint8_t rpmFilterFade = 30; + RpmFilterConfig rpmFilter; uint8_t rescueConfigDelay = 30; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index dd4b5f76..01a05320 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -1089,8 +1089,8 @@ class MspProcessor r.writeU16(_model.config.dynamicFilter.q); // dyn_notch_q r.writeU16(_model.config.dynamicFilter.min_freq); // dyn_notch_min_hz // rpm filter - r.writeU8(_model.config.rpmFilterHarmonics); // gyro_rpm_notch_harmonics - r.writeU8(_model.config.rpmFilterMinFreq); // gyro_rpm_notch_min + r.writeU8(_model.config.rpmFilter.harmonics); // gyro_rpm_notch_harmonics + r.writeU8(_model.config.rpmFilter.minFreq); // gyro_rpm_notch_min // 1.43+ r.writeU16(_model.config.dynamicFilter.max_freq); // dyn_notch_max_hz break; @@ -1134,8 +1134,8 @@ class MspProcessor _model.config.dynamicFilter.width = m.readU8(); // dyn_notch_width_percent _model.config.dynamicFilter.q = m.readU16(); // dyn_notch_q _model.config.dynamicFilter.min_freq = m.readU16(); // dyn_notch_min_hz - _model.config.rpmFilterHarmonics = m.readU8(); // gyro_rpm_notch_harmonics - _model.config.rpmFilterMinFreq = m.readU8(); // gyro_rpm_notch_min + _model.config.rpmFilter.harmonics = m.readU8(); // gyro_rpm_notch_harmonics + _model.config.rpmFilter.minFreq = m.readU8(); // gyro_rpm_notch_min } // 1.43+ if (m.remain() >= 1) { diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index b99f046b..08dc6877 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -33,16 +33,16 @@ int GyroSensor::begin() _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_enabled = _model.config.rpmFilter.harmonics > 0 && _model.config.output.dshotTelemetry; _rpm_motor_index = 0; - _rpm_fade_inv = 1.0f / _model.config.rpmFilterFade; - _rpm_min_freq = _model.config.rpmFilterMinFreq; + _rpm_fade_inv = 1.0f / _model.config.rpmFilter.fade; + _rpm_min_freq = _model.config.rpmFilter.minFreq; _rpm_max_freq = 0.48f * _model.state.loopTimer.rate; - _rpm_q = _model.config.rpmFilterQ * 0.01f; + _rpm_q = _model.config.rpmFilter.q * 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); + _rpm_weights[i] = Math::clamp(0.01f * _model.config.rpmFilter.weights[i], 0.0f, 1.0f); } for (size_t i = 0; i < 3; i++) { @@ -111,7 +111,7 @@ int FAST_CODE_ATTR GyroSensor::filter() { for (size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) { - for (size_t n = 0; n < _model.config.rpmFilterHarmonics; n++) + for (size_t n = 0; n < _model.config.rpmFilter.harmonics; n++) { _model.state.gyro = Utils::applyFilter(_model.state.rpmFilter[m][n], _model.state.gyro); } @@ -165,12 +165,12 @@ void FAST_CODE_ATTR GyroSensor::rpmFilterUpdate() 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++) + for (size_t n = 0; n < _model.config.rpmFilter.harmonics; 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) + if (freqMargin < _model.config.rpmFilter.fade) { weight *= freqMargin * _rpm_fade_inv; } From 7740f9eecfcecc201efa735e67347eac0fbf4445 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 27 Oct 2024 00:10:28 +0200 Subject: [PATCH 19/68] vbat ibat config refactor --- lib/Espfc/src/Actuator.h | 2 +- lib/Espfc/src/Blackbox/Blackbox.cpp | 6 ++--- lib/Espfc/src/Cli.h | 18 +++++++------- lib/Espfc/src/ModelConfig.h | 27 ++++++++++++++------- lib/Espfc/src/Msp/MspProcessor.h | 36 ++++++++++++++-------------- lib/Espfc/src/Sensor/VoltageSensor.h | 14 +++++------ 6 files changed, 56 insertions(+), 47 deletions(-) diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h index d750272a..83d77af4 100644 --- a/lib/Espfc/src/Actuator.h +++ b/lib/Espfc/src/Actuator.h @@ -203,7 +203,7 @@ class Actuator { _model.state.buzzer.play(BUZZER_RX_LOST); } - if(_model.state.battery.warn(_model.config.vbatCellWarning)) + if(_model.state.battery.warn(_model.config.vbat.cellWarning)) { _model.state.buzzer.play(BUZZER_BAT_LOW); } diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index 8e6a9d67..bb179c10 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -170,9 +170,9 @@ int Blackbox::begin() 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()->currentMeterSource = (currentMeterSource_e)_model.config.ibat.source; + batteryConfigMutable()->voltageMeterSource = (voltageMeterSource_e)_model.config.vbat.source; + batteryConfigMutable()->vbatwarningcellvoltage = _model.config.vbat.cellWarning; batteryConfigMutable()->vbatmaxcellvoltage = 420; batteryConfigMutable()->vbatmincellvoltage = 340; diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index a9cef0df..27bdbfc1 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -494,15 +494,15 @@ class Cli Param(PSTR("board_align_pitch"), &c.boardAlignment[1]), Param(PSTR("board_align_yaw"), &c.boardAlignment[2]), - Param(PSTR("vbat_source"), &c.vbatSource, voltageSourceChoices), - Param(PSTR("vbat_scale"), &c.vbatScale), - Param(PSTR("vbat_mul"), &c.vbatResMult), - Param(PSTR("vbat_div"), &c.vbatResDiv), - Param(PSTR("vbat_cell_warn"), &c.vbatCellWarning), - - Param(PSTR("ibat_source"), &c.ibatSource, currentSourceChoices), - Param(PSTR("ibat_scale"), &c.ibatScale), - Param(PSTR("ibat_offset"), &c.ibatOffset), + Param(PSTR("vbat_source"), &c.vbat.source, voltageSourceChoices), + Param(PSTR("vbat_scale"), &c.vbat.scale), + Param(PSTR("vbat_mul"), &c.vbat.resMult), + Param(PSTR("vbat_div"), &c.vbat.resDiv), + Param(PSTR("vbat_cell_warn"), &c.vbat.cellWarning), + + Param(PSTR("ibat_source"), &c.ibat.source, currentSourceChoices), + Param(PSTR("ibat_scale"), &c.ibat.scale), + Param(PSTR("ibat_offset"), &c.ibat.offset), Param(PSTR("fusion_mode"), &c.fusion.mode, fusionModeChoices), Param(PSTR("fusion_gain_p"), &c.fusion.gain), diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 74332b65..77ad9053 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -542,6 +542,22 @@ struct RpmFilterConfig uint8_t fade = 30; }; +struct VBatConfig +{ + int16_t cellWarning = 350; + uint8_t scale = 100; + uint8_t resDiv = 10; + uint8_t resMult = 1; + int8_t source = 0; +}; + +struct IBatConfig +{ + int8_t source = 0; + int16_t scale = 100; + int16_t offset = 0; +}; + // persistent data class ModelConfig { @@ -637,15 +653,8 @@ class ModelConfig char modelName[MODEL_NAME_LEN + 1]; - int16_t vbatCellWarning = 350; - uint8_t vbatScale = 100; - uint8_t vbatResDiv = 10; - uint8_t vbatResMult = 1; - int8_t vbatSource = 0; - - int8_t ibatSource = 0; - int16_t ibatScale = 100; - int16_t ibatOffset = 0; + VBatConfig vbat; + IBatConfig ibat; int8_t debugMode = DEBUG_NONE; uint8_t debugAxis = 1; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 01a05320..b1f723da 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -351,27 +351,27 @@ class MspProcessor case MSP_BATTERY_CONFIG: r.writeU8(34); // vbatmincellvoltage r.writeU8(42); // vbatmaxcellvoltage - r.writeU8((_model.config.vbatCellWarning + 5) / 10); // vbatwarningcellvoltage + r.writeU8((_model.config.vbat.cellWarning + 5) / 10); // vbatwarningcellvoltage r.writeU16(0); // batteryCapacity - r.writeU8(_model.config.vbatSource); // voltageMeterSource - r.writeU8(_model.config.ibatSource); // currentMeterSource + r.writeU8(_model.config.vbat.source); // voltageMeterSource + r.writeU8(_model.config.ibat.source); // currentMeterSource r.writeU16(340); // vbatmincellvoltage r.writeU16(420); // vbatmaxcellvoltage - r.writeU16(_model.config.vbatCellWarning); // vbatwarningcellvoltage + r.writeU16(_model.config.vbat.cellWarning); // vbatwarningcellvoltage break; case MSP_SET_BATTERY_CONFIG: m.readU8(); // vbatmincellvoltage m.readU8(); // vbatmaxcellvoltage - _model.config.vbatCellWarning = m.readU8() * 10; // vbatwarningcellvoltage + _model.config.vbat.cellWarning = m.readU8() * 10; // vbatwarningcellvoltage m.readU16(); // batteryCapacity - _model.config.vbatSource = toVbatSource(m.readU8()); // voltageMeterSource - _model.config.ibatSource = toIbatSource(m.readU8()); // currentMeterSource + _model.config.vbat.source = toVbatSource(m.readU8()); // voltageMeterSource + _model.config.ibat.source = toIbatSource(m.readU8()); // currentMeterSource if(m.remain() >= 6) { m.readU16(); // vbatmincellvoltage m.readU16(); // vbatmaxcellvoltage - _model.config.vbatCellWarning = m.readU16(); + _model.config.vbat.cellWarning = m.readU16(); } break; @@ -414,9 +414,9 @@ class MspProcessor r.writeU8(5); // frame size (5) r.writeU8(i + 10); // id (10-19 vbat adc) r.writeU8(0); // type resistor divider - r.writeU8(_model.config.vbatScale); // scale - r.writeU8(_model.config.vbatResDiv); // resdivval - r.writeU8(_model.config.vbatResMult); // resdivmultiplier + r.writeU8(_model.config.vbat.scale); // scale + r.writeU8(_model.config.vbat.resDiv); // resdivval + r.writeU8(_model.config.vbat.resMult); // resdivmultiplier } break; @@ -425,9 +425,9 @@ class MspProcessor int id = m.readU8(); if(id == 10 + 0) // id (10-19 vbat adc, allow only 10) { - _model.config.vbatScale = m.readU8(); - _model.config.vbatResDiv = m.readU8(); - _model.config.vbatResMult = m.readU8(); + _model.config.vbat.scale = m.readU8(); + _model.config.vbat.resDiv = m.readU8(); + _model.config.vbat.resMult = m.readU8(); } } break; @@ -439,8 +439,8 @@ class MspProcessor r.writeU8(6); // frame size (6) r.writeU8(i + 10); // id (10-19 ibat adc) r.writeU8(1); // type adc - r.writeU16(_model.config.ibatScale); // scale - r.writeU16(_model.config.ibatOffset); // offset + r.writeU16(_model.config.ibat.scale); // scale + r.writeU16(_model.config.ibat.offset); // offset } break; @@ -449,8 +449,8 @@ class MspProcessor int id = m.readU8(); if(id == 10 + 0) // id (10-19 ibat adc, allow only 10) { - _model.config.ibatScale = m.readU16(); - _model.config.ibatOffset = m.readU16(); + _model.config.ibat.scale = m.readU16(); + _model.config.ibat.offset = m.readU16(); } } break; diff --git a/lib/Espfc/src/Sensor/VoltageSensor.h b/lib/Espfc/src/Sensor/VoltageSensor.h index aff06666..ba7a63a4 100644 --- a/lib/Espfc/src/Sensor/VoltageSensor.h +++ b/lib/Espfc/src/Sensor/VoltageSensor.h @@ -55,7 +55,7 @@ class VoltageSensor: public BaseSensor int readVbat() { #ifdef ESPFC_ADC_0 - if(_model.config.vbatSource != 1 || _model.config.pin[PIN_INPUT_ADC_0] == -1) return 0; + if(_model.config.vbat.source != 1 || _model.config.pin[PIN_INPUT_ADC_0] == -1) return 0; // wemos d1 mini has divider 3.2:1 (220k:100k) // additionaly I've used divider 5.7:1 (4k7:1k) // total should equals ~18.24:1, 73:4 resDiv:resMult should be ideal, @@ -63,9 +63,9 @@ class VoltageSensor: public BaseSensor _model.state.battery.rawVoltage = analogRead(_model.config.pin[PIN_INPUT_ADC_0]); float volts = _vFilterFast.update(_model.state.battery.rawVoltage * ESPFC_ADC_SCALE); - volts *= _model.config.vbatScale * 0.1f; - volts *= _model.config.vbatResMult; - volts /= _model.config.vbatResDiv; + volts *= _model.config.vbat.scale * 0.1f; + volts *= _model.config.vbat.resMult; + volts /= _model.config.vbat.resDiv; _model.state.battery.voltageUnfiltered = volts; _model.state.battery.voltage = _vFilter.update(_model.state.battery.voltageUnfiltered); @@ -94,14 +94,14 @@ class VoltageSensor: public BaseSensor int readIbat() { #ifdef ESPFC_ADC_1 - if(_model.config.ibatSource != 1 && _model.config.pin[PIN_INPUT_ADC_1] == -1) return 0; + if(_model.config.ibat.source != 1 && _model.config.pin[PIN_INPUT_ADC_1] == -1) return 0; _model.state.battery.rawCurrent = analogRead(_model.config.pin[PIN_INPUT_ADC_1]); float volts = _iFilterFast.update(_model.state.battery.rawCurrent * ESPFC_ADC_SCALE); float milivolts = volts * 1000.0f; - volts += _model.config.ibatOffset * 0.001f; - volts *= _model.config.ibatScale * 0.1f; + volts += _model.config.ibat.offset * 0.001f; + volts *= _model.config.ibat.scale * 0.1f; _model.state.battery.currentUnfiltered = volts; _model.state.battery.current = _iFilter.update(_model.state.battery.currentUnfiltered); From efdf95242a231cf381a849745b4fbf2c9b3bb0e9 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 27 Oct 2024 00:45:46 +0200 Subject: [PATCH 20/68] define to constexpr --- lib/Espfc/src/Control/Pid.h | 33 +++++++++++++++++---------------- lib/Espfc/src/Control/Rates.cpp | 3 +++ lib/Espfc/src/Control/Rates.h | 3 --- lib/Espfc/src/Filter.cpp | 28 ++++++++++++++++++---------- lib/Espfc/src/ModelState.h | 6 ++---- 5 files changed, 40 insertions(+), 33 deletions(-) diff --git a/lib/Espfc/src/Control/Pid.h b/lib/Espfc/src/Control/Pid.h index 465cf87a..deb3711b 100644 --- a/lib/Espfc/src/Control/Pid.h +++ b/lib/Espfc/src/Control/Pid.h @@ -2,25 +2,26 @@ #include #include "Filter.h" - -// bataflight scalers -#define PTERM_SCALE_BETAFLIGHT 0.032029f -#define ITERM_SCALE_BETAFLIGHT 0.244381f -#define DTERM_SCALE_BETAFLIGHT 0.000529f -#define FTERM_SCALE_BETAFLIGHT 0.00013754f - -#define PTERM_SCALE (PTERM_SCALE_BETAFLIGHT * RAD_TO_DEG * 0.001f) // ~ 0.00183 = 0.032029f * 57.29 / 1000 -#define ITERM_SCALE (ITERM_SCALE_BETAFLIGHT * RAD_TO_DEG * 0.001f) // ~ 0.014f -#define DTERM_SCALE (DTERM_SCALE_BETAFLIGHT * RAD_TO_DEG * 0.001f) // ~ 0.0000303f -#define FTERM_SCALE (FTERM_SCALE_BETAFLIGHT * RAD_TO_DEG * 0.001f) // - -#define LEVEL_PTERM_SCALE 0.1f // 1/10 -#define LEVEL_ITERM_SCALE 0.1f // 1/10 -#define LEVEL_DTERM_SCALE 0.001f // 1/1000 -#define LEVEL_FTERM_SCALE 0.001f // 1/1000 +#include "Math/Utils.h" namespace Espfc { +// bataflight scalers +constexpr float PTERM_SCALE_BETAFLIGHT = 0.032029f; +constexpr float ITERM_SCALE_BETAFLIGHT = 0.244381f; +constexpr float DTERM_SCALE_BETAFLIGHT = 0.000529f; +constexpr float FTERM_SCALE_BETAFLIGHT = 0.00013754f; + +constexpr float PTERM_SCALE = PTERM_SCALE_BETAFLIGHT * Math::toDeg(1.0f) * 0.001f; // ~ 0.00183 = 0.032029f * 57.29 / 1000 +constexpr float ITERM_SCALE = ITERM_SCALE_BETAFLIGHT * Math::toDeg(1.0f) * 0.001f; // ~ 0.014f +constexpr float DTERM_SCALE = DTERM_SCALE_BETAFLIGHT * Math::toDeg(1.0f) * 0.001f; // ~ 0.0000303f +constexpr float FTERM_SCALE = FTERM_SCALE_BETAFLIGHT * Math::toDeg(1.0f) * 0.001f; // + +constexpr float LEVEL_PTERM_SCALE = 0.1f; // 1/10 +constexpr float LEVEL_ITERM_SCALE = 0.1f; // 1/10 +constexpr float LEVEL_DTERM_SCALE = 0.001f; // 1/1000 +constexpr float LEVEL_FTERM_SCALE = 0.001f; // 1/1000 + enum ItermRelaxType { ITERM_RELAX_OFF, ITERM_RELAX_RP, diff --git a/lib/Espfc/src/Control/Rates.cpp b/lib/Espfc/src/Control/Rates.cpp index 0b44b1d1..b2191ba2 100644 --- a/lib/Espfc/src/Control/Rates.cpp +++ b/lib/Espfc/src/Control/Rates.cpp @@ -3,6 +3,9 @@ namespace Espfc { +constexpr float SETPOINT_RATE_LIMIT = 1998.0f; +constexpr float RC_RATE_INCREMENTAL = 14.54f; + void Rates::begin(const InputConfig& config) { rateType = (RateType)config.rateType; diff --git a/lib/Espfc/src/Control/Rates.h b/lib/Espfc/src/Control/Rates.h index 3378d0eb..3831dd65 100644 --- a/lib/Espfc/src/Control/Rates.h +++ b/lib/Espfc/src/Control/Rates.h @@ -3,9 +3,6 @@ #include "Math/Utils.h" #include "ModelConfig.h" -#define SETPOINT_RATE_LIMIT 1998.0f -#define RC_RATE_INCREMENTAL 14.54f - namespace Espfc { diff --git a/lib/Espfc/src/Filter.cpp b/lib/Espfc/src/Filter.cpp index 77039617..0746dfb5 100644 --- a/lib/Espfc/src/Filter.cpp +++ b/lib/Espfc/src/Filter.cpp @@ -3,16 +3,7 @@ #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) +static inline float pt1Gain(float rate, float freq) { float rc = 1.f / (2.f * Espfc::Math::pi() * freq); float dt = 1.f / rate; @@ -215,6 +206,23 @@ void FAST_CODE_ATTR FilterStateMedian::reconfigure(const FilterStateMedian& from { } +// 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; } + +template +static inline void QMF_SORT(T& a, T& b) { if (a > b) std::swap(a, b); } + +template +static inline void QMF_COPY(T* p, T* v, size_t n) { for(size_t i = 0; i < n; i++) p[i] = v[i]; } + +static inline void QMF_SORTF(float& a, float& b) { QMF_SORT(a, b); } + float FAST_CODE_ATTR FilterStateMedian::update(float n) { v[0] = v[1]; diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index a56f0c31..9e0b0775 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -130,10 +130,8 @@ class FailsafeState uint32_t timeout; }; -#define ACCEL_G (9.80665f) -#define ACCEL_G_INV (1.f / ACCEL_G) -//#define ACCEL_G (1.f) -//#define ACCEL_G_INV (1.f) +constexpr float ACCEL_G = 9.80665f; +constexpr float ACCEL_G_INV = 1.f / ACCEL_G; enum RescueConfigMode { RESCUE_CONFIG_PENDING, From fada2d20af4d8acfbdc879604420aa703b2252b8 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 27 Oct 2024 20:09:35 +0100 Subject: [PATCH 21/68] gyro config refactor --- lib/Espfc/src/Actuator.h | 4 +- lib/Espfc/src/Blackbox/Blackbox.cpp | 22 ++++----- lib/Espfc/src/Cli.h | 44 ++++++++--------- lib/Espfc/src/Hardware.h | 4 +- lib/Espfc/src/Model.h | 20 ++++---- lib/Espfc/src/ModelConfig.h | 30 +++++++----- lib/Espfc/src/Msp/MspProcessor.h | 74 ++++++++++++++--------------- lib/Espfc/src/Sensor/AccelSensor.h | 2 +- lib/Espfc/src/Sensor/GyroSensor.cpp | 8 ++-- test/test_fc/test_fc.cpp | 12 ++--- 10 files changed, 113 insertions(+), 107 deletions(-) diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h index 83d77af4..147bd764 100644 --- a/lib/Espfc/src/Actuator.h +++ b/lib/Espfc/src/Actuator.h @@ -221,8 +221,8 @@ class Actuator { return; // temporary disable int scale = Math::clamp((int)_model.state.inputUs[AXIS_THRUST], 1000, 2000); - if(_model.config.gyroDynLpfFilter.cutoff > 0) { - int gyroFreq = Math::map(scale, 1000, 2000, _model.config.gyroDynLpfFilter.cutoff, _model.config.gyroDynLpfFilter.freq); + if(_model.config.gyro.dynLpfFilter.cutoff > 0) { + int gyroFreq = Math::map(scale, 1000, 2000, _model.config.gyro.dynLpfFilter.cutoff, _model.config.gyro.dynLpfFilter.freq); for(size_t i = 0; i <= AXIS_YAW; i++) { _model.state.gyroFilter[i].reconfigure(gyroFreq); } diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index bb179c10..e31340c2 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -110,18 +110,18 @@ int Blackbox::begin() 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_hardware_lpf = _model.config.gyro.dlpf; + gyroConfigMutable()->gyro_lpf1_type = _model.config.gyro.filter.type; + gyroConfigMutable()->gyro_lpf1_static_hz = _model.config.gyro.filter.freq; + gyroConfigMutable()->gyro_lpf1_dyn_min_hz = _model.config.gyro.dynLpfFilter.cutoff; + gyroConfigMutable()->gyro_lpf1_dyn_max_hz = _model.config.gyro.dynLpfFilter.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_lpf2_type = _model.config.gyro.filter2.type; + gyroConfigMutable()->gyro_lpf2_static_hz = _model.config.gyro.filter2.freq; + gyroConfigMutable()->gyro_soft_notch_cutoff_1 = _model.config.gyro.notch1Filter.cutoff; + gyroConfigMutable()->gyro_soft_notch_hz_1 = _model.config.gyro.notch1Filter.freq; + gyroConfigMutable()->gyro_soft_notch_cutoff_2 = _model.config.gyro.notch2Filter.cutoff; + gyroConfigMutable()->gyro_soft_notch_hz_2 = _model.config.gyro.notch2Filter.freq; gyroConfigMutable()->gyro_sync_denom = 1; dynNotchConfigMutable()->dyn_notch_count = _model.config.dynamicFilter.width; diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 27bdbfc1..29e59077 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -432,22 +432,22 @@ class Cli Param(PSTR("debug_mode"), &c.debugMode, debugModeChoices), Param(PSTR("debug_axis"), &c.debugAxis), - Param(PSTR("gyro_bus"), &c.gyroBus, busDevChoices), - Param(PSTR("gyro_dev"), &c.gyroDev, gyroDevChoices), - Param(PSTR("gyro_dlpf"), &c.gyroDlpf, gyroDlpfChoices), - Param(PSTR("gyro_align"), &c.gyroAlign, alignChoices), - Param(PSTR("gyro_lpf_type"), &c.gyroFilter.type, filterTypeChoices), - Param(PSTR("gyro_lpf_freq"), &c.gyroFilter.freq), - Param(PSTR("gyro_lpf2_type"), &c.gyroFilter2.type, filterTypeChoices), - Param(PSTR("gyro_lpf2_freq"), &c.gyroFilter2.freq), - Param(PSTR("gyro_lpf3_type"), &c.gyroFilter3.type, filterTypeChoices), - Param(PSTR("gyro_lpf3_freq"), &c.gyroFilter3.freq), - Param(PSTR("gyro_notch1_freq"), &c.gyroNotch1Filter.freq), - Param(PSTR("gyro_notch1_cutoff"), &c.gyroNotch1Filter.cutoff), - Param(PSTR("gyro_notch2_freq"), &c.gyroNotch2Filter.freq), - Param(PSTR("gyro_notch2_cutoff"), &c.gyroNotch2Filter.cutoff), - Param(PSTR("gyro_dyn_lpf_min"), &c.gyroDynLpfFilter.cutoff), - Param(PSTR("gyro_dyn_lpf_max"), &c.gyroDynLpfFilter.freq), + Param(PSTR("gyro_bus"), &c.gyro.bus, busDevChoices), + Param(PSTR("gyro_dev"), &c.gyro.dev, gyroDevChoices), + Param(PSTR("gyro_dlpf"), &c.gyro.dlpf, gyroDlpfChoices), + Param(PSTR("gyro_align"), &c.gyro.align, alignChoices), + Param(PSTR("gyro_lpf_type"), &c.gyro.filter.type, filterTypeChoices), + Param(PSTR("gyro_lpf_freq"), &c.gyro.filter.freq), + Param(PSTR("gyro_lpf2_type"), &c.gyro.filter2.type, filterTypeChoices), + Param(PSTR("gyro_lpf2_freq"), &c.gyro.filter2.freq), + Param(PSTR("gyro_lpf3_type"), &c.gyro.filter3.type, filterTypeChoices), + Param(PSTR("gyro_lpf3_freq"), &c.gyro.filter3.freq), + Param(PSTR("gyro_notch1_freq"), &c.gyro.notch1Filter.freq), + Param(PSTR("gyro_notch1_cutoff"), &c.gyro.notch1Filter.cutoff), + Param(PSTR("gyro_notch2_freq"), &c.gyro.notch2Filter.freq), + Param(PSTR("gyro_notch2_cutoff"), &c.gyro.notch2Filter.cutoff), + Param(PSTR("gyro_dyn_lpf_min"), &c.gyro.dynLpfFilter.cutoff), + Param(PSTR("gyro_dyn_lpf_max"), &c.gyro.dynLpfFilter.freq), Param(PSTR("gyro_dyn_notch_q"), &c.dynamicFilter.q), Param(PSTR("gyro_dyn_notch_count"), &c.dynamicFilter.width), Param(PSTR("gyro_dyn_notch_min"), &c.dynamicFilter.min_freq), @@ -460,9 +460,9 @@ class Cli Param(PSTR("gyro_rpm_weight_2"), &c.rpmFilter.weights[1]), Param(PSTR("gyro_rpm_weight_3"), &c.rpmFilter.weights[2]), Param(PSTR("gyro_rpm_tlm_lpf_freq"), &c.rpmFilter.freqLpf), - Param(PSTR("gyro_offset_x"), &c.gyroBias[0]), - Param(PSTR("gyro_offset_y"), &c.gyroBias[1]), - Param(PSTR("gyro_offset_z"), &c.gyroBias[2]), + Param(PSTR("gyro_offset_x"), &c.gyro.bias[0]), + Param(PSTR("gyro_offset_y"), &c.gyro.bias[1]), + Param(PSTR("gyro_offset_z"), &c.gyro.bias[2]), Param(PSTR("accel_bus"), &c.accelBus, busDevChoices), Param(PSTR("accel_dev"), &c.accelDev, gyroDevChoices), @@ -1037,9 +1037,9 @@ class Cli if(!cmd.args[1]) { s.print(F(" gyro offset: ")); - s.print(_model.config.gyroBias[0]); s.print(' '); - s.print(_model.config.gyroBias[1]); s.print(' '); - s.print(_model.config.gyroBias[2]); s.print(F(" [")); + s.print(_model.config.gyro.bias[0]); s.print(' '); + s.print(_model.config.gyro.bias[1]); s.print(' '); + s.print(_model.config.gyro.bias[2]); s.print(F(" [")); s.print(Math::toDeg(_model.state.gyroBias[0])); s.print(' '); s.print(Math::toDeg(_model.state.gyroBias[1])); s.print(' '); s.print(Math::toDeg(_model.state.gyroBias[2])); s.println(F("]")); diff --git a/lib/Espfc/src/Hardware.h b/lib/Espfc/src/Hardware.h index 3a702399..4a41b225 100644 --- a/lib/Espfc/src/Hardware.h +++ b/lib/Espfc/src/Hardware.h @@ -96,7 +96,7 @@ class Hardware void detectGyro() { - if(_model.config.gyroDev == GYRO_NONE) return; + if(_model.config.gyro.dev == GYRO_NONE) return; Device::GyroDevice * detectedGyro = nullptr; #if defined(ESPFC_SPI_0) @@ -126,7 +126,7 @@ class Hardware #endif if(!detectedGyro) return; - detectedGyro->setDLPFMode(_model.config.gyroDlpf); + detectedGyro->setDLPFMode(_model.config.gyro.dlpf); _model.state.gyroDev = detectedGyro; _model.state.gyroPresent = (bool)detectedGyro; _model.state.accelPresent = _model.state.gyroPresent && _model.config.accelDev != GYRO_NONE; diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index c41df055..b8ffadc8 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -109,7 +109,7 @@ class Model bool gyroActive() const /* IRAM_ATTR */ { - return state.gyroPresent && config.gyroDev != GYRO_NONE; + return state.gyroPresent && config.gyro.dev != GYRO_NONE; } bool accelActive() const @@ -461,18 +461,18 @@ class Model state.gyroDynNotchFilter[p][i].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 380), gyroFilterRate); } } - state.gyroNotch1Filter[i].begin(config.gyroNotch1Filter, gyroFilterRate); - state.gyroNotch2Filter[i].begin(config.gyroNotch2Filter, gyroFilterRate); - if(config.gyroDynLpfFilter.cutoff > 0) + state.gyroNotch1Filter[i].begin(config.gyro.notch1Filter, gyroFilterRate); + state.gyroNotch2Filter[i].begin(config.gyro.notch2Filter, gyroFilterRate); + if(config.gyro.dynLpfFilter.cutoff > 0) { - state.gyroFilter[i].begin(FilterConfig((FilterType)config.gyroFilter.type, config.gyroDynLpfFilter.cutoff), gyroFilterRate); + state.gyroFilter[i].begin(FilterConfig((FilterType)config.gyro.filter.type, config.gyro.dynLpfFilter.cutoff), gyroFilterRate); } else { - state.gyroFilter[i].begin(config.gyroFilter, gyroFilterRate); + state.gyroFilter[i].begin(config.gyro.filter, gyroFilterRate); } - state.gyroFilter2[i].begin(config.gyroFilter2, gyroFilterRate); - state.gyroFilter3[i].begin(config.gyroFilter3, gyroPreFilterRate); + state.gyroFilter2[i].begin(config.gyro.filter2, gyroFilterRate); + state.gyroFilter3[i].begin(config.gyro.filter3, gyroPreFilterRate); state.accelFilter[i].begin(config.accelFilter, gyroFilterRate); state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accelTimer.rate / 3), gyroFilterRate); for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) @@ -574,7 +574,7 @@ class Model // load current sensor calibration for(size_t i = 0; i <= AXIS_YAW; i++) { - state.gyroBias.set(i, config.gyroBias[i] / 1000.0f); + state.gyroBias.set(i, config.gyro.bias[i] / 1000.0f); state.accelBias.set(i, config.accelBias[i] / 1000.0f); state.magCalibrationOffset.set(i, config.magCalibrationOffset[i] / 10.0f); state.magCalibrationScale.set(i, config.magCalibrationScale[i] / 1000.0f); @@ -586,7 +586,7 @@ class Model // store current sensor calibration for(size_t i = 0; i < 3; i++) { - config.gyroBias[i] = lrintf(state.gyroBias[i] * 1000.0f); + config.gyro.bias[i] = lrintf(state.gyroBias[i] * 1000.0f); config.accelBias[i] = lrintf(state.accelBias[i] * 1000.0f); config.magCalibrationOffset[i] = lrintf(state.magCalibrationOffset[i] * 10.0f); config.magCalibrationScale[i] = lrintf(state.magCalibrationScale[i] * 1000.0f); diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 77ad9053..23e9c580 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -558,21 +558,28 @@ struct IBatConfig int16_t offset = 0; }; +struct GyroConfig +{ + int8_t bus = BUS_AUTO; + int8_t dev = GYRO_AUTO; + int8_t dlpf = GYRO_DLPF_256; + int8_t align = ALIGN_DEFAULT; + + FilterConfig filter{FILTER_PT1, 100}; + FilterConfig filter2{FILTER_PT1, 213}; + FilterConfig filter3{FILTER_FO, 150}; + FilterConfig notch1Filter{FILTER_NOTCH, 0, 0}; + FilterConfig notch2Filter{FILTER_NOTCH, 0, 0}; + FilterConfig dynLpfFilter{FILTER_PT1, 425, 170}; + + int16_t bias[3] = { 0, 0, 0 }; +}; + // persistent data class ModelConfig { public: - int8_t gyroBus = BUS_AUTO; - int8_t gyroDev = GYRO_AUTO; - int8_t gyroDlpf = GYRO_DLPF_256; - int8_t gyroAlign = ALIGN_DEFAULT; - - FilterConfig gyroFilter{FILTER_PT1, 100}; - FilterConfig gyroFilter2{FILTER_PT1, 213}; - FilterConfig gyroFilter3{FILTER_FO, 150}; - FilterConfig gyroNotch1Filter{FILTER_NOTCH, 0, 0}; - FilterConfig gyroNotch2Filter{FILTER_NOTCH, 0, 0}; - FilterConfig gyroDynLpfFilter{FILTER_PT1, 425, 170}; + GyroConfig gyro; int8_t accelBus = BUS_AUTO; int8_t accelDev = GYRO_AUTO; @@ -646,7 +653,6 @@ class ModelConfig FusionConfig fusion; - int16_t gyroBias[3] = { 0, 0, 0 }; int16_t accelBias[3] = { 0, 0, 0 }; int16_t magCalibrationScale[3] = { 0, 0, 0 }; int16_t magCalibrationOffset[3] = { 1000, 1000, 1000 }; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index b1f723da..0a8f2845 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -532,13 +532,13 @@ class MspProcessor break; case MSP_SENSOR_ALIGNMENT: - r.writeU8(_model.config.gyroAlign); // gyro align - r.writeU8(_model.config.gyroAlign); // acc align, Starting with 4.0 gyro and acc alignment are the same + r.writeU8(_model.config.gyro.align); // gyro align + r.writeU8(_model.config.gyro.align); // acc align, Starting with 4.0 gyro and acc alignment are the same r.writeU8(_model.config.magAlign); // mag align //1.41+ r.writeU8(_model.state.gyroPresent ? 1 : 0); // gyro detection mask GYRO_1_MASK r.writeU8(0); // gyro_to_use - r.writeU8(_model.config.gyroAlign); // gyro 1 + r.writeU8(_model.config.gyro.align); // gyro 1 r.writeU8(0); // gyro 2 break; @@ -554,7 +554,7 @@ class MspProcessor gyroAlign = m.readU8(); // gyro 1 align m.readU8(); // gyro 2 align } - _model.config.gyroAlign = gyroAlign; + _model.config.gyro.align = gyroAlign; } break; @@ -1060,29 +1060,29 @@ class MspProcessor // break; case MSP_FILTER_CONFIG: - r.writeU8(_model.config.gyroFilter.freq); // gyro lpf - r.writeU16(_model.config.dtermFilter.freq); // dterm lpf - r.writeU16(_model.config.yawFilter.freq); // yaw lpf - r.writeU16(_model.config.gyroNotch1Filter.freq); // gyro notch 1 hz - r.writeU16(_model.config.gyroNotch1Filter.cutoff); // gyro notch 1 cutoff - r.writeU16(_model.config.dtermNotchFilter.freq); // dterm notch hz - r.writeU16(_model.config.dtermNotchFilter.cutoff); // dterm notch cutoff - r.writeU16(_model.config.gyroNotch2Filter.freq); // gyro notch 2 hz - r.writeU16(_model.config.gyroNotch2Filter.cutoff); // gyro notch 2 cutoff - r.writeU8(_model.config.dtermFilter.type); // dterm type - r.writeU8(fromGyroDlpf(_model.config.gyroDlpf)); - r.writeU8(0); // dlfp 32khz type - r.writeU16(_model.config.gyroFilter.freq); // lowpass1 freq - r.writeU16(_model.config.gyroFilter2.freq); // lowpass2 freq - r.writeU8(_model.config.gyroFilter.type); // lowpass1 type - r.writeU8(_model.config.gyroFilter2.type); // lowpass2 type - r.writeU16(_model.config.dtermFilter2.freq); // dterm lopwass2 freq + r.writeU8(_model.config.gyro.filter.freq); // gyro lpf + r.writeU16(_model.config.dtermFilter.freq); // dterm lpf + r.writeU16(_model.config.yawFilter.freq); // yaw lpf + r.writeU16(_model.config.gyro.notch1Filter.freq); // gyro notch 1 hz + r.writeU16(_model.config.gyro.notch1Filter.cutoff); // gyro notch 1 cutoff + r.writeU16(_model.config.dtermNotchFilter.freq); // dterm notch hz + r.writeU16(_model.config.dtermNotchFilter.cutoff); // dterm notch cutoff + r.writeU16(_model.config.gyro.notch2Filter.freq); // gyro notch 2 hz + r.writeU16(_model.config.gyro.notch2Filter.cutoff); // gyro notch 2 cutoff + r.writeU8(_model.config.dtermFilter.type); // dterm type + r.writeU8(fromGyroDlpf(_model.config.gyro.dlpf)); + r.writeU8(0); // dlfp 32khz type + r.writeU16(_model.config.gyro.filter.freq); // lowpass1 freq + r.writeU16(_model.config.gyro.filter2.freq); // lowpass2 freq + r.writeU8(_model.config.gyro.filter.type); // lowpass1 type + r.writeU8(_model.config.gyro.filter2.type); // lowpass2 type + r.writeU16(_model.config.dtermFilter2.freq); // dterm lopwass2 freq // 1.41+ - r.writeU8(_model.config.dtermFilter2.type); // dterm lopwass2 type - r.writeU16(_model.config.gyroDynLpfFilter.cutoff); // dyn lpf gyro min - r.writeU16(_model.config.gyroDynLpfFilter.freq); // dyn lpf gyro max - r.writeU16(_model.config.dtermDynLpfFilter.cutoff); // dyn lpf dterm min - r.writeU16(_model.config.dtermDynLpfFilter.freq); // dyn lpf dterm max + r.writeU8(_model.config.dtermFilter2.type); // dterm lopwass2 type + r.writeU16(_model.config.gyro.dynLpfFilter.cutoff); // dyn lpf gyro min + r.writeU16(_model.config.gyro.dynLpfFilter.freq); // dyn lpf gyro max + r.writeU16(_model.config.dtermDynLpfFilter.cutoff); // dyn lpf dterm min + r.writeU16(_model.config.dtermDynLpfFilter.freq); // dyn lpf dterm max // gyro analyse r.writeU8(3); // deprecated dyn notch range r.writeU8(_model.config.dynamicFilter.width); // dyn_notch_width_percent @@ -1096,18 +1096,18 @@ class MspProcessor break; case MSP_SET_FILTER_CONFIG: - _model.config.gyroFilter.freq = m.readU8(); + _model.config.gyro.filter.freq = m.readU8(); _model.config.dtermFilter.freq = m.readU16(); _model.config.yawFilter.freq = m.readU16(); if (m.remain() >= 8) { - _model.config.gyroNotch1Filter.freq = m.readU16(); - _model.config.gyroNotch1Filter.cutoff = m.readU16(); + _model.config.gyro.notch1Filter.freq = m.readU16(); + _model.config.gyro.notch1Filter.cutoff = m.readU16(); _model.config.dtermNotchFilter.freq = m.readU16(); _model.config.dtermNotchFilter.cutoff = m.readU16(); } if (m.remain() >= 4) { - _model.config.gyroNotch2Filter.freq = m.readU16(); - _model.config.gyroNotch2Filter.cutoff = m.readU16(); + _model.config.gyro.notch2Filter.freq = m.readU16(); + _model.config.gyro.notch2Filter.cutoff = m.readU16(); } if (m.remain() >= 1) { _model.config.dtermFilter.type = (FilterType)m.readU8(); @@ -1115,17 +1115,17 @@ class MspProcessor if (m.remain() >= 10) { m.readU8(); // dlfp type m.readU8(); // 32k dlfp type - _model.config.gyroFilter.freq = m.readU16(); - _model.config.gyroFilter2.freq = m.readU16(); - _model.config.gyroFilter.type = m.readU8(); - _model.config.gyroFilter2.type = m.readU8(); + _model.config.gyro.filter.freq = m.readU16(); + _model.config.gyro.filter2.freq = m.readU16(); + _model.config.gyro.filter.type = m.readU8(); + _model.config.gyro.filter2.type = m.readU8(); _model.config.dtermFilter2.freq = m.readU16(); } // 1.41+ if (m.remain() >= 9) { _model.config.dtermFilter2.type = m.readU8(); - _model.config.gyroDynLpfFilter.cutoff = m.readU16(); // dyn gyro lpf min - _model.config.gyroDynLpfFilter.freq = m.readU16(); // dyn gyro lpf max + _model.config.gyro.dynLpfFilter.cutoff = m.readU16(); // dyn gyro lpf min + _model.config.gyro.dynLpfFilter.freq = m.readU16(); // dyn gyro lpf max _model.config.dtermDynLpfFilter.cutoff = m.readU16(); // dyn dterm lpf min _model.config.dtermDynLpfFilter.freq = m.readU16(); // dyn dterm lpf min } diff --git a/lib/Espfc/src/Sensor/AccelSensor.h b/lib/Espfc/src/Sensor/AccelSensor.h index 91221e9e..8e9d1005 100644 --- a/lib/Espfc/src/Sensor/AccelSensor.h +++ b/lib/Espfc/src/Sensor/AccelSensor.h @@ -64,7 +64,7 @@ class AccelSensor: public BaseSensor _model.state.accel = (VectorFloat)_model.state.accelRaw * _model.state.accelScale; - align(_model.state.accel, _model.config.gyroAlign); + align(_model.state.accel, _model.config.gyro.align); _model.state.accel = _model.state.boardAlignment.apply(_model.state.accel); for(size_t i = 0; i < 3; i++) diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index 08dc6877..6583bb53 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -19,7 +19,7 @@ int GyroSensor::begin() _gyro = _model.state.gyroDev; if (!_gyro) return 0; - _gyro->setDLPFMode(_model.config.gyroDlpf); + _gyro->setDLPFMode(_model.config.gyro.dlpf); _gyro->setRate(_gyro->getRate()); _model.state.gyroScale = Math::toRad(2000.f) / 32768.f; @@ -53,7 +53,7 @@ int GyroSensor::begin() #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); + _model.logger.info().log(F("GYRO INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_gyro->getAddress()).log(_model.config.gyro.dlpf).log(_gyro->getRate()).log(_model.state.gyroTimer.rate).logln(_model.state.gyroTimer.interval); return 1; } @@ -68,10 +68,10 @@ int FAST_CODE_ATTR GyroSensor::read() VectorFloat input = static_cast(_model.state.gyroRaw) * _model.state.gyroScale; - align(input, _model.config.gyroAlign); + align(input, _model.config.gyro.align); input = _model.state.boardAlignment.apply(input); - if (_model.config.gyroFilter3.freq) + if (_model.config.gyro.filter3.freq) { _model.state.gyroSampled = Utils::applyFilter(_model.state.gyroFilter3, input); } diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index b0797108..ca8740c2 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -106,7 +106,7 @@ void test_model_gyro_init_1k_256dlpf() { Model model; model.state.gyroClock = 8000; - model.config.gyroDlpf = GYRO_DLPF_256; + model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 1; model.config.mixerSync = 1; model.begin(); @@ -123,7 +123,7 @@ void test_model_gyro_init_1k_188dlpf() { Model model; model.state.gyroClock = 1000; - model.config.gyroDlpf = GYRO_DLPF_188; + model.config.gyro.dlpf = GYRO_DLPF_188; model.config.loopSync = 2; model.config.mixerSync = 2; model.begin(); @@ -140,7 +140,7 @@ void test_model_inner_pid_init() { Model model; model.state.gyroClock = 1000; - model.config.gyroDlpf = GYRO_DLPF_256; + model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 1; model.config.mixerSync = 1; model.config.mixerType = FC_MIXER_QUADX; @@ -172,7 +172,7 @@ void test_model_outer_pid_init() { Model model; model.state.gyroClock = 8000; - model.config.gyroDlpf = GYRO_DLPF_256; + model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 1; model.config.mixerSync = 1; model.config.mixerType = FC_MIXER_QUADX; @@ -196,7 +196,7 @@ void test_controller_rates() { Model model; model.state.gyroClock = 8000; - model.config.gyroDlpf = GYRO_DLPF_256; + model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 8; model.config.mixerSync = 1; model.config.mixerType = FC_MIXER_QUADX; @@ -246,7 +246,7 @@ void test_controller_rates_limit() { Model model; model.state.gyroClock = 8000; - model.config.gyroDlpf = GYRO_DLPF_256; + model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 8; model.config.mixerSync = 1; model.config.mixerType = FC_MIXER_QUADX; From 2f1b947c706a0b54d525285a8c386445326363d1 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 27 Oct 2024 20:25:19 +0100 Subject: [PATCH 22/68] mag baro config refactor --- lib/Espfc/src/Blackbox/Blackbox.cpp | 8 ++-- lib/Espfc/src/Cli.h | 67 ++++++++++++++--------------- lib/Espfc/src/Hardware.h | 6 +-- lib/Espfc/src/Model.h | 24 +++++------ lib/Espfc/src/ModelConfig.h | 47 +++++++++++--------- lib/Espfc/src/Msp/MspProcessor.h | 16 +++---- lib/Espfc/src/Sensor/BaroSensor.h | 4 +- lib/Espfc/src/Sensor/MagSensor.h | 2 +- 8 files changed, 90 insertions(+), 84 deletions(-) diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index e31340c2..56eeafea 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -129,10 +129,10 @@ int Blackbox::begin() 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; + accelerometerConfigMutable()->acc_lpf_hz = _model.config.accel.filter.freq; + accelerometerConfigMutable()->acc_hardware = _model.config.accel.dev; + barometerConfigMutable()->baro_hardware = _model.config.baro.dev; + compassConfigMutable()->mag_hardware = _model.config.mag.dev; motorConfigMutable()->dev.useUnsyncedPwm = _model.config.output.async; motorConfigMutable()->dev.motorPwmProtocol = _model.config.output.protocol; diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 29e59077..1c31ab61 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -464,31 +464,30 @@ class Cli Param(PSTR("gyro_offset_y"), &c.gyro.bias[1]), Param(PSTR("gyro_offset_z"), &c.gyro.bias[2]), - Param(PSTR("accel_bus"), &c.accelBus, busDevChoices), - Param(PSTR("accel_dev"), &c.accelDev, gyroDevChoices), - //Param(PSTR("accel_align"), &c.accelAlign, alignChoices), - Param(PSTR("accel_lpf_type"), &c.accelFilter.type, filterTypeChoices), - Param(PSTR("accel_lpf_freq"), &c.accelFilter.freq), - Param(PSTR("accel_offset_x"), &c.accelBias[0]), - Param(PSTR("accel_offset_y"), &c.accelBias[1]), - Param(PSTR("accel_offset_z"), &c.accelBias[2]), - - Param(PSTR("mag_bus"), &c.magBus, busDevChoices), - Param(PSTR("mag_dev"), &c.magDev, magDevChoices), - Param(PSTR("mag_align"), &c.magAlign, alignChoices), - Param(PSTR("mag_filter_type"), &c.magFilter.type, filterTypeChoices), - Param(PSTR("mag_filter_lpf"), &c.magFilter.freq), - Param(PSTR("mag_offset_x"), &c.magCalibrationOffset[0]), - Param(PSTR("mag_offset_y"), &c.magCalibrationOffset[1]), - Param(PSTR("mag_offset_z"), &c.magCalibrationOffset[2]), - Param(PSTR("mag_scale_x"), &c.magCalibrationScale[0]), - Param(PSTR("mag_scale_y"), &c.magCalibrationScale[1]), - Param(PSTR("mag_scale_z"), &c.magCalibrationScale[2]), - - Param(PSTR("baro_bus"), &c.baroBus, busDevChoices), - Param(PSTR("baro_dev"), &c.baroDev, baroDevChoices), - Param(PSTR("baro_lpf_type"), &c.baroFilter.type, filterTypeChoices), - Param(PSTR("baro_lpf_freq"), &c.baroFilter.freq), + Param(PSTR("accel_bus"), &c.accel.bus, busDevChoices), + Param(PSTR("accel_dev"), &c.accel.dev, gyroDevChoices), + Param(PSTR("accel_lpf_type"), &c.accel.filter.type, filterTypeChoices), + Param(PSTR("accel_lpf_freq"), &c.accel.filter.freq), + Param(PSTR("accel_offset_x"), &c.accel.bias[0]), + Param(PSTR("accel_offset_y"), &c.accel.bias[1]), + Param(PSTR("accel_offset_z"), &c.accel.bias[2]), + + Param(PSTR("mag_bus"), &c.mag.bus, busDevChoices), + Param(PSTR("mag_dev"), &c.mag.dev, magDevChoices), + Param(PSTR("mag_align"), &c.mag.align, alignChoices), + Param(PSTR("mag_filter_type"), &c.mag.filter.type, filterTypeChoices), + Param(PSTR("mag_filter_lpf"), &c.mag.filter.freq), + Param(PSTR("mag_offset_x"), &c.mag.offset[0]), + Param(PSTR("mag_offset_y"), &c.mag.offset[1]), + Param(PSTR("mag_offset_z"), &c.mag.offset[2]), + Param(PSTR("mag_scale_x"), &c.mag.scale[0]), + Param(PSTR("mag_scale_y"), &c.mag.scale[1]), + Param(PSTR("mag_scale_z"), &c.mag.scale[2]), + + Param(PSTR("baro_bus"), &c.baro.bus, busDevChoices), + Param(PSTR("baro_dev"), &c.baro.dev, baroDevChoices), + Param(PSTR("baro_lpf_type"), &c.baro.filter.type, filterTypeChoices), + Param(PSTR("baro_lpf_freq"), &c.baro.filter.freq), Param(PSTR("board_align_roll"), &c.boardAlignment[0]), Param(PSTR("board_align_pitch"), &c.boardAlignment[1]), @@ -1045,25 +1044,25 @@ class Cli s.print(Math::toDeg(_model.state.gyroBias[2])); s.println(F("]")); s.print(F("accel offset: ")); - s.print(_model.config.accelBias[0]); s.print(' '); - s.print(_model.config.accelBias[1]); s.print(' '); - s.print(_model.config.accelBias[2]); s.print(F(" [")); + s.print(_model.config.accel.bias[0]); s.print(' '); + s.print(_model.config.accel.bias[1]); s.print(' '); + s.print(_model.config.accel.bias[2]); s.print(F(" [")); s.print(_model.state.accelBias[0]); s.print(' '); s.print(_model.state.accelBias[1]); s.print(' '); s.print(_model.state.accelBias[2]); s.println(F("]")); s.print(F(" mag offset: ")); - s.print(_model.config.magCalibrationOffset[0]); s.print(' '); - s.print(_model.config.magCalibrationOffset[1]); s.print(' '); - s.print(_model.config.magCalibrationOffset[2]); s.print(F(" [")); + s.print(_model.config.mag.offset[0]); s.print(' '); + s.print(_model.config.mag.offset[1]); s.print(' '); + s.print(_model.config.mag.offset[2]); s.print(F(" [")); s.print(_model.state.magCalibrationOffset[0]); s.print(' '); s.print(_model.state.magCalibrationOffset[1]); s.print(' '); s.print(_model.state.magCalibrationOffset[2]); s.println(F("]")); s.print(F(" mag scale: ")); - s.print(_model.config.magCalibrationScale[0]); s.print(' '); - s.print(_model.config.magCalibrationScale[1]); s.print(' '); - s.print(_model.config.magCalibrationScale[2]); s.print(F(" [")); + s.print(_model.config.mag.scale[0]); s.print(' '); + s.print(_model.config.mag.scale[1]); s.print(' '); + s.print(_model.config.mag.scale[2]); s.print(F(" [")); s.print(_model.state.magCalibrationScale[0]); s.print(' '); s.print(_model.state.magCalibrationScale[1]); s.print(' '); s.print(_model.state.magCalibrationScale[2]); s.println(F("]")); diff --git a/lib/Espfc/src/Hardware.h b/lib/Espfc/src/Hardware.h index 4a41b225..307a51a0 100644 --- a/lib/Espfc/src/Hardware.h +++ b/lib/Espfc/src/Hardware.h @@ -129,13 +129,13 @@ class Hardware detectedGyro->setDLPFMode(_model.config.gyro.dlpf); _model.state.gyroDev = detectedGyro; _model.state.gyroPresent = (bool)detectedGyro; - _model.state.accelPresent = _model.state.gyroPresent && _model.config.accelDev != GYRO_NONE; + _model.state.accelPresent = _model.state.gyroPresent && _model.config.accel.dev != GYRO_NONE; _model.state.gyroClock = detectedGyro->getRate(); } void detectMag() { - if(_model.config.magDev == MAG_NONE) return; + if(_model.config.mag.dev == MAG_NONE) return; Device::MagDevice * detectedMag = nullptr; #if defined(ESPFC_I2C_0) @@ -160,7 +160,7 @@ class Hardware void detectBaro() { - if(_model.config.baroDev == BARO_NONE) return; + if(_model.config.baro.dev == BARO_NONE) return; Device::BaroDevice * detectedBaro = nullptr; #if defined(ESPFC_SPI_0) diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index b8ffadc8..b83892f2 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -114,17 +114,17 @@ class Model bool accelActive() const { - return state.accelPresent && config.accelDev != GYRO_NONE; + return state.accelPresent && config.accel.dev != GYRO_NONE; } bool magActive() const { - return state.magPresent && config.magDev != MAG_NONE; + return state.magPresent && config.mag.dev != MAG_NONE; } bool baroActive() const { - return state.baroPresent && config.baroDev != BARO_NONE; + return state.baroPresent && config.baro.dev != BARO_NONE; } bool calibrationActive() const @@ -314,7 +314,7 @@ class Model } int loopSyncMax = 1; - //if(config.magDev != MAG_NONE || config.baroDev != BARO_NONE) loopSyncMax /= 2; + //if(config.mag.dev != MAG_NONE || config.baro.dev != BARO_NONE) loopSyncMax /= 2; config.loopSync = std::max((int)config.loopSync, loopSyncMax); state.loopRate = state.gyroRate / config.loopSync; @@ -473,7 +473,7 @@ class Model } state.gyroFilter2[i].begin(config.gyro.filter2, gyroFilterRate); state.gyroFilter3[i].begin(config.gyro.filter3, gyroPreFilterRate); - state.accelFilter[i].begin(config.accelFilter, gyroFilterRate); + state.accelFilter[i].begin(config.accel.filter, gyroFilterRate); state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accelTimer.rate / 3), gyroFilterRate); for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) { @@ -486,7 +486,7 @@ class Model } if(magActive()) { - state.magFilter[i].begin(config.magFilter, state.magTimer.rate); + state.magFilter[i].begin(config.mag.filter, state.magTimer.rate); } } @@ -575,9 +575,9 @@ class Model for(size_t i = 0; i <= AXIS_YAW; i++) { state.gyroBias.set(i, config.gyro.bias[i] / 1000.0f); - state.accelBias.set(i, config.accelBias[i] / 1000.0f); - state.magCalibrationOffset.set(i, config.magCalibrationOffset[i] / 10.0f); - state.magCalibrationScale.set(i, config.magCalibrationScale[i] / 1000.0f); + state.accelBias.set(i, config.accel.bias[i] / 1000.0f); + state.magCalibrationOffset.set(i, config.mag.offset[i] / 10.0f); + state.magCalibrationScale.set(i, config.mag.scale[i] / 1000.0f); } } @@ -587,9 +587,9 @@ class Model for(size_t i = 0; i < 3; i++) { config.gyro.bias[i] = lrintf(state.gyroBias[i] * 1000.0f); - config.accelBias[i] = lrintf(state.accelBias[i] * 1000.0f); - config.magCalibrationOffset[i] = lrintf(state.magCalibrationOffset[i] * 10.0f); - config.magCalibrationScale[i] = lrintf(state.magCalibrationScale[i] * 1000.0f); + config.accel.bias[i] = lrintf(state.accelBias[i] * 1000.0f); + config.mag.offset[i] = lrintf(state.magCalibrationOffset[i] * 10.0f); + config.mag.scale[i] = lrintf(state.magCalibrationScale[i] * 1000.0f); } } diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 23e9c580..21338d2a 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -564,42 +564,53 @@ struct GyroConfig int8_t dev = GYRO_AUTO; int8_t dlpf = GYRO_DLPF_256; int8_t align = ALIGN_DEFAULT; - FilterConfig filter{FILTER_PT1, 100}; FilterConfig filter2{FILTER_PT1, 213}; FilterConfig filter3{FILTER_FO, 150}; FilterConfig notch1Filter{FILTER_NOTCH, 0, 0}; FilterConfig notch2Filter{FILTER_NOTCH, 0, 0}; FilterConfig dynLpfFilter{FILTER_PT1, 425, 170}; + int16_t bias[3] = { 0, 0, 0 }; +}; +struct AccelConfig +{ + int8_t bus = BUS_AUTO; + int8_t dev = GYRO_AUTO; + FilterConfig filter{FILTER_BIQUAD, 15}; int16_t bias[3] = { 0, 0, 0 }; }; +struct BaroConfig +{ + int8_t bus = BUS_AUTO; + int8_t dev = BARO_NONE; + FilterConfig filter{FILTER_BIQUAD, 5}; +}; + +struct MagConfig +{ + int8_t bus = BUS_AUTO; + int8_t dev = MAG_NONE; + int8_t align = ALIGN_DEFAULT; + FilterConfig filter{FILTER_BIQUAD, 10}; + int16_t scale[3] = { 0, 0, 0 }; + int16_t offset[3] = { 1000, 1000, 1000 }; +}; + // persistent data class ModelConfig { public: GyroConfig gyro; - - int8_t accelBus = BUS_AUTO; - int8_t accelDev = GYRO_AUTO; - int8_t accelAlign = ALIGN_DEFAULT; - FilterConfig accelFilter{FILTER_BIQUAD, 15}; - - int8_t magBus = BUS_AUTO; - int8_t magDev = MAG_NONE; - int8_t magAlign = ALIGN_DEFAULT; - FilterConfig magFilter{FILTER_BIQUAD, 10}; - - int8_t baroBus = BUS_AUTO; - int8_t baroDev = BARO_NONE; - FilterConfig baroFilter{FILTER_BIQUAD, 10}; + AccelConfig accel; + BaroConfig baro; + MagConfig mag; InputConfig input; FailsafeConfig failsafe; ActuatorCondition conditions[ACTUATOR_CONDITIONS]; - ScalerConfig scaler[SCALER_COUNT]; OutputConfig output; @@ -653,10 +664,6 @@ class ModelConfig FusionConfig fusion; - int16_t accelBias[3] = { 0, 0, 0 }; - int16_t magCalibrationScale[3] = { 0, 0, 0 }; - int16_t magCalibrationOffset[3] = { 1000, 1000, 1000 }; - char modelName[MODEL_NAME_LEN + 1]; VBatConfig vbat; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 0a8f2845..3d12f626 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -519,22 +519,22 @@ class MspProcessor break; case MSP_SENSOR_CONFIG: - r.writeU8(_model.config.accelDev); // 3 acc mpu6050 - r.writeU8(_model.config.baroDev); // 2 baro bmp085 - r.writeU8(_model.config.magDev); // 3 mag hmc5883l + r.writeU8(_model.config.accel.dev); // 3 acc mpu6050 + r.writeU8(_model.config.baro.dev); // 2 baro bmp085 + r.writeU8(_model.config.mag.dev); // 3 mag hmc5883l break; case MSP_SET_SENSOR_CONFIG: - _model.config.accelDev = m.readU8(); // 3 acc mpu6050 - _model.config.baroDev = m.readU8(); // 2 baro bmp085 - _model.config.magDev = m.readU8(); // 3 mag hmc5883l + _model.config.accel.dev = m.readU8(); // 3 acc mpu6050 + _model.config.baro.dev = m.readU8(); // 2 baro bmp085 + _model.config.mag.dev = m.readU8(); // 3 mag hmc5883l _model.reload(); break; case MSP_SENSOR_ALIGNMENT: r.writeU8(_model.config.gyro.align); // gyro align r.writeU8(_model.config.gyro.align); // acc align, Starting with 4.0 gyro and acc alignment are the same - r.writeU8(_model.config.magAlign); // mag align + r.writeU8(_model.config.mag.align); // mag align //1.41+ r.writeU8(_model.state.gyroPresent ? 1 : 0); // gyro detection mask GYRO_1_MASK r.writeU8(0); // gyro_to_use @@ -546,7 +546,7 @@ class MspProcessor { uint8_t gyroAlign = m.readU8(); // gyro align m.readU8(); // discard deprecated acc align - _model.config.magAlign = m.readU8(); // mag align + _model.config.mag.align = m.readU8(); // mag align // API >= 1.41 - support the gyro_to_use and alignment for gyros 1 & 2 if(m.remain() >= 3) { diff --git a/lib/Espfc/src/Sensor/BaroSensor.h b/lib/Espfc/src/Sensor/BaroSensor.h index a789b5c1..05704c10 100644 --- a/lib/Espfc/src/Sensor/BaroSensor.h +++ b/lib/Espfc/src/Sensor/BaroSensor.h @@ -38,9 +38,9 @@ class BaroSensor: public BaseSensor _temperatureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baroRate * 0.1f), _model.state.baroRate); _pressureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baroRate * 0.1f), _model.state.baroRate); - _altitudeFilter.begin(_model.config.baroFilter, _model.state.baroRate); + _altitudeFilter.begin(_model.config.baro.filter, _model.state.baroRate); - _model.logger.info().log(F("BARO INIT")).log(FPSTR(Device::BaroDevice::getName(_baro->getType()))).log(_baro->getAddress()).log(toGyroRate).log(_model.state.baroRate).logln(_model.config.baroFilter.freq); + _model.logger.info().log(F("BARO INIT")).log(FPSTR(Device::BaroDevice::getName(_baro->getType()))).log(_baro->getAddress()).log(toGyroRate).log(_model.state.baroRate).logln(_model.config.baro.filter.freq); return 1; } diff --git a/lib/Espfc/src/Sensor/MagSensor.h b/lib/Espfc/src/Sensor/MagSensor.h index 99853d0c..f228c69c 100644 --- a/lib/Espfc/src/Sensor/MagSensor.h +++ b/lib/Espfc/src/Sensor/MagSensor.h @@ -60,7 +60,7 @@ class MagSensor: public BaseSensor _model.state.mag = _mag->convert(_model.state.magRaw); - align(_model.state.mag, _model.config.magAlign); + align(_model.state.mag, _model.config.mag.align); _model.state.mag = _model.state.boardAlignment.apply(_model.state.mag); for(size_t i = 0; i < 3; i++) From e9386327185e08d05273b0fd73c6d19e13bd1b2e Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 27 Oct 2024 21:03:10 +0100 Subject: [PATCH 23/68] dterm iterm yaw mixer level config refactor --- lib/Espfc/src/Actuator.h | 4 +- lib/Espfc/src/Blackbox/Blackbox.cpp | 24 +++++----- lib/Espfc/src/Cli.h | 48 +++++++++---------- lib/Espfc/src/Controller.cpp | 18 ++++---- lib/Espfc/src/Model.h | 28 +++++------ lib/Espfc/src/ModelConfig.h | 72 ++++++++++++++++++----------- lib/Espfc/src/Msp/MspProcessor.h | 64 ++++++++++++------------- lib/Espfc/src/Output/Mixer.cpp | 4 +- test/test_fc/test_fc.cpp | 8 ++-- 9 files changed, 144 insertions(+), 126 deletions(-) diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h index 147bd764..f3d22c13 100644 --- a/lib/Espfc/src/Actuator.h +++ b/lib/Espfc/src/Actuator.h @@ -227,8 +227,8 @@ class Actuator _model.state.gyroFilter[i].reconfigure(gyroFreq); } } - if(_model.config.dtermDynLpfFilter.cutoff > 0) { - int dtermFreq = Math::map(scale, 1000, 2000, _model.config.dtermDynLpfFilter.cutoff, _model.config.dtermDynLpfFilter.freq); + if(_model.config.dterm.dynLpfFilter.cutoff > 0) { + int dtermFreq = Math::map(scale, 1000, 2000, _model.config.dterm.dynLpfFilter.cutoff, _model.config.dterm.dynLpfFilter.freq); for(size_t i = 0; i <= AXIS_YAW; i++) { _model.state.innerPid[i].dtermFilter.reconfigure(dtermFreq); } diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index 56eeafea..5a913461 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -66,15 +66,15 @@ int Blackbox::begin() } } 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->dterm_lpf1_type = _model.config.dterm.filter.type; + cp->dterm_lpf1_static_hz = _model.config.dterm.filter.freq; + cp->dterm_lpf1_dyn_min_hz = _model.config.dterm.dynLpfFilter.cutoff; + cp->dterm_lpf1_dyn_max_hz = _model.config.dterm.dynLpfFilter.freq; + cp->dterm_lpf2_type = _model.config.dterm.filter2.type; + cp->dterm_lpf2_static_hz = _model.config.dterm.filter2.freq; + cp->dterm_notch_hz = _model.config.dterm.notchFilter.freq; + cp->dterm_notch_cutoff = _model.config.dterm.notchFilter.cutoff; + cp->yaw_lowpass_hz = _model.config.yaw.filter.freq; cp->itermWindupPointPercent = 80; cp->antiGravityMode = 0; cp->pidSumLimit = 660; @@ -92,14 +92,14 @@ int Blackbox::begin() cp->anti_gravity_cutoff_hz = 100; cp->d_min_gain = 0; cp->d_min_advance = 0; - cp->angle_limit = _model.config.angleLimit; + cp->angle_limit = _model.config.level.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 = _model.config.iterm.relax; cp->iterm_relax_type = 1; - cp->iterm_relax_cutoff = _model.config.itermRelaxCutoff; + cp->iterm_relax_cutoff = _model.config.iterm.relaxCutoff; cp->dterm_lpf1_dyn_expo = 5; cp->tpa_low_rate = 20; cp->tpa_low_breakpoint = 1050; diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 1c31ab61..f47934f0 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -609,34 +609,34 @@ class Cli Param(PSTR("pid_level_i"), &c.pid[FC_PID_LEVEL].I), Param(PSTR("pid_level_d"), &c.pid[FC_PID_LEVEL].D), - Param(PSTR("pid_level_angle_limit"), &c.angleLimit), - Param(PSTR("pid_level_rate_limit"), &c.angleRateLimit), - Param(PSTR("pid_level_lpf_type"), &c.levelPtermFilter.type, filterTypeChoices), - Param(PSTR("pid_level_lpf_freq"), &c.levelPtermFilter.freq), - - Param(PSTR("pid_yaw_lpf_type"), &c.yawFilter.type, filterTypeChoices), - Param(PSTR("pid_yaw_lpf_freq"), &c.yawFilter.freq), - - Param(PSTR("pid_dterm_lpf_type"), &c.dtermFilter.type, filterTypeChoices), - Param(PSTR("pid_dterm_lpf_freq"), &c.dtermFilter.freq), - Param(PSTR("pid_dterm_lpf2_type"), &c.dtermFilter2.type, filterTypeChoices), - Param(PSTR("pid_dterm_lpf2_freq"), &c.dtermFilter2.freq), - Param(PSTR("pid_dterm_notch_freq"), &c.dtermNotchFilter.freq), - Param(PSTR("pid_dterm_notch_cutoff"), &c.dtermNotchFilter.cutoff), - Param(PSTR("pid_dterm_dyn_lpf_min"), &c.dtermDynLpfFilter.cutoff), - Param(PSTR("pid_dterm_dyn_lpf_max"), &c.dtermDynLpfFilter.freq), - - Param(PSTR("pid_dterm_weight"), &c.dtermSetpointWeight), - 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), + Param(PSTR("pid_level_angle_limit"), &c.level.angleLimit), + Param(PSTR("pid_level_rate_limit"), &c.level.rateLimit), + Param(PSTR("pid_level_lpf_type"), &c.level.ptermFilter.type, filterTypeChoices), + Param(PSTR("pid_level_lpf_freq"), &c.level.ptermFilter.freq), + + Param(PSTR("pid_yaw_lpf_type"), &c.yaw.filter.type, filterTypeChoices), + Param(PSTR("pid_yaw_lpf_freq"), &c.yaw.filter.freq), + + Param(PSTR("pid_dterm_lpf_type"), &c.dterm.filter.type, filterTypeChoices), + Param(PSTR("pid_dterm_lpf_freq"), &c.dterm.filter.freq), + Param(PSTR("pid_dterm_lpf2_type"), &c.dterm.filter2.type, filterTypeChoices), + Param(PSTR("pid_dterm_lpf2_freq"), &c.dterm.filter2.freq), + Param(PSTR("pid_dterm_notch_freq"), &c.dterm.notchFilter.freq), + Param(PSTR("pid_dterm_notch_cutoff"), &c.dterm.notchFilter.cutoff), + Param(PSTR("pid_dterm_dyn_lpf_min"), &c.dterm.dynLpfFilter.cutoff), + Param(PSTR("pid_dterm_dyn_lpf_max"), &c.dterm.dynLpfFilter.freq), + + Param(PSTR("pid_dterm_weight"), &c.dterm.setpointWeight), + Param(PSTR("pid_iterm_limit"), &c.iterm.limit), + Param(PSTR("pid_iterm_zero"), &c.iterm.lowThrottleZeroIterm), + Param(PSTR("pid_iterm_relax"), &c.iterm.relax, inputItermRelaxChoices), + Param(PSTR("pid_iterm_relax_cutoff"), &c.iterm.relaxCutoff), Param(PSTR("pid_tpa_scale"), &c.tpaScale), Param(PSTR("pid_tpa_breakpoint"), &c.tpaBreakpoint), Param(PSTR("mixer_sync"), &c.mixerSync), - Param(PSTR("mixer_type"), &c.mixerType, mixerTypeChoices), - Param(PSTR("mixer_yaw_reverse"), &c.yawReverse), + Param(PSTR("mixer_type"), &c.mixer.type, mixerTypeChoices), + Param(PSTR("mixer_yaw_reverse"), &c.mixer.yawReverse), Param(PSTR("mixer_throttle_limit_type"), &c.output.throttleLimitType, throtleLimitTypeChoices), Param(PSTR("mixer_throttle_limit_percent"), &c.output.throttleLimitPercent), Param(PSTR("mixer_output_limit"), &c.output.motorLimit), diff --git a/lib/Espfc/src/Controller.cpp b/lib/Espfc/src/Controller.cpp index e3e81014..d44fb634 100644 --- a/lib/Espfc/src/Controller.cpp +++ b/lib/Espfc/src/Controller.cpp @@ -24,7 +24,7 @@ int FAST_CODE_ATTR Controller::update() { Stats::Measure(_model.state.stats, COUNTER_OUTER_PID); resetIterm(); - if(_model.config.mixerType == FC_MIXER_GIMBAL) + if(_model.config.mixer.type == FC_MIXER_GIMBAL) { outerLoopRobot(); } @@ -36,7 +36,7 @@ int FAST_CODE_ATTR Controller::update() { Stats::Measure(_model.state.stats, COUNTER_INNER_PID); - if(_model.config.mixerType == FC_MIXER_GIMBAL) + if(_model.config.mixer.type == FC_MIXER_GIMBAL) { innerLoopRobot(); } @@ -63,14 +63,14 @@ void Controller::outerLoopRobot() if(true || _model.isActive(MODE_ANGLE)) { - angle = _model.state.input[AXIS_PITCH] * radians(_model.config.angleLimit); + angle = _model.state.input[AXIS_PITCH] * Math::toRad(_model.config.level.angleLimit); } else { - angle = _model.state.outerPid[AXIS_PITCH].update(_model.state.input[AXIS_PITCH], speed) * radians(_model.config.angleRateLimit); + angle = _model.state.outerPid[AXIS_PITCH].update(_model.state.input[AXIS_PITCH], speed) * Math::toRad(_model.config.level.rateLimit); } _model.state.desiredAngle.set(AXIS_PITCH, angle); - _model.state.desiredRate[AXIS_YAW] = _model.state.input[AXIS_YAW] * radians(_model.config.angleRateLimit); + _model.state.desiredRate[AXIS_YAW] = _model.state.input[AXIS_YAW] * Math::toRad(_model.config.level.rateLimit); if(_model.config.debugMode == DEBUG_ANGLERATE) { @@ -86,7 +86,7 @@ void Controller::innerLoopRobot() //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); + const bool stabilize = angle < Math::toRad(_model.config.level.angleLimit); if(stabilize) { _model.state.output[AXIS_PITCH] = _model.state.innerPid[AXIS_PITCH].update(_model.state.desiredAngle[AXIS_PITCH], _model.state.angle[AXIS_PITCH]); @@ -111,8 +111,8 @@ 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.input[AXIS_ROLL] * Math::toRad(_model.config.level.angleLimit), + _model.state.input[AXIS_PITCH] * Math::toRad(_model.config.level.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]); @@ -167,7 +167,7 @@ float Controller::getTpaFactor() const 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) + || (!_model.isAirModeActive() && _model.config.iterm.lowThrottleZeroIterm && _model.isThrottleLow()) // on low throttle (not in air mode) ) { for(size_t i = 0; i < AXES; i++) diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index b83892f2..d1467067 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -512,7 +512,7 @@ class Model // configure PIDs float pidScale[] = { 1.f, 1.f, 1.f }; - if(config.mixerType == FC_MIXER_GIMBAL) + if(config.mixer.type == FC_MIXER_GIMBAL) { pidScale[AXIS_YAW] = 0.2f; // ROBOT pidScale[AXIS_PITCH] = 20.f; // ROBOT @@ -526,23 +526,23 @@ 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 = config.itermLimit * 0.01f; + pid.iLimit = config.iterm.limit * 0.01f; pid.oLimit = 0.66f; pid.rate = state.loopTimer.rate; - pid.dtermNotchFilter.begin(config.dtermNotchFilter, pidFilterRate); - if(config.dtermDynLpfFilter.cutoff > 0) { - pid.dtermFilter.begin(FilterConfig((FilterType)config.dtermFilter.type, config.dtermDynLpfFilter.cutoff), pidFilterRate); + pid.dtermNotchFilter.begin(config.dterm.notchFilter, pidFilterRate); + if(config.dterm.dynLpfFilter.cutoff > 0) { + pid.dtermFilter.begin(FilterConfig((FilterType)config.dterm.filter.type, config.dterm.dynLpfFilter.cutoff), pidFilterRate); } else { - pid.dtermFilter.begin(config.dtermFilter, pidFilterRate); + pid.dtermFilter.begin(config.dterm.filter, pidFilterRate); } - pid.dtermFilter2.begin(config.dtermFilter2, pidFilterRate); + pid.dtermFilter2.begin(config.dterm.filter2, pidFilterRate); pid.ftermFilter.begin(config.input.filterDerivative, pidFilterRate); - pid.itermRelaxFilter.begin(FilterConfig(FILTER_PT1, config.itermRelaxCutoff), pidFilterRate); + pid.itermRelaxFilter.begin(FilterConfig(FILTER_PT1, config.iterm.relaxCutoff), pidFilterRate); if(i == AXIS_YAW) { - pid.itermRelax = config.itermRelax == ITERM_RELAX_RPY || config.itermRelax == ITERM_RELAX_RPY_INC ? config.itermRelax : ITERM_RELAX_OFF; - pid.ptermFilter.begin(config.yawFilter, pidFilterRate); + pid.itermRelax = config.iterm.relax == ITERM_RELAX_RPY || config.iterm.relax == ITERM_RELAX_RPY_INC ? config.iterm.relax : ITERM_RELAX_OFF; + pid.ptermFilter.begin(config.yaw.filter, pidFilterRate); } else { - pid.itermRelax = config.itermRelax; + pid.itermRelax = config.iterm.relax; } pid.begin(); } @@ -555,10 +555,10 @@ class Model pid.Ki = (float)pc.I * LEVEL_ITERM_SCALE; pid.Kd = (float)pc.D * LEVEL_DTERM_SCALE; pid.Kf = (float)pc.F * LEVEL_FTERM_SCALE; - pid.iLimit = Math::toRad(config.angleRateLimit) * 0.1f; - pid.oLimit = Math::toRad(config.angleRateLimit); + pid.iLimit = Math::toRad(config.level.rateLimit) * 0.1f; + pid.oLimit = Math::toRad(config.level.rateLimit); pid.rate = state.loopTimer.rate; - pid.ptermFilter.begin(config.levelPtermFilter, pidFilterRate); + pid.ptermFilter.begin(config.level.ptermFilter, pidFilterRate); //pid.iLimit = 0.3f; // ROBOT //pid.oLimit = 1.f; // ROBOT pid.begin(); diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 21338d2a..9b071e12 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -598,6 +598,41 @@ struct MagConfig int16_t offset[3] = { 1000, 1000, 1000 }; }; +struct YawConfig +{ + FilterConfig filter{FILTER_PT1, 90}; +}; + +struct DtermConfig +{ + FilterConfig filter{FILTER_PT1, 128}; + FilterConfig filter2{FILTER_PT1, 128}; + FilterConfig notchFilter{FILTER_NOTCH, 0, 0}; + FilterConfig dynLpfFilter{FILTER_PT1, 145, 60}; + int16_t setpointWeight = 30; +}; + +struct ItermConfig +{ + int8_t limit = 30; + int8_t relax = ITERM_RELAX_RP; + int8_t relaxCutoff = 15; + bool lowThrottleZeroIterm = true; +}; + +struct LevelConfig +{ + FilterConfig ptermFilter{FILTER_PT1, 90}; + int8_t angleLimit = 55; + int16_t rateLimit = 300; +}; + +struct MixerConfiguration +{ + int8_t type = FC_MIXER_QUADX; + bool yawReverse = 0; +}; + // persistent data class ModelConfig { @@ -615,9 +650,6 @@ class ModelConfig OutputConfig output; - int8_t mixerType = FC_MIXER_QUADX; - bool yawReverse = 0; - PidConfig pid[FC_PID_ITEM_COUNT] = { [FC_PID_ROLL] = { .P = 42, .I = 85, .D = 24, .F = 72 }, // ROLL [FC_PID_PITCH] = { .P = 46, .I = 90, .D = 26, .F = 76 }, // PITCH @@ -631,32 +663,18 @@ class ModelConfig [FC_PID_VEL] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // VEL }; - FilterConfig yawFilter{FILTER_PT1, 90}; - - FilterConfig dtermFilter{FILTER_PT1, 128}; - FilterConfig dtermFilter2{FILTER_PT1, 128}; - FilterConfig dtermNotchFilter{FILTER_NOTCH, 0, 0}; - FilterConfig dtermDynLpfFilter{FILTER_PT1, 145, 60}; - FilterConfig levelPtermFilter{FILTER_PT1, 90}; - - int16_t dtermSetpointWeight = 30; - int8_t itermLimit = 30; - int8_t itermRelax = ITERM_RELAX_RP; - int8_t itermRelaxCutoff = 15; - - int8_t angleLimit = 55; - int16_t angleRateLimit = 300; + MixerConfiguration mixer; + YawConfig yaw; + LevelConfig level; + DtermConfig dterm; + ItermConfig iterm; int8_t loopSync = 8; // MPU 1000Hz int8_t mixerSync = 1; int32_t featureMask = ESPFC_FEATURE_MASK; - - bool lowThrottleZeroIterm = true; - bool telemetry = 0; int32_t telemetryInterval = 1000; - int8_t telemetryPort; // unused BlackboxConfig blackbox; @@ -854,7 +872,7 @@ class ModelConfig void brobot() { - mixerType = FC_MIXER_GIMBAL; + mixer.type = FC_MIXER_GIMBAL; pin[PIN_OUTPUT_0] = 14; // D5 // ROBOT pin[PIN_OUTPUT_1] = 12; // D6 // ROBOT @@ -865,10 +883,10 @@ class ModelConfig //fusionMode = FUSION_COMPLEMENTARY; // ROBOT //accelFilter.freq = 30; // ROBOT - lowThrottleZeroIterm = false; // ROBOT - itermLimit = 10; // ROBOT - dtermSetpointWeight = 0; // ROBOT - angleLimit = 10; // deg // ROBOT + iterm.lowThrottleZeroIterm = false; // ROBOT + iterm.limit = 10; // ROBOT + dterm.setpointWeight = 0; // ROBOT + level.angleLimit = 10; // deg // ROBOT output.protocol = ESC_PROTOCOL_PWM; // ROBOT output.rate = 100; // ROBOT diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 3d12f626..e4d15bf9 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -509,13 +509,13 @@ class MspProcessor break; case MSP_MIXER_CONFIG: - r.writeU8(_model.config.mixerType); // mixerMode, QUAD_X - r.writeU8(_model.config.yawReverse); // yaw_motors_reversed + r.writeU8(_model.config.mixer.type); // mixerMode, QUAD_X + r.writeU8(_model.config.mixer.yawReverse); // yaw_motors_reversed break; case MSP_SET_MIXER_CONFIG: - _model.config.mixerType = m.readU8(); // mixerMode, QUAD_X - _model.config.yawReverse = m.readU8(); // yaw_motors_reversed + _model.config.mixer.type = m.readU8(); // mixerMode, QUAD_X + _model.config.mixer.yawReverse = m.readU8(); // yaw_motors_reversed break; case MSP_SENSOR_CONFIG: @@ -1061,28 +1061,28 @@ class MspProcessor case MSP_FILTER_CONFIG: r.writeU8(_model.config.gyro.filter.freq); // gyro lpf - r.writeU16(_model.config.dtermFilter.freq); // dterm lpf - r.writeU16(_model.config.yawFilter.freq); // yaw lpf + r.writeU16(_model.config.dterm.filter.freq); // dterm lpf + r.writeU16(_model.config.yaw.filter.freq); // yaw lpf r.writeU16(_model.config.gyro.notch1Filter.freq); // gyro notch 1 hz r.writeU16(_model.config.gyro.notch1Filter.cutoff); // gyro notch 1 cutoff - r.writeU16(_model.config.dtermNotchFilter.freq); // dterm notch hz - r.writeU16(_model.config.dtermNotchFilter.cutoff); // dterm notch cutoff + r.writeU16(_model.config.dterm.notchFilter.freq); // dterm notch hz + r.writeU16(_model.config.dterm.notchFilter.cutoff); // dterm notch cutoff r.writeU16(_model.config.gyro.notch2Filter.freq); // gyro notch 2 hz r.writeU16(_model.config.gyro.notch2Filter.cutoff); // gyro notch 2 cutoff - r.writeU8(_model.config.dtermFilter.type); // dterm type + r.writeU8(_model.config.dterm.filter.type); // dterm type r.writeU8(fromGyroDlpf(_model.config.gyro.dlpf)); r.writeU8(0); // dlfp 32khz type r.writeU16(_model.config.gyro.filter.freq); // lowpass1 freq r.writeU16(_model.config.gyro.filter2.freq); // lowpass2 freq r.writeU8(_model.config.gyro.filter.type); // lowpass1 type r.writeU8(_model.config.gyro.filter2.type); // lowpass2 type - r.writeU16(_model.config.dtermFilter2.freq); // dterm lopwass2 freq + r.writeU16(_model.config.dterm.filter2.freq); // dterm lopwass2 freq // 1.41+ - r.writeU8(_model.config.dtermFilter2.type); // dterm lopwass2 type + r.writeU8(_model.config.dterm.filter2.type); // dterm lopwass2 type r.writeU16(_model.config.gyro.dynLpfFilter.cutoff); // dyn lpf gyro min r.writeU16(_model.config.gyro.dynLpfFilter.freq); // dyn lpf gyro max - r.writeU16(_model.config.dtermDynLpfFilter.cutoff); // dyn lpf dterm min - r.writeU16(_model.config.dtermDynLpfFilter.freq); // dyn lpf dterm max + r.writeU16(_model.config.dterm.dynLpfFilter.cutoff); // dyn lpf dterm min + r.writeU16(_model.config.dterm.dynLpfFilter.freq); // dyn lpf dterm max // gyro analyse r.writeU8(3); // deprecated dyn notch range r.writeU8(_model.config.dynamicFilter.width); // dyn_notch_width_percent @@ -1097,20 +1097,20 @@ class MspProcessor case MSP_SET_FILTER_CONFIG: _model.config.gyro.filter.freq = m.readU8(); - _model.config.dtermFilter.freq = m.readU16(); - _model.config.yawFilter.freq = m.readU16(); + _model.config.dterm.filter.freq = m.readU16(); + _model.config.yaw.filter.freq = m.readU16(); if (m.remain() >= 8) { _model.config.gyro.notch1Filter.freq = m.readU16(); _model.config.gyro.notch1Filter.cutoff = m.readU16(); - _model.config.dtermNotchFilter.freq = m.readU16(); - _model.config.dtermNotchFilter.cutoff = m.readU16(); + _model.config.dterm.notchFilter.freq = m.readU16(); + _model.config.dterm.notchFilter.cutoff = m.readU16(); } if (m.remain() >= 4) { _model.config.gyro.notch2Filter.freq = m.readU16(); _model.config.gyro.notch2Filter.cutoff = m.readU16(); } if (m.remain() >= 1) { - _model.config.dtermFilter.type = (FilterType)m.readU8(); + _model.config.dterm.filter.type = (FilterType)m.readU8(); } if (m.remain() >= 10) { m.readU8(); // dlfp type @@ -1119,15 +1119,15 @@ class MspProcessor _model.config.gyro.filter2.freq = m.readU16(); _model.config.gyro.filter.type = m.readU8(); _model.config.gyro.filter2.type = m.readU8(); - _model.config.dtermFilter2.freq = m.readU16(); + _model.config.dterm.filter2.freq = m.readU16(); } // 1.41+ if (m.remain() >= 9) { - _model.config.dtermFilter2.type = m.readU8(); + _model.config.dterm.filter2.type = m.readU8(); _model.config.gyro.dynLpfFilter.cutoff = m.readU16(); // dyn gyro lpf min _model.config.gyro.dynLpfFilter.freq = m.readU16(); // dyn gyro lpf max - _model.config.dtermDynLpfFilter.cutoff = m.readU16(); // dyn dterm lpf min - _model.config.dtermDynLpfFilter.freq = m.readU16(); // dyn dterm lpf min + _model.config.dterm.dynLpfFilter.cutoff = m.readU16(); // dyn dterm lpf min + _model.config.dterm.dynLpfFilter.freq = m.readU16(); // dyn dterm lpf min } if (m.remain() >= 8) { m.readU8(); // deprecated dyn_notch_range @@ -1178,20 +1178,20 @@ class MspProcessor r.writeU8(0); // reserved r.writeU8(0); // vbatPidCompensation; r.writeU8(0); // feedForwardTransition; - r.writeU8((uint8_t)std::min(_model.config.dtermSetpointWeight, (int16_t)255)); // was low byte of dtermSetpointWeight + r.writeU8((uint8_t)std::min(_model.config.dterm.setpointWeight, (int16_t)255)); // was low byte of dtermSetpointWeight r.writeU8(0); // reserved r.writeU8(0); // reserved r.writeU8(0); // reserved r.writeU16(0); // rateAccelLimit; r.writeU16(0); // yawRateAccelLimit; - r.writeU8(_model.config.angleLimit); // levelAngleLimit; + r.writeU8(_model.config.level.angleLimit); // levelAngleLimit; r.writeU8(0); // was pidProfile.levelSensitivity r.writeU16(0); // itermThrottleThreshold; r.writeU16(1000); // itermAcceleratorGain; anti_gravity_gain, 0 in 1.45+ - r.writeU16(_model.config.dtermSetpointWeight); + r.writeU16(_model.config.dterm.setpointWeight); r.writeU8(0); // iterm rotation r.writeU8(0); // smart feed forward - r.writeU8(_model.config.itermRelax); // iterm relax + r.writeU8(_model.config.iterm.relax); // iterm relax r.writeU8(1); // iterm relax type (setpoint only) r.writeU8(0); // abs control gain r.writeU8(0); // throttle boost @@ -1209,7 +1209,7 @@ class MspProcessor r.writeU8(0); // use_integrated_yaw r.writeU8(0); // integrated_yaw_relax // 1.42+ - r.writeU8(_model.config.itermRelaxCutoff); // iterm_relax_cutoff + r.writeU8(_model.config.iterm.relaxCutoff); // iterm_relax_cutoff // 1.43+ r.writeU8(_model.config.output.motorLimit); // motor_output_limit r.writeU8(0); // auto_profile_cell_count @@ -1223,14 +1223,14 @@ class MspProcessor m.readU8(); // reserved m.readU8(); m.readU8(); - _model.config.dtermSetpointWeight = m.readU8(); + _model.config.dterm.setpointWeight = m.readU8(); m.readU8(); // reserved m.readU8(); // reserved m.readU8(); // reserved m.readU16(); m.readU16(); if (m.remain() >= 2) { - _model.config.angleLimit = m.readU8(); + _model.config.level.angleLimit = m.readU8(); m.readU8(); // was pidProfile.levelSensitivity } if (m.remain() >= 4) { @@ -1238,12 +1238,12 @@ class MspProcessor m.readU16(); // itermAcceleratorGain; anti_gravity_gain } if (m.remain() >= 2) { - _model.config.dtermSetpointWeight = m.readU16(); + _model.config.dterm.setpointWeight = m.readU16(); } if (m.remain() >= 14) { m.readU8(); //iterm rotation m.readU8(); //smart feed forward - _model.config.itermRelax = m.readU8(); //iterm relax + _model.config.iterm.relax = m.readU8(); //iterm relax m.readU8(); //iterm relax type m.readU8(); //abs control gain m.readU8(); //throttle boost @@ -1265,7 +1265,7 @@ class MspProcessor } // 1.42+ if (m.remain() >= 1) { - _model.config.itermRelaxCutoff = m.readU8(); // iterm_relax_cutoff + _model.config.iterm.relaxCutoff = m.readU8(); // iterm_relax_cutoff } // 1.43+ if (m.remain() >= 3) { diff --git a/lib/Espfc/src/Output/Mixer.cpp b/lib/Espfc/src/Output/Mixer.cpp index 988a01b3..eccbed01 100644 --- a/lib/Espfc/src/Output/Mixer.cpp +++ b/lib/Espfc/src/Output/Mixer.cpp @@ -75,7 +75,7 @@ int Mixer::begin() _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); + _model.state.currentMixer = Mixers::getMixer((MixerType)_model.config.mixer.type, _model.state.customMixer); return 1; } @@ -115,7 +115,7 @@ void FAST_CODE_ATTR Mixer::updateMixer(const MixerConfig& mixer, float * outputs 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_YAW] = _model.state.output[AXIS_YAW] * (_model.config.mixer.yawReverse ? 1.f : -1.f); sources[MIXER_SOURCE_THRUST] = _model.state.output[AXIS_THRUST]; sources[MIXER_SOURCE_RC_ROLL] = _model.state.input[AXIS_ROLL]; diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index ca8740c2..538b5f21 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -143,7 +143,7 @@ void test_model_inner_pid_init() model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 1; model.config.mixerSync = 1; - model.config.mixerType = FC_MIXER_QUADX; + model.config.mixer.type = FC_MIXER_QUADX; model.config.pid[FC_PID_ROLL] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; model.config.pid[FC_PID_PITCH] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; model.config.pid[FC_PID_YAW] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; @@ -175,7 +175,7 @@ void test_model_outer_pid_init() model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 1; model.config.mixerSync = 1; - model.config.mixerType = FC_MIXER_QUADX; + model.config.mixer.type = FC_MIXER_QUADX; model.config.pid[FC_PID_LEVEL] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; model.begin(); @@ -199,7 +199,7 @@ void test_controller_rates() model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 8; model.config.mixerSync = 1; - model.config.mixerType = FC_MIXER_QUADX; + model.config.mixer.type = FC_MIXER_QUADX; model.config.input.rateType = RATES_TYPE_BETAFLIGHT; model.config.input.rate[AXIS_ROLL] = 70; @@ -249,7 +249,7 @@ void test_controller_rates_limit() model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 8; model.config.mixerSync = 1; - model.config.mixerType = FC_MIXER_QUADX; + model.config.mixer.type = FC_MIXER_QUADX; model.config.input.rateType = RATES_TYPE_BETAFLIGHT; model.config.input.rate[AXIS_ROLL] = 70; From 893c374447e14072ed7b34e8d4f3260e2960b154 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 27 Oct 2024 21:16:23 +0100 Subject: [PATCH 24/68] move rpm and dynamic filter to gyro config --- lib/Espfc/src/Blackbox/Blackbox.cpp | 24 +++++++++---------- lib/Espfc/src/Cli.h | 24 +++++++++---------- lib/Espfc/src/Model.h | 12 +++++----- lib/Espfc/src/ModelConfig.h | 13 +++++------ lib/Espfc/src/Msp/MspProcessor.h | 24 +++++++++---------- lib/Espfc/src/Sensor/GyroSensor.cpp | 36 ++++++++++++++--------------- 6 files changed, 66 insertions(+), 67 deletions(-) diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index 5a913461..a7250eca 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -124,10 +124,10 @@ int Blackbox::begin() gyroConfigMutable()->gyro_soft_notch_hz_2 = _model.config.gyro.notch2Filter.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; + dynNotchConfigMutable()->dyn_notch_count = _model.config.gyro.dynamicFilter.width; + dynNotchConfigMutable()->dyn_notch_q = _model.config.gyro.dynamicFilter.q; + dynNotchConfigMutable()->dyn_notch_min_hz = _model.config.gyro.dynamicFilter.min_freq; + dynNotchConfigMutable()->dyn_notch_max_hz = _model.config.gyro.dynamicFilter.max_freq; accelerometerConfigMutable()->acc_lpf_hz = _model.config.accel.filter.freq; accelerometerConfigMutable()->acc_hardware = _model.config.accel.dev; @@ -182,14 +182,14 @@ int Blackbox::begin() rxConfigMutable()->airModeActivateThreshold = 40; rxConfigMutable()->serialrx_provider = _model.config.input.serialRxProvider; - rpmFilterConfigMutable()->rpm_filter_harmonics = _model.config.rpmFilter.harmonics; - rpmFilterConfigMutable()->rpm_filter_q = _model.config.rpmFilter.q; - rpmFilterConfigMutable()->rpm_filter_min_hz = _model.config.rpmFilter.minFreq; - rpmFilterConfigMutable()->rpm_filter_fade_range_hz = _model.config.rpmFilter.fade; - rpmFilterConfigMutable()->rpm_filter_lpf_hz = _model.config.rpmFilter.freqLpf; - rpmFilterConfigMutable()->rpm_filter_weights[0] = _model.config.rpmFilter.weights[0]; - rpmFilterConfigMutable()->rpm_filter_weights[1] = _model.config.rpmFilter.weights[1]; - rpmFilterConfigMutable()->rpm_filter_weights[2] = _model.config.rpmFilter.weights[2]; + rpmFilterConfigMutable()->rpm_filter_harmonics = _model.config.gyro.rpmFilter.harmonics; + rpmFilterConfigMutable()->rpm_filter_q = _model.config.gyro.rpmFilter.q; + rpmFilterConfigMutable()->rpm_filter_min_hz = _model.config.gyro.rpmFilter.minFreq; + rpmFilterConfigMutable()->rpm_filter_fade_range_hz = _model.config.gyro.rpmFilter.fade; + rpmFilterConfigMutable()->rpm_filter_lpf_hz = _model.config.gyro.rpmFilter.freqLpf; + rpmFilterConfigMutable()->rpm_filter_weights[0] = _model.config.gyro.rpmFilter.weights[0]; + rpmFilterConfigMutable()->rpm_filter_weights[1] = _model.config.gyro.rpmFilter.weights[1]; + rpmFilterConfigMutable()->rpm_filter_weights[2] = _model.config.gyro.rpmFilter.weights[2]; updateModeFlag(&rcModeActivationPresent, BOXARM, _model.state.modeMaskPresent & 1 << MODE_ARMED); updateModeFlag(&rcModeActivationPresent, BOXANGLE, _model.state.modeMaskPresent & 1 << MODE_ANGLE); diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index f47934f0..2df27caa 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -448,18 +448,18 @@ class Cli Param(PSTR("gyro_notch2_cutoff"), &c.gyro.notch2Filter.cutoff), Param(PSTR("gyro_dyn_lpf_min"), &c.gyro.dynLpfFilter.cutoff), Param(PSTR("gyro_dyn_lpf_max"), &c.gyro.dynLpfFilter.freq), - Param(PSTR("gyro_dyn_notch_q"), &c.dynamicFilter.q), - Param(PSTR("gyro_dyn_notch_count"), &c.dynamicFilter.width), - Param(PSTR("gyro_dyn_notch_min"), &c.dynamicFilter.min_freq), - Param(PSTR("gyro_dyn_notch_max"), &c.dynamicFilter.max_freq), - Param(PSTR("gyro_rpm_harmonics"), &c.rpmFilter.harmonics), - Param(PSTR("gyro_rpm_q"), &c.rpmFilter.q), - Param(PSTR("gyro_rpm_min_freq"), &c.rpmFilter.minFreq), - Param(PSTR("gyro_rpm_fade"), &c.rpmFilter.fade), - Param(PSTR("gyro_rpm_weight_1"), &c.rpmFilter.weights[0]), - Param(PSTR("gyro_rpm_weight_2"), &c.rpmFilter.weights[1]), - Param(PSTR("gyro_rpm_weight_3"), &c.rpmFilter.weights[2]), - Param(PSTR("gyro_rpm_tlm_lpf_freq"), &c.rpmFilter.freqLpf), + Param(PSTR("gyro_dyn_notch_q"), &c.gyro.dynamicFilter.q), + Param(PSTR("gyro_dyn_notch_count"), &c.gyro.dynamicFilter.width), + Param(PSTR("gyro_dyn_notch_min"), &c.gyro.dynamicFilter.min_freq), + Param(PSTR("gyro_dyn_notch_max"), &c.gyro.dynamicFilter.max_freq), + Param(PSTR("gyro_rpm_harmonics"), &c.gyro.rpmFilter.harmonics), + Param(PSTR("gyro_rpm_q"), &c.gyro.rpmFilter.q), + Param(PSTR("gyro_rpm_min_freq"), &c.gyro.rpmFilter.minFreq), + Param(PSTR("gyro_rpm_fade"), &c.gyro.rpmFilter.fade), + Param(PSTR("gyro_rpm_weight_1"), &c.gyro.rpmFilter.weights[0]), + Param(PSTR("gyro_rpm_weight_2"), &c.gyro.rpmFilter.weights[1]), + Param(PSTR("gyro_rpm_weight_3"), &c.gyro.rpmFilter.weights[2]), + Param(PSTR("gyro_rpm_tlm_lpf_freq"), &c.gyro.rpmFilter.freqLpf), Param(PSTR("gyro_offset_x"), &c.gyro.bias[0]), Param(PSTR("gyro_offset_y"), &c.gyro.bias[1]), Param(PSTR("gyro_offset_z"), &c.gyro.bias[2]), diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index d1467067..3a21d101 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -416,9 +416,9 @@ class Model 1 << (BUZZER_ARMING - 1) | 1 << (BUZZER_BAT_LOW - 1); - if(config.dynamicFilter.width > 6) + if(config.gyro.dynamicFilter.width > 6) { - config.dynamicFilter.width = 6; + config.gyro.dynamicFilter.width = 6; } } @@ -456,7 +456,7 @@ class Model { if(isActive(FEATURE_DYNAMIC_FILTER)) { - for(size_t p = 0; p < (size_t)config.dynamicFilter.width; p++) + for(size_t p = 0; p < (size_t)config.gyro.dynamicFilter.width; p++) { state.gyroDynNotchFilter[p][i].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 380), gyroFilterRate); } @@ -477,10 +477,10 @@ class Model state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accelTimer.rate / 3), gyroFilterRate); for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) { - state.rpmFreqFilter[m].begin(FilterConfig(FILTER_PT1, config.rpmFilter.freqLpf), gyroFilterRate); - for(size_t n = 0; n < config.rpmFilter.harmonics; n++) + state.rpmFreqFilter[m].begin(FilterConfig(FILTER_PT1, config.gyro.rpmFilter.freqLpf), gyroFilterRate); + for(size_t n = 0; n < config.gyro.rpmFilter.harmonics; n++) { - int center = Math::mapi(m * RPM_FILTER_HARMONICS_MAX + n, 0, RPM_FILTER_MOTOR_MAX * config.rpmFilter.harmonics, config.rpmFilter.minFreq, gyroFilterRate / 2); + int center = Math::mapi(m * RPM_FILTER_HARMONICS_MAX + n, 0, RPM_FILTER_MOTOR_MAX * config.gyro.rpmFilter.harmonics, config.gyro.rpmFilter.minFreq, gyroFilterRate / 2); state.rpmFilter[m][n][i].begin(FilterConfig(FILTER_NOTCH_DF1, center, center * 0.98f), gyroFilterRate); } } diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 9b071e12..4e54bc95 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -564,21 +564,23 @@ struct GyroConfig int8_t dev = GYRO_AUTO; int8_t dlpf = GYRO_DLPF_256; int8_t align = ALIGN_DEFAULT; + int16_t bias[3] = { 0, 0, 0 }; FilterConfig filter{FILTER_PT1, 100}; FilterConfig filter2{FILTER_PT1, 213}; FilterConfig filter3{FILTER_FO, 150}; FilterConfig notch1Filter{FILTER_NOTCH, 0, 0}; FilterConfig notch2Filter{FILTER_NOTCH, 0, 0}; FilterConfig dynLpfFilter{FILTER_PT1, 425, 170}; - int16_t bias[3] = { 0, 0, 0 }; + DynamicFilterConfig dynamicFilter{4, 300, 80, 400}; + RpmFilterConfig rpmFilter; }; struct AccelConfig { int8_t bus = BUS_AUTO; int8_t dev = GYRO_AUTO; - FilterConfig filter{FILTER_BIQUAD, 15}; int16_t bias[3] = { 0, 0, 0 }; + FilterConfig filter{FILTER_BIQUAD, 15}; }; struct BaroConfig @@ -593,9 +595,9 @@ struct MagConfig int8_t bus = BUS_AUTO; int8_t dev = MAG_NONE; int8_t align = ALIGN_DEFAULT; - FilterConfig filter{FILTER_BIQUAD, 10}; int16_t scale[3] = { 0, 0, 0 }; int16_t offset[3] = { 1000, 1000, 1000 }; + FilterConfig filter{FILTER_BIQUAD, 10}; }; struct YawConfig @@ -694,6 +696,7 @@ class ModelConfig int8_t pin[PIN_COUNT]; int16_t i2cSpeed = 800; + int8_t tpaScale = 10; int16_t tpaBreakpoint = 1650; @@ -702,10 +705,6 @@ class ModelConfig WirelessConfig wireless; - DynamicFilterConfig dynamicFilter{4, 300, 80, 400}; - - RpmFilterConfig rpmFilter; - uint8_t rescueConfigDelay = 30; int16_t boardAlignment[3] = {0, 0, 0}; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index e4d15bf9..89e61aea 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -1085,14 +1085,14 @@ class MspProcessor r.writeU16(_model.config.dterm.dynLpfFilter.freq); // dyn lpf dterm max // gyro analyse r.writeU8(3); // deprecated dyn notch range - r.writeU8(_model.config.dynamicFilter.width); // dyn_notch_width_percent - r.writeU16(_model.config.dynamicFilter.q); // dyn_notch_q - r.writeU16(_model.config.dynamicFilter.min_freq); // dyn_notch_min_hz + r.writeU8(_model.config.gyro.dynamicFilter.width); // dyn_notch_width_percent + r.writeU16(_model.config.gyro.dynamicFilter.q); // dyn_notch_q + r.writeU16(_model.config.gyro.dynamicFilter.min_freq); // dyn_notch_min_hz // rpm filter - r.writeU8(_model.config.rpmFilter.harmonics); // gyro_rpm_notch_harmonics - r.writeU8(_model.config.rpmFilter.minFreq); // gyro_rpm_notch_min + r.writeU8(_model.config.gyro.rpmFilter.harmonics); // gyro_rpm_notch_harmonics + r.writeU8(_model.config.gyro.rpmFilter.minFreq); // gyro_rpm_notch_min // 1.43+ - r.writeU16(_model.config.dynamicFilter.max_freq); // dyn_notch_max_hz + r.writeU16(_model.config.gyro.dynamicFilter.max_freq); // dyn_notch_max_hz break; case MSP_SET_FILTER_CONFIG: @@ -1131,15 +1131,15 @@ class MspProcessor } if (m.remain() >= 8) { m.readU8(); // deprecated dyn_notch_range - _model.config.dynamicFilter.width = m.readU8(); // dyn_notch_width_percent - _model.config.dynamicFilter.q = m.readU16(); // dyn_notch_q - _model.config.dynamicFilter.min_freq = m.readU16(); // dyn_notch_min_hz - _model.config.rpmFilter.harmonics = m.readU8(); // gyro_rpm_notch_harmonics - _model.config.rpmFilter.minFreq = m.readU8(); // gyro_rpm_notch_min + _model.config.gyro.dynamicFilter.width = m.readU8(); // dyn_notch_width_percent + _model.config.gyro.dynamicFilter.q = m.readU16(); // dyn_notch_q + _model.config.gyro.dynamicFilter.min_freq = m.readU16(); // dyn_notch_min_hz + _model.config.gyro.rpmFilter.harmonics = m.readU8(); // gyro_rpm_notch_harmonics + _model.config.gyro.rpmFilter.minFreq = m.readU8(); // gyro_rpm_notch_min } // 1.43+ if (m.remain() >= 1) { - _model.config.dynamicFilter.max_freq = m.readU16(); // dyn_notch_max_hz + _model.config.gyro.dynamicFilter.max_freq = m.readU16(); // dyn_notch_max_hz } _model.reload(); break; diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index 6583bb53..92753d79 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -30,26 +30,26 @@ int GyroSensor::begin() _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_enabled = _model.isActive(FEATURE_DYNAMIC_FILTER) && _model.config.gyro.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.rpmFilter.harmonics > 0 && _model.config.output.dshotTelemetry; + _rpm_enabled = _model.config.gyro.rpmFilter.harmonics > 0 && _model.config.output.dshotTelemetry; _rpm_motor_index = 0; - _rpm_fade_inv = 1.0f / _model.config.rpmFilter.fade; - _rpm_min_freq = _model.config.rpmFilter.minFreq; + _rpm_fade_inv = 1.0f / _model.config.gyro.rpmFilter.fade; + _rpm_min_freq = _model.config.gyro.rpmFilter.minFreq; _rpm_max_freq = 0.48f * _model.state.loopTimer.rate; - _rpm_q = _model.config.rpmFilter.q * 0.01f; + _rpm_q = _model.config.gyro.rpmFilter.q * 0.01f; for (size_t i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) { - _rpm_weights[i] = Math::clamp(0.01f * _model.config.rpmFilter.weights[i], 0.0f, 1.0f); + _rpm_weights[i] = Math::clamp(0.01f * _model.config.gyro.rpmFilter.weights[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); + _fft[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.gyro.dynamicFilter, i); #else - _freqAnalyzer[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.dynamicFilter); + _freqAnalyzer[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.gyro.dynamicFilter); #endif } @@ -111,7 +111,7 @@ int FAST_CODE_ATTR GyroSensor::filter() { for (size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) { - for (size_t n = 0; n < _model.config.rpmFilter.harmonics; n++) + for (size_t n = 0; n < _model.config.gyro.rpmFilter.harmonics; n++) { _model.state.gyro = Utils::applyFilter(_model.state.rpmFilter[m][n], _model.state.gyro); } @@ -133,7 +133,7 @@ int FAST_CODE_ATTR GyroSensor::filter() if (_dyn_notch_enabled) { - for (size_t p = 0; p < (size_t)_model.config.dynamicFilter.width; p++) + for (size_t p = 0; p < (size_t)_model.config.gyro.dynamicFilter.width; p++) { _model.state.gyro = Utils::applyFilter(_model.state.gyroDynNotchFilter[p], _model.state.gyro); } @@ -165,12 +165,12 @@ void FAST_CODE_ATTR GyroSensor::rpmFilterUpdate() 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.rpmFilter.harmonics; n++) + for (size_t n = 0; n < _model.config.gyro.rpmFilter.harmonics; 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.rpmFilter.fade) + if (freqMargin < _model.config.gyro.rpmFilter.fade) { weight *= freqMargin * _rpm_fade_inv; } @@ -200,7 +200,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() { Stats::Measure measure(_model.state.stats, COUNTER_GYRO_FFT); - const float q = _model.config.dynamicFilter.q * 0.01; + const float q = _model.config.gyro.dynamicFilter.q * 0.01; bool feed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0; bool update = _model.state.dynamicFilterTimer.check(); @@ -227,10 +227,10 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() } if (_dyn_notch_enabled && status) { - for (size_t p = 0; p < (size_t)_model.config.dynamicFilter.width; p++) + for (size_t p = 0; p < (size_t)_model.config.gyro.dynamicFilter.width; p++) { float freq = _fft[i].peaks[p].freq; - if (freq >= _model.config.dynamicFilter.min_freq && freq <= _model.config.dynamicFilter.max_freq) + if (freq >= _model.config.gyro.dynamicFilter.min_freq && freq <= _model.config.gyro.dynamicFilter.max_freq) { _model.state.gyroDynNotchFilter[p][i].reconfigure(freq, freq, q); } @@ -258,13 +258,13 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() } if (_dyn_notch_enabled && update) { - if (freq >= _model.config.dynamicFilter.min_freq && freq <= _model.config.dynamicFilter.max_freq) + if (freq >= _model.config.gyro.dynamicFilter.min_freq && freq <= _model.config.gyro.dynamicFilter.max_freq) { - for (size_t p = 0; p < (size_t)_model.config.dynamicFilter.width; p++) + for (size_t p = 0; p < (size_t)_model.config.gyro.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); + int16_t f = Math::clamp((int16_t)lrintf(freq * harmonic), _model.config.gyro.dynamicFilter.min_freq, _model.config.gyro.dynamicFilter.max_freq); _model.state.gyroDynNotchFilter[p][x].reconfigure(f, f, q); } } From 3adc1030bc5962b94f18dddc66a4ebfdf077eb4e Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 28 Oct 2024 15:51:40 +0100 Subject: [PATCH 25/68] controller debug pin and serial config refactor --- lib/Espfc/src/Actuator.h | 2 +- lib/Espfc/src/Blackbox/Blackbox.cpp | 14 +-- lib/Espfc/src/Cli.h | 8 +- lib/Espfc/src/Controller.cpp | 18 +-- lib/Espfc/src/Fusion.cpp | 2 +- lib/Espfc/src/Input.cpp | 16 +-- lib/Espfc/src/Model.h | 2 +- lib/Espfc/src/ModelConfig.h | 175 ++++++++++++--------------- lib/Espfc/src/Msp/MspProcessor.h | 12 +- lib/Espfc/src/Output/Mixer.cpp | 6 +- lib/Espfc/src/Sensor/AccelSensor.h | 2 +- lib/Espfc/src/Sensor/BaroSensor.h | 4 +- lib/Espfc/src/Sensor/GyroSensor.cpp | 20 +-- lib/Espfc/src/Sensor/VoltageSensor.h | 4 +- 14 files changed, 135 insertions(+), 150 deletions(-) diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h index f3d22c13..c054898c 100644 --- a/lib/Espfc/src/Actuator.h +++ b/lib/Espfc/src/Actuator.h @@ -42,7 +42,7 @@ class Actuator updateDynLpf(); updateRescueConfig(); - if(_model.config.debugMode == DEBUG_PIDLOOP) + if(_model.config.debug.mode == DEBUG_PIDLOOP) { _model.state.debug[4] = micros() - startTime; } diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index a7250eca..a2c40e83 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -38,7 +38,7 @@ int Blackbox::begin() } systemConfigMutable()->activeRateProfile = 0; - systemConfigMutable()->debug_mode = debugMode = _model.config.debugMode; + systemConfigMutable()->debug_mode = debugMode = _model.config.debug.mode; controlRateConfig_t *rp = controlRateProfilesMutable(systemConfig()->activeRateProfile); for(int i = 0; i <= AXIS_YAW; i++) @@ -50,8 +50,8 @@ int Blackbox::begin() } rp->thrMid8 = 50; rp->thrExpo8 = 0; - rp->dynThrPID = _model.config.tpaScale; - rp->tpa_breakpoint = _model.config.tpaBreakpoint; + rp->dynThrPID = _model.config.controller.tpaScale; + rp->tpa_breakpoint = _model.config.controller.tpaBreakpoint; rp->rates_type = _model.config.input.rateType; pidProfile_s * cp = currentPidProfile = &_pidProfile; @@ -82,8 +82,8 @@ int Blackbox::begin() 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->tpa_rate = _model.config.controller.tpaScale; + cp->tpa_breakpoint = _model.config.controller.tpaBreakpoint; cp->motor_output_limit = _model.config.output.motorLimit; cp->throttle_boost = 0; cp->throttle_boost_cutoff = 100; @@ -225,7 +225,7 @@ int FAST_CODE_ATTR Blackbox::update() } //PIN_DEBUG(LOW); - if(_model.config.debugMode == DEBUG_PIDLOOP) + if(_model.config.debug.mode == DEBUG_PIDLOOP) { _model.state.debug[5] = micros() - startTime; } @@ -263,7 +263,7 @@ void FAST_CODE_ATTR Blackbox::updateData() motor[i] = PWM_TO_DSHOT(motor[i]); } } - if(_model.config.debugMode != DEBUG_NONE && _model.config.debugMode != DEBUG_BLACKBOX_OUTPUT) + if(_model.config.debug.mode != DEBUG_NONE && _model.config.debug.mode != DEBUG_BLACKBOX_OUTPUT) { for(size_t i = 0; i < 8; i++) { diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 2df27caa..2d2c949f 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -429,8 +429,8 @@ class Cli Param(PSTR("feature_soft_serial"), &c.featureMask, 6), Param(PSTR("feature_telemetry"), &c.featureMask, 10), - Param(PSTR("debug_mode"), &c.debugMode, debugModeChoices), - Param(PSTR("debug_axis"), &c.debugAxis), + Param(PSTR("debug_mode"), &c.debug.mode, debugModeChoices), + Param(PSTR("debug_axis"), &c.debug.axis), Param(PSTR("gyro_bus"), &c.gyro.bus, busDevChoices), Param(PSTR("gyro_dev"), &c.gyro.dev, gyroDevChoices), @@ -631,8 +631,8 @@ class Cli Param(PSTR("pid_iterm_zero"), &c.iterm.lowThrottleZeroIterm), Param(PSTR("pid_iterm_relax"), &c.iterm.relax, inputItermRelaxChoices), Param(PSTR("pid_iterm_relax_cutoff"), &c.iterm.relaxCutoff), - Param(PSTR("pid_tpa_scale"), &c.tpaScale), - Param(PSTR("pid_tpa_breakpoint"), &c.tpaBreakpoint), + Param(PSTR("pid_tpa_scale"), &c.controller.tpaScale), + Param(PSTR("pid_tpa_breakpoint"), &c.controller.tpaBreakpoint), Param(PSTR("mixer_sync"), &c.mixerSync), Param(PSTR("mixer_type"), &c.mixer.type, mixerTypeChoices), diff --git a/lib/Espfc/src/Controller.cpp b/lib/Espfc/src/Controller.cpp index d44fb634..afa8d52e 100644 --- a/lib/Espfc/src/Controller.cpp +++ b/lib/Espfc/src/Controller.cpp @@ -15,7 +15,7 @@ int Controller::begin() int FAST_CODE_ATTR Controller::update() { uint32_t startTime = 0; - if(_model.config.debugMode == DEBUG_PIDLOOP) + if(_model.config.debug.mode == DEBUG_PIDLOOP) { startTime = micros(); _model.state.debug[0] = startTime - _model.state.loopTimer.last; @@ -46,7 +46,7 @@ int FAST_CODE_ATTR Controller::update() } } - if(_model.config.debugMode == DEBUG_PIDLOOP) + if(_model.config.debug.mode == DEBUG_PIDLOOP) { _model.state.debug[2] = micros() - startTime; } @@ -72,7 +72,7 @@ void Controller::outerLoopRobot() _model.state.desiredAngle.set(AXIS_PITCH, angle); _model.state.desiredRate[AXIS_YAW] = _model.state.input[AXIS_YAW] * Math::toRad(_model.config.level.rateLimit); - if(_model.config.debugMode == DEBUG_ANGLERATE) + if(_model.config.debug.mode == DEBUG_ANGLERATE) { _model.state.debug[0] = speed * 1000; _model.state.debug[1] = lrintf(degrees(angle) * 10); @@ -99,7 +99,7 @@ void Controller::innerLoopRobot() _model.state.output[AXIS_YAW] = 0.f; } - if(_model.config.debugMode == DEBUG_ANGLERATE) + if(_model.config.debug.mode == 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); @@ -129,7 +129,7 @@ void FAST_CODE_ATTR Controller::outerLoop() _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) + if(_model.config.debug.mode == DEBUG_ANGLERATE) { for(size_t i = 0; i < 3; ++i) { @@ -148,7 +148,7 @@ void FAST_CODE_ATTR Controller::innerLoop() } _model.state.output[AXIS_THRUST] = _model.state.desiredRate[AXIS_THRUST]; - if(_model.config.debugMode == DEBUG_ITERM_RELAX) + if(_model.config.debug.mode == 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); @@ -159,9 +159,9 @@ void FAST_CODE_ATTR Controller::innerLoop() 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)); + if(_model.config.controller.tpaScale == 0) return 1.f; + float t = Math::clamp(_model.state.inputUs[AXIS_THRUST], (float)_model.config.controller.tpaBreakpoint, 2000.f); + return Math::map(t, (float)_model.config.controller.tpaBreakpoint, 2000.f, 1.f, 1.f - ((float)_model.config.controller.tpaScale * 0.01f)); } void Controller::resetIterm() diff --git a/lib/Espfc/src/Fusion.cpp b/lib/Espfc/src/Fusion.cpp index 99be1d3b..8fe3b804 100644 --- a/lib/Espfc/src/Fusion.cpp +++ b/lib/Espfc/src/Fusion.cpp @@ -70,7 +70,7 @@ int FAST_CODE_ATTR Fusion::update() } //else madgwickFusion1(); - if(_model.config.debugMode == DEBUG_ALTITUDE) + if(_model.config.debug.mode == DEBUG_ALTITUDE) { _model.state.debug[0] = lrintf(degrees(_model.state.angle[0]) * 10); _model.state.debug[1] = lrintf(degrees(_model.state.angle[1]) * 10); diff --git a/lib/Espfc/src/Input.cpp b/lib/Espfc/src/Input.cpp index 95654fbd..c9199313 100644 --- a/lib/Espfc/src/Input.cpp +++ b/lib/Espfc/src/Input.cpp @@ -88,7 +88,7 @@ int FAST_CODE_ATTR Input::update() filterInputs(status); } - if(_model.config.debugMode == DEBUG_PIDLOOP) + if(_model.config.debug.mode == DEBUG_PIDLOOP) { _model.state.debug[1] = micros() - startTime; } @@ -103,7 +103,7 @@ InputStatus FAST_CODE_ATTR Input::readInputs() InputStatus status = _device->update(); - if(_model.config.debugMode == DEBUG_RX_TIMING) + if(_model.config.debug.mode == DEBUG_RX_TIMING) { _model.state.debug[0] = micros() - startTime; } @@ -118,7 +118,7 @@ InputStatus FAST_CODE_ATTR Input::readInputs() processInputs(); - if(_model.config.debugMode == DEBUG_RX_SIGNAL_LOSS) + if(_model.config.debug.mode == DEBUG_RX_SIGNAL_LOSS) { _model.state.debug[0] = !_model.state.inputRxLoss; _model.state.debug[1] = _model.state.inputRxFailSafe; @@ -175,7 +175,7 @@ void FAST_CODE_ATTR Input::processInputs() _model.state.inputBuffer[c] = v; } - if(_model.config.debugMode == DEBUG_RX_TIMING) + if(_model.config.debug.mode == DEBUG_RX_TIMING) { _model.state.debug[2] = micros() - startTime; } @@ -279,7 +279,7 @@ void FAST_CODE_ATTR Input::filterInputs(InputStatus status) setInput((Axis)c, v, newFrame); } - if(_model.config.debugMode == DEBUG_RX_TIMING) + if(_model.config.debug.mode == DEBUG_RX_TIMING) { _model.state.debug[3] = micros() - startTime; } @@ -300,7 +300,7 @@ void FAST_CODE_ATTR Input::updateFrameRate() _model.state.inputInterpolationStep = _model.state.loopTimer.intervalf / _model.state.inputInterpolationDelta; } - if(_model.config.debugMode == DEBUG_RC_SMOOTHING_RATE) + if(_model.config.debug.mode == DEBUG_RC_SMOOTHING_RATE) { _model.state.debug[0] = _model.state.inputFrameDelta / 10; _model.state.debug[1] = _model.state.inputFrameRate; @@ -311,7 +311,7 @@ void FAST_CODE_ATTR Input::updateFrameRate() 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) + if(_model.config.debug.mode == DEBUG_RC_SMOOTHING_RATE) { _model.state.debug[2] = lrintf(freq); _model.state.debug[3] = lrintf(_model.state.inputAutoFreq); @@ -331,7 +331,7 @@ void FAST_CODE_ATTR Input::updateFrameRate() } } - if(_model.config.debugMode == DEBUG_RX_TIMING) + if(_model.config.debug.mode == DEBUG_RX_TIMING) { _model.state.debug[1] = micros() - now; } diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 3a21d101..669aa962 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -212,7 +212,7 @@ class Model void inline setDebug(DebugMode mode, size_t index, int16_t value) { if(index >= 8) return; - if(config.debugMode != mode) return; + if(config.debug.mode != mode) return; state.debug[index] = value; } diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 4e54bc95..fb6a201f 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -532,6 +532,12 @@ struct BlackboxConfig int8_t mode = 0; }; +struct DebugConfig +{ + int8_t mode = DEBUG_NONE; + uint8_t axis = 1; +}; + struct RpmFilterConfig { uint8_t harmonics = 3; @@ -635,23 +641,31 @@ struct MixerConfiguration bool yawReverse = 0; }; +struct ControllerConfig +{ + int8_t tpaScale = 10; + int16_t tpaBreakpoint = 1650; +}; + // persistent data class ModelConfig { public: + // inputs and sensors GyroConfig gyro; AccelConfig accel; BaroConfig baro; MagConfig mag; - InputConfig input; FailsafeConfig failsafe; + FusionConfig fusion; + VBatConfig vbat; + IBatConfig ibat; ActuatorCondition conditions[ACTUATOR_CONDITIONS]; ScalerConfig scaler[SCALER_COUNT]; - OutputConfig output; - + // pid controller PidConfig pid[FC_PID_ITEM_COUNT] = { [FC_PID_ROLL] = { .P = 42, .I = 85, .D = 24, .F = 72 }, // ROLL [FC_PID_PITCH] = { .P = 46, .I = 90, .D = 26, .F = 76 }, // PITCH @@ -664,137 +678,108 @@ class ModelConfig [FC_PID_MAG] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // MAG [FC_PID_VEL] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // VEL }; - - MixerConfiguration mixer; YawConfig yaw; LevelConfig level; DtermConfig dterm; ItermConfig iterm; + ControllerConfig controller; - int8_t loopSync = 8; // MPU 1000Hz - int8_t mixerSync = 1; - - int32_t featureMask = ESPFC_FEATURE_MASK; - bool telemetry = 0; - int32_t telemetryInterval = 1000; - - BlackboxConfig blackbox; - - SerialPortConfig serial[SERIAL_UART_COUNT]; - - FusionConfig fusion; - - char modelName[MODEL_NAME_LEN + 1]; - - VBatConfig vbat; - IBatConfig ibat; - - int8_t debugMode = DEBUG_NONE; - uint8_t debugAxis = 1; - - BuzzerConfig buzzer; - - int8_t pin[PIN_COUNT]; - int16_t i2cSpeed = 800; - - int8_t tpaScale = 10; - int16_t tpaBreakpoint = 1650; - - int8_t customMixerCount = 0; - MixerEntry customMixes[MIXER_RULE_MAX]; - - WirelessConfig wireless; - - uint8_t rescueConfigDelay = 30; - - int16_t boardAlignment[3] = {0, 0, 0}; - - ModelConfig() - { + // hardware + int8_t pin[PIN_COUNT] = { #ifdef ESPFC_INPUT - pin[PIN_INPUT_RX] = ESPFC_INPUT_PIN; + [PIN_INPUT_RX] = ESPFC_INPUT_PIN, #endif - pin[PIN_OUTPUT_0] = ESPFC_OUTPUT_0; - pin[PIN_OUTPUT_1] = ESPFC_OUTPUT_1; - pin[PIN_OUTPUT_2] = ESPFC_OUTPUT_2; - pin[PIN_OUTPUT_3] = ESPFC_OUTPUT_3; + [PIN_OUTPUT_0] = ESPFC_OUTPUT_0, + [PIN_OUTPUT_1] = ESPFC_OUTPUT_1, + [PIN_OUTPUT_2] = ESPFC_OUTPUT_2, + [PIN_OUTPUT_3] = ESPFC_OUTPUT_3, #if ESPFC_OUTPUT_COUNT > 4 - pin[PIN_OUTPUT_4] = ESPFC_OUTPUT_4; + [PIN_OUTPUT_4] = ESPFC_OUTPUT_4, #endif #if ESPFC_OUTPUT_COUNT > 5 - pin[PIN_OUTPUT_5] = ESPFC_OUTPUT_5; + [PIN_OUTPUT_5] = ESPFC_OUTPUT_5, #endif #if ESPFC_OUTPUT_COUNT > 6 - pin[PIN_OUTPUT_6] = ESPFC_OUTPUT_6; + [PIN_OUTPUT_6] = ESPFC_OUTPUT_6, #endif #if ESPFC_OUTPUT_COUNT > 7 - pin[PIN_OUTPUT_7] = ESPFC_OUTPUT_7; + [PIN_OUTPUT_7] = ESPFC_OUTPUT_7, #endif - pin[PIN_BUZZER] = ESPFC_BUZZER_PIN; + [PIN_BUZZER] = ESPFC_BUZZER_PIN, #ifdef ESPFC_SERIAL_0 - pin[PIN_SERIAL_0_TX] = ESPFC_SERIAL_0_TX; - pin[PIN_SERIAL_0_RX] = ESPFC_SERIAL_0_RX; + [PIN_SERIAL_0_TX] = ESPFC_SERIAL_0_TX, + [PIN_SERIAL_0_RX] = ESPFC_SERIAL_0_RX, #endif #ifdef ESPFC_SERIAL_1 - pin[PIN_SERIAL_1_TX] = ESPFC_SERIAL_1_TX; - pin[PIN_SERIAL_1_RX] = ESPFC_SERIAL_1_RX; + [PIN_SERIAL_1_TX] = ESPFC_SERIAL_1_TX, + [PIN_SERIAL_1_RX] = ESPFC_SERIAL_1_RX, #endif #ifdef ESPFC_SERIAL_2 - pin[PIN_SERIAL_2_TX] = ESPFC_SERIAL_2_TX; - pin[PIN_SERIAL_2_RX] = ESPFC_SERIAL_2_RX; + [PIN_SERIAL_2_TX] = ESPFC_SERIAL_2_TX, + [PIN_SERIAL_2_RX] = ESPFC_SERIAL_2_RX, #endif #ifdef ESPFC_I2C_0 - pin[PIN_I2C_0_SCL] = ESPFC_I2C_0_SCL; - pin[PIN_I2C_0_SDA] = ESPFC_I2C_0_SDA; + [PIN_I2C_0_SCL] = ESPFC_I2C_0_SCL, + [PIN_I2C_0_SDA] = ESPFC_I2C_0_SDA, #endif #ifdef ESPFC_ADC_0 - pin[PIN_INPUT_ADC_0] = ESPFC_ADC_0_PIN; + [PIN_INPUT_ADC_0] = ESPFC_ADC_0_PIN, #endif #ifdef ESPFC_ADC_1 - pin[PIN_INPUT_ADC_1] = ESPFC_ADC_1_PIN; + [PIN_INPUT_ADC_1] = ESPFC_ADC_1_PIN, #endif #ifdef ESPFC_SPI_0 - pin[PIN_SPI_0_SCK] = ESPFC_SPI_0_SCK; - pin[PIN_SPI_0_MOSI] = ESPFC_SPI_0_MOSI; - pin[PIN_SPI_0_MISO] = ESPFC_SPI_0_MISO; + [PIN_SPI_0_SCK] = ESPFC_SPI_0_SCK, + [PIN_SPI_0_MOSI] = ESPFC_SPI_0_MOSI, + [PIN_SPI_0_MISO] = ESPFC_SPI_0_MISO, #endif #ifdef ESPFC_SPI_0 - pin[PIN_SPI_CS0] = ESPFC_SPI_CS_GYRO; - pin[PIN_SPI_CS1] = ESPFC_SPI_CS_BARO; - pin[PIN_SPI_CS2] = -1; + [PIN_SPI_CS0] = ESPFC_SPI_CS_GYRO, + [PIN_SPI_CS1] = ESPFC_SPI_CS_BARO, + [PIN_SPI_CS2] = -1, +#endif + }; + SerialPortConfig serial[SERIAL_UART_COUNT] = { +#ifdef ESPFC_SERIAL_USB + [SERIAL_USB] = { .id = SERIAL_ID_USB_VCP, .functionMask = ESPFC_SERIAL_USB_FN, .baud = SERIAL_SPEED_115200, .blackboxBaud = SERIAL_SPEED_NONE }, #endif - #ifdef ESPFC_SERIAL_0 - serial[SERIAL_UART_0].id = SERIAL_ID_UART_1; - serial[SERIAL_UART_0].functionMask = ESPFC_SERIAL_0_FN; - serial[SERIAL_UART_0].baud = ESPFC_SERIAL_0_BAUD; - serial[SERIAL_UART_0].blackboxBaud = ESPFC_SERIAL_0_BBAUD; + [SERIAL_UART_0] = { .id = SERIAL_ID_UART_1, .functionMask = ESPFC_SERIAL_0_FN, .baud = ESPFC_SERIAL_0_BAUD, .blackboxBaud = ESPFC_SERIAL_0_BBAUD }, #endif #ifdef ESPFC_SERIAL_1 - serial[SERIAL_UART_1].id = SERIAL_ID_UART_2; - serial[SERIAL_UART_1].functionMask = ESPFC_SERIAL_1_FN; - serial[SERIAL_UART_1].baud = ESPFC_SERIAL_1_BAUD; - serial[SERIAL_UART_1].blackboxBaud = ESPFC_SERIAL_1_BBAUD; + [SERIAL_UART_1] = { .id = SERIAL_ID_UART_2, .functionMask = ESPFC_SERIAL_1_FN, .baud = ESPFC_SERIAL_1_BAUD, .blackboxBaud = ESPFC_SERIAL_1_BBAUD }, #endif #ifdef ESPFC_SERIAL_2 - serial[SERIAL_UART_2].id = SERIAL_ID_UART_3; - serial[SERIAL_UART_2].functionMask = ESPFC_SERIAL_2_FN; - serial[SERIAL_UART_2].baud = ESPFC_SERIAL_2_BAUD; - serial[SERIAL_UART_2].blackboxBaud = ESPFC_SERIAL_2_BBAUD; -#endif -#ifdef ESPFC_SERIAL_USB - serial[SERIAL_USB].id = SERIAL_ID_USB_VCP; - serial[SERIAL_USB].functionMask = ESPFC_SERIAL_USB_FN; - serial[SERIAL_USB].baud = SERIAL_SPEED_115200; - serial[SERIAL_USB].blackboxBaud = SERIAL_SPEED_NONE; + [SERIAL_UART_2] = { .id = SERIAL_ID_UART_3, .functionMask = ESPFC_SERIAL_2_FN, .baud = ESPFC_SERIAL_2_BAUD, .blackboxBaud = ESPFC_SERIAL_2_BBAUD }, #endif #ifdef ESPFC_SERIAL_SOFT_0 - serial[SERIAL_SOFT_0].id = SERIAL_ID_SOFTSERIAL_1; - serial[SERIAL_SOFT_0].functionMask = ESPFC_SERIAL_SOFT_0_FN; - serial[SERIAL_SOFT_0].baud = SERIAL_SPEED_115200; - serial[SERIAL_SOFT_0].blackboxBaud = SERIAL_SPEED_NONE; + [SERIAL_SOFT_0] = { .id = SERIAL_ID_SOFTSERIAL_1, .functionMask = ESPFC_SERIAL_SOFT_0_FN, .baud = SERIAL_SPEED_115200, .blackboxBaud = SERIAL_SPEED_NONE }, #endif + }; + BuzzerConfig buzzer; + WirelessConfig wireless; + // mixer and outputs + int8_t customMixerCount = 0; + MixerEntry customMixes[MIXER_RULE_MAX]; + MixerConfiguration mixer; + OutputConfig output; + BlackboxConfig blackbox; + DebugConfig debug; + + // not classified yet + int16_t i2cSpeed = 800; + int8_t loopSync = 8; // MPU 1000Hz + int8_t mixerSync = 1; + int32_t featureMask = ESPFC_FEATURE_MASK; + bool telemetry = 0; + int32_t telemetryInterval = 1000; + uint8_t rescueConfigDelay = 30; + int16_t boardAlignment[3] = {0, 0, 0}; + char modelName[MODEL_NAME_LEN + 1]; + + ModelConfig() + { for(size_t i = 0; i < INPUT_CHANNELS; i++) { input.channel[i].map = i; @@ -826,7 +811,7 @@ class ModelConfig { #ifdef ESPFC_DEV_PRESET_BLACKBOX blackbox.dev = BLACKBOX_DEV_SERIAL; // serial - debugMode = DEBUG_GYRO_SCALED; + debug.mode = DEBUG_GYRO_SCALED; serial[ESPFC_DEV_PRESET_BLACKBOX].functionMask |= SERIAL_FUNCTION_BLACKBOX; serial[ESPFC_DEV_PRESET_BLACKBOX].blackboxBaud = SERIAL_SPEED_250000; serial[ESPFC_DEV_PRESET_BLACKBOX].baud = SERIAL_SPEED_250000; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 89e61aea..fc1dbc23 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -910,10 +910,10 @@ class MspProcessor { r.writeU8(_model.config.input.superRate[i]); } - r.writeU8(_model.config.tpaScale); // dyn thr pid + r.writeU8(_model.config.controller.tpaScale); // dyn thr pid r.writeU8(50); // thrMid8 r.writeU8(0); // thr expo - r.writeU16(_model.config.tpaBreakpoint); // tpa breakpoint + r.writeU16(_model.config.controller.tpaBreakpoint); // tpa breakpoint r.writeU8(_model.config.input.expo[AXIS_YAW]); // yaw expo r.writeU8(_model.config.input.rate[AXIS_YAW]); // yaw rate r.writeU8(_model.config.input.rate[AXIS_PITCH]); // pitch rate @@ -951,10 +951,10 @@ class MspProcessor { _model.config.input.superRate[i] = m.readU8(); } - _model.config.tpaScale = Math::clamp(m.readU8(), (uint8_t)0, (uint8_t)90); // dyn thr pid + _model.config.controller.tpaScale = Math::clamp(m.readU8(), (uint8_t)0, (uint8_t)90); // dyn thr pid m.readU8(); // thrMid8 m.readU8(); // thr expo - _model.config.tpaBreakpoint = Math::clamp(m.readU16(), (uint16_t)1000, (uint16_t)2000); // tpa breakpoint + _model.config.controller.tpaBreakpoint = Math::clamp(m.readU16(), (uint16_t)1000, (uint16_t)2000); // tpa breakpoint if(m.remain() >= 1) { _model.config.input.expo[AXIS_YAW] = m.readU8(); // yaw expo @@ -1012,7 +1012,7 @@ class MspProcessor r.writeU16(125); // gyro cal duration (1.25s) r.writeU16(0); // gyro offset yaw r.writeU8(0); // check overflow - r.writeU8(_model.config.debugMode); + r.writeU8(_model.config.debug.mode); r.writeU8(DEBUG_COUNT); break; @@ -1040,7 +1040,7 @@ class MspProcessor m.readU8(); // check overflow } if(m.remain()) { - _model.config.debugMode = m.readU8(); + _model.config.debug.mode = m.readU8(); } _model.reload(); break; diff --git a/lib/Espfc/src/Output/Mixer.cpp b/lib/Espfc/src/Output/Mixer.cpp index eccbed01..c612f9e5 100644 --- a/lib/Espfc/src/Output/Mixer.cpp +++ b/lib/Espfc/src/Output/Mixer.cpp @@ -90,14 +90,14 @@ int FAST_CODE_ATTR Mixer::update() updateMixer(mixer, outputs); writeOutput(mixer, outputs); - if(_model.config.debugMode == DEBUG_PIDLOOP) + if(_model.config.debug.mode == DEBUG_PIDLOOP) { _model.state.debug[3] = micros() - startTime; } _model.state.stats.loopTick(); - if(_model.config.debugMode == DEBUG_CYCLETIME) + if(_model.config.debug.mode == DEBUG_CYCLETIME) { _model.state.debug[0] = _model.state.stats.loopTime(); _model.state.debug[1] = lrintf(_model.state.stats.getCpuLoad()); @@ -350,7 +350,7 @@ void FAST_CODE_ATTR Mixer::readTelemetry() 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) + if(_model.config.debug.mode == DEBUG_DSHOT_RPM_ERRORS) { _model.state.debug[i] = _model.state.outputTelemetryErrors[i]; } diff --git a/lib/Espfc/src/Sensor/AccelSensor.h b/lib/Espfc/src/Sensor/AccelSensor.h index 8e9d1005..5ef22037 100644 --- a/lib/Espfc/src/Sensor/AccelSensor.h +++ b/lib/Espfc/src/Sensor/AccelSensor.h @@ -69,7 +69,7 @@ class AccelSensor: public BaseSensor for(size_t i = 0; i < 3; i++) { - if(_model.config.debugMode == DEBUG_ACCELEROMETER) + if(_model.config.debug.mode == DEBUG_ACCELEROMETER) { _model.state.debug[i] = _model.state.accelRaw[i]; } diff --git a/lib/Espfc/src/Sensor/BaroSensor.h b/lib/Espfc/src/Sensor/BaroSensor.h index 05704c10..351b4873 100644 --- a/lib/Espfc/src/Sensor/BaroSensor.h +++ b/lib/Espfc/src/Sensor/BaroSensor.h @@ -60,7 +60,7 @@ class BaroSensor: public BaseSensor Stats::Measure measure(_model.state.stats, COUNTER_BARO); - if(_model.config.debugMode == DEBUG_BARO) + if(_model.config.debug.mode == DEBUG_BARO) { _model.state.debug[0] = _state; } @@ -127,7 +127,7 @@ class BaroSensor: public BaseSensor } _model.state.baroAltitude = _model.state.baroAltitudeRaw - _model.state.baroAltitudeBias; - if(_model.config.debugMode == DEBUG_BARO) + if(_model.config.debug.mode == DEBUG_BARO) { _model.state.debug[1] = lrintf(_model.state.baroPressureRaw * 0.1f); // hPa x 10 _model.state.debug[2] = lrintf(_model.state.baroTemperatureRaw * 100.f); // deg C x 100 diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index 92753d79..b002620c 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -31,7 +31,7 @@ int GyroSensor::begin() _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.gyro.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; + _dyn_notch_debug = _model.config.debug.mode == DEBUG_FFT_FREQ || _model.config.debug.mode == DEBUG_FFT_TIME; _rpm_enabled = _model.config.gyro.rpmFilter.harmonics > 0 && _model.config.output.dshotTelemetry; _rpm_motor_index = 0; @@ -101,11 +101,11 @@ int FAST_CODE_ATTR GyroSensor::filter() _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.setDebug(DEBUG_GYRO_SAMPLE, 0, lrintf(degrees(_model.state.gyro[_model.config.debug.axis]))); _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]))); + _model.setDebug(DEBUG_GYRO_SAMPLE, 1, lrintf(degrees(_model.state.gyro[_model.config.debug.axis]))); if (_rpm_enabled) { @@ -118,13 +118,13 @@ int FAST_CODE_ATTR GyroSensor::filter() } } - _model.setDebug(DEBUG_GYRO_SAMPLE, 2, lrintf(degrees(_model.state.gyro[_model.config.debugAxis]))); + _model.setDebug(DEBUG_GYRO_SAMPLE, 2, lrintf(degrees(_model.state.gyro[_model.config.debug.axis]))); _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]))); + _model.setDebug(DEBUG_GYRO_SAMPLE, 3, lrintf(degrees(_model.state.gyro[_model.config.debug.axis]))); if (_dyn_notch_enabled || _dyn_notch_debug) { @@ -212,13 +212,13 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() { uint32_t startTime = micros(); int status = _fft[i].update(_model.state.gyroDynNotch[i]); - if (_model.config.debugMode == DEBUG_FFT_TIME) + if (_model.config.debug.mode == 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) + if (_model.config.debug.mode == DEBUG_FFT_FREQ && i == _model.config.debug.axis) { _model.state.debug[0] = lrintf(_fft[i].peaks[0].freq); _model.state.debug[1] = lrintf(_fft[i].peaks[1].freq); @@ -243,17 +243,17 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() uint32_t startTime = micros(); _freqAnalyzer[i].update(_model.state.gyroDynNotch[i]); float freq = _freqAnalyzer[i].freq; - if (_model.config.debugMode == DEBUG_FFT_TIME) + if (_model.config.debug.mode == 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 (_model.config.debug.mode == DEBUG_FFT_FREQ) { if (update) _model.state.debug[i] = lrintf(freq); - if (i == _model.config.debugAxis) + if (i == _model.config.debug.axis) _model.state.debug[3] = lrintf(degrees(_model.state.gyroDynNotch[i])); } if (_dyn_notch_enabled && update) diff --git a/lib/Espfc/src/Sensor/VoltageSensor.h b/lib/Espfc/src/Sensor/VoltageSensor.h index ba7a63a4..3e4af7ad 100644 --- a/lib/Espfc/src/Sensor/VoltageSensor.h +++ b/lib/Espfc/src/Sensor/VoltageSensor.h @@ -80,7 +80,7 @@ class VoltageSensor: public BaseSensor _model.state.battery.cellVoltage = _model.state.battery.voltage / constrain(_model.state.battery.cells, 1, 6); _model.state.battery.percentage = Math::clamp(Math::map(_model.state.battery.cellVoltage, 3.4f, 4.2f, 0.0f, 100.0f), 0.0f, 100.0f); - if(_model.config.debugMode == DEBUG_BATTERY) + if(_model.config.debug.mode == DEBUG_BATTERY) { _model.state.debug[0] = constrain(lrintf(_model.state.battery.voltageUnfiltered * 100.0f), 0, 32000); _model.state.debug[1] = constrain(lrintf(_model.state.battery.voltage * 100.0f), 0, 32000); @@ -106,7 +106,7 @@ class VoltageSensor: public BaseSensor _model.state.battery.currentUnfiltered = volts; _model.state.battery.current = _iFilter.update(_model.state.battery.currentUnfiltered); - if(_model.config.debugMode == DEBUG_CURRENT_SENSOR) + if(_model.config.debug.mode == DEBUG_CURRENT_SENSOR) { _model.state.debug[0] = lrintf(milivolts); _model.state.debug[1] = constrain(lrintf(_model.state.battery.currentUnfiltered * 100.0f), 0, 32000); From 767f0898dd4494e32f027e9c1a769c0fb6528e25 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 28 Oct 2024 15:57:45 +0100 Subject: [PATCH 26/68] usb serial in cli --- lib/Espfc/src/Cli.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 2d2c949f..aa08fdb8 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -574,6 +574,9 @@ class Cli #ifdef ESPFC_SERIAL_SOFT_0 Param(PSTR("serial_soft_0"), &c.serial[SERIAL_SOFT_0]), #endif +#ifdef ESPFC_SERIAL_USB + Param(PSTR("serial_usb"), &c.serial[SERIAL_USB]), +#endif Param(PSTR("scaler_0"), &c.scaler[0]), Param(PSTR("scaler_1"), &c.scaler[1]), From 3821e7822451151bbf9b657689eabd426b34c0a9 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 28 Oct 2024 21:51:34 +0100 Subject: [PATCH 27/68] input output state refactor --- lib/Espfc/src/Actuator.h | 12 +-- lib/Espfc/src/Blackbox/Blackbox.cpp | 6 +- lib/Espfc/src/Blackbox/BlackboxBridge.cpp | 6 +- lib/Espfc/src/Cli.h | 10 +- lib/Espfc/src/Controller.cpp | 36 +++---- lib/Espfc/src/Input.cpp | 116 +++++++++++----------- lib/Espfc/src/Model.h | 14 +-- lib/Espfc/src/ModelState.h | 95 ++++++++++-------- lib/Espfc/src/Msp/MspProcessor.h | 22 ++-- lib/Espfc/src/Output/Mixer.cpp | 82 +++++++-------- lib/Espfc/src/Sensor/GyroSensor.cpp | 4 +- test/test_fc/test_fc.cpp | 6 +- 12 files changed, 213 insertions(+), 196 deletions(-) diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h index c054898c..29b61293 100644 --- a/lib/Espfc/src/Actuator.h +++ b/lib/Espfc/src/Actuator.h @@ -64,7 +64,7 @@ class Actuator short c = _model.config.scaler[i].channel; if(c < AXIS_AUX_1) continue; - float v = _model.state.input[c]; + float v = _model.state.input.ch[c]; float min = _model.config.scaler[i].minScale * 0.01f; float max = _model.config.scaler[i].maxScale * 0.01f; float scale = Math::map3(v, -1.f, 0.f, 1.f, min, min < 0 ? 0.f : 1.f, max); @@ -100,7 +100,7 @@ class Actuator _model.setArmingDisabled(ARMING_DISABLED_NO_GYRO, !_model.state.gyroPresent || errors); _model.setArmingDisabled(ARMING_DISABLED_FAILSAFE, _model.state.failsafe.phase != FC_FAILSAFE_IDLE); - _model.setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE, _model.state.inputRxLoss || _model.state.inputRxFailSafe); + _model.setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE, _model.state.input.rxLoss || _model.state.input.rxFailSafe); _model.setArmingDisabled(ARMING_DISABLED_THROTTLE, !_model.isThrottleLow()); _model.setArmingDisabled(ARMING_DISABLED_CALIBRATING, _model.calibrationActive()); _model.setArmingDisabled(ARMING_DISABLED_MOTOR_PROTOCOL, _model.config.output.protocol == ESC_PROTOCOL_DISABLED); @@ -120,7 +120,7 @@ class Actuator size_t ch = c->ch; // + AXIS_AUX_1; if(ch < AXIS_AUX_1 || ch >= AXIS_COUNT) continue; // invalid channel - int16_t val = _model.state.inputUs[ch]; + int16_t val = _model.state.input.us[ch]; if(val > min && val < max) { newMask |= 1 << c->id; @@ -191,7 +191,7 @@ class Actuator { _model.state.airmodeAllowed = false; } - if(armed && !_model.state.airmodeAllowed && _model.state.inputUs[AXIS_THRUST] > 1400) // activate airmode in the air + if(armed && !_model.state.airmodeAllowed && _model.state.input.us[AXIS_THRUST] > 1400) // activate airmode in the air { _model.state.airmodeAllowed = true; } @@ -220,7 +220,7 @@ class Actuator void updateDynLpf() { return; // temporary disable - int scale = Math::clamp((int)_model.state.inputUs[AXIS_THRUST], 1000, 2000); + int scale = Math::clamp((int)_model.state.input.us[AXIS_THRUST], 1000, 2000); if(_model.config.gyro.dynLpfFilter.cutoff > 0) { int gyroFreq = Math::map(scale, 1000, 2000, _model.config.gyro.dynLpfFilter.cutoff, _model.config.gyro.dynLpfFilter.freq); for(size_t i = 0; i <= AXIS_YAW; i++) { @@ -241,7 +241,7 @@ class Actuator { case RESCUE_CONFIG_PENDING: // if some rc frames are received, disable to prevent activate later - if(_model.state.inputFrameCount > 100) + if(_model.state.input.frameCount > 100) { _model.state.rescueConfigMode = RESCUE_CONFIG_DISABLED; } diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index a2c40e83..7f662a5f 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -243,7 +243,7 @@ void FAST_CODE_ATTR Blackbox::updateData() 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); + rcCommand[i] = (_model.state.input.buffer[i] - 1500) * (i == AXIS_YAW ? -1 : 1); if(_model.accelActive()) { acc.accADC[i] = _model.state.accel[i] * ACCEL_G_INV * acc.dev.acc_1G; } @@ -254,10 +254,10 @@ void FAST_CODE_ATTR Blackbox::updateData() baro.altitude = lrintf(_model.state.baroAltitude * 100.f); // cm } } - rcCommand[AXIS_THRUST] = _model.state.inputBuffer[AXIS_THRUST]; + rcCommand[AXIS_THRUST] = _model.state.input.buffer[AXIS_THRUST]; for(size_t i = 0; i < 4; i++) { - motor[i] = Math::clamp(_model.state.outputUs[i], (int16_t)1000, (int16_t)2000); + motor[i] = Math::clamp(_model.state.output.us[i], (int16_t)1000, (int16_t)2000); if(_model.state.digitalOutput) { motor[i] = PWM_TO_DSHOT(motor[i]); diff --git a/lib/Espfc/src/Blackbox/BlackboxBridge.cpp b/lib/Espfc/src/Blackbox/BlackboxBridge.cpp index c07a4114..183f20d5 100644 --- a/lib/Espfc/src/Blackbox/BlackboxBridge.cpp +++ b/lib/Espfc/src/Blackbox/BlackboxBridge.cpp @@ -24,7 +24,7 @@ int32_t getAmperageLatest(void) bool rxIsReceivingSignal(void) { if(!_model_ptr) return false; - return !((*_model_ptr).state.inputRxLoss || (*_model_ptr).state.inputRxFailSafe); + return !((*_model_ptr).state.input.rxLoss || (*_model_ptr).state.input.rxFailSafe); } bool isRssiConfigured(void) @@ -70,7 +70,7 @@ float pidGetPreviousSetpoint(int axis) float mixerGetThrottle(void) { - return (_model_ptr->state.output[Espfc::AXIS_THRUST] + 1.0f) * 0.5f; + return (_model_ptr->state.output.ch[Espfc::AXIS_THRUST] + 1.0f) * 0.5f; } int16_t getMotorOutputLow() @@ -90,5 +90,5 @@ bool areMotorsRunning(void) uint16_t getDshotErpm(uint8_t i) { - return _model_ptr->state.outputTelemetryErpm[i]; + return _model_ptr->state.output.telemetry.erpm[i]; } diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index aa08fdb8..72e9f0b3 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -1197,7 +1197,7 @@ class Cli uint32_t mode = _model.config.scaler[i].dimension; if(!mode) continue; short c = _model.config.scaler[i].channel; - float v = _model.state.input[c]; + float v = _model.state.input.ch[c]; float min = _model.config.scaler[i].minScale * 0.01f; float max = _model.config.scaler[i].maxScale * 0.01f; float scale = Math::map3(v, -1.f, 0.f, 1.f, min, min < 0 ? 0.f : 1.f, max); @@ -1280,11 +1280,11 @@ class Cli s.println(); s.print(F(" input: ")); - s.print(_model.state.inputFrameRate); + s.print(_model.state.input.frameRate); s.print(F(" Hz, ")); - s.print(_model.state.inputAutoFreq); + s.print(_model.state.input.autoFreq); s.print(F(" Hz, ")); - s.println(_model.state.inputAutoFactor); + s.println(_model.state.input.autoFactor); static const char* armingDisableNames[] = { PSTR("NO_GYRO"), PSTR("FAILSAFE"), PSTR("RX_FAILSAFE"), PSTR("BAD_RX_RECOVERY"), @@ -1384,7 +1384,7 @@ class Cli } else { s.print(_model.config.pin[i + PIN_OUTPUT_0]); s.print(' '); - s.println(_model.state.outputUs[i]); + s.println(_model.state.output.us[i]); } } } diff --git a/lib/Espfc/src/Controller.cpp b/lib/Espfc/src/Controller.cpp index afa8d52e..13b30c3e 100644 --- a/lib/Espfc/src/Controller.cpp +++ b/lib/Espfc/src/Controller.cpp @@ -58,19 +58,19 @@ 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); + const float speed = _speedFilter.update(_model.state.output.ch[AXIS_PITCH] * speedScale + _model.state.gyro[AXIS_PITCH] * gyroScale); float angle = 0; if(true || _model.isActive(MODE_ANGLE)) { - angle = _model.state.input[AXIS_PITCH] * Math::toRad(_model.config.level.angleLimit); + angle = _model.state.input.ch[AXIS_PITCH] * Math::toRad(_model.config.level.angleLimit); } else { - angle = _model.state.outerPid[AXIS_PITCH].update(_model.state.input[AXIS_PITCH], speed) * Math::toRad(_model.config.level.rateLimit); + angle = _model.state.outerPid[AXIS_PITCH].update(_model.state.input.ch[AXIS_PITCH], speed) * Math::toRad(_model.config.level.rateLimit); } _model.state.desiredAngle.set(AXIS_PITCH, angle); - _model.state.desiredRate[AXIS_YAW] = _model.state.input[AXIS_YAW] * Math::toRad(_model.config.level.rateLimit); + _model.state.desiredRate[AXIS_YAW] = _model.state.input.ch[AXIS_YAW] * Math::toRad(_model.config.level.rateLimit); if(_model.config.debug.mode == DEBUG_ANGLERATE) { @@ -89,20 +89,20 @@ void Controller::innerLoopRobot() const bool stabilize = angle < Math::toRad(_model.config.level.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]); + _model.state.output.ch[AXIS_PITCH] = _model.state.innerPid[AXIS_PITCH].update(_model.state.desiredAngle[AXIS_PITCH], _model.state.angle[AXIS_PITCH]); + _model.state.output.ch[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; + _model.state.output.ch[AXIS_PITCH] = 0.f; + _model.state.output.ch[AXIS_YAW] = 0.f; } if(_model.config.debug.mode == 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); + _model.state.debug[3] = lrintf(_model.state.output.ch[AXIS_PITCH] * 1000); } } @@ -111,8 +111,8 @@ void FAST_CODE_ATTR Controller::outerLoop() if(_model.isActive(MODE_ANGLE)) { _model.state.desiredAngle = VectorFloat( - _model.state.input[AXIS_ROLL] * Math::toRad(_model.config.level.angleLimit), - _model.state.input[AXIS_PITCH] * Math::toRad(_model.config.level.angleLimit), + _model.state.input.ch[AXIS_ROLL] * Math::toRad(_model.config.level.angleLimit), + _model.state.input.ch[AXIS_PITCH] * Math::toRad(_model.config.level.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]); @@ -123,11 +123,11 @@ void FAST_CODE_ATTR Controller::outerLoop() } 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_ROLL] = calculateSetpointRate(AXIS_ROLL, _model.state.input.ch[AXIS_ROLL]); + _model.state.desiredRate[AXIS_PITCH] = calculateSetpointRate(AXIS_PITCH, _model.state.input.ch[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]; + _model.state.desiredRate[AXIS_YAW] = calculateSetpointRate(AXIS_YAW, _model.state.input.ch[AXIS_YAW]); + _model.state.desiredRate[AXIS_THRUST] = _model.state.input.ch[AXIS_THRUST]; if(_model.config.debug.mode == DEBUG_ANGLERATE) { @@ -143,10 +143,10 @@ 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.output.ch[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]; + _model.state.output.ch[AXIS_THRUST] = _model.state.desiredRate[AXIS_THRUST]; if(_model.config.debug.mode == DEBUG_ITERM_RELAX) { @@ -160,7 +160,7 @@ void FAST_CODE_ATTR Controller::innerLoop() float Controller::getTpaFactor() const { if(_model.config.controller.tpaScale == 0) return 1.f; - float t = Math::clamp(_model.state.inputUs[AXIS_THRUST], (float)_model.config.controller.tpaBreakpoint, 2000.f); + float t = Math::clamp(_model.state.input.us[AXIS_THRUST], (float)_model.config.controller.tpaBreakpoint, 2000.f); return Math::map(t, (float)_model.config.controller.tpaBreakpoint, 2000.f, 1.f, 1.f - ((float)_model.config.controller.tpaScale * 0.01f)); } diff --git a/lib/Espfc/src/Input.cpp b/lib/Espfc/src/Input.cpp index c9199313..83442ab5 100644 --- a/lib/Espfc/src/Input.cpp +++ b/lib/Espfc/src/Input.cpp @@ -10,34 +10,34 @@ Input::Input(Model& model, TelemetryManager& telemetry): _model(model), _telemet 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); + _model.state.input.channelCount = _device ? _device->getChannelCount() : INPUT_CHANNELS; + _model.state.input.frameDelta = FRAME_TIME_DEFAULT_US; + _model.state.input.frameRate = 1000000ul / _model.state.input.frameDelta; + _model.state.input.frameCount = 0; + _model.state.input.autoFactor = 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 + _model.state.input.interpolationDelta = Math::clamp(_model.state.input.frameDelta, (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 + _model.state.input.interpolationDelta = _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; + _model.state.input.interpolationDelta = FRAME_TIME_DEFAULT_US * 0.000001f; break; } - _model.state.inputInterpolationStep = _model.state.loopTimer.intervalf / _model.state.inputInterpolationDelta; + _model.state.input.interpolationStep = _model.state.loopTimer.intervalf / _model.state.input.interpolationDelta; _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; + _model.state.input.raw[c] = v; + _model.state.input.buffer[c] = v; + _model.state.input.bufferPrevious[c] = v; setInput((Axis)c, v, true, true); } return 1; @@ -55,7 +55,7 @@ int16_t FAST_CODE_ATTR Input::getFailsafeValue(uint8_t c) case FAILSAFE_MODE_INVALID: case FAILSAFE_MODE_HOLD: default: - return _model.state.inputBuffer[c]; + return _model.state.input.buffer[c]; } } @@ -65,13 +65,13 @@ void FAST_CODE_ATTR Input::setInput(Axis i, float v, bool newFrame, bool noFilte 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); + _model.state.input.us[i] = nv; + _model.state.input.ch[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); + _model.state.input.us[i] = v; + _model.state.input.ch[i] = Math::map(v, ich.min, ich.max, -1.f, 1.f); } } @@ -110,9 +110,9 @@ InputStatus FAST_CODE_ATTR Input::readInputs() if(status == INPUT_IDLE) return status; - _model.state.inputRxLoss = (status == INPUT_LOST || status == INPUT_FAILSAFE); - _model.state.inputRxFailSafe = (status == INPUT_FAILSAFE); - _model.state.inputFrameCount++; + _model.state.input.rxLoss = (status == INPUT_LOST || status == INPUT_FAILSAFE); + _model.state.input.rxFailSafe = (status == INPUT_FAILSAFE); + _model.state.input.frameCount++; updateFrameRate(); @@ -120,10 +120,10 @@ InputStatus FAST_CODE_ATTR Input::readInputs() if(_model.config.debug.mode == 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); + _model.state.debug[0] = !_model.state.input.rxLoss; + _model.state.debug[1] = _model.state.input.rxFailSafe; + _model.state.debug[2] = _model.state.input.channelsValid; + _model.state.debug[3] = _model.state.input.lossTime / (100 * 1000); } return status; @@ -131,20 +131,20 @@ InputStatus FAST_CODE_ATTR Input::readInputs() void FAST_CODE_ATTR Input::processInputs() { - if(_model.state.inputFrameCount < 5) return; // ignore few first frames that might be garbage + if(_model.state.input.frameCount < 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); + _device->get(channels, _model.state.input.channelCount); - _model.state.inputChannelsValid = true; - for(size_t c = 0; c < _model.state.inputChannelCount; c++) + _model.state.input.channelsValid = true; + for(size_t c = 0; c < _model.state.input.channelCount; c++) { const InputChannelConfig& ich = _model.config.input.channel[c]; // remap channels - int16_t v = _model.state.inputRaw[c] = (int16_t)channels[ich.map]; + int16_t v = _model.state.input.raw[c] = (int16_t)channels[ich.map]; // adj midrc v -= _model.config.input.midRc - PWM_RANGE_MID; @@ -167,12 +167,12 @@ void FAST_CODE_ATTR Input::processInputs() if(v < _model.config.input.minRc || v > _model.config.input.maxRc) { v = getFailsafeValue(c); - if(c <= AXIS_THRUST) _model.state.inputChannelsValid = false; + if(c <= AXIS_THRUST) _model.state.input.channelsValid = false; } // update input buffer - _model.state.inputBufferPrevious[c] = _model.state.inputBuffer[c]; - _model.state.inputBuffer[c] = v; + _model.state.input.bufferPrevious[c] = _model.state.input.buffer[c]; + _model.state.input.buffer[c] = v; } if(_model.config.debug.mode == DEBUG_RX_TIMING) @@ -204,15 +204,15 @@ bool FAST_CODE_ATTR Input::failsafe(InputStatus status) } // 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) + _model.state.input.lossTime = micros() - _model.state.input.frameTime; + if(_model.state.input.lossTime > 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) + if(_model.state.input.lossTime >= 2 * TENTH_TO_US) { failsafeStage1(); return true; @@ -224,14 +224,14 @@ bool FAST_CODE_ATTR Input::failsafe(InputStatus status) void FAST_CODE_ATTR Input::failsafeIdle() { _model.state.failsafe.phase = FC_FAILSAFE_IDLE; - _model.state.inputLossTime = 0; + _model.state.input.lossTime = 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++) + _model.state.input.rxLoss = true; + for(size_t i = 0; i < _model.state.input.channelCount; i++) { setInput((Axis)i, getFailsafeValue(i), true, true); } @@ -240,8 +240,8 @@ void FAST_CODE_ATTR Input::failsafeStage1() void FAST_CODE_ATTR Input::failsafeStage2() { _model.state.failsafe.phase = FC_FAILSAFE_RX_LOSS_DETECTED; - _model.state.inputRxLoss = true; - _model.state.inputRxFailSafe = true; + _model.state.input.rxLoss = true; + _model.state.input.rxFailSafe = true; if(_model.isModeActive(MODE_ARMED)) { _model.state.failsafe.phase = FC_FAILSAFE_LANDED; @@ -265,16 +265,16 @@ void FAST_CODE_ATTR Input::filterInputs(InputStatus status) } if(_step < 1.f) { - _step += _model.state.inputInterpolationStep; + _step += _model.state.input.interpolationStep; } } - for(size_t c = 0; c < _model.state.inputChannelCount; c++) + for(size_t c = 0; c < _model.state.input.channelCount; c++) { - float v = _model.state.inputBuffer[c]; + float v = _model.state.input.buffer[c]; if(c <= AXIS_THRUST) { - v = interpolation ? _interpolate(_model.state.inputBufferPrevious[c], v, _step) : v; + v = interpolation ? _interpolate(_model.state.input.bufferPrevious[c], v, _step) : v; } setInput((Axis)c, v, newFrame); } @@ -288,36 +288,36 @@ void FAST_CODE_ATTR Input::filterInputs(InputStatus status) void FAST_CODE_ATTR Input::updateFrameRate() { const uint32_t now = micros(); - const uint32_t frameDelta = now - _model.state.inputFrameTime; + const uint32_t frameDelta = now - _model.state.input.frameTime; - _model.state.inputFrameTime = now; - _model.state.inputFrameDelta += (((int)frameDelta - (int)_model.state.inputFrameDelta) >> 3); // avg * 0.125 - _model.state.inputFrameRate = 1000000ul / _model.state.inputFrameDelta; + _model.state.input.frameTime = now; + _model.state.input.frameDelta += (((int)frameDelta - (int)_model.state.input.frameDelta) >> 3); // avg * 0.125 + _model.state.input.frameRate = 1000000ul / _model.state.input.frameDelta; 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; + _model.state.input.interpolationDelta = Math::clamp(_model.state.input.frameDelta, (uint32_t)4000, (uint32_t)40000) * 0.000001f; // estimate real interval + _model.state.input.interpolationStep = _model.state.loopTimer.intervalf / _model.state.input.interpolationDelta; } if(_model.config.debug.mode == DEBUG_RC_SMOOTHING_RATE) { - _model.state.debug[0] = _model.state.inputFrameDelta / 10; - _model.state.debug[1] = _model.state.inputFrameRate; + _model.state.debug[0] = _model.state.input.frameDelta / 10; + _model.state.debug[1] = _model.state.input.frameRate; } // 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) + float freq = std::max(_model.state.input.frameRate * _model.state.input.autoFactor, 15.f); // no lower than 15Hz + if(freq > _model.state.input.autoFreq * 1.1f || freq < _model.state.input.autoFreq * 0.9f) { - _model.state.inputAutoFreq += 0.25f * (freq - _model.state.inputAutoFreq); + _model.state.input.autoFreq += 0.25f * (freq - _model.state.input.autoFreq); if(_model.config.debug.mode == DEBUG_RC_SMOOTHING_RATE) { _model.state.debug[2] = lrintf(freq); - _model.state.debug[3] = lrintf(_model.state.inputAutoFreq); + _model.state.debug[3] = lrintf(_model.state.input.autoFreq); } - FilterConfig conf((FilterType)_model.config.input.filter.type, _model.state.inputAutoFreq); - FilterConfig confDerivative((FilterType)_model.config.input.filterDerivative.type, _model.state.inputAutoFreq); + FilterConfig conf((FilterType)_model.config.input.filter.type, _model.state.input.autoFreq); + FilterConfig confDerivative((FilterType)_model.config.input.filterDerivative.type, _model.state.input.autoFreq); for(size_t i = 0; i <= AXIS_THRUST; i++) { if(_model.config.input.filter.freq == 0) diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 669aa962..aa6d3437 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -98,7 +98,7 @@ class Model bool isThrottleLow() const { - return state.inputUs[AXIS_THRUST] < config.input.minCheck; + return state.input.us[AXIS_THRUST] < config.input.minCheck; } bool blackboxEnabled() const @@ -189,7 +189,7 @@ class Model void setOutputSaturated(bool val) { - state.outputSaturated = val; + state.output.saturated = val; for(size_t i = 0; i < 3; i++) { state.innerPid[i].outputSaturated = val; @@ -203,8 +203,8 @@ class Model for(size_t i = 0; i < count; i++) { if(config.output.channel[i].servo) continue; - if(state.outputDisarmed[i] != config.output.minCommand) return true; - //if(state.outputUs[i] != config.output.minCommand) return true; + if(state.output.disarmed[i] != config.output.minCommand) return true; + //if(state.output.us[i] != config.output.minCommand) return true; } return false; } @@ -257,8 +257,8 @@ class Model uint16_t getRssi() const { size_t channel = config.input.rssiChannel; - if(channel < 4 || channel > state.inputChannelCount) return 0; - float value = state.input[channel - 1]; + if(channel < 4 || channel > state.input.channelCount) return 0; + float value = state.input.ch[channel - 1]; return Math::clamp(lrintf(Math::map(value, -1.0f, 1.0f, 0.0f, 1023.0f)), 0l, 1023l); } @@ -505,7 +505,7 @@ class Model // ensure disarmed pulses for(size_t i = 0; i < OUTPUT_CHANNELS; i++) { - state.outputDisarmed[i] = config.output.channel[i].servo ? config.output.channel[i].neutral : config.output.minCommand; // ROBOT + state.output.disarmed[i] = config.output.channel[i].servo ? config.output.channel[i].neutral : config.output.minCommand; // ROBOT } state.buzzer.beeperMask = config.buzzer.beeperMask; diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 9e0b0775..d0bae642 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -139,6 +139,60 @@ enum RescueConfigMode { RESCUE_CONFIG_DISABLED, }; +struct OutputTelemetryState +{ + int16_t errors[OUTPUT_CHANNELS]; + int32_t errorsSum[OUTPUT_CHANNELS]; + int32_t errorsCount[OUTPUT_CHANNELS]; + + uint32_t erpm[OUTPUT_CHANNELS]; + float rpm[OUTPUT_CHANNELS]; + float freq[OUTPUT_CHANNELS]; + + int8_t temperature[OUTPUT_CHANNELS]; + int8_t voltage[OUTPUT_CHANNELS]; + int8_t current[OUTPUT_CHANNELS]; + int8_t debug1[OUTPUT_CHANNELS]; + int8_t debug2[OUTPUT_CHANNELS]; + int8_t debug3[OUTPUT_CHANNELS]; + int8_t events[OUTPUT_CHANNELS]; +}; + +struct OutputState +{ + float ch[OUTPUT_CHANNELS]; + int16_t us[OUTPUT_CHANNELS]; + int16_t disarmed[OUTPUT_CHANNELS]; + bool saturated; + OutputTelemetryState telemetry; +}; + +struct InputState +{ + size_t channelCount; + bool channelsValid; + bool rxLoss; + bool rxFailSafe; + + uint32_t frameTime; + uint32_t frameDelta; + uint32_t frameRate; + uint32_t frameCount; + uint32_t lossTime; + + float interpolationDelta; + float interpolationStep; + float autoFactor; + float autoFreq; + + int16_t raw[INPUT_CHANNELS]; + int16_t buffer[INPUT_CHANNELS]; + int16_t bufferPrevious[INPUT_CHANNELS]; + + float us[INPUT_CHANNELS]; + float ch[INPUT_CHANNELS]; +}; + // working data struct ModelState { @@ -201,47 +255,10 @@ struct ModelState Control::Pid innerPid[AXES]; Control::Pid outerPid[AXES]; - size_t inputChannelCount; - bool inputChannelsValid; - bool inputRxLoss; - bool inputRxFailSafe; - - uint32_t inputFrameTime; - uint32_t inputFrameDelta; - uint32_t inputFrameRate; - uint32_t inputFrameCount; - uint32_t inputLossTime; - float inputInterpolationDelta; - float inputInterpolationStep; - float inputAutoFactor; - float inputAutoFreq; - - int16_t inputRaw[INPUT_CHANNELS]; - int16_t inputBuffer[INPUT_CHANNELS]; - int16_t inputBufferPrevious[INPUT_CHANNELS]; - - float inputUs[INPUT_CHANNELS]; - float input[INPUT_CHANNELS]; + InputState input; FailsafeState failsafe; - float output[OUTPUT_CHANNELS]; - int16_t outputUs[OUTPUT_CHANNELS]; - int16_t outputDisarmed[OUTPUT_CHANNELS]; - bool outputSaturated; - - int16_t outputTelemetryErrors[OUTPUT_CHANNELS]; - int32_t outputTelemetryErrorsSum[OUTPUT_CHANNELS]; - int32_t outputTelemetryErrorsCount[OUTPUT_CHANNELS]; - uint32_t outputTelemetryErpm[OUTPUT_CHANNELS]; - float outputTelemetryRpm[OUTPUT_CHANNELS]; - float outputTelemetryFreq[OUTPUT_CHANNELS]; - int8_t outputTelemetryTemperature[OUTPUT_CHANNELS]; - int8_t outputTelemetryVoltage[OUTPUT_CHANNELS]; - int8_t outputTelemetryCurrent[OUTPUT_CHANNELS]; - int8_t outputTelemetryDebug1[OUTPUT_CHANNELS]; - int8_t outputTelemetryDebug2[OUTPUT_CHANNELS]; - int8_t outputTelemetryDebug3[OUTPUT_CHANNELS]; - int8_t outputTelemetryEvents[OUTPUT_CHANNELS]; + OutputState output; // other state Kalman kalman[AXES]; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index fc1dbc23..880846d2 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -874,7 +874,7 @@ class MspProcessor break; case MSP_RXFAIL_CONFIG: - for (size_t i = 0; i < _model.state.inputChannelCount; i++) + for (size_t i = 0; i < _model.state.input.channelCount; i++) { r.writeU8(_model.config.input.channel[i].fsMode); r.writeU16(_model.config.input.channel[i].fsValue); @@ -897,9 +897,9 @@ class MspProcessor break; case MSP_RC: - for(size_t i = 0; i < _model.state.inputChannelCount; i++) + for(size_t i = 0; i < _model.state.input.channelCount; i++) { - r.writeU16(lrintf(_model.state.inputUs[i])); + r.writeU16(lrintf(_model.state.input.us[i])); } break; @@ -1299,7 +1299,7 @@ class MspProcessor r.writeU16(0); continue; } - r.writeU16(_model.state.outputUs[i]); + r.writeU16(_model.state.output.us[i]); } break; @@ -1316,11 +1316,11 @@ class MspProcessor if (_model.config.pin[i + PIN_OUTPUT_0] != -1) { - rpm = lrintf(_model.state.outputTelemetryRpm[i]); - invalidPct = _model.state.outputTelemetryErrors[i]; - escTemperature = _model.state.outputTelemetryTemperature[i]; - escVoltage = _model.state.outputTelemetryVoltage[i]; - escCurrent = _model.state.outputTelemetryCurrent[i]; + rpm = lrintf(_model.state.output.telemetry.rpm[i]); + invalidPct = _model.state.output.telemetry.errors[i]; + escTemperature = _model.state.output.telemetry.temperature[i]; + escVoltage = _model.state.output.telemetry.voltage[i]; + escCurrent = _model.state.output.telemetry.current[i]; } r.writeU32(rpm); @@ -1335,7 +1335,7 @@ class MspProcessor case MSP_SET_MOTOR: for(size_t i = 0; i < OUTPUT_CHANNELS; i++) { - _model.state.outputDisarmed[i] = m.readU16(); + _model.state.output.disarmed[i] = m.readU16(); } break; @@ -1347,7 +1347,7 @@ class MspProcessor r.writeU16(1500); continue; } - r.writeU16(_model.state.outputUs[i]); + r.writeU16(_model.state.output.us[i]); } break; diff --git a/lib/Espfc/src/Output/Mixer.cpp b/lib/Espfc/src/Output/Mixer.cpp index c612f9e5..96dfeedb 100644 --- a/lib/Espfc/src/Output/Mixer.cpp +++ b/lib/Espfc/src/Output/Mixer.cpp @@ -55,15 +55,15 @@ int Mixer::begin() _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; + _model.state.output.telemetry.errors[i] = 0; + _model.state.output.telemetry.errorsSum[i] = 0; + _model.state.output.telemetry.errorsCount[i] = 0; + _model.state.output.telemetry.temperature[i] = 0; + _model.state.output.telemetry.current[i] = 0; + _model.state.output.telemetry.voltage[i] = 0; + _model.state.output.telemetry.erpm[i] = 0; + _model.state.output.telemetry.rpm[i] = 0; + _model.state.output.telemetry.freq[i] = 0; } motorInitEscDevice(_motor); @@ -113,19 +113,19 @@ void FAST_CODE_ATTR Mixer::updateMixer(const MixerConfig& mixer, float * outputs 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.mixer.yawReverse ? 1.f : -1.f); - sources[MIXER_SOURCE_THRUST] = _model.state.output[AXIS_THRUST]; + sources[MIXER_SOURCE_ROLL] = _model.state.output.ch[AXIS_ROLL]; + sources[MIXER_SOURCE_PITCH] = _model.state.output.ch[AXIS_PITCH]; + sources[MIXER_SOURCE_YAW] = _model.state.output.ch[AXIS_YAW] * (_model.config.mixer.yawReverse ? 1.f : -1.f); + sources[MIXER_SOURCE_THRUST] = _model.state.output.ch[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]; + sources[MIXER_SOURCE_RC_ROLL] = _model.state.input.ch[AXIS_ROLL]; + sources[MIXER_SOURCE_RC_PITCH] = _model.state.input.ch[AXIS_PITCH]; + sources[MIXER_SOURCE_RC_YAW] = _model.state.input.ch[AXIS_YAW]; + sources[MIXER_SOURCE_RC_THRUST] = _model.state.input.ch[AXIS_THRUST]; for(size_t i = 0; i < 3; i++) { - sources[MIXER_SOURCE_RC_AUX1 + i] = _model.state.input[AXIS_AUX_1 + i]; + sources[MIXER_SOURCE_RC_AUX1 + i] = _model.state.input.ch[AXIS_AUX_1 + i]; } for(size_t i = 0; i < OUTPUT_CHANNELS; i++) @@ -244,19 +244,19 @@ void FAST_CODE_ATTR Mixer::writeOutput(const MixerConfig& mixer, float * out) 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]; + _model.state.output.us[i] = och.servo && _model.state.output.disarmed[i] == 1000 ? och.neutral : _model.state.output.disarmed[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); + _model.state.output.us[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)); + _model.state.output.us[i] = lrintf(Math::map(v, -1.f, 1.f, _model.state.minThrottle, _model.state.maxThrottle)); } } } @@ -266,11 +266,11 @@ void FAST_CODE_ATTR Mixer::writeOutput(const MixerConfig& mixer, float * out) const OutputChannelConfig& och = _model.config.output.channel[i]; if(och.servo) { - if(_servo) _servo->write(i, _model.state.outputUs[i]); + if(_servo) _servo->write(i, _model.state.output.us[i]); } else { - if(_motor) _motor->write(i, _model.state.outputUs[i]); + if(_motor) _motor->write(i, _model.state.output.us[i]); } } @@ -289,11 +289,11 @@ void FAST_CODE_ATTR Mixer::readTelemetry() uint32_t value = _motor->telemetry(i); value = EscDriver::gcrToRawValue(value); - _model.state.outputTelemetryErrorsCount[i]++; + _model.state.output.telemetry.errorsCount[i]++; if(value == EscDriver::INVALID_TELEMETRY_VALUE) { - _model.state.outputTelemetryErrorsSum[i]++; + _model.state.output.telemetry.errorsSum[i]++; continue; } @@ -302,44 +302,44 @@ void FAST_CODE_ATTR Mixer::readTelemetry() { case 0x0200: // Temperature range (in degree Celsius, just like Blheli_32 and KISS) - _model.state.outputTelemetryTemperature[i] = value & 0x00ff; + _model.state.output.telemetry.temperature[i] = value & 0x00ff; break; case 0x0400: // Voltage range (0-63,75V step 0,25V) - _model.state.outputTelemetryVoltage[i] = value & 0x00ff; + _model.state.output.telemetry.voltage[i] = value & 0x00ff; break; case 0x0600: // Current range (0-255A step 1A) - _model.state.outputTelemetryCurrent[i] = value & 0x00ff; + _model.state.output.telemetry.current[i] = value & 0x00ff; break; case 0x0800: // Debug 1 value - _model.state.outputTelemetryDebug1[i] = value & 0x00ff; + _model.state.output.telemetry.debug1[i] = value & 0x00ff; break; case 0x0A00: // Debug 2 value - _model.state.outputTelemetryDebug2[i] = value & 0x00ff; + _model.state.output.telemetry.debug2[i] = value & 0x00ff; break; case 0x0C00: // Debug 3 value - _model.state.outputTelemetryDebug3[i] = value & 0x00ff; + _model.state.output.telemetry.debug3[i] = value & 0x00ff; break; case 0x0E00: // State / Events - _model.state.outputTelemetryEvents[i] = value & 0x00ff; + _model.state.output.telemetry.events[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]); + _model.state.output.telemetry.erpm[i] = EscDriver::convertToErpm(value); + _model.state.output.telemetry.rpm[i] = erpmToRpm(_model.state.output.telemetry.erpm[i]); + _model.setDebug(DEBUG_DSHOT_RPM_TELEMETRY, i, _model.state.output.telemetry.erpm[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])); + _model.state.output.telemetry.freq[i] = _model.state.rpmFreqFilter[i].update(erpmToHz(_model.state.output.telemetry.erpm[i])); } _statsCounter++; @@ -347,16 +347,16 @@ void FAST_CODE_ATTR Mixer::readTelemetry() { for(size_t i = 0; i < OUTPUT_CHANNELS; i++) { - if(_model.state.outputTelemetryErrorsCount[i]) + if(_model.state.output.telemetry.errorsCount[i]) { - _model.state.outputTelemetryErrors[i] = _model.state.outputTelemetryErrorsSum[i] * 10000 / _model.state.outputTelemetryErrorsCount[i]; + _model.state.output.telemetry.errors[i] = _model.state.output.telemetry.errorsSum[i] * 10000 / _model.state.output.telemetry.errorsCount[i]; if(_model.config.debug.mode == DEBUG_DSHOT_RPM_ERRORS) { - _model.state.debug[i] = _model.state.outputTelemetryErrors[i]; + _model.state.debug[i] = _model.state.output.telemetry.errors[i]; } } - _model.state.outputTelemetryErrorsCount[i] = 0; - _model.state.outputTelemetryErrorsSum[i] = 0; + _model.state.output.telemetry.errorsCount[i] = 0; + _model.state.output.telemetry.errorsSum[i] = 0; } _statsCounter = 0; } diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index b002620c..7cb9e48f 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -164,7 +164,7 @@ void FAST_CODE_ATTR GyroSensor::rpmFilterUpdate() Stats::Measure measure(_model.state.stats, COUNTER_RPM_UPDATE); - const float motorFreq = _model.state.outputTelemetryFreq[_rpm_motor_index]; + const float motorFreq = _model.state.output.telemetry.freq[_rpm_motor_index]; for (size_t n = 0; n < _model.config.gyro.rpmFilter.harmonics; n++) { const float freq = Math::clamp(motorFreq * (n + 1), _rpm_min_freq, _rpm_max_freq); @@ -182,7 +182,7 @@ void FAST_CODE_ATTR GyroSensor::rpmFilterUpdate() } } - _model.setDebug(DEBUG_RPM_FILTER, _rpm_motor_index, lrintf(_model.state.outputTelemetryFreq[_rpm_motor_index])); + _model.setDebug(DEBUG_RPM_FILTER, _rpm_motor_index, lrintf(_model.state.output.telemetry.freq[_rpm_motor_index])); _rpm_motor_index++; if (_rpm_motor_index >= RPM_FILTER_MOTOR_MAX) diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index 538b5f21..2946673b 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -468,8 +468,8 @@ void test_actuator_arming_failsafe() model.config.output.protocol = ESC_PROTOCOL_DSHOT150; model.state.failsafe.phase = FC_FAILSAFE_RX_LOSS_DETECTED; model.state.gyroCalibrationState = CALIBRATION_UPDATE; - model.state.inputRxFailSafe = true; - model.state.inputRxLoss = true; + model.state.input.rxFailSafe = true; + model.state.input.rxLoss = true; //model.begin(); @@ -488,7 +488,7 @@ void test_actuator_arming_throttle() Model model; model.config.output.protocol = ESC_PROTOCOL_DSHOT150; model.config.input.minCheck = 1050; - model.state.inputUs[AXIS_THRUST] = 1100; + model.state.input.us[AXIS_THRUST] = 1100; model.state.gyroPresent = true; //model.begin(); From 9c2db28363a0a351ea0c6f5c48d10a7510a66101 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 28 Oct 2024 22:15:33 +0100 Subject: [PATCH 28/68] mixer state refactor --- lib/Espfc/src/Actuator.h | 4 ++-- lib/Espfc/src/Blackbox/Blackbox.cpp | 8 ++++---- lib/Espfc/src/Blackbox/BlackboxBridge.cpp | 4 ++-- lib/Espfc/src/Cli.h | 2 +- lib/Espfc/src/Controller.cpp | 2 +- lib/Espfc/src/Espfc.cpp | 6 +++--- lib/Espfc/src/Hardware.h | 4 ++-- lib/Espfc/src/Input.cpp | 6 +++--- lib/Espfc/src/Model.h | 20 +++++++++--------- lib/Espfc/src/ModelConfig.h | 3 +++ lib/Espfc/src/ModelState.h | 25 +++++++++++++++-------- lib/Espfc/src/Output/Mixer.cpp | 20 +++++++++--------- test/test_fc/test_fc.cpp | 4 ++-- 13 files changed, 59 insertions(+), 49 deletions(-) diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h index 29b61293..3164de51 100644 --- a/lib/Espfc/src/Actuator.h +++ b/lib/Espfc/src/Actuator.h @@ -223,13 +223,13 @@ class Actuator int scale = Math::clamp((int)_model.state.input.us[AXIS_THRUST], 1000, 2000); if(_model.config.gyro.dynLpfFilter.cutoff > 0) { int gyroFreq = Math::map(scale, 1000, 2000, _model.config.gyro.dynLpfFilter.cutoff, _model.config.gyro.dynLpfFilter.freq); - for(size_t i = 0; i <= AXIS_YAW; i++) { + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { _model.state.gyroFilter[i].reconfigure(gyroFreq); } } if(_model.config.dterm.dynLpfFilter.cutoff > 0) { int dtermFreq = Math::map(scale, 1000, 2000, _model.config.dterm.dynLpfFilter.cutoff, _model.config.dterm.dynLpfFilter.freq); - for(size_t i = 0; i <= AXIS_YAW; i++) { + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { _model.state.innerPid[i].dtermFilter.reconfigure(dtermFreq); } } diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index 7f662a5f..5865599f 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -41,7 +41,7 @@ int Blackbox::begin() systemConfigMutable()->debug_mode = debugMode = _model.config.debug.mode; controlRateConfig_t *rp = controlRateProfilesMutable(systemConfig()->activeRateProfile); - for(int i = 0; i <= AXIS_YAW; i++) + for(int i = 0; i < AXIS_COUNT_RPY; i++) { rp->rcRates[i] = _model.config.input.rate[i]; rp->rcExpo[i] = _model.config.input.expo[i]; @@ -139,8 +139,8 @@ int Blackbox::begin() 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()->minthrottle = _model.state.mixer.minThrottle; + motorConfigMutable()->maxthrottle = _model.state.mixer.maxThrottle; motorConfigMutable()->dev.useDshotTelemetry = _model.config.output.dshotTelemetry; motorConfigMutable()->motorPoleCount = _model.config.output.motorPoles; @@ -258,7 +258,7 @@ void FAST_CODE_ATTR Blackbox::updateData() for(size_t i = 0; i < 4; i++) { motor[i] = Math::clamp(_model.state.output.us[i], (int16_t)1000, (int16_t)2000); - if(_model.state.digitalOutput) + if(_model.state.mixer.digitalOutput) { motor[i] = PWM_TO_DSHOT(motor[i]); } diff --git a/lib/Espfc/src/Blackbox/BlackboxBridge.cpp b/lib/Espfc/src/Blackbox/BlackboxBridge.cpp index 183f20d5..b44bde26 100644 --- a/lib/Espfc/src/Blackbox/BlackboxBridge.cpp +++ b/lib/Espfc/src/Blackbox/BlackboxBridge.cpp @@ -75,12 +75,12 @@ float mixerGetThrottle(void) int16_t getMotorOutputLow() { - return _model_ptr->state.digitalOutput ? PWM_TO_DSHOT(1000) : 1000; + return _model_ptr->state.mixer.digitalOutput ? PWM_TO_DSHOT(1000) : 1000; } int16_t getMotorOutputHigh() { - return _model_ptr->state.digitalOutput ? PWM_TO_DSHOT(2000) : 2000; + return _model_ptr->state.mixer.digitalOutput ? PWM_TO_DSHOT(2000) : 2000; } bool areMotorsRunning(void) diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 72e9f0b3..60163af7 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -1531,7 +1531,7 @@ class Cli s.println(F(" Hz")); s.print(F(" mixer rate: ")); - s.print(_model.state.mixerTimer.rate); + s.print(_model.state.mixer.timer.rate); s.println(F(" Hz")); s.print(F(" accel rate: ")); diff --git a/lib/Espfc/src/Controller.cpp b/lib/Espfc/src/Controller.cpp index 13b30c3e..7f62d801 100644 --- a/lib/Espfc/src/Controller.cpp +++ b/lib/Espfc/src/Controller.cpp @@ -141,7 +141,7 @@ void FAST_CODE_ATTR Controller::outerLoop() void FAST_CODE_ATTR Controller::innerLoop() { const float tpaFactor = getTpaFactor(); - for(size_t i = 0; i <= AXIS_YAW; ++i) + for(size_t i = 0; i < AXIS_COUNT_RPY; ++i) { _model.state.output.ch[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); diff --git a/lib/Espfc/src/Espfc.cpp b/lib/Espfc/src/Espfc.cpp index d8c3e77e..e3e9ffe8 100644 --- a/lib/Espfc/src/Espfc.cpp +++ b/lib/Espfc/src/Espfc.cpp @@ -49,7 +49,7 @@ int FAST_CODE_ATTR Espfc::update(bool externalTrigger) #if defined(ESPFC_MULTI_CORE) _sensor.read(); - if(_model.state.inputTimer.syncTo(_model.state.gyroTimer, 1u)) + if(_model.state.input.timer.syncTo(_model.state.gyroTimer, 1u)) { _input.update(); } @@ -68,12 +68,12 @@ int FAST_CODE_ATTR Espfc::update(bool externalTrigger) if(_model.state.loopTimer.syncTo(_model.state.gyroTimer)) { _controller.update(); - if(_model.state.mixerTimer.syncTo(_model.state.loopTimer)) + if(_model.state.mixer.timer.syncTo(_model.state.loopTimer)) { _mixer.update(); } _blackbox.update(); - if(_model.state.inputTimer.syncTo(_model.state.gyroTimer, 1u)) + if(_model.state.input.timer.syncTo(_model.state.gyroTimer, 1u)) { _input.update(); } diff --git a/lib/Espfc/src/Hardware.h b/lib/Espfc/src/Hardware.h index 307a51a0..9aae4cdd 100644 --- a/lib/Espfc/src/Hardware.h +++ b/lib/Espfc/src/Hardware.h @@ -230,8 +230,8 @@ class Hardware static void restart(const Model& model) { - if(model.state.escMotor) model.state.escMotor->end(); - if(model.state.escServo) model.state.escServo->end(); + if(model.state.mixer.escMotor) model.state.mixer.escMotor->end(); + if(model.state.mixer.escServo) model.state.mixer.escServo->end(); #ifdef ESPFC_SERIAL_SOFT_0_WIFI WiFi.disconnect(); WiFi.softAPdisconnect(); diff --git a/lib/Espfc/src/Input.cpp b/lib/Espfc/src/Input.cpp index 83442ab5..38f04616 100644 --- a/lib/Espfc/src/Input.cpp +++ b/lib/Espfc/src/Input.cpp @@ -64,7 +64,7 @@ void FAST_CODE_ATTR Input::setInput(Axis i, float v, bool newFrame, bool noFilte const InputChannelConfig& ich = _model.config.input.channel[i]; if(i <= AXIS_THRUST) { - const float nv = noFilter ? v : _model.state.inputFilter[i].update(v); + const float nv = noFilter ? v : _model.state.input.filter[i].update(v); _model.state.input.us[i] = nv; _model.state.input.ch[i] = Math::map(nv, ich.min, ich.max, -1.f, 1.f); } @@ -318,11 +318,11 @@ void FAST_CODE_ATTR Input::updateFrameRate() } FilterConfig conf((FilterType)_model.config.input.filter.type, _model.state.input.autoFreq); FilterConfig confDerivative((FilterType)_model.config.input.filterDerivative.type, _model.state.input.autoFreq); - for(size_t i = 0; i <= AXIS_THRUST; i++) + for(size_t i = 0; i < AXIS_COUNT_RPYT; i++) { if(_model.config.input.filter.freq == 0) { - _model.state.inputFilter[i].reconfigure(conf, _model.state.loopTimer.rate); + _model.state.input.filter[i].reconfigure(conf, _model.state.loopTimer.rate); } if(_model.config.input.filterDerivative.freq == 0) { diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index aa6d3437..5e766617 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -432,9 +432,9 @@ class Model 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.mixer.timer.setRate(state.loopTimer.rate, config.mixerSync); int inputRate = Math::alignToClock(state.gyroTimer.rate, 1000); - state.inputTimer.setRate(state.gyroTimer.rate, state.gyroTimer.rate / inputRate); + state.input.timer.setRate(state.gyroTimer.rate, state.gyroTimer.rate / inputRate); state.actuatorTimer.setRate(50); state.dynamicFilterTimer.setRate(50); state.telemetryTimer.setInterval(config.telemetryInterval * 1000); @@ -448,11 +448,11 @@ class Model const uint32_t gyroPreFilterRate = state.gyroTimer.rate; const uint32_t gyroFilterRate = state.loopTimer.rate; - const uint32_t inputFilterRate = state.inputTimer.rate; + const uint32_t inputFilterRate = state.input.timer.rate; const uint32_t pidFilterRate = state.loopTimer.rate; // configure filters - for(size_t i = 0; i <= AXIS_YAW; i++) + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { if(isActive(FEATURE_DYNAMIC_FILTER)) { @@ -494,11 +494,11 @@ class Model { if (config.input.filterType == INPUT_FILTER) { - state.inputFilter[i].begin(config.input.filter, inputFilterRate); + state.input.filter[i].begin(config.input.filter, inputFilterRate); } else { - state.inputFilter[i].begin(FilterConfig(FILTER_PT3, 25), inputFilterRate); + state.input.filter[i].begin(FilterConfig(FILTER_PT3, 25), inputFilterRate); } } @@ -518,7 +518,7 @@ class Model pidScale[AXIS_PITCH] = 20.f; // ROBOT } - for(size_t i = 0; i <= AXIS_YAW; i++) // rpy + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) // rpy { const PidConfig& pc = config.pid[i]; Control::Pid& pid = state.innerPid[i]; @@ -547,7 +547,7 @@ class Model pid.begin(); } - for(size_t i = 0; i < AXIS_YAW; i++) + for(size_t i = 0; i < AXIS_COUNT_RP; i++) { PidConfig& pc = config.pid[FC_PID_LEVEL]; Control::Pid& pid = state.outerPid[i]; @@ -572,7 +572,7 @@ class Model void postLoad() { // load current sensor calibration - for(size_t i = 0; i <= AXIS_YAW; i++) + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { state.gyroBias.set(i, config.gyro.bias[i] / 1000.0f); state.accelBias.set(i, config.accel.bias[i] / 1000.0f); @@ -584,7 +584,7 @@ class Model void preSave() { // store current sensor calibration - for(size_t i = 0; i < 3; i++) + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { config.gyro.bias[i] = lrintf(state.gyroBias[i] * 1000.0f); config.accel.bias[i] = lrintf(state.accelBias[i] * 1000.0f); diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index fb6a201f..27c440d5 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -200,6 +200,9 @@ enum Axis { AXIS_AUX_11, AXIS_AUX_12, AXIS_COUNT, + AXIS_COUNT_RP = AXIS_YAW, // RP axis count + AXIS_COUNT_RPY = AXIS_THRUST, // RPY axis count + AXIS_COUNT_RPYT = AXIS_AUX_1, // RPYT axis count }; enum Feature { diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index d0bae642..6831b564 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -191,6 +191,21 @@ struct InputState float us[INPUT_CHANNELS]; float ch[INPUT_CHANNELS]; + + Filter filter[AXIS_COUNT_RPYT]; + + Timer timer; +}; + +struct MixerState +{ + Timer timer; + float minThrottle; + float maxThrottle; + bool digitalOutput; + + EscDriver * escMotor; + EscDriver * escServo; }; // working data @@ -240,7 +255,6 @@ struct ModelState Filter accelFilter[3]; Filter magFilter[3]; - Filter inputFilter[4]; Filter rpmFreqFilter[RPM_FILTER_MOTOR_MAX]; Filter rpmFilter[RPM_FILTER_MOTOR_MAX][RPM_FILTER_HARMONICS_MAX][3]; @@ -258,6 +272,7 @@ struct ModelState InputState input; FailsafeState failsafe; + MixerState mixer; OutputState output; // other state @@ -288,12 +303,6 @@ struct ModelState int32_t loopRate; Timer loopTimer; - Timer mixerTimer; - float minThrottle; - float maxThrottle; - bool digitalOutput; - - Timer inputTimer; Timer actuatorTimer; Timer magTimer; @@ -355,8 +364,6 @@ struct ModelState Timer serialTimer; Target::Queue appQueue; - EscDriver * escMotor; - EscDriver * escServo; }; } diff --git a/lib/Espfc/src/Output/Mixer.cpp b/lib/Espfc/src/Output/Mixer.cpp index 96dfeedb..8515d8bc 100644 --- a/lib/Espfc/src/Output/Mixer.cpp +++ b/lib/Espfc/src/Output/Mixer.cpp @@ -19,7 +19,7 @@ int Mixer::begin() .dshotTelemetry = !!_model.config.output.dshotTelemetry, }; escMotor.begin(motorConf); - _model.state.escMotor = _motor = &escMotor; + _model.state.mixer.escMotor = _motor = &escMotor; _model.logger.info().log(F("MOTOR CONF")).log(_model.config.output.protocol).log(_model.config.output.async).log(_model.config.output.rate).log(_model.config.output.dshotTelemetry).logln(ESC_DRIVER_MOTOR_TIMER); if(_model.config.output.servoRate) @@ -32,11 +32,11 @@ int Mixer::begin() .dshotTelemetry = false, }; escServo.begin(servoConf); - _model.state.escServo = _servo = &escServo; + _model.state.mixer.escServo = _servo = &escServo; _model.logger.info().log(F("SERVO CONF")).log(ESC_PROTOCOL_PWM).log(true).logln(_model.config.output.servoRate).logln(ESC_DRIVER_SERVO_TIMER); } _erpmToHz = EscDriver::getErpmToHzRatio(_model.config.output.motorPoles); - _statsCounterMax = _model.state.mixerTimer.rate / 2; + _statsCounterMax = _model.state.mixer.timer.rate / 2; _statsCounter = 0; for(size_t i = 0; i < OUTPUT_CHANNELS; ++i) @@ -67,13 +67,13 @@ int Mixer::begin() } 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.mixer.minThrottle = _model.config.output.minThrottle; + _model.state.mixer.maxThrottle = _model.config.output.maxThrottle; + _model.state.mixer.digitalOutput = _model.config.output.protocol >= ESC_PROTOCOL_DSHOT150; + if(_model.state.mixer.digitalOutput) { - _model.state.minThrottle = (_model.config.output.dshotIdle * 0.1f) + 1001.f; - _model.state.maxThrottle = 2000.f; + _model.state.mixer.minThrottle = (_model.config.output.dshotIdle * 0.1f) + 1001.f; + _model.state.mixer.maxThrottle = 2000.f; } _model.state.currentMixer = Mixers::getMixer((MixerType)_model.config.mixer.type, _model.state.customMixer); return 1; @@ -256,7 +256,7 @@ void FAST_CODE_ATTR Mixer::writeOutput(const MixerConfig& mixer, float * out) else { float v = Math::clamp(out[i], -1.f, 1.f); - _model.state.output.us[i] = lrintf(Math::map(v, -1.f, 1.f, _model.state.minThrottle, _model.state.maxThrottle)); + _model.state.output.us[i] = lrintf(Math::map(v, -1.f, 1.f, _model.state.mixer.minThrottle, _model.state.mixer.maxThrottle)); } } } diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index 2946673b..953a2042 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -116,7 +116,7 @@ void test_model_gyro_init_1k_256dlpf() TEST_ASSERT_EQUAL_INT32(2000, model.state.gyroTimer.rate); TEST_ASSERT_EQUAL_INT32(2000, model.state.loopRate); TEST_ASSERT_EQUAL_INT32(2000, model.state.loopTimer.rate); - TEST_ASSERT_EQUAL_INT32(2000, model.state.mixerTimer.rate); + TEST_ASSERT_EQUAL_INT32(2000, model.state.mixer.timer.rate); } void test_model_gyro_init_1k_188dlpf() @@ -133,7 +133,7 @@ void test_model_gyro_init_1k_188dlpf() TEST_ASSERT_EQUAL_INT32(1000, model.state.gyroTimer.rate); TEST_ASSERT_EQUAL_INT32( 500, model.state.loopRate); TEST_ASSERT_EQUAL_INT32( 500, model.state.loopTimer.rate); - TEST_ASSERT_EQUAL_INT32( 250, model.state.mixerTimer.rate); + TEST_ASSERT_EQUAL_INT32( 250, model.state.mixer.timer.rate); } void test_model_inner_pid_init() From ef9416b6ef5f6a23423e4886761019223f01ece9 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 28 Oct 2024 22:54:52 +0100 Subject: [PATCH 29/68] mag state refactor --- lib/Espfc/src/Blackbox/Blackbox.cpp | 2 +- lib/Espfc/src/Cli.h | 20 +++---- lib/Espfc/src/Device/MagAK8963.h | 2 +- lib/Espfc/src/Fusion.cpp | 10 ++-- lib/Espfc/src/Hardware.h | 6 +-- lib/Espfc/src/Model.h | 24 ++++----- lib/Espfc/src/ModelConfig.h | 4 +- lib/Espfc/src/ModelState.h | 43 ++++++++------- lib/Espfc/src/Msp/MspProcessor.h | 2 +- lib/Espfc/src/Sensor/MagSensor.h | 82 ++++++++++++++++------------- 10 files changed, 104 insertions(+), 91 deletions(-) diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index 5865599f..eb6d03e1 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -248,7 +248,7 @@ void FAST_CODE_ATTR Blackbox::updateData() 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; + mag.magADC[i] = _model.state.mag.adc[i] * 1090; } if(_model.baroActive()) { baro.altitude = lrintf(_model.state.baroAltitude * 100.f); // cm diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 60163af7..ba8bbcf5 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -1058,17 +1058,17 @@ class Cli s.print(_model.config.mag.offset[0]); s.print(' '); s.print(_model.config.mag.offset[1]); s.print(' '); s.print(_model.config.mag.offset[2]); s.print(F(" [")); - s.print(_model.state.magCalibrationOffset[0]); s.print(' '); - s.print(_model.state.magCalibrationOffset[1]); s.print(' '); - s.print(_model.state.magCalibrationOffset[2]); s.println(F("]")); + s.print(_model.state.mag.calibrationOffset[0]); s.print(' '); + s.print(_model.state.mag.calibrationOffset[1]); s.print(' '); + s.print(_model.state.mag.calibrationOffset[2]); s.println(F("]")); s.print(F(" mag scale: ")); s.print(_model.config.mag.scale[0]); s.print(' '); s.print(_model.config.mag.scale[1]); s.print(' '); s.print(_model.config.mag.scale[2]); s.print(F(" [")); - s.print(_model.state.magCalibrationScale[0]); s.print(' '); - s.print(_model.state.magCalibrationScale[1]); s.print(' '); - s.print(_model.state.magCalibrationScale[2]); s.println(F("]")); + s.print(_model.state.mag.calibrationScale[0]); s.print(' '); + s.print(_model.state.mag.calibrationScale[1]); s.print(' '); + s.print(_model.state.mag.calibrationScale[2]); s.println(F("]")); } else if(strcmp_P(cmd.args[1], PSTR("gyro")) == 0) { @@ -1092,8 +1092,8 @@ class Cli } else if(strcmp_P(cmd.args[1], PSTR("reset_mag")) == 0 || strcmp_P(cmd.args[1], PSTR("reset_all")) == 0) { - _model.state.magCalibrationOffset = VectorFloat(); - _model.state.magCalibrationScale = VectorFloat(1.f, 1.f, 1.f); + _model.state.mag.calibrationOffset = VectorFloat(); + _model.state.mag.calibrationScale = VectorFloat(1.f, 1.f, 1.f); s.println(F("OK")); } } @@ -1241,7 +1241,7 @@ class Cli Device::GyroDevice * gyro = _model.state.gyroDev; Device::BaroDevice * baro = _model.state.baroDev; - Device::MagDevice * mag = _model.state.magDev; + Device::MagDevice * mag = _model.state.mag.dev; s.print(F(" devices: ")); if(gyro) { @@ -1543,7 +1543,7 @@ class Cli s.println(F(" Hz")); s.print(F(" mag rate: ")); - s.print(_model.state.magTimer.rate); + s.print(_model.state.mag.timer.rate); s.println(F(" Hz")); } diff --git a/lib/Espfc/src/Device/MagAK8963.h b/lib/Espfc/src/Device/MagAK8963.h index da1515b6..ae8d17de 100644 --- a/lib/Espfc/src/Device/MagAK8963.h +++ b/lib/Espfc/src/Device/MagAK8963.h @@ -81,7 +81,7 @@ class MagAK8963: public MagDevice const VectorFloat convert(const VectorInt16& v) const override { - return (VectorFloat)v * scale; + return static_cast(v) * scale; } int getRate() const override diff --git a/lib/Espfc/src/Fusion.cpp b/lib/Espfc/src/Fusion.cpp index 8fe3b804..96da4795 100644 --- a/lib/Espfc/src/Fusion.cpp +++ b/lib/Espfc/src/Fusion.cpp @@ -218,8 +218,8 @@ void Fusion::updatePoseFromAccelMag() q.y = cosX2 * sinY2; q.z = -sinX2 * sinY2; - VectorFloat m = _model.state.mag.getRotated(q); - _model.state.magPose = m; + VectorFloat m = _model.state.mag.adc.getRotated(q); + _model.state.mag.pose = m; _model.state.pose.z = -atan2(m.y, m.x); } else @@ -265,7 +265,7 @@ void Fusion::lerpFusion() if(_model.magActive()) { // use yaw from mag - VectorFloat mag = _model.state.mag.getRotated(_model.state.accelPoseQ); + VectorFloat mag = _model.state.mag.adc.getRotated(_model.state.accelPoseQ); _model.state.accelPose.z = -atan2(mag.y, mag.x); } else @@ -294,7 +294,7 @@ void Fusion::madgwickFusion() _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 + _model.state.mag.adc.x, _model.state.mag.adc.y, _model.state.mag.adc.z ); } else @@ -315,7 +315,7 @@ void FAST_CODE_ATTR Fusion::mahonyFusion() _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 + _model.state.mag.adc.x, _model.state.mag.adc.y, _model.state.mag.adc.z ); } else diff --git a/lib/Espfc/src/Hardware.h b/lib/Espfc/src/Hardware.h index 9aae4cdd..954ae207 100644 --- a/lib/Espfc/src/Hardware.h +++ b/lib/Espfc/src/Hardware.h @@ -153,9 +153,9 @@ class Hardware if(!detectedMag && detectDevice(qmc5883l, gyroSlaveBus)) detectedMag = &qmc5883l; } - _model.state.magDev = detectedMag; - _model.state.magPresent = (bool)detectedMag; - _model.state.magRate = detectedMag ? detectedMag->getRate() : 0; + _model.state.mag.dev = detectedMag; + _model.state.mag.present = (bool)detectedMag; + _model.state.mag.rate = detectedMag ? detectedMag->getRate() : 0; } void detectBaro() diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 5e766617..897d700a 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -119,7 +119,7 @@ class Model bool magActive() const { - return state.magPresent && config.mag.dev != MAG_NONE; + return state.mag.present && config.mag.dev != MAG_NONE; } bool baroActive() const @@ -129,7 +129,7 @@ class Model bool calibrationActive() const { - return state.accelCalibrationState != CALIBRATION_IDLE || state.gyroCalibrationState != CALIBRATION_IDLE || state.magCalibrationState != CALIBRATION_IDLE; + return state.accelCalibrationState != CALIBRATION_IDLE || state.gyroCalibrationState != CALIBRATION_IDLE || state.mag.calibrationState != CALIBRATION_IDLE; } void calibrateGyro() @@ -143,7 +143,7 @@ class Model void calibrateMag() { - state.magCalibrationState = CALIBRATION_START; + state.mag.calibrationState = CALIBRATION_START; } void finishCalibration() @@ -159,11 +159,11 @@ class Model save(); logger.info().log(F("ACCEL BIAS")).log(state.accelBias.x).log(state.accelBias.y).logln(state.accelBias.z); } - if(state.magCalibrationState == CALIBRATION_SAVE) + if(state.mag.calibrationState == CALIBRATION_SAVE) { save(); - logger.info().log(F("MAG BIAS")).log(state.magCalibrationOffset.x).log(state.magCalibrationOffset.y).logln(state.magCalibrationOffset.z); - logger.info().log(F("MAG SCALE")).log(state.magCalibrationScale.x).log(state.magCalibrationScale.y).logln(state.magCalibrationScale.z); + logger.info().log(F("MAG BIAS")).log(state.mag.calibrationOffset.x).log(state.mag.calibrationOffset.y).logln(state.mag.calibrationOffset.z); + logger.info().log(F("MAG SCALE")).log(state.mag.calibrationScale.x).log(state.mag.calibrationScale.y).logln(state.mag.calibrationScale.z); } } @@ -441,7 +441,7 @@ class Model state.stats.timer.setRate(3); if(magActive()) { - state.magTimer.setRate(state.magRate); + state.mag.timer.setRate(state.mag.rate); } state.boardAlignment.init(VectorFloat(Math::toRad(config.boardAlignment[0]), Math::toRad(config.boardAlignment[1]), Math::toRad(config.boardAlignment[2]))); @@ -486,7 +486,7 @@ class Model } if(magActive()) { - state.magFilter[i].begin(config.mag.filter, state.magTimer.rate); + state.mag.filter[i].begin(config.mag.filter, state.mag.timer.rate); } } @@ -576,8 +576,8 @@ class Model { state.gyroBias.set(i, config.gyro.bias[i] / 1000.0f); state.accelBias.set(i, config.accel.bias[i] / 1000.0f); - state.magCalibrationOffset.set(i, config.mag.offset[i] / 10.0f); - state.magCalibrationScale.set(i, config.mag.scale[i] / 1000.0f); + state.mag.calibrationOffset.set(i, config.mag.offset[i] / 10.0f); + state.mag.calibrationScale.set(i, config.mag.scale[i] / 1000.0f); } } @@ -588,8 +588,8 @@ class Model { config.gyro.bias[i] = lrintf(state.gyroBias[i] * 1000.0f); config.accel.bias[i] = lrintf(state.accelBias[i] * 1000.0f); - config.mag.offset[i] = lrintf(state.magCalibrationOffset[i] * 10.0f); - config.mag.scale[i] = lrintf(state.magCalibrationScale[i] * 1000.0f); + config.mag.offset[i] = lrintf(state.mag.calibrationOffset[i] * 10.0f); + config.mag.scale[i] = lrintf(state.mag.calibrationScale[i] * 1000.0f); } } diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 27c440d5..0d15099b 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -604,8 +604,8 @@ struct MagConfig int8_t bus = BUS_AUTO; int8_t dev = MAG_NONE; int8_t align = ALIGN_DEFAULT; - int16_t scale[3] = { 0, 0, 0 }; - int16_t offset[3] = { 1000, 1000, 1000 }; + int16_t offset[3] = { 0, 0, 0 }; + int16_t scale[3] = { 1000, 1000, 1000 }; FilterConfig filter{FILTER_BIQUAD, 10}; }; diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 6831b564..44ceef24 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -208,11 +208,34 @@ struct MixerState EscDriver * escServo; }; +struct MagState +{ + Device::MagDevice* dev; + bool present; + int rate; + + VectorInt16 raw; + VectorFloat adc; + Filter filter[3]; + Timer timer; + + int calibrationSamples; + int calibrationState; + bool calibrationValid; + VectorFloat calibrationMin; + VectorFloat calibrationMax; + VectorFloat calibrationScale; + VectorFloat calibrationOffset; + + VectorFloat pose; +}; + // working data struct ModelState { + MagState mag; + Device::GyroDevice* gyroDev; - Device::MagDevice* magDev; Device::BaroDevice* baroDev; VectorInt16 gyroRaw; @@ -222,18 +245,15 @@ struct ModelState VectorFloat gyroImu; VectorInt16 accelRaw; - VectorInt16 magRaw; VectorFloat gyro; VectorFloat accel; - VectorFloat mag; VectorFloat gyroPose; Quaternion gyroPoseQ; VectorFloat accelPose; VectorFloat accelPose2; Quaternion accelPoseQ; - VectorFloat magPose; bool imuUpdate; bool loopUpdate; @@ -254,7 +274,6 @@ struct ModelState Filter gyroImuFilter[3]; Filter accelFilter[3]; - Filter magFilter[3]; Filter rpmFreqFilter[RPM_FILTER_MOTOR_MAX]; Filter rpmFilter[RPM_FILTER_MOTOR_MAX][RPM_FILTER_HARMONICS_MAX][3]; @@ -304,19 +323,6 @@ struct ModelState Timer loopTimer; Timer actuatorTimer; - - Timer magTimer; - int magRate; - - int magCalibrationSamples; - int magCalibrationState; - bool magCalibrationValid; - - VectorFloat magCalibrationMin; - VectorFloat magCalibrationMax; - VectorFloat magCalibrationScale; - VectorFloat magCalibrationOffset; - Timer telemetryTimer; Stats stats; @@ -343,7 +349,6 @@ struct ModelState bool gyroPresent; bool accelPresent; - bool magPresent; bool baroPresent; float baroTemperatureRaw; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 880846d2..4abdf302 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -1287,7 +1287,7 @@ class MspProcessor } for (int i = 0; i < 3; i++) { - r.writeU16(lrintf(_model.state.mag[i] * 1090)); + r.writeU16(lrintf(_model.state.mag.adc[i] * 1090)); } break; diff --git a/lib/Espfc/src/Sensor/MagSensor.h b/lib/Espfc/src/Sensor/MagSensor.h index f228c69c..ab1208f5 100644 --- a/lib/Espfc/src/Sensor/MagSensor.h +++ b/lib/Espfc/src/Sensor/MagSensor.h @@ -19,16 +19,16 @@ class MagSensor: public BaseSensor { if(!_model.magActive()) return 0; - _mag = _model.state.magDev; + _mag = _model.state.mag.dev; if(!_mag) return 0; - if(_model.state.magTimer.rate < 5) return 0; + if(_model.state.mag.timer.rate < 5) return 0; // by default use eeprom calibration settings - _model.state.magCalibrationState = CALIBRATION_IDLE; - _model.state.magCalibrationValid = true; + _model.state.mag.calibrationState = CALIBRATION_IDLE; + _model.state.mag.calibrationValid = true; - _model.logger.info().log(F("MAG INIT")).log(FPSTR(Device::MagDevice::getName(_mag->getType()))).log(_mag->getAddress()).logln(_model.state.magTimer.rate); + _model.logger.info().log(F("MAG INIT")).log(FPSTR(Device::MagDevice::getName(_mag->getType()))).log(_mag->getAddress()).logln(_model.state.mag.timer.rate); return 1; } @@ -44,10 +44,10 @@ class MagSensor: public BaseSensor int read() { - if(!_mag || !_model.magActive() || !_model.state.magTimer.check()) return 0; + if(!_mag || !_model.magActive() || !_model.state.mag.timer.check()) return 0; Stats::Measure measure(_model.state.stats, COUNTER_MAG_READ); - _mag->readMag(_model.state.magRaw); + _mag->readMag(_model.state.mag.raw); return 1; } @@ -58,70 +58,78 @@ class MagSensor: public BaseSensor Stats::Measure measure(_model.state.stats, COUNTER_MAG_FILTER); - _model.state.mag = _mag->convert(_model.state.magRaw); + _model.state.mag.adc = _mag->convert(_model.state.mag.raw); - align(_model.state.mag, _model.config.mag.align); - _model.state.mag = _model.state.boardAlignment.apply(_model.state.mag); - for(size_t i = 0; i < 3; i++) + align(_model.state.mag.adc, _model.config.mag.align); + _model.state.mag.adc = _model.state.boardAlignment.apply(_model.state.mag.adc); + + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { - _model.state.mag.set(i, _model.state.magFilter[i].update(_model.state.mag[i])); + _model.state.mag.adc.set(i, _model.state.mag.filter[i].update(_model.state.mag.adc[i])); } calibrate(); + /*Serial.print(_model.state.mag.adc.x); + Serial.print(' '); + Serial.print(_model.state.mag.adc.y); + Serial.print(' '); + Serial.print(_model.state.mag.adc.z); + Serial.print('\n');*/ + return 1; } private: void calibrate() { - switch(_model.state.magCalibrationState) + switch(_model.state.mag.calibrationState) { case CALIBRATION_IDLE: - if(_model.state.magCalibrationValid) + if(_model.state.mag.calibrationValid) { - _model.state.mag -= _model.state.magCalibrationOffset; - _model.state.mag *= _model.state.magCalibrationScale; + _model.state.mag.adc -= _model.state.mag.calibrationOffset; + _model.state.mag.adc *= _model.state.mag.calibrationScale; } break; case CALIBRATION_START: resetCalibration(); - _model.state.magCalibrationSamples = 30 * _model.state.magTimer.rate; - _model.state.magCalibrationState = CALIBRATION_UPDATE; + _model.state.mag.calibrationSamples = 30 * _model.state.mag.timer.rate; + _model.state.mag.calibrationState = CALIBRATION_UPDATE; break; case CALIBRATION_UPDATE: updateCalibration(); - _model.state.magCalibrationSamples--; - if(_model.state.magCalibrationSamples <= 0) _model.state.magCalibrationState = CALIBRATION_APPLY; + _model.state.mag.calibrationSamples--; + if(_model.state.mag.calibrationSamples <= 0) _model.state.mag.calibrationState = CALIBRATION_APPLY; break; case CALIBRATION_APPLY: applyCalibration(); - _model.state.magCalibrationState = CALIBRATION_SAVE; + _model.state.mag.calibrationState = CALIBRATION_SAVE; break; case CALIBRATION_SAVE: _model.finishCalibration(); - _model.state.magCalibrationState = CALIBRATION_IDLE; + _model.state.mag.calibrationState = CALIBRATION_IDLE; break; default: - _model.state.magCalibrationState = CALIBRATION_IDLE; + _model.state.mag.calibrationState = CALIBRATION_IDLE; break; } } void resetCalibration() { - _model.state.magCalibrationMin = VectorFloat(); - _model.state.magCalibrationMax = VectorFloat(); - _model.state.magCalibrationValid = false; + _model.state.mag.calibrationMin = VectorFloat(); + _model.state.mag.calibrationMax = VectorFloat(); + _model.state.mag.calibrationValid = false; } void updateCalibration() { for(int i = 0; i < 3; i++) { - if(_model.state.mag[i] < _model.state.magCalibrationMin[i]) _model.state.magCalibrationMin.set(i, _model.state.mag[i]); - if(_model.state.mag[i] > _model.state.magCalibrationMax[i]) _model.state.magCalibrationMax.set(i, _model.state.mag[i]); + if(_model.state.mag.adc[i] < _model.state.mag.calibrationMin[i]) _model.state.mag.calibrationMin.set(i, _model.state.mag.adc[i]); + if(_model.state.mag.adc[i] > _model.state.mag.calibrationMax[i]) _model.state.mag.calibrationMax.set(i, _model.state.mag.adc[i]); } } @@ -133,11 +141,11 @@ class MagSensor: public BaseSensor float maxRange = -1; for(int i = 0; i < 3; i++) { - if(_model.state.magCalibrationMin[i] > -EPSILON) return; - if(_model.state.magCalibrationMax[i] < EPSILON) return; - if((_model.state.magCalibrationMax[i] - _model.state.magCalibrationMin[i]) > maxRange) + if(_model.state.mag.calibrationMin[i] > -EPSILON) return; + if(_model.state.mag.calibrationMax[i] < EPSILON) return; + if((_model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]) > maxRange) { - maxRange = _model.state.magCalibrationMax[i] - _model.state.magCalibrationMin[i]; + maxRange = _model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]; } } @@ -149,8 +157,8 @@ class MagSensor: public BaseSensor for (int i = 0; i < 3; i++) { - const float range = (_model.state.magCalibrationMax[i] - _model.state.magCalibrationMin[i]); - const float bias = (_model.state.magCalibrationMax[i] + _model.state.magCalibrationMin[i]) * 0.5f; + const float range = (_model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]); + const float bias = (_model.state.mag.calibrationMax[i] + _model.state.mag.calibrationMin[i]) * 0.5f; if(abs(range) < EPSILON) return; // incomplete data @@ -158,9 +166,9 @@ class MagSensor: public BaseSensor offset.set(i, bias); // level bias } - _model.state.magCalibrationScale = scale; - _model.state.magCalibrationOffset = offset; - _model.state.magCalibrationValid = true; + _model.state.mag.calibrationScale = scale; + _model.state.mag.calibrationOffset = offset; + _model.state.mag.calibrationValid = true; } Model& _model; From cefb1db1bc4cb86d558e2d662c41af42493ac46b Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 28 Oct 2024 22:55:36 +0100 Subject: [PATCH 30/68] CI python 3.12 --- .github/workflows/platformio.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/platformio.yml b/.github/workflows/platformio.yml index 249e104b..3bbc1d6d 100644 --- a/.github/workflows/platformio.yml +++ b/.github/workflows/platformio.yml @@ -22,7 +22,7 @@ jobs: - name: Set up Python uses: actions/setup-python@v5 with: - python-version: '3.11' + python-version: '3.12' - name: Install Dependencies run: pip install --upgrade platformio - name: Run PlatformIO Test @@ -64,7 +64,7 @@ jobs: - name: Set up Python uses: actions/setup-python@v5 with: - python-version: '3.11' + python-version: '3.12' - name: Install Dependencies run: pip install --upgrade platformio From 7b183ad7c4da6a5584c410581c2e11a118974de7 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 28 Oct 2024 23:12:15 +0100 Subject: [PATCH 31/68] baro state refactor --- lib/Espfc/src/Blackbox/Blackbox.cpp | 2 +- lib/Espfc/src/Cli.h | 4 +-- lib/Espfc/src/Hardware.h | 4 +-- lib/Espfc/src/Model.h | 2 +- lib/Espfc/src/ModelState.h | 33 ++++++++++++----------- lib/Espfc/src/Msp/MspProcessor.h | 2 +- lib/Espfc/src/Sensor/BaroSensor.h | 41 +++++++++++++++-------------- lib/Espfc/src/Sensor/MagSensor.h | 7 ----- 8 files changed, 46 insertions(+), 49 deletions(-) diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index eb6d03e1..a611cfb6 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -251,7 +251,7 @@ void FAST_CODE_ATTR Blackbox::updateData() mag.magADC[i] = _model.state.mag.adc[i] * 1090; } if(_model.baroActive()) { - baro.altitude = lrintf(_model.state.baroAltitude * 100.f); // cm + baro.altitude = lrintf(_model.state.baro.altitude * 100.f); // cm } } rcCommand[AXIS_THRUST] = _model.state.input.buffer[AXIS_THRUST]; diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index ba8bbcf5..6a79e0cd 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -1240,7 +1240,7 @@ class Cli s.println(); Device::GyroDevice * gyro = _model.state.gyroDev; - Device::BaroDevice * baro = _model.state.baroDev; + Device::BaroDevice * baro = _model.state.baro.dev; Device::MagDevice * mag = _model.state.mag.dev; s.print(F(" devices: ")); if(gyro) @@ -1539,7 +1539,7 @@ class Cli s.println(F(" Hz")); s.print(F(" baro rate: ")); - s.print(_model.state.baroRate); + s.print(_model.state.baro.rate); s.println(F(" Hz")); s.print(F(" mag rate: ")); diff --git a/lib/Espfc/src/Hardware.h b/lib/Espfc/src/Hardware.h index 954ae207..a214a473 100644 --- a/lib/Espfc/src/Hardware.h +++ b/lib/Espfc/src/Hardware.h @@ -188,8 +188,8 @@ class Hardware if(!detectedBaro && detectDevice(spl06, gyroSlaveBus)) detectedBaro = &spl06; } - _model.state.baroDev = detectedBaro; - _model.state.baroPresent = (bool)detectedBaro; + _model.state.baro.dev = detectedBaro; + _model.state.baro.present = (bool)detectedBaro; } #if defined(ESPFC_SPI_0) diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 897d700a..488368ff 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -124,7 +124,7 @@ class Model bool baroActive() const { - return state.baroPresent && config.baro.dev != BARO_NONE; + return state.baro.present && config.baro.dev != BARO_NONE; } bool calibrationActive() const diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 44ceef24..813757a4 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -230,23 +230,37 @@ struct MagState VectorFloat pose; }; +struct BaroState +{ + Device::BaroDevice* dev; + bool present; + int32_t rate; + + float temperatureRaw; + float temperature; + float pressureRaw; + float pressure; + float altitudeRaw; + float altitude; + float altitudeBias; + int32_t altitudeBiasSamples; +}; + // working data struct ModelState { MagState mag; + BaroState baro; Device::GyroDevice* gyroDev; - Device::BaroDevice* baroDev; - VectorInt16 gyroRaw; + VectorFloat gyro; VectorFloat gyroSampled; VectorFloat gyroScaled; VectorFloat gyroDynNotch; VectorFloat gyroImu; VectorInt16 accelRaw; - - VectorFloat gyro; VectorFloat accel; VectorFloat gyroPose; @@ -349,17 +363,6 @@ struct ModelState bool gyroPresent; bool accelPresent; - bool baroPresent; - - float baroTemperatureRaw; - float baroTemperature; - float baroPressureRaw; - float baroPressure; - float baroAltitudeRaw; - float baroAltitude; - float baroAltitudeBias; - int32_t baroAltitudeBiasSamples; - int32_t baroRate; uint32_t armingDisabledFlags; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 4abdf302..266c2f01 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -685,7 +685,7 @@ class MspProcessor break; case MSP_ALTITUDE: - r.writeU32(lrintf(_model.state.baroAltitude * 100.f)); // alt [cm] + r.writeU32(lrintf(_model.state.baro.altitude * 100.f)); // alt [cm] r.writeU16(0); // vario break; diff --git a/lib/Espfc/src/Sensor/BaroSensor.h b/lib/Espfc/src/Sensor/BaroSensor.h index 351b4873..468ada71 100644 --- a/lib/Espfc/src/Sensor/BaroSensor.h +++ b/lib/Espfc/src/Sensor/BaroSensor.h @@ -23,24 +23,25 @@ class BaroSensor: public BaseSensor int begin() { - _model.state.baroRate = 0; + _model.state.baro.rate = 0; if(!_model.baroActive()) return 0; - _baro = _model.state.baroDev; + _baro = _model.state.baro.dev; if(!_baro) return 0; _baro->setMode(BARO_MODE_TEMP); int delay = _baro->getDelay(BARO_MODE_TEMP) + _baro->getDelay(BARO_MODE_PRESS); int toGyroRate = (delay / _model.state.gyroTimer.interval) + 1; // number of gyro readings per cycle int interval = _model.state.gyroTimer.interval * toGyroRate; - _model.state.baroRate = 1000000 / interval; + _model.state.baro.rate = 1000000 / interval; - _model.state.baroAltitudeBiasSamples = 2 * _model.state.baroRate; + _model.state.baro.altitudeBiasSamples = 2 * _model.state.baro.rate; - _temperatureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baroRate * 0.1f), _model.state.baroRate); - _pressureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baroRate * 0.1f), _model.state.baroRate); - _altitudeFilter.begin(_model.config.baro.filter, _model.state.baroRate); + // TODO: move filters to BaroState + _temperatureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baro.rate * 0.1f), _model.state.baro.rate); + _pressureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baro.rate * 0.1f), _model.state.baro.rate); + _altitudeFilter.begin(_model.config.baro.filter, _model.state.baro.rate); - _model.logger.info().log(F("BARO INIT")).log(FPSTR(Device::BaroDevice::getName(_baro->getType()))).log(_baro->getAddress()).log(toGyroRate).log(_model.state.baroRate).logln(_model.config.baro.filter.freq); + _model.logger.info().log(F("BARO INIT")).log(FPSTR(Device::BaroDevice::getName(_baro->getType()))).log(_baro->getAddress()).log(toGyroRate).log(_model.state.baro.rate).logln(_model.config.baro.filter.freq); return 1; } @@ -107,31 +108,31 @@ class BaroSensor: public BaseSensor private: void readTemperature() { - _model.state.baroTemperatureRaw = _baro->readTemperature(); - _model.state.baroTemperature = _temperatureFilter.update(_model.state.baroTemperatureRaw); + _model.state.baro.temperatureRaw = _baro->readTemperature(); + _model.state.baro.temperature = _temperatureFilter.update(_model.state.baro.temperatureRaw); } void readPressure() { - _model.state.baroPressureRaw = _baro->readPressure(); - _model.state.baroPressure = _pressureFilter.update(_model.state.baroPressureRaw); + _model.state.baro.pressureRaw = _baro->readPressure(); + _model.state.baro.pressure = _pressureFilter.update(_model.state.baro.pressureRaw); } void updateAltitude() { - _model.state.baroAltitudeRaw = _altitudeFilter.update(Math::toAltitude(_model.state.baroPressure)); - if(_model.state.baroAltitudeBiasSamples > 0) + _model.state.baro.altitudeRaw = _altitudeFilter.update(Math::toAltitude(_model.state.baro.pressure)); + if(_model.state.baro.altitudeBiasSamples > 0) { - _model.state.baroAltitudeBiasSamples--; - _model.state.baroAltitudeBias += (_model.state.baroAltitudeRaw - _model.state.baroAltitudeBias) * 0.2f; + _model.state.baro.altitudeBiasSamples--; + _model.state.baro.altitudeBias += (_model.state.baro.altitudeRaw - _model.state.baro.altitudeBias) * 0.2f; } - _model.state.baroAltitude = _model.state.baroAltitudeRaw - _model.state.baroAltitudeBias; + _model.state.baro.altitude = _model.state.baro.altitudeRaw - _model.state.baro.altitudeBias; if(_model.config.debug.mode == DEBUG_BARO) { - _model.state.debug[1] = lrintf(_model.state.baroPressureRaw * 0.1f); // hPa x 10 - _model.state.debug[2] = lrintf(_model.state.baroTemperatureRaw * 100.f); // deg C x 100 - _model.state.debug[3] = lrintf(_model.state.baroAltitudeRaw * 10.f); + _model.state.debug[1] = lrintf(_model.state.baro.pressureRaw * 0.1f); // hPa x 10 + _model.state.debug[2] = lrintf(_model.state.baro.temperatureRaw * 100.f); // deg C x 100 + _model.state.debug[3] = lrintf(_model.state.baro.altitudeRaw * 10.f); } } diff --git a/lib/Espfc/src/Sensor/MagSensor.h b/lib/Espfc/src/Sensor/MagSensor.h index ab1208f5..1f3c90e5 100644 --- a/lib/Espfc/src/Sensor/MagSensor.h +++ b/lib/Espfc/src/Sensor/MagSensor.h @@ -71,13 +71,6 @@ class MagSensor: public BaseSensor calibrate(); - /*Serial.print(_model.state.mag.adc.x); - Serial.print(' '); - Serial.print(_model.state.mag.adc.y); - Serial.print(' '); - Serial.print(_model.state.mag.adc.z); - Serial.print('\n');*/ - return 1; } From eefcb1585b07988cfdca579db0d51b81bc9defa9 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Tue, 29 Oct 2024 21:22:59 +0100 Subject: [PATCH 32/68] gyro accel state refactor --- lib/Espfc/src/Actuator.h | 4 +- lib/Espfc/src/Blackbox/Blackbox.cpp | 8 +- lib/Espfc/src/Cli.h | 24 +++--- lib/Espfc/src/Controller.cpp | 12 +-- lib/Espfc/src/Espfc.cpp | 10 +-- lib/Espfc/src/Espfc.h | 2 +- lib/Espfc/src/Fusion.cpp | 52 +++++------ lib/Espfc/src/Hardware.h | 8 +- lib/Espfc/src/Model.h | 76 ++++++++-------- lib/Espfc/src/ModelState.h | 110 +++++++++++++----------- lib/Espfc/src/Msp/MspProcessor.h | 18 ++-- lib/Espfc/src/Output/Mixer.cpp | 2 +- lib/Espfc/src/Sensor/AccelSensor.h | 54 ++++++------ lib/Espfc/src/Sensor/BaroSensor.h | 4 +- lib/Espfc/src/Sensor/GyroSensor.cpp | 100 ++++++++++----------- lib/Espfc/src/SensorManager.cpp | 10 +-- lib/Espfc/src/Stats.h | 3 +- lib/Espfc/src/Telemetry/TelemetryText.h | 2 +- test/test_fc/test_fc.cpp | 30 +++---- 19 files changed, 267 insertions(+), 262 deletions(-) diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h index 3164de51..c46be816 100644 --- a/lib/Espfc/src/Actuator.h +++ b/lib/Espfc/src/Actuator.h @@ -98,7 +98,7 @@ class Actuator int errors = _model.state.i2cErrorDelta; _model.state.i2cErrorDelta = 0; - _model.setArmingDisabled(ARMING_DISABLED_NO_GYRO, !_model.state.gyroPresent || errors); + _model.setArmingDisabled(ARMING_DISABLED_NO_GYRO, !_model.state.gyro.present || errors); _model.setArmingDisabled(ARMING_DISABLED_FAILSAFE, _model.state.failsafe.phase != FC_FAILSAFE_IDLE); _model.setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE, _model.state.input.rxLoss || _model.state.input.rxFailSafe); _model.setArmingDisabled(ARMING_DISABLED_THROTTLE, !_model.isThrottleLow()); @@ -224,7 +224,7 @@ class Actuator if(_model.config.gyro.dynLpfFilter.cutoff > 0) { int gyroFreq = Math::map(scale, 1000, 2000, _model.config.gyro.dynLpfFilter.cutoff, _model.config.gyro.dynLpfFilter.freq); for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { - _model.state.gyroFilter[i].reconfigure(gyroFreq); + _model.state.gyro.filter[i].reconfigure(gyroFreq); } } if(_model.config.dterm.dynLpfFilter.cutoff > 0) { diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index a611cfb6..75f23150 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -152,7 +152,7 @@ int Blackbox::begin() if(_model.magActive()) sensorsSet(SENSOR_MAG); if(_model.baroActive()) sensorsSet(SENSOR_BARO); - gyro.sampleLooptime = _model.state.gyroTimer.interval; + gyro.sampleLooptime = _model.state.gyro.timer.interval; targetPidLooptime = _model.state.loopTimer.interval; activePidLoopDenom = _model.config.loopSync; @@ -237,15 +237,15 @@ 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]); + gyro.gyroADCf[i] = Math::toDeg(_model.state.gyro.adc[i]); + gyro.gyroADC[i] = Math::toDeg(_model.state.gyro.scaled[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.input.buffer[i] - 1500) * (i == AXIS_YAW ? -1 : 1); if(_model.accelActive()) { - acc.accADC[i] = _model.state.accel[i] * ACCEL_G_INV * acc.dev.acc_1G; + acc.accADC[i] = _model.state.accel.adc[i] * ACCEL_G_INV * acc.dev.acc_1G; } if(_model.magActive()) { mag.magADC[i] = _model.state.mag.adc[i] * 1090; diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 6a79e0cd..7e112c05 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -1042,17 +1042,17 @@ class Cli s.print(_model.config.gyro.bias[0]); s.print(' '); s.print(_model.config.gyro.bias[1]); s.print(' '); s.print(_model.config.gyro.bias[2]); s.print(F(" [")); - s.print(Math::toDeg(_model.state.gyroBias[0])); s.print(' '); - s.print(Math::toDeg(_model.state.gyroBias[1])); s.print(' '); - s.print(Math::toDeg(_model.state.gyroBias[2])); s.println(F("]")); + s.print(Math::toDeg(_model.state.gyro.bias[0])); s.print(' '); + s.print(Math::toDeg(_model.state.gyro.bias[1])); s.print(' '); + s.print(Math::toDeg(_model.state.gyro.bias[2])); s.println(F("]")); s.print(F("accel offset: ")); s.print(_model.config.accel.bias[0]); s.print(' '); s.print(_model.config.accel.bias[1]); s.print(' '); s.print(_model.config.accel.bias[2]); s.print(F(" [")); - s.print(_model.state.accelBias[0]); s.print(' '); - s.print(_model.state.accelBias[1]); s.print(' '); - s.print(_model.state.accelBias[2]); s.println(F("]")); + s.print(_model.state.accel.bias[0]); s.print(' '); + s.print(_model.state.accel.bias[1]); s.print(' '); + s.print(_model.state.accel.bias[2]); s.println(F("]")); s.print(F(" mag offset: ")); s.print(_model.config.mag.offset[0]); s.print(' '); @@ -1082,12 +1082,12 @@ class Cli } else if(strcmp_P(cmd.args[1], PSTR("reset_accel")) == 0 || strcmp_P(cmd.args[1], PSTR("reset_all")) == 0) { - _model.state.accelBias = VectorFloat(); + _model.state.accel.bias = VectorFloat(); s.println(F("OK")); } else if(strcmp_P(cmd.args[1], PSTR("reset_gyro")) == 0 || strcmp_P(cmd.args[1], PSTR("reset_all")) == 0) { - _model.state.gyroBias = VectorFloat(); + _model.state.gyro.bias = VectorFloat(); s.println(F("OK")); } else if(strcmp_P(cmd.args[1], PSTR("reset_mag")) == 0 || strcmp_P(cmd.args[1], PSTR("reset_all")) == 0) @@ -1239,7 +1239,7 @@ class Cli printStats(s); s.println(); - Device::GyroDevice * gyro = _model.state.gyroDev; + Device::GyroDevice * gyro = _model.state.gyro.dev; Device::BaroDevice * baro = _model.state.baro.dev; Device::MagDevice * mag = _model.state.mag.dev; s.print(F(" devices: ")); @@ -1519,11 +1519,11 @@ class Cli s.println(F(" MHz")); s.print(F(" gyro clock: ")); - s.print(_model.state.gyroClock); + s.print(_model.state.gyro.clock); s.println(F(" Hz")); s.print(F(" gyro rate: ")); - s.print(_model.state.gyroTimer.rate); + s.print(_model.state.gyro.timer.rate); s.println(F(" Hz")); s.print(F(" loop rate: ")); @@ -1535,7 +1535,7 @@ class Cli s.println(F(" Hz")); s.print(F(" accel rate: ")); - s.print(_model.state.accelTimer.rate); + s.print(_model.state.accel.timer.rate); s.println(F(" Hz")); s.print(F(" baro rate: ")); diff --git a/lib/Espfc/src/Controller.cpp b/lib/Espfc/src/Controller.cpp index 7f62d801..d0449ed8 100644 --- a/lib/Espfc/src/Controller.cpp +++ b/lib/Espfc/src/Controller.cpp @@ -58,7 +58,7 @@ void Controller::outerLoopRobot() { const float speedScale = 2.f; const float gyroScale = 0.1f; - const float speed = _speedFilter.update(_model.state.output.ch[AXIS_PITCH] * speedScale + _model.state.gyro[AXIS_PITCH] * gyroScale); + const float speed = _speedFilter.update(_model.state.output.ch[AXIS_PITCH] * speedScale + _model.state.gyro.adc[AXIS_PITCH] * gyroScale); float angle = 0; if(true || _model.isActive(MODE_ANGLE)) @@ -75,7 +75,7 @@ void Controller::outerLoopRobot() if(_model.config.debug.mode == DEBUG_ANGLERATE) { _model.state.debug[0] = speed * 1000; - _model.state.debug[1] = lrintf(degrees(angle) * 10); + _model.state.debug[1] = lrintf(Math::toDeg(angle) * 10); } } @@ -90,7 +90,7 @@ void Controller::innerLoopRobot() if(stabilize) { _model.state.output.ch[AXIS_PITCH] = _model.state.innerPid[AXIS_PITCH].update(_model.state.desiredAngle[AXIS_PITCH], _model.state.angle[AXIS_PITCH]); - _model.state.output.ch[AXIS_YAW] = _model.state.innerPid[AXIS_YAW].update(_model.state.desiredRate[AXIS_YAW], _model.state.gyro[AXIS_YAW]); + _model.state.output.ch[AXIS_YAW] = _model.state.innerPid[AXIS_YAW].update(_model.state.desiredRate[AXIS_YAW], _model.state.gyro.adc[AXIS_YAW]); } else { @@ -101,7 +101,7 @@ void Controller::innerLoopRobot() if(_model.config.debug.mode == DEBUG_ANGLERATE) { - _model.state.debug[2] = lrintf(degrees(_model.state.angle[AXIS_PITCH]) * 10); + _model.state.debug[2] = lrintf(Math::toDeg(_model.state.angle[AXIS_PITCH]) * 10); _model.state.debug[3] = lrintf(_model.state.output.ch[AXIS_PITCH] * 1000); } } @@ -133,7 +133,7 @@ void FAST_CODE_ATTR Controller::outerLoop() { for(size_t i = 0; i < 3; ++i) { - _model.state.debug[i] = lrintf(degrees(_model.state.desiredRate[i])); + _model.state.debug[i] = lrintf(Math::toDeg(_model.state.desiredRate[i])); } } } @@ -143,7 +143,7 @@ void FAST_CODE_ATTR Controller::innerLoop() const float tpaFactor = getTpaFactor(); for(size_t i = 0; i < AXIS_COUNT_RPY; ++i) { - _model.state.output.ch[i] = _model.state.innerPid[i].update(_model.state.desiredRate[i], _model.state.gyro[i]) * tpaFactor; + _model.state.output.ch[i] = _model.state.innerPid[i].update(_model.state.desiredRate[i], _model.state.gyro.adc[i]) * tpaFactor; //_model.state.debug[i] = lrintf(_model.state.innerPid[i].fTerm * 1000); } _model.state.output.ch[AXIS_THRUST] = _model.state.desiredRate[AXIS_THRUST]; diff --git a/lib/Espfc/src/Espfc.cpp b/lib/Espfc/src/Espfc.cpp index e3e9ffe8..1714b307 100644 --- a/lib/Espfc/src/Espfc.cpp +++ b/lib/Espfc/src/Espfc.cpp @@ -38,18 +38,18 @@ int FAST_CODE_ATTR Espfc::update(bool externalTrigger) { if(externalTrigger) { - _model.state.gyroTimer.update(); + _model.state.gyro.timer.update(); } else { - if(!_model.state.gyroTimer.check()) return 0; + if(!_model.state.gyro.timer.check()) return 0; } Stats::Measure measure(_model.state.stats, COUNTER_CPU_0); #if defined(ESPFC_MULTI_CORE) _sensor.read(); - if(_model.state.input.timer.syncTo(_model.state.gyroTimer, 1u)) + if(_model.state.input.timer.syncTo(_model.state.gyro.timer, 1u)) { _input.update(); } @@ -65,7 +65,7 @@ int FAST_CODE_ATTR Espfc::update(bool externalTrigger) #else _sensor.update(); - if(_model.state.loopTimer.syncTo(_model.state.gyroTimer)) + if(_model.state.loopTimer.syncTo(_model.state.gyro.timer)) { _controller.update(); if(_model.state.mixer.timer.syncTo(_model.state.loopTimer)) @@ -73,7 +73,7 @@ int FAST_CODE_ATTR Espfc::update(bool externalTrigger) _mixer.update(); } _blackbox.update(); - if(_model.state.input.timer.syncTo(_model.state.gyroTimer, 1u)) + if(_model.state.input.timer.syncTo(_model.state.gyro.timer, 1u)) { _input.update(); } diff --git a/lib/Espfc/src/Espfc.h b/lib/Espfc/src/Espfc.h index e25f1dec..43fa0e7d 100644 --- a/lib/Espfc/src/Espfc.h +++ b/lib/Espfc/src/Espfc.h @@ -26,7 +26,7 @@ class Espfc int getGyroInterval() const { - return _model.state.gyroTimer.interval; + return _model.state.gyro.timer.interval; } private: diff --git a/lib/Espfc/src/Fusion.cpp b/lib/Espfc/src/Fusion.cpp index 96da4795..9359d165 100644 --- a/lib/Espfc/src/Fusion.cpp +++ b/lib/Espfc/src/Fusion.cpp @@ -9,10 +9,10 @@ int Fusion::begin() { _model.state.gyroPoseQ = Quaternion(); - _madgwick.begin(_model.state.accelTimer.rate); + _madgwick.begin(_model.state.accel.timer.rate); _madgwick.setKp(_model.config.fusion.gain * 0.05f); - _mahony.begin(_model.state.accelTimer.rate); + _mahony.begin(_model.state.accel.timer.rate); _mahony.setKp(_model.config.fusion.gain * 0.1f); _mahony.setKi(_model.config.fusion.gainI * 0.1f); @@ -72,9 +72,9 @@ int FAST_CODE_ATTR Fusion::update() if(_model.config.debug.mode == 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); + _model.state.debug[0] = lrintf(Math::toDeg(_model.state.angle[0]) * 10); + _model.state.debug[1] = lrintf(Math::toDeg(_model.state.angle[1]) * 10); + _model.state.debug[2] = lrintf(Math::toDeg(_model.state.angle[2]) * 10); } return 1; } @@ -82,29 +82,29 @@ int FAST_CODE_ATTR Fusion::update() void Fusion::experimentalFusion() { // Experiment: workaround for 90 deg limit on pitch[y] axis - Quaternion r = Quaternion::lerp(Quaternion(), _model.state.accel.accelToQuaternion(), 0.5); + Quaternion r = Quaternion::lerp(Quaternion(), _model.state.accel.adc.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.pose = _model.state.accel.adc.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; + _model.state.angle.z += _model.state.gyro.timer.intervalf * _model.state.gyro.adc.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 = _model.state.accel.adc.accelToEuler(); _model.state.pose.z = _model.state.angle.z; - const float dt = _model.state.gyroTimer.intervalf; + const float dt = _model.state.gyro.timer.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); + float angle = _model.state.kalman[i].getAngle(_model.state.pose.get(i), _model.state.gyro.adc.get(i), dt); _model.state.angle.set(i, angle); //_model.state.rate.set(i, _model.state.kalman[i].getRate()); } @@ -113,13 +113,13 @@ void Fusion::kalmanFusion() void Fusion::complementaryFusion() { - _model.state.pose = _model.state.accel.accelToEuler(); + _model.state.pose = _model.state.accel.adc.accelToEuler(); _model.state.pose.z = _model.state.angle.z; - const float dt = _model.state.gyroTimer.intervalf; + const float dt = _model.state.gyro.timer.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; + float angle = (_model.state.angle[i] + _model.state.gyro.adc[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); @@ -131,10 +131,10 @@ void Fusion::complementaryFusion() void Fusion::complementaryFusionOld() { const float alpha = 0.01f; - const float dt = _model.state.gyroTimer.intervalf; - _model.state.pose = _model.state.accel.accelToEuler(); + const float dt = _model.state.gyro.timer.intervalf; + _model.state.pose = _model.state.accel.adc.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.angle = (_model.state.angle + _model.state.gyro.adc * dt) * (1.f - alpha) + _model.state.pose * alpha; _model.state.angleQ = _model.state.angle.eulerToQuaternion(); } @@ -149,10 +149,10 @@ void Fusion::rtqfFusion() return; } - float timeDelta = _model.state.gyroTimer.intervalf; + float timeDelta = _model.state.gyro.timer.intervalf; Quaternion measuredQPose = _model.state.poseQ; Quaternion fusionQPose = _model.state.angleQ; - VectorFloat fusionGyro = _model.state.gyro; + VectorFloat fusionGyro = _model.state.gyro.adc; float qs, qx, qy, qz; qs = fusionQPose.w; @@ -200,7 +200,7 @@ void Fusion::rtqfFusion() void Fusion::updatePoseFromAccelMag() { - _model.state.pose = _model.state.accel.accelToEuler(); + _model.state.pose = _model.state.accel.adc.accelToEuler(); //_model.state.accelPose = _model.state.pose; if(_model.magActive()) @@ -259,7 +259,7 @@ void Fusion::lerpFusion() { float correctionAlpha = 0.001f; // 0 - 1 => gyro - accel - _model.state.accelPose = _model.state.accel.accelToEuler(); + _model.state.accelPose = _model.state.accel.adc.accelToEuler(); _model.state.accelPoseQ = _model.state.accelPose.eulerToQuaternion(); if(_model.magActive()) @@ -277,7 +277,7 @@ void Fusion::lerpFusion() //_model.state.accelPose.eulerFromQuaternion(_model.state.accelPoseQ); // predict new state - Quaternion rotation = (_model.state.gyro * _model.state.gyroTimer.intervalf).eulerToQuaternion(); + Quaternion rotation = (_model.state.gyro.adc * _model.state.gyro.timer.intervalf).eulerToQuaternion(); _model.state.gyroPoseQ = (_model.state.gyroPoseQ * rotation).getNormalized(); // drift compensation @@ -293,7 +293,7 @@ void Fusion::madgwickFusion() { _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.accel.adc.x, _model.state.accel.adc.y, _model.state.accel.adc.z, _model.state.mag.adc.x, _model.state.mag.adc.y, _model.state.mag.adc.z ); } @@ -301,7 +301,7 @@ void Fusion::madgwickFusion() { _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.accel.adc.x, _model.state.accel.adc.y, _model.state.accel.adc.z ); } _model.state.angleQ = _madgwick.getQuaternion(); @@ -314,7 +314,7 @@ void FAST_CODE_ATTR Fusion::mahonyFusion() { _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.accel.adc.x, _model.state.accel.adc.y, _model.state.accel.adc.z, _model.state.mag.adc.x, _model.state.mag.adc.y, _model.state.mag.adc.z ); } @@ -322,7 +322,7 @@ void FAST_CODE_ATTR Fusion::mahonyFusion() { _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.accel.adc.x, _model.state.accel.adc.y, _model.state.accel.adc.z ); } _model.state.angleQ = _mahony.getQuaternion(); diff --git a/lib/Espfc/src/Hardware.h b/lib/Espfc/src/Hardware.h index a214a473..e32c0f3f 100644 --- a/lib/Espfc/src/Hardware.h +++ b/lib/Espfc/src/Hardware.h @@ -127,10 +127,10 @@ class Hardware if(!detectedGyro) return; detectedGyro->setDLPFMode(_model.config.gyro.dlpf); - _model.state.gyroDev = detectedGyro; - _model.state.gyroPresent = (bool)detectedGyro; - _model.state.accelPresent = _model.state.gyroPresent && _model.config.accel.dev != GYRO_NONE; - _model.state.gyroClock = detectedGyro->getRate(); + _model.state.gyro.dev = detectedGyro; + _model.state.gyro.present = (bool)detectedGyro; + _model.state.accel.present = _model.state.gyro.present && _model.config.accel.dev != GYRO_NONE; + _model.state.gyro.clock = detectedGyro->getRate(); } void detectMag() diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 488368ff..9a538c5a 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -109,12 +109,12 @@ class Model bool gyroActive() const /* IRAM_ATTR */ { - return state.gyroPresent && config.gyro.dev != GYRO_NONE; + return state.gyro.present && config.gyro.dev != GYRO_NONE; } bool accelActive() const { - return state.accelPresent && config.accel.dev != GYRO_NONE; + return state.accel.present && config.accel.dev != GYRO_NONE; } bool magActive() const @@ -129,15 +129,15 @@ class Model bool calibrationActive() const { - return state.accelCalibrationState != CALIBRATION_IDLE || state.gyroCalibrationState != CALIBRATION_IDLE || state.mag.calibrationState != CALIBRATION_IDLE; + return state.accel.calibrationState != CALIBRATION_IDLE || state.gyro.calibrationState != CALIBRATION_IDLE || state.mag.calibrationState != CALIBRATION_IDLE; } void calibrateGyro() { - state.gyroCalibrationState = CALIBRATION_START; + state.gyro.calibrationState = CALIBRATION_START; if(accelActive()) { - state.accelCalibrationState = CALIBRATION_START; + state.accel.calibrationState = CALIBRATION_START; } } @@ -148,16 +148,16 @@ class Model void finishCalibration() { - if(state.gyroCalibrationState == CALIBRATION_SAVE) + if(state.gyro.calibrationState == CALIBRATION_SAVE) { //save(); 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)); + logger.info().log(F("GYRO BIAS")).log(Math::toDeg(state.gyro.bias.x)).log(Math::toDeg(state.gyro.bias.y)).logln(Math::toDeg(state.gyro.bias.z)); } - if(state.accelCalibrationState == CALIBRATION_SAVE) + if(state.accel.calibrationState == CALIBRATION_SAVE) { save(); - logger.info().log(F("ACCEL BIAS")).log(state.accelBias.x).log(state.accelBias.y).logln(state.accelBias.z); + logger.info().log(F("ACCEL BIAS")).log(state.accel.bias.x).log(state.accel.bias.y).logln(state.accel.bias.z); } if(state.mag.calibrationState == CALIBRATION_SAVE) { @@ -299,13 +299,13 @@ class Model void sanitize() { // for spi gyro allow full speed mode - if (state.gyroDev && state.gyroDev->getBus()->isSPI()) + if (state.gyro.dev && state.gyro.dev->getBus()->isSPI()) { - state.gyroRate = Math::alignToClock(state.gyroClock, ESPFC_GYRO_SPI_RATE_MAX); + state.gyro.rate = Math::alignToClock(state.gyro.clock, ESPFC_GYRO_SPI_RATE_MAX); } else { - state.gyroRate = Math::alignToClock(state.gyroClock, ESPFC_GYRO_I2C_RATE_MAX); + state.gyro.rate = Math::alignToClock(state.gyro.clock, ESPFC_GYRO_I2C_RATE_MAX); // first usage if(_storageResult == STORAGE_ERR_BAD_MAGIC || _storageResult == STORAGE_ERR_BAD_SIZE || _storageResult == STORAGE_ERR_BAD_VERSION) { @@ -317,7 +317,7 @@ class Model //if(config.mag.dev != MAG_NONE || config.baro.dev != BARO_NONE) loopSyncMax /= 2; config.loopSync = std::max((int)config.loopSync, loopSyncMax); - state.loopRate = state.gyroRate / config.loopSync; + state.loopRate = state.gyro.rate / config.loopSync; config.output.protocol = ESC_PROTOCOL_SANITIZE(config.output.protocol); @@ -363,7 +363,7 @@ class Model if(config.output.protocol == ESC_PROTOCOL_PWM && state.loopRate > 500) { config.loopSync = std::max(config.loopSync, (int8_t)((state.loopRate + 499) / 500)); // align loop rate to lower than 500Hz - state.loopRate = state.gyroRate / config.loopSync; + state.loopRate = state.gyro.rate / config.loopSync; if(state.loopRate > 480 && config.output.maxThrottle > 1940) { config.output.maxThrottle = 1940; @@ -373,7 +373,7 @@ class Model if(config.output.protocol == ESC_PROTOCOL_ONESHOT125 && state.loopRate > 2000) { config.loopSync = std::max(config.loopSync, (int8_t)((state.loopRate + 1999) / 2000)); // align loop rate to lower than 2000Hz - state.loopRate = state.gyroRate / config.loopSync; + state.loopRate = state.gyro.rate / config.loopSync; } } @@ -428,15 +428,15 @@ class Model // init timers // sample rate = clock / ( divider + 1) - state.gyroTimer.setRate(state.gyroRate); - 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.gyro.timer.setRate(state.gyro.rate); + int accelRate = Math::alignToClock(state.gyro.timer.rate, 500); + state.accel.timer.setRate(state.gyro.timer.rate, state.gyro.timer.rate / accelRate); + state.loopTimer.setRate(state.gyro.timer.rate, config.loopSync); state.mixer.timer.setRate(state.loopTimer.rate, config.mixerSync); - int inputRate = Math::alignToClock(state.gyroTimer.rate, 1000); - state.input.timer.setRate(state.gyroTimer.rate, state.gyroTimer.rate / inputRate); + int inputRate = Math::alignToClock(state.gyro.timer.rate, 1000); + state.input.timer.setRate(state.gyro.timer.rate, state.gyro.timer.rate / inputRate); state.actuatorTimer.setRate(50); - state.dynamicFilterTimer.setRate(50); + state.gyro.dynamicFilterTimer.setRate(50); state.telemetryTimer.setInterval(config.telemetryInterval * 1000); state.stats.timer.setRate(3); if(magActive()) @@ -446,7 +446,7 @@ class Model state.boardAlignment.init(VectorFloat(Math::toRad(config.boardAlignment[0]), Math::toRad(config.boardAlignment[1]), Math::toRad(config.boardAlignment[2]))); - const uint32_t gyroPreFilterRate = state.gyroTimer.rate; + const uint32_t gyroPreFilterRate = state.gyro.timer.rate; const uint32_t gyroFilterRate = state.loopTimer.rate; const uint32_t inputFilterRate = state.input.timer.rate; const uint32_t pidFilterRate = state.loopTimer.rate; @@ -458,30 +458,30 @@ class Model { for(size_t p = 0; p < (size_t)config.gyro.dynamicFilter.width; p++) { - state.gyroDynNotchFilter[p][i].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 380), gyroFilterRate); + state.gyro.dynNotchFilter[p][i].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 380), gyroFilterRate); } } - state.gyroNotch1Filter[i].begin(config.gyro.notch1Filter, gyroFilterRate); - state.gyroNotch2Filter[i].begin(config.gyro.notch2Filter, gyroFilterRate); + state.gyro.notch1Filter[i].begin(config.gyro.notch1Filter, gyroFilterRate); + state.gyro.notch2Filter[i].begin(config.gyro.notch2Filter, gyroFilterRate); if(config.gyro.dynLpfFilter.cutoff > 0) { - state.gyroFilter[i].begin(FilterConfig((FilterType)config.gyro.filter.type, config.gyro.dynLpfFilter.cutoff), gyroFilterRate); + state.gyro.filter[i].begin(FilterConfig((FilterType)config.gyro.filter.type, config.gyro.dynLpfFilter.cutoff), gyroFilterRate); } else { - state.gyroFilter[i].begin(config.gyro.filter, gyroFilterRate); + state.gyro.filter[i].begin(config.gyro.filter, gyroFilterRate); } - state.gyroFilter2[i].begin(config.gyro.filter2, gyroFilterRate); - state.gyroFilter3[i].begin(config.gyro.filter3, gyroPreFilterRate); - state.accelFilter[i].begin(config.accel.filter, gyroFilterRate); - state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accelTimer.rate / 3), gyroFilterRate); + state.gyro.filter2[i].begin(config.gyro.filter2, gyroFilterRate); + state.gyro.filter3[i].begin(config.gyro.filter3, gyroPreFilterRate); + state.accel.filter[i].begin(config.accel.filter, gyroFilterRate); + state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accel.timer.rate / 3), gyroFilterRate); for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) { - state.rpmFreqFilter[m].begin(FilterConfig(FILTER_PT1, config.gyro.rpmFilter.freqLpf), gyroFilterRate); + state.gyro.rpmFreqFilter[m].begin(FilterConfig(FILTER_PT1, config.gyro.rpmFilter.freqLpf), gyroFilterRate); for(size_t n = 0; n < config.gyro.rpmFilter.harmonics; n++) { int center = Math::mapi(m * RPM_FILTER_HARMONICS_MAX + n, 0, RPM_FILTER_MOTOR_MAX * config.gyro.rpmFilter.harmonics, config.gyro.rpmFilter.minFreq, gyroFilterRate / 2); - state.rpmFilter[m][n][i].begin(FilterConfig(FILTER_NOTCH_DF1, center, center * 0.98f), gyroFilterRate); + state.gyro.rpmFilter[m][n][i].begin(FilterConfig(FILTER_NOTCH_DF1, center, center * 0.98f), gyroFilterRate); } } if(magActive()) @@ -574,8 +574,8 @@ class Model // load current sensor calibration for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { - state.gyroBias.set(i, config.gyro.bias[i] / 1000.0f); - state.accelBias.set(i, config.accel.bias[i] / 1000.0f); + state.gyro.bias.set(i, config.gyro.bias[i] / 1000.0f); + state.accel.bias.set(i, config.accel.bias[i] / 1000.0f); state.mag.calibrationOffset.set(i, config.mag.offset[i] / 10.0f); state.mag.calibrationScale.set(i, config.mag.scale[i] / 1000.0f); } @@ -586,8 +586,8 @@ class Model // store current sensor calibration for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { - config.gyro.bias[i] = lrintf(state.gyroBias[i] * 1000.0f); - config.accel.bias[i] = lrintf(state.accelBias[i] * 1000.0f); + config.gyro.bias[i] = lrintf(state.gyro.bias[i] * 1000.0f); + config.accel.bias[i] = lrintf(state.accel.bias[i] * 1000.0f); config.mag.offset[i] = lrintf(state.mag.calibrationOffset[i] * 10.0f); config.mag.scale[i] = lrintf(state.mag.calibrationScale[i] * 1000.0f); } diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 813757a4..d6bd5509 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -8,7 +8,6 @@ #endif #include "ModelConfig.h" -#include "Stats.h" #include "helper_3dmath.h" #include "Control/Pid.h" #include "Kalman.h" @@ -21,13 +20,15 @@ namespace Espfc { -static const size_t CLI_BUFF_SIZE = 128; -static const size_t CLI_ARGS_SIZE = 12; +constexpr size_t CLI_BUFF_SIZE = 128; +constexpr size_t CLI_ARGS_SIZE = 12; class CliCmd { public: - CliCmd(): buff{0}, index(0) { for(size_t i = 0; i < CLI_ARGS_SIZE; ++i) args[i] = nullptr; } + CliCmd(): buff{0}, index{0} { + std::fill_n(args, CLI_ARGS_SIZE, nullptr); + } const char * args[CLI_ARGS_SIZE]; char buff[CLI_BUFF_SIZE]; size_t index; @@ -246,51 +247,81 @@ struct BaroState int32_t altitudeBiasSamples; }; +struct GyroState +{ + Device::GyroDevice* dev; + bool present; + int32_t rate; + int32_t clock = 1000; + + VectorInt16 raw; + VectorFloat adc; + VectorFloat sampled; + VectorFloat scaled; + VectorFloat dynNotch; + + float scale; + VectorFloat bias; + float biasAlpha; + int biasSamples; + int calibrationState; + int calibrationRate; + + Filter filter[AXIS_COUNT_RPY]; + Filter filter2[AXIS_COUNT_RPY]; + Filter filter3[AXIS_COUNT_RPY]; + Filter notch1Filter[AXIS_COUNT_RPY]; + Filter notch2Filter[AXIS_COUNT_RPY]; + Filter dynNotchFilter[6][AXIS_COUNT_RPY]; + Filter rpmFilter[RPM_FILTER_MOTOR_MAX][RPM_FILTER_HARMONICS_MAX][AXIS_COUNT_RPY]; + Filter rpmFreqFilter[RPM_FILTER_MOTOR_MAX]; + + Timer timer; + Timer dynamicFilterTimer; +}; + +struct AccelState +{ + bool present; + VectorInt16 raw; + VectorFloat adc; + VectorFloat prev; + Filter filter[3]; + Timer timer; + + float scale; + VectorFloat bias; + float biasAlpha; + int biasSamples; + int calibrationState; +}; + // working data struct ModelState { + GyroState gyro; + AccelState accel; MagState mag; BaroState baro; - Device::GyroDevice* gyroDev; - VectorInt16 gyroRaw; - VectorFloat gyro; - VectorFloat gyroSampled; - VectorFloat gyroScaled; - VectorFloat gyroDynNotch; - VectorFloat gyroImu; - - VectorInt16 accelRaw; - VectorFloat accel; - VectorFloat gyroPose; Quaternion gyroPoseQ; VectorFloat accelPose; VectorFloat accelPose2; Quaternion accelPoseQ; - bool imuUpdate; bool loopUpdate; VectorFloat pose; Quaternion poseQ; - VectorFloat angle; Quaternion angleQ; RotationMatrixFloat boardAlignment; - Filter gyroFilter[3]; - Filter gyroFilter2[3]; - Filter gyroFilter3[3]; - Filter gyroNotch1Filter[3]; - Filter gyroNotch2Filter[3]; - Filter gyroDynNotchFilter[6][3]; + bool imuUpdate; + VectorFloat gyroImu; Filter gyroImuFilter[3]; - Filter accelFilter[3]; - Filter rpmFreqFilter[RPM_FILTER_MOTOR_MAX]; - Filter rpmFilter[RPM_FILTER_MOTOR_MAX][RPM_FILTER_HARMONICS_MAX][3]; - VectorFloat velocity; VectorFloat desiredVelocity; @@ -310,28 +341,6 @@ struct ModelState // other state Kalman kalman[AXES]; - VectorFloat accelPrev; - - float accelScale; - VectorFloat accelBias; - float accelBiasAlpha; - int accelBiasSamples; - int accelCalibrationState; - - float gyroScale; - VectorFloat gyroBias; - float gyroBiasAlpha; - int gyroBiasSamples; - int gyroCalibrationState; - int gyroCalibrationRate; - - int32_t gyroClock = 1000; - int32_t gyroRate; - - Timer gyroTimer; - Timer dynamicFilterTimer; - - Timer accelTimer; int32_t loopRate; Timer loopTimer; @@ -361,9 +370,6 @@ struct ModelState int16_t i2cErrorCount; int16_t i2cErrorDelta; - bool gyroPresent; - bool accelPresent; - uint32_t armingDisabledFlags; RescueConfigMode rescueConfigMode; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 266c2f01..ecc6c9f5 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -208,10 +208,10 @@ class MspProcessor // 1.42 r.writeU8(2); // configuration state: configured // 1.43 - r.writeU16(_model.state.gyroPresent ? _model.state.gyroTimer.rate : 0); // sample rate + r.writeU16(_model.state.gyro.present ? _model.state.gyro.timer.rate : 0); // sample rate { uint32_t problems = 0; - if(_model.state.accelBias.x == 0 && _model.state.accelBias.y == 0 && _model.state.accelBias.z == 0) { + if(_model.state.accel.bias.x == 0 && _model.state.accel.bias.y == 0 && _model.state.accel.bias.z == 0) { problems |= 1 << 0; // acc calibration required } if(_model.config.output.protocol == ESC_PROTOCOL_DISABLED) { @@ -250,7 +250,7 @@ class MspProcessor r.writeU8(1); // max profile count r.writeU8(0); // current rate profile index } else { // MSP_STATUS - //r.writeU16(_model.state.gyroTimer.interval); // gyro cycle time + //r.writeU16(_model.state.gyro.timer.interval); // gyro cycle time r.writeU16(0); } @@ -536,7 +536,7 @@ class MspProcessor r.writeU8(_model.config.gyro.align); // acc align, Starting with 4.0 gyro and acc alignment are the same r.writeU8(_model.config.mag.align); // mag align //1.41+ - r.writeU8(_model.state.gyroPresent ? 1 : 0); // gyro detection mask GYRO_1_MASK + r.writeU8(_model.state.gyro.present ? 1 : 0); // gyro detection mask GYRO_1_MASK r.writeU8(0); // gyro_to_use r.writeU8(_model.config.gyro.align); // gyro 1 r.writeU8(0); // gyro 2 @@ -679,9 +679,9 @@ class MspProcessor break; case MSP_ATTITUDE: - r.writeU16(lrintf(degrees(_model.state.angle.x) * 10.f)); // roll [decidegrees] - r.writeU16(lrintf(degrees(_model.state.angle.y) * 10.f)); // pitch [decidegrees] - r.writeU16(lrintf(degrees(-_model.state.angle.z))); // yaw [degrees] + r.writeU16(lrintf(Math::toDeg(_model.state.angle.x) * 10.f)); // roll [decidegrees] + r.writeU16(lrintf(Math::toDeg(_model.state.angle.y) * 10.f)); // pitch [decidegrees] + r.writeU16(lrintf(Math::toDeg(-_model.state.angle.z))); // yaw [degrees] break; case MSP_ALTITUDE: @@ -1279,11 +1279,11 @@ class MspProcessor case MSP_RAW_IMU: for (int i = 0; i < 3; i++) { - r.writeU16(lrintf(_model.state.accel[i] * ACCEL_G_INV * 2048.f)); + r.writeU16(lrintf(_model.state.accel.adc[i] * ACCEL_G_INV * 2048.f)); } for (int i = 0; i < 3; i++) { - r.writeU16(lrintf(Math::toDeg(_model.state.gyro[i]))); + r.writeU16(lrintf(Math::toDeg(_model.state.gyro.adc[i]))); } for (int i = 0; i < 3; i++) { diff --git a/lib/Espfc/src/Output/Mixer.cpp b/lib/Espfc/src/Output/Mixer.cpp index 8515d8bc..561353f1 100644 --- a/lib/Espfc/src/Output/Mixer.cpp +++ b/lib/Espfc/src/Output/Mixer.cpp @@ -339,7 +339,7 @@ void FAST_CODE_ATTR Mixer::readTelemetry() for(size_t i = 0; i < OUTPUT_CHANNELS; i++) { - _model.state.output.telemetry.freq[i] = _model.state.rpmFreqFilter[i].update(erpmToHz(_model.state.output.telemetry.erpm[i])); + _model.state.output.telemetry.freq[i] = _model.state.gyro.rpmFreqFilter[i].update(erpmToHz(_model.state.output.telemetry.erpm[i])); } _statsCounter++; diff --git a/lib/Espfc/src/Sensor/AccelSensor.h b/lib/Espfc/src/Sensor/AccelSensor.h index 5ef22037..9b4da449 100644 --- a/lib/Espfc/src/Sensor/AccelSensor.h +++ b/lib/Espfc/src/Sensor/AccelSensor.h @@ -15,22 +15,22 @@ class AccelSensor: public BaseSensor int begin() { - _model.state.accel.z = ACCEL_G; + _model.state.accel.adc.z = ACCEL_G; - _gyro = _model.state.gyroDev; + _gyro = _model.state.gyro.dev; if(!_gyro) return 0; - _model.state.accelScale = 16.f * ACCEL_G / 32768.f; + _model.state.accel.scale = 16.f * ACCEL_G / 32768.f; for(size_t i = 0; i < 3; i++) { - _filter[i].begin(FilterConfig(FILTER_FIR2, 1), _model.state.accelTimer.rate); + _filter[i].begin(FilterConfig(FILTER_FIR2, 1), _model.state.accel.timer.rate); } - _model.state.accelBiasAlpha = _model.state.accelTimer.rate > 0 ? 5.0f / _model.state.accelTimer.rate : 0; - _model.state.accelCalibrationState = CALIBRATION_IDLE; + _model.state.accel.biasAlpha = _model.state.accel.timer.rate > 0 ? 5.0f / _model.state.accel.timer.rate : 0; + _model.state.accel.calibrationState = CALIBRATION_IDLE; - _model.logger.info().log(F("ACCEL INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_gyro->getAddress()).log(_model.state.accelTimer.rate).log(_model.state.accelTimer.interval).logln(_model.state.accelPresent); + _model.logger.info().log(F("ACCEL INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_gyro->getAddress()).log(_model.state.accel.timer.rate).log(_model.state.accel.timer.interval).logln(_model.state.accel.present); return 1; } @@ -48,10 +48,10 @@ class AccelSensor: public BaseSensor { if(!_model.accelActive()) return 0; - //if(!_model.state.accelTimer.check()) return 0; + //if(!_model.state.accel.timer.check()) return 0; Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_READ); - _gyro->readAccel(_model.state.accelRaw); + _gyro->readAccel(_model.state.accel.raw); return 1; } @@ -62,19 +62,19 @@ class AccelSensor: public BaseSensor Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_FILTER); - _model.state.accel = (VectorFloat)_model.state.accelRaw * _model.state.accelScale; + _model.state.accel.adc = (VectorFloat)_model.state.accel.raw * _model.state.accel.scale; - align(_model.state.accel, _model.config.gyro.align); - _model.state.accel = _model.state.boardAlignment.apply(_model.state.accel); + align(_model.state.accel.adc, _model.config.gyro.align); + _model.state.accel.adc = _model.state.boardAlignment.apply(_model.state.accel.adc); for(size_t i = 0; i < 3; i++) { if(_model.config.debug.mode == DEBUG_ACCELEROMETER) { - _model.state.debug[i] = _model.state.accelRaw[i]; + _model.state.debug[i] = _model.state.accel.raw[i]; } - _model.state.accel.set(i, _filter[i].update(_model.state.accel[i])); - _model.state.accel.set(i, _model.state.accelFilter[i].update(_model.state.accel[i])); + _model.state.accel.adc.set(i, _filter[i].update(_model.state.accel.adc[i])); + _model.state.accel.adc.set(i, _model.state.accel.filter[i].update(_model.state.accel.adc[i])); } calibrate(); @@ -85,31 +85,31 @@ class AccelSensor: public BaseSensor private: void calibrate() { - switch(_model.state.accelCalibrationState) + switch(_model.state.accel.calibrationState) { case CALIBRATION_IDLE: - _model.state.accel -= _model.state.accelBias; + _model.state.accel.adc -= _model.state.accel.bias; break; case CALIBRATION_START: - _model.state.accelBias = VectorFloat(0, 0, ACCEL_G); - _model.state.accelBiasSamples = 2 * _model.state.accelTimer.rate; - _model.state.accelCalibrationState = CALIBRATION_UPDATE; + _model.state.accel.bias = VectorFloat(0, 0, ACCEL_G); + _model.state.accel.biasSamples = 2 * _model.state.accel.timer.rate; + _model.state.accel.calibrationState = CALIBRATION_UPDATE; break; case CALIBRATION_UPDATE: - _model.state.accelBias += (_model.state.accel - _model.state.accelBias) * _model.state.accelBiasAlpha; - _model.state.accelBiasSamples--; - if(_model.state.accelBiasSamples <= 0) _model.state.accelCalibrationState = CALIBRATION_APPLY; + _model.state.accel.bias += (_model.state.accel.adc - _model.state.accel.bias) * _model.state.accel.biasAlpha; + _model.state.accel.biasSamples--; + if(_model.state.accel.biasSamples <= 0) _model.state.accel.calibrationState = CALIBRATION_APPLY; break; case CALIBRATION_APPLY: - _model.state.accelBias.z -= ACCEL_G; - _model.state.accelCalibrationState = CALIBRATION_SAVE; + _model.state.accel.bias.z -= ACCEL_G; + _model.state.accel.calibrationState = CALIBRATION_SAVE; break; case CALIBRATION_SAVE: _model.finishCalibration(); - _model.state.accelCalibrationState = CALIBRATION_IDLE; + _model.state.accel.calibrationState = CALIBRATION_IDLE; break; default: - _model.state.accelCalibrationState = CALIBRATION_IDLE; + _model.state.accel.calibrationState = CALIBRATION_IDLE; break; } } diff --git a/lib/Espfc/src/Sensor/BaroSensor.h b/lib/Espfc/src/Sensor/BaroSensor.h index 468ada71..dbeda6df 100644 --- a/lib/Espfc/src/Sensor/BaroSensor.h +++ b/lib/Espfc/src/Sensor/BaroSensor.h @@ -30,8 +30,8 @@ class BaroSensor: public BaseSensor _baro->setMode(BARO_MODE_TEMP); int delay = _baro->getDelay(BARO_MODE_TEMP) + _baro->getDelay(BARO_MODE_PRESS); - int toGyroRate = (delay / _model.state.gyroTimer.interval) + 1; // number of gyro readings per cycle - int interval = _model.state.gyroTimer.interval * toGyroRate; + int toGyroRate = (delay / _model.state.gyro.timer.interval) + 1; // number of gyro readings per cycle + int interval = _model.state.gyro.timer.interval * toGyroRate; _model.state.baro.rate = 1000000 / interval; _model.state.baro.altitudeBiasSamples = 2 * _model.state.baro.rate; diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index 7cb9e48f..1b66bf21 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -16,16 +16,16 @@ GyroSensor::GyroSensor(Model &model) : _dyn_notch_denom(1), _model(model) int GyroSensor::begin() { - _gyro = _model.state.gyroDev; + _gyro = _model.state.gyro.dev; if (!_gyro) return 0; _gyro->setDLPFMode(_model.config.gyro.dlpf); _gyro->setRate(_gyro->getRate()); - _model.state.gyroScale = Math::toRad(2000.f) / 32768.f; + _model.state.gyro.scale = Math::toRad(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; + _model.state.gyro.calibrationState = CALIBRATION_START; // calibrate gyro on start + _model.state.gyro.calibrationRate = _model.state.loopTimer.rate; + _model.state.gyro.biasAlpha = 5.0f / _model.state.gyro.calibrationRate; _sma.begin(_model.config.loopSync); _dyn_notch_denom = std::max((uint32_t)1, _model.state.loopTimer.rate / 1000); @@ -53,7 +53,7 @@ int GyroSensor::begin() #endif } - _model.logger.info().log(F("GYRO INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_gyro->getAddress()).log(_model.config.gyro.dlpf).log(_gyro->getRate()).log(_model.state.gyroTimer.rate).logln(_model.state.gyroTimer.interval); + _model.logger.info().log(F("GYRO INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_gyro->getAddress()).log(_model.config.gyro.dlpf).log(_gyro->getRate()).log(_model.state.gyro.timer.rate).logln(_model.state.gyro.timer.interval); return 1; } @@ -64,20 +64,20 @@ int FAST_CODE_ATTR GyroSensor::read() Stats::Measure measure(_model.state.stats, COUNTER_GYRO_READ); - _gyro->readGyro(_model.state.gyroRaw); + _gyro->readGyro(_model.state.gyro.raw); - VectorFloat input = static_cast(_model.state.gyroRaw) * _model.state.gyroScale; + VectorFloat input = static_cast(_model.state.gyro.raw) * _model.state.gyro.scale; align(input, _model.config.gyro.align); input = _model.state.boardAlignment.apply(input); if (_model.config.gyro.filter3.freq) { - _model.state.gyroSampled = Utils::applyFilter(_model.state.gyroFilter3, input); + _model.state.gyro.sampled = Utils::applyFilter(_model.state.gyro.filter3, input); } else { - _model.state.gyroSampled = _sma.update(input); + _model.state.gyro.sampled = _sma.update(input); } return 1; @@ -89,23 +89,23 @@ int FAST_CODE_ATTR GyroSensor::filter() Stats::Measure measure(_model.state.stats, COUNTER_GYRO_FILTER); - _model.state.gyro = _model.state.gyroSampled; + _model.state.gyro.adc = _model.state.gyro.sampled; calibrate(); - _model.state.gyroScaled = _model.state.gyro; // must be after calibration + _model.state.gyro.scaled = _model.state.gyro.adc; // 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_RAW, i, _model.state.gyro.raw[i]); + _model.setDebug(DEBUG_GYRO_SCALED, i, lrintf(Math::toDeg(_model.state.gyro.scaled[i]))); } - _model.setDebug(DEBUG_GYRO_SAMPLE, 0, lrintf(degrees(_model.state.gyro[_model.config.debug.axis]))); + _model.setDebug(DEBUG_GYRO_SAMPLE, 0, lrintf(Math::toDeg(_model.state.gyro.adc[_model.config.debug.axis]))); - _model.state.gyro = Utils::applyFilter(_model.state.gyroFilter2, _model.state.gyro); + _model.state.gyro.adc = Utils::applyFilter(_model.state.gyro.filter2, _model.state.gyro.adc); - _model.setDebug(DEBUG_GYRO_SAMPLE, 1, lrintf(degrees(_model.state.gyro[_model.config.debug.axis]))); + _model.setDebug(DEBUG_GYRO_SAMPLE, 1, lrintf(Math::toDeg(_model.state.gyro.adc[_model.config.debug.axis]))); if (_rpm_enabled) { @@ -113,40 +113,40 @@ int FAST_CODE_ATTR GyroSensor::filter() { for (size_t n = 0; n < _model.config.gyro.rpmFilter.harmonics; n++) { - _model.state.gyro = Utils::applyFilter(_model.state.rpmFilter[m][n], _model.state.gyro); + _model.state.gyro.adc = Utils::applyFilter(_model.state.gyro.rpmFilter[m][n], _model.state.gyro.adc); } } } - _model.setDebug(DEBUG_GYRO_SAMPLE, 2, lrintf(degrees(_model.state.gyro[_model.config.debug.axis]))); + _model.setDebug(DEBUG_GYRO_SAMPLE, 2, lrintf(Math::toDeg(_model.state.gyro.adc[_model.config.debug.axis]))); - _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.state.gyro.adc = Utils::applyFilter(_model.state.gyro.notch1Filter, _model.state.gyro.adc); + _model.state.gyro.adc = Utils::applyFilter(_model.state.gyro.notch2Filter, _model.state.gyro.adc); + _model.state.gyro.adc = Utils::applyFilter(_model.state.gyro.filter, _model.state.gyro.adc); - _model.setDebug(DEBUG_GYRO_SAMPLE, 3, lrintf(degrees(_model.state.gyro[_model.config.debug.axis]))); + _model.setDebug(DEBUG_GYRO_SAMPLE, 3, lrintf(Math::toDeg(_model.state.gyro.adc[_model.config.debug.axis]))); if (_dyn_notch_enabled || _dyn_notch_debug) { - _model.state.gyroDynNotch = _dyn_notch_sma.update(_model.state.gyro); + _model.state.gyro.dynNotch = _dyn_notch_sma.update(_model.state.gyro.adc); } if (_dyn_notch_enabled) { for (size_t p = 0; p < (size_t)_model.config.gyro.dynamicFilter.width; p++) { - _model.state.gyro = Utils::applyFilter(_model.state.gyroDynNotchFilter[p], _model.state.gyro); + _model.state.gyro.adc = Utils::applyFilter(_model.state.gyro.dynNotchFilter[p], _model.state.gyro.adc); } } for (size_t i = 0; i < 3; ++i) { - _model.setDebug(DEBUG_GYRO_FILTERED, i, lrintf(degrees(_model.state.gyro[i]))); + _model.setDebug(DEBUG_GYRO_FILTERED, i, lrintf(Math::toDeg(_model.state.gyro.adc[i]))); } if (_model.accelActive()) { - _model.state.gyroImu = Utils::applyFilter(_model.state.gyroImuFilter, _model.state.gyro); + _model.state.gyroImu = Utils::applyFilter(_model.state.gyroImuFilter, _model.state.gyro.adc); } return 1; @@ -174,11 +174,11 @@ void FAST_CODE_ATTR GyroSensor::rpmFilterUpdate() { weight *= freqMargin * _rpm_fade_inv; } - _model.state.rpmFilter[_rpm_motor_index][n][0].reconfigure(freq, freq, _rpm_q, weight); + _model.state.gyro.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.state.gyro.rpmFilter[_rpm_motor_index][n][i].reconfigure(_model.state.gyro.rpmFilter[_rpm_motor_index][n][0]); } } @@ -202,7 +202,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() const float q = _model.config.gyro.dynamicFilter.q * 0.01; bool feed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0; - bool update = _model.state.dynamicFilterTimer.check(); + bool update = _model.state.gyro.dynamicFilterTimer.check(); for (size_t i = 0; i < 3; ++i) { @@ -211,7 +211,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() if (feed) { uint32_t startTime = micros(); - int status = _fft[i].update(_model.state.gyroDynNotch[i]); + int status = _fft[i].update(_model.state.gyro.dynNotch[i]); if (_model.config.debug.mode == DEBUG_FFT_TIME) { if (i == 0) @@ -232,7 +232,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() float freq = _fft[i].peaks[p].freq; if (freq >= _model.config.gyro.dynamicFilter.min_freq && freq <= _model.config.gyro.dynamicFilter.max_freq) { - _model.state.gyroDynNotchFilter[p][i].reconfigure(freq, freq, q); + _model.state.gyro.dynNotchFilter[p][i].reconfigure(freq, freq, q); } } } @@ -241,7 +241,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() if (feed) { uint32_t startTime = micros(); - _freqAnalyzer[i].update(_model.state.gyroDynNotch[i]); + _freqAnalyzer[i].update(_model.state.gyro.dynNotch[i]); float freq = _freqAnalyzer[i].freq; if (_model.config.debug.mode == DEBUG_FFT_TIME) { @@ -254,7 +254,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() if (update) _model.state.debug[i] = lrintf(freq); if (i == _model.config.debug.axis) - _model.state.debug[3] = lrintf(degrees(_model.state.gyroDynNotch[i])); + _model.state.debug[3] = lrintf(Math::toDeg(_model.state.gyro.dynNotch[i])); } if (_dyn_notch_enabled && update) { @@ -265,7 +265,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() size_t x = (p + i) % 3; int harmonic = (p / 3) + 1; int16_t f = Math::clamp((int16_t)lrintf(freq * harmonic), _model.config.gyro.dynamicFilter.min_freq, _model.config.gyro.dynamicFilter.max_freq); - _model.state.gyroDynNotchFilter[p][x].reconfigure(f, f, q); + _model.state.gyro.dynNotchFilter[p][x].reconfigure(f, f, q); } } } @@ -277,38 +277,38 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() void FAST_CODE_ATTR GyroSensor::calibrate() { - switch (_model.state.gyroCalibrationState) + switch (_model.state.gyro.calibrationState) { case CALIBRATION_IDLE: - _model.state.gyro -= _model.state.gyroBias; + _model.state.gyro.adc -= _model.state.gyro.bias; break; case CALIBRATION_START: - //_model.state.gyroBias = VectorFloat(); - _model.state.gyroBiasSamples = 2 * _model.state.gyroCalibrationRate; - _model.state.gyroCalibrationState = CALIBRATION_UPDATE; + //_model.state.gyro.bias = VectorFloat(); + _model.state.gyro.biasSamples = 2 * _model.state.gyro.calibrationRate; + _model.state.gyro.calibrationState = 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) + VectorFloat deltaAccel = _model.state.accel.adc - _model.state.accel.prev; + _model.state.accel.prev = _model.state.accel.adc; + if (deltaAccel.getMagnitude() < ESPFC_FUZZY_ACCEL_ZERO && _model.state.gyro.adc.getMagnitude() < ESPFC_FUZZY_GYRO_ZERO) { - _model.state.gyroBias += (_model.state.gyro - _model.state.gyroBias) * _model.state.gyroBiasAlpha; - _model.state.gyroBiasSamples--; + _model.state.gyro.bias += (_model.state.gyro.adc - _model.state.gyro.bias) * _model.state.gyro.biasAlpha; + _model.state.gyro.biasSamples--; } - if (_model.state.gyroBiasSamples <= 0) - _model.state.gyroCalibrationState = CALIBRATION_APPLY; + if (_model.state.gyro.biasSamples <= 0) + _model.state.gyro.calibrationState = CALIBRATION_APPLY; } break; case CALIBRATION_APPLY: - _model.state.gyroCalibrationState = CALIBRATION_SAVE; + _model.state.gyro.calibrationState = CALIBRATION_SAVE; break; case CALIBRATION_SAVE: _model.finishCalibration(); - _model.state.gyroCalibrationState = CALIBRATION_IDLE; + _model.state.gyro.calibrationState = CALIBRATION_IDLE; break; default: - _model.state.gyroCalibrationState = CALIBRATION_IDLE; + _model.state.gyro.calibrationState = CALIBRATION_IDLE; break; } } diff --git a/lib/Espfc/src/SensorManager.cpp b/lib/Espfc/src/SensorManager.cpp index e74d0102..4600b37b 100644 --- a/lib/Espfc/src/SensorManager.cpp +++ b/lib/Espfc/src/SensorManager.cpp @@ -20,12 +20,12 @@ int FAST_CODE_ATTR SensorManager::read() { _gyro.read(); - if(_model.state.loopTimer.syncTo(_model.state.gyroTimer)) + if(_model.state.loopTimer.syncTo(_model.state.gyro.timer)) { _model.state.appQueue.send(Event(EVENT_GYRO_READ)); } - if(_model.state.accelTimer.syncTo(_model.state.gyroTimer)) + if(_model.state.accel.timer.syncTo(_model.state.gyro.timer)) { _accel.update(); _model.state.appQueue.send(Event(EVENT_ACCEL_READ)); @@ -44,9 +44,9 @@ int FAST_CODE_ATTR SensorManager::read() int FAST_CODE_ATTR SensorManager::preLoop() { _gyro.filter(); - if(_model.state.gyroBiasSamples == 0) + if(_model.state.gyro.biasSamples == 0) { - _model.state.gyroBiasSamples = -1; + _model.state.gyro.biasSamples = -1; _fusion.restoreGain(); } return 1; @@ -77,7 +77,7 @@ int SensorManager::updateDelayed() // update at most one sensor besides gyro int status = 0; - if(_model.state.accelTimer.syncTo(_model.state.gyroTimer)) + if(_model.state.accel.timer.syncTo(_model.state.gyro.timer)) { _accel.update(); status = 1; diff --git a/lib/Espfc/src/Stats.h b/lib/Espfc/src/Stats.h index 20725b15..20f3d102 100644 --- a/lib/Espfc/src/Stats.h +++ b/lib/Espfc/src/Stats.h @@ -1,12 +1,11 @@ #ifndef _ESPFC_STATS_H_ #define _ESPFC_STATS_H_ -#include "Arduino.h" #include "Timer.h" namespace Espfc { -enum StatCounter : int8_t { +enum StatCounter { COUNTER_GYRO_READ, COUNTER_GYRO_FILTER, COUNTER_GYRO_FFT, diff --git a/lib/Espfc/src/Telemetry/TelemetryText.h b/lib/Espfc/src/Telemetry/TelemetryText.h index bd7976d2..e83be251 100644 --- a/lib/Espfc/src/Telemetry/TelemetryText.h +++ b/lib/Espfc/src/Telemetry/TelemetryText.h @@ -13,7 +13,7 @@ class TelemetryText int process(Stream& s) const { - //print(s, _model.state.gyro.x, 3); + //print(s, _model.state.gyro.adc.x, 3); //println(s); return 1; diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index 953a2042..ac19b1c3 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -105,15 +105,15 @@ void test_timer_check_micros() void test_model_gyro_init_1k_256dlpf() { Model model; - model.state.gyroClock = 8000; + model.state.gyro.clock = 8000; model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 1; model.config.mixerSync = 1; model.begin(); - TEST_ASSERT_EQUAL_INT32(8000, model.state.gyroClock); - TEST_ASSERT_EQUAL_INT32(2000, model.state.gyroRate); - TEST_ASSERT_EQUAL_INT32(2000, model.state.gyroTimer.rate); + TEST_ASSERT_EQUAL_INT32(8000, model.state.gyro.clock); + TEST_ASSERT_EQUAL_INT32(2000, model.state.gyro.rate); + TEST_ASSERT_EQUAL_INT32(2000, model.state.gyro.timer.rate); TEST_ASSERT_EQUAL_INT32(2000, model.state.loopRate); TEST_ASSERT_EQUAL_INT32(2000, model.state.loopTimer.rate); TEST_ASSERT_EQUAL_INT32(2000, model.state.mixer.timer.rate); @@ -122,15 +122,15 @@ void test_model_gyro_init_1k_256dlpf() void test_model_gyro_init_1k_188dlpf() { Model model; - model.state.gyroClock = 1000; + model.state.gyro.clock = 1000; model.config.gyro.dlpf = GYRO_DLPF_188; model.config.loopSync = 2; model.config.mixerSync = 2; model.begin(); - TEST_ASSERT_EQUAL_INT32(1000, model.state.gyroClock); - TEST_ASSERT_EQUAL_INT32(1000, model.state.gyroRate); - TEST_ASSERT_EQUAL_INT32(1000, model.state.gyroTimer.rate); + TEST_ASSERT_EQUAL_INT32(1000, model.state.gyro.clock); + TEST_ASSERT_EQUAL_INT32(1000, model.state.gyro.rate); + TEST_ASSERT_EQUAL_INT32(1000, model.state.gyro.timer.rate); TEST_ASSERT_EQUAL_INT32( 500, model.state.loopRate); TEST_ASSERT_EQUAL_INT32( 500, model.state.loopTimer.rate); TEST_ASSERT_EQUAL_INT32( 250, model.state.mixer.timer.rate); @@ -139,7 +139,7 @@ void test_model_gyro_init_1k_188dlpf() void test_model_inner_pid_init() { Model model; - model.state.gyroClock = 1000; + model.state.gyro.clock = 1000; model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 1; model.config.mixerSync = 1; @@ -171,7 +171,7 @@ void test_model_inner_pid_init() void test_model_outer_pid_init() { Model model; - model.state.gyroClock = 8000; + model.state.gyro.clock = 8000; model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 1; model.config.mixerSync = 1; @@ -195,7 +195,7 @@ void test_model_outer_pid_init() void test_controller_rates() { Model model; - model.state.gyroClock = 8000; + model.state.gyro.clock = 8000; model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 8; model.config.mixerSync = 1; @@ -245,7 +245,7 @@ void test_controller_rates() void test_controller_rates_limit() { Model model; - model.state.gyroClock = 8000; + model.state.gyro.clock = 8000; model.config.gyro.dlpf = GYRO_DLPF_256; model.config.loopSync = 8; model.config.mixerSync = 1; @@ -464,10 +464,10 @@ void test_actuator_arming_gyro_motor_calbration() void test_actuator_arming_failsafe() { Model model; - model.state.gyroPresent = true; + model.state.gyro.present = true; model.config.output.protocol = ESC_PROTOCOL_DSHOT150; model.state.failsafe.phase = FC_FAILSAFE_RX_LOSS_DETECTED; - model.state.gyroCalibrationState = CALIBRATION_UPDATE; + model.state.gyro.calibrationState = CALIBRATION_UPDATE; model.state.input.rxFailSafe = true; model.state.input.rxLoss = true; @@ -489,7 +489,7 @@ void test_actuator_arming_throttle() model.config.output.protocol = ESC_PROTOCOL_DSHOT150; model.config.input.minCheck = 1050; model.state.input.us[AXIS_THRUST] = 1100; - model.state.gyroPresent = true; + model.state.gyro.present = true; //model.begin(); From d947406939fccf61bd555bca8a6302d1510dfc5d Mon Sep 17 00:00:00 2001 From: rtlopez Date: Tue, 29 Oct 2024 21:42:57 +0100 Subject: [PATCH 33/68] attitude state refactor --- lib/Espfc/src/Controller.cpp | 14 ++--- lib/Espfc/src/Fusion.cpp | 74 ++++++++++++------------- lib/Espfc/src/Model.h | 2 +- lib/Espfc/src/ModelState.h | 20 ++++--- lib/Espfc/src/Msp/MspProcessor.h | 6 +- lib/Espfc/src/Sensor/GyroSensor.cpp | 2 +- lib/Espfc/src/Telemetry/TelemetryCRSF.h | 6 +- 7 files changed, 63 insertions(+), 61 deletions(-) diff --git a/lib/Espfc/src/Controller.cpp b/lib/Espfc/src/Controller.cpp index d0449ed8..f7b29e6a 100644 --- a/lib/Espfc/src/Controller.cpp +++ b/lib/Espfc/src/Controller.cpp @@ -82,14 +82,14 @@ void Controller::outerLoopRobot() void Controller::innerLoopRobot() { //VectorFloat v(0.f, 0.f, 1.f); - //v.rotate(_model.state.angleQ); + //v.rotate(_model.state.attitude.quaternion); //const float angle = acos(v.z); - const float angle = std::max(abs(_model.state.angle[AXIS_PITCH]), abs(_model.state.angle[AXIS_ROLL])); + const float angle = std::max(abs(_model.state.attitude.euler[AXIS_PITCH]), abs(_model.state.attitude.euler[AXIS_ROLL])); const bool stabilize = angle < Math::toRad(_model.config.level.angleLimit); if(stabilize) { - _model.state.output.ch[AXIS_PITCH] = _model.state.innerPid[AXIS_PITCH].update(_model.state.desiredAngle[AXIS_PITCH], _model.state.angle[AXIS_PITCH]); + _model.state.output.ch[AXIS_PITCH] = _model.state.innerPid[AXIS_PITCH].update(_model.state.desiredAngle[AXIS_PITCH], _model.state.attitude.euler[AXIS_PITCH]); _model.state.output.ch[AXIS_YAW] = _model.state.innerPid[AXIS_YAW].update(_model.state.desiredRate[AXIS_YAW], _model.state.gyro.adc[AXIS_YAW]); } else @@ -101,7 +101,7 @@ void Controller::innerLoopRobot() if(_model.config.debug.mode == DEBUG_ANGLERATE) { - _model.state.debug[2] = lrintf(Math::toDeg(_model.state.angle[AXIS_PITCH]) * 10); + _model.state.debug[2] = lrintf(Math::toDeg(_model.state.attitude.euler[AXIS_PITCH]) * 10); _model.state.debug[3] = lrintf(_model.state.output.ch[AXIS_PITCH] * 1000); } } @@ -113,10 +113,10 @@ void FAST_CODE_ATTR Controller::outerLoop() _model.state.desiredAngle = VectorFloat( _model.state.input.ch[AXIS_ROLL] * Math::toRad(_model.config.level.angleLimit), _model.state.input.ch[AXIS_PITCH] * Math::toRad(_model.config.level.angleLimit), - _model.state.angle[AXIS_YAW] + _model.state.attitude.euler[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]); + _model.state.desiredRate[AXIS_ROLL] = _model.state.outerPid[AXIS_ROLL].update(_model.state.desiredAngle[AXIS_ROLL], _model.state.attitude.euler[AXIS_ROLL]); + _model.state.desiredRate[AXIS_PITCH] = _model.state.outerPid[AXIS_PITCH].update(_model.state.desiredAngle[AXIS_PITCH], _model.state.attitude.euler[AXIS_PITCH]); // disable fterm in angle mode _model.state.innerPid[AXIS_ROLL].fScale = 0.f; _model.state.innerPid[AXIS_PITCH].fScale = 0.f; diff --git a/lib/Espfc/src/Fusion.cpp b/lib/Espfc/src/Fusion.cpp index 9359d165..5c2eae12 100644 --- a/lib/Espfc/src/Fusion.cpp +++ b/lib/Espfc/src/Fusion.cpp @@ -72,9 +72,9 @@ int FAST_CODE_ATTR Fusion::update() if(_model.config.debug.mode == DEBUG_ALTITUDE) { - _model.state.debug[0] = lrintf(Math::toDeg(_model.state.angle[0]) * 10); - _model.state.debug[1] = lrintf(Math::toDeg(_model.state.angle[1]) * 10); - _model.state.debug[2] = lrintf(Math::toDeg(_model.state.angle[2]) * 10); + _model.state.debug[0] = lrintf(Math::toDeg(_model.state.attitude.euler[0]) * 10); + _model.state.debug[1] = lrintf(Math::toDeg(_model.state.attitude.euler[1]) * 10); + _model.state.debug[2] = lrintf(Math::toDeg(_model.state.attitude.euler[2]) * 10); } return 1; } @@ -83,49 +83,49 @@ void Fusion::experimentalFusion() { // Experiment: workaround for 90 deg limit on pitch[y] axis Quaternion r = Quaternion::lerp(Quaternion(), _model.state.accel.adc.accelToQuaternion(), 0.5); - _model.state.angle.eulerFromQuaternion(r); - _model.state.angle *= 2.f; + _model.state.attitude.euler.eulerFromQuaternion(r); + _model.state.attitude.euler *= 2.f; } void Fusion::simpleFusion() { _model.state.pose = _model.state.accel.adc.accelToEuler(); - _model.state.angle.x = _model.state.pose.x; - _model.state.angle.y = _model.state.pose.y; - _model.state.angle.z += _model.state.gyro.timer.intervalf * _model.state.gyro.adc.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; + _model.state.attitude.euler.x = _model.state.pose.x; + _model.state.attitude.euler.y = _model.state.pose.y; + _model.state.attitude.euler.z += _model.state.gyro.timer.intervalf * _model.state.gyro.adc.z; + if(_model.state.attitude.euler.z > PI) _model.state.attitude.euler.z -= TWO_PI; + if(_model.state.attitude.euler.z < -PI) _model.state.attitude.euler.z += TWO_PI; } void Fusion::kalmanFusion() { _model.state.pose = _model.state.accel.adc.accelToEuler(); - _model.state.pose.z = _model.state.angle.z; + _model.state.pose.z = _model.state.attitude.euler.z; const float dt = _model.state.gyro.timer.intervalf; for(size_t i = 0; i < 3; i++) { float angle = _model.state.kalman[i].getAngle(_model.state.pose.get(i), _model.state.gyro.adc.get(i), dt); - _model.state.angle.set(i, angle); + _model.state.attitude.euler.set(i, angle); //_model.state.rate.set(i, _model.state.kalman[i].getRate()); } - _model.state.angleQ = _model.state.angle.eulerToQuaternion(); + _model.state.attitude.quaternion = _model.state.attitude.euler.eulerToQuaternion(); } void Fusion::complementaryFusion() { _model.state.pose = _model.state.accel.adc.accelToEuler(); - _model.state.pose.z = _model.state.angle.z; + _model.state.pose.z = _model.state.attitude.euler.z; const float dt = _model.state.gyro.timer.intervalf; const float alpha = 0.002f; for(size_t i = 0; i < 3; i++) { - float angle = (_model.state.angle[i] + _model.state.gyro.adc[i] * dt) * (1.f - alpha) + _model.state.pose[i] * alpha; + float angle = (_model.state.attitude.euler[i] + _model.state.gyro.adc[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.attitude.euler.set(i, angle); } - _model.state.angleQ = _model.state.angle.eulerToQuaternion(); - //_model.state.angle.eulerFromQuaternion(_model.state.angleQ); // causes NaN + _model.state.attitude.quaternion = _model.state.attitude.euler.eulerToQuaternion(); + //_model.state.attitude.euler.eulerFromQuaternion(_model.state.attitude.quaternion); // causes NaN } void Fusion::complementaryFusionOld() @@ -133,9 +133,9 @@ void Fusion::complementaryFusionOld() const float alpha = 0.01f; const float dt = _model.state.gyro.timer.intervalf; _model.state.pose = _model.state.accel.adc.accelToEuler(); - _model.state.pose.z = _model.state.angle.z; - _model.state.angle = (_model.state.angle + _model.state.gyro.adc * dt) * (1.f - alpha) + _model.state.pose * alpha; - _model.state.angleQ = _model.state.angle.eulerToQuaternion(); + _model.state.pose.z = _model.state.attitude.euler.z; + _model.state.attitude.euler = (_model.state.attitude.euler + _model.state.gyro.adc * dt) * (1.f - alpha) + _model.state.pose * alpha; + _model.state.attitude.quaternion = _model.state.attitude.euler.eulerToQuaternion(); } void Fusion::rtqfFusion() @@ -143,15 +143,15 @@ void Fusion::rtqfFusion() float slerpPower = 0.001f; if(_first) { - _model.state.angle = _model.state.pose; - _model.state.angleQ = _model.state.poseQ; + _model.state.attitude.euler = _model.state.pose; + _model.state.attitude.quaternion = _model.state.poseQ; _first = false; return; } float timeDelta = _model.state.gyro.timer.intervalf; Quaternion measuredQPose = _model.state.poseQ; - Quaternion fusionQPose = _model.state.angleQ; + Quaternion fusionQPose = _model.state.attitude.quaternion; VectorFloat fusionGyro = _model.state.gyro.adc; float qs, qx, qy, qz; @@ -194,8 +194,8 @@ void Fusion::rtqfFusion() fusionQPose = fusionQPose * rotationPower; fusionQPose.normalize(); - _model.state.angleQ = fusionQPose; - _model.state.angle.eulerFromQuaternion(fusionQPose); + _model.state.attitude.quaternion = fusionQPose; + _model.state.attitude.euler.eulerFromQuaternion(fusionQPose); } void Fusion::updatePoseFromAccelMag() @@ -224,7 +224,7 @@ void Fusion::updatePoseFromAccelMag() } else { - _model.state.pose.z = _model.state.angle.z; + _model.state.pose.z = _model.state.attitude.euler.z; } _model.state.poseQ = _model.state.pose.eulerToQuaternion(); _model.state.pose.eulerFromQuaternion(_model.state.poseQ); @@ -244,8 +244,8 @@ void Fusion::updatePoseFromAccelMag() // 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))) { + if(((_model.state.poseQ.get(maxIndex) < 0) && (_model.state.attitude.quaternion.get(maxIndex) > 0)) || + ((_model.state.poseQ.get(maxIndex) > 0) && (_model.state.attitude.quaternion.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; @@ -292,7 +292,7 @@ void Fusion::madgwickFusion() if(false && _model.magActive()) { _madgwick.update( - _model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z, + _model.state.attitude.rate.x, _model.state.attitude.rate.y, _model.state.attitude.rate.z, _model.state.accel.adc.x, _model.state.accel.adc.y, _model.state.accel.adc.z, _model.state.mag.adc.x, _model.state.mag.adc.y, _model.state.mag.adc.z ); @@ -300,12 +300,12 @@ void Fusion::madgwickFusion() else { _madgwick.update( - _model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z, + _model.state.attitude.rate.x, _model.state.attitude.rate.y, _model.state.attitude.rate.z, _model.state.accel.adc.x, _model.state.accel.adc.y, _model.state.accel.adc.z ); } - _model.state.angleQ = _madgwick.getQuaternion(); - _model.state.angle = _madgwick.getEuler(); + _model.state.attitude.quaternion = _madgwick.getQuaternion(); + _model.state.attitude.euler = _madgwick.getEuler(); } void FAST_CODE_ATTR Fusion::mahonyFusion() @@ -313,7 +313,7 @@ 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.attitude.rate.x, _model.state.attitude.rate.y, _model.state.attitude.rate.z, _model.state.accel.adc.x, _model.state.accel.adc.y, _model.state.accel.adc.z, _model.state.mag.adc.x, _model.state.mag.adc.y, _model.state.mag.adc.z ); @@ -321,12 +321,12 @@ void FAST_CODE_ATTR Fusion::mahonyFusion() else { _mahony.update( - _model.state.gyroImu.x, _model.state.gyroImu.y, _model.state.gyroImu.z, + _model.state.attitude.rate.x, _model.state.attitude.rate.y, _model.state.attitude.rate.z, _model.state.accel.adc.x, _model.state.accel.adc.y, _model.state.accel.adc.z ); } - _model.state.angleQ = _mahony.getQuaternion(); - _model.state.angle = _mahony.getEuler(); + _model.state.attitude.quaternion = _mahony.getQuaternion(); + _model.state.attitude.euler = _mahony.getEuler(); } } diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 9a538c5a..47912c96 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -474,7 +474,7 @@ class Model state.gyro.filter2[i].begin(config.gyro.filter2, gyroFilterRate); state.gyro.filter3[i].begin(config.gyro.filter3, gyroPreFilterRate); state.accel.filter[i].begin(config.accel.filter, gyroFilterRate); - state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accel.timer.rate / 3), gyroFilterRate); + state.attitude.filter[i].begin(FilterConfig(FILTER_PT1, state.accel.timer.rate / 3), gyroFilterRate); for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) { state.gyro.rpmFreqFilter[m].begin(FilterConfig(FILTER_PT1, config.gyro.rpmFilter.freqLpf), gyroFilterRate); diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index d6bd5509..bf6a8fa3 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -286,7 +286,7 @@ struct AccelState VectorInt16 raw; VectorFloat adc; VectorFloat prev; - Filter filter[3]; + Filter filter[AXIS_COUNT_RPY]; Timer timer; float scale; @@ -296,6 +296,14 @@ struct AccelState int calibrationState; }; +struct AttitudeState +{ + VectorFloat rate; + Filter filter[AXIS_COUNT_RPY]; + VectorFloat euler; + Quaternion quaternion; +}; + // working data struct ModelState { @@ -304,24 +312,18 @@ struct ModelState MagState mag; BaroState baro; + AttitudeState attitude; + VectorFloat gyroPose; Quaternion gyroPoseQ; VectorFloat accelPose; VectorFloat accelPose2; Quaternion accelPoseQ; - - bool loopUpdate; VectorFloat pose; Quaternion poseQ; - VectorFloat angle; - Quaternion angleQ; RotationMatrixFloat boardAlignment; - bool imuUpdate; - VectorFloat gyroImu; - Filter gyroImuFilter[3]; - VectorFloat velocity; VectorFloat desiredVelocity; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index ecc6c9f5..651b1ba4 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -679,9 +679,9 @@ class MspProcessor break; case MSP_ATTITUDE: - r.writeU16(lrintf(Math::toDeg(_model.state.angle.x) * 10.f)); // roll [decidegrees] - r.writeU16(lrintf(Math::toDeg(_model.state.angle.y) * 10.f)); // pitch [decidegrees] - r.writeU16(lrintf(Math::toDeg(-_model.state.angle.z))); // yaw [degrees] + r.writeU16(lrintf(Math::toDeg(_model.state.attitude.euler.x) * 10.f)); // roll [decidegrees] + r.writeU16(lrintf(Math::toDeg(_model.state.attitude.euler.y) * 10.f)); // pitch [decidegrees] + r.writeU16(lrintf(Math::toDeg(-_model.state.attitude.euler.z))); // yaw [degrees] break; case MSP_ALTITUDE: diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index 1b66bf21..6e6b941e 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -146,7 +146,7 @@ int FAST_CODE_ATTR GyroSensor::filter() if (_model.accelActive()) { - _model.state.gyroImu = Utils::applyFilter(_model.state.gyroImuFilter, _model.state.gyro.adc); + _model.state.attitude.rate = Utils::applyFilter(_model.state.attitude.filter, _model.state.gyro.adc); } return 1; diff --git a/lib/Espfc/src/Telemetry/TelemetryCRSF.h b/lib/Espfc/src/Telemetry/TelemetryCRSF.h index ff85d3bc..c724a7c5 100644 --- a/lib/Espfc/src/Telemetry/TelemetryCRSF.h +++ b/lib/Espfc/src/Telemetry/TelemetryCRSF.h @@ -100,9 +100,9 @@ class TelemetryCRSF { msg.prepare(Rc::CRSF_FRAMETYPE_ATTITUDE); - int16_t r = toAngle(_model.state.angle.x); - int16_t p = toAngle(_model.state.angle.y); - int16_t y = toAngle(_model.state.angle.z); + int16_t r = toAngle(_model.state.attitude.euler.x); + int16_t p = toAngle(_model.state.attitude.euler.y); + int16_t y = toAngle(_model.state.attitude.euler.z); msg.writeU16(Math::toBigEndian16(r)); msg.writeU16(Math::toBigEndian16(p)); From 3f3f6f5dd49abfedb1995fab33808deb23f925b1 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Tue, 29 Oct 2024 21:51:55 +0100 Subject: [PATCH 34/68] setpoint state refactor --- lib/Espfc/src/Blackbox/BlackboxBridge.cpp | 2 +- lib/Espfc/src/Controller.cpp | 28 +++++++-------- lib/Espfc/src/ModelState.h | 43 +++++++++++------------ 3 files changed, 35 insertions(+), 38 deletions(-) diff --git a/lib/Espfc/src/Blackbox/BlackboxBridge.cpp b/lib/Espfc/src/Blackbox/BlackboxBridge.cpp index b44bde26..823d6dca 100644 --- a/lib/Espfc/src/Blackbox/BlackboxBridge.cpp +++ b/lib/Espfc/src/Blackbox/BlackboxBridge.cpp @@ -65,7 +65,7 @@ bool sensors(uint32_t mask) float pidGetPreviousSetpoint(int axis) { - return Espfc::Math::toDeg(_model_ptr->state.desiredRate[axis]); + return Espfc::Math::toDeg(_model_ptr->state.setpoint.rate[axis]); } float mixerGetThrottle(void) diff --git a/lib/Espfc/src/Controller.cpp b/lib/Espfc/src/Controller.cpp index f7b29e6a..d8bb07ce 100644 --- a/lib/Espfc/src/Controller.cpp +++ b/lib/Espfc/src/Controller.cpp @@ -69,8 +69,8 @@ void Controller::outerLoopRobot() { angle = _model.state.outerPid[AXIS_PITCH].update(_model.state.input.ch[AXIS_PITCH], speed) * Math::toRad(_model.config.level.rateLimit); } - _model.state.desiredAngle.set(AXIS_PITCH, angle); - _model.state.desiredRate[AXIS_YAW] = _model.state.input.ch[AXIS_YAW] * Math::toRad(_model.config.level.rateLimit); + _model.state.setpoint.angle.set(AXIS_PITCH, angle); + _model.state.setpoint.rate[AXIS_YAW] = _model.state.input.ch[AXIS_YAW] * Math::toRad(_model.config.level.rateLimit); if(_model.config.debug.mode == DEBUG_ANGLERATE) { @@ -89,8 +89,8 @@ void Controller::innerLoopRobot() const bool stabilize = angle < Math::toRad(_model.config.level.angleLimit); if(stabilize) { - _model.state.output.ch[AXIS_PITCH] = _model.state.innerPid[AXIS_PITCH].update(_model.state.desiredAngle[AXIS_PITCH], _model.state.attitude.euler[AXIS_PITCH]); - _model.state.output.ch[AXIS_YAW] = _model.state.innerPid[AXIS_YAW].update(_model.state.desiredRate[AXIS_YAW], _model.state.gyro.adc[AXIS_YAW]); + _model.state.output.ch[AXIS_PITCH] = _model.state.innerPid[AXIS_PITCH].update(_model.state.setpoint.angle[AXIS_PITCH], _model.state.attitude.euler[AXIS_PITCH]); + _model.state.output.ch[AXIS_YAW] = _model.state.innerPid[AXIS_YAW].update(_model.state.setpoint.rate[AXIS_YAW], _model.state.gyro.adc[AXIS_YAW]); } else { @@ -110,30 +110,30 @@ void FAST_CODE_ATTR Controller::outerLoop() { if(_model.isActive(MODE_ANGLE)) { - _model.state.desiredAngle = VectorFloat( + _model.state.setpoint.angle = VectorFloat( _model.state.input.ch[AXIS_ROLL] * Math::toRad(_model.config.level.angleLimit), _model.state.input.ch[AXIS_PITCH] * Math::toRad(_model.config.level.angleLimit), _model.state.attitude.euler[AXIS_YAW] ); - _model.state.desiredRate[AXIS_ROLL] = _model.state.outerPid[AXIS_ROLL].update(_model.state.desiredAngle[AXIS_ROLL], _model.state.attitude.euler[AXIS_ROLL]); - _model.state.desiredRate[AXIS_PITCH] = _model.state.outerPid[AXIS_PITCH].update(_model.state.desiredAngle[AXIS_PITCH], _model.state.attitude.euler[AXIS_PITCH]); + _model.state.setpoint.rate[AXIS_ROLL] = _model.state.outerPid[AXIS_ROLL].update(_model.state.setpoint.angle[AXIS_ROLL], _model.state.attitude.euler[AXIS_ROLL]); + _model.state.setpoint.rate[AXIS_PITCH] = _model.state.outerPid[AXIS_PITCH].update(_model.state.setpoint.angle[AXIS_PITCH], _model.state.attitude.euler[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.ch[AXIS_ROLL]); - _model.state.desiredRate[AXIS_PITCH] = calculateSetpointRate(AXIS_PITCH, _model.state.input.ch[AXIS_PITCH]); + _model.state.setpoint.rate[AXIS_ROLL] = calculateSetpointRate(AXIS_ROLL, _model.state.input.ch[AXIS_ROLL]); + _model.state.setpoint.rate[AXIS_PITCH] = calculateSetpointRate(AXIS_PITCH, _model.state.input.ch[AXIS_PITCH]); } - _model.state.desiredRate[AXIS_YAW] = calculateSetpointRate(AXIS_YAW, _model.state.input.ch[AXIS_YAW]); - _model.state.desiredRate[AXIS_THRUST] = _model.state.input.ch[AXIS_THRUST]; + _model.state.setpoint.rate[AXIS_YAW] = calculateSetpointRate(AXIS_YAW, _model.state.input.ch[AXIS_YAW]); + _model.state.setpoint.rate[AXIS_THRUST] = _model.state.input.ch[AXIS_THRUST]; if(_model.config.debug.mode == DEBUG_ANGLERATE) { for(size_t i = 0; i < 3; ++i) { - _model.state.debug[i] = lrintf(Math::toDeg(_model.state.desiredRate[i])); + _model.state.debug[i] = lrintf(Math::toDeg(_model.state.setpoint.rate[i])); } } } @@ -143,10 +143,10 @@ void FAST_CODE_ATTR Controller::innerLoop() const float tpaFactor = getTpaFactor(); for(size_t i = 0; i < AXIS_COUNT_RPY; ++i) { - _model.state.output.ch[i] = _model.state.innerPid[i].update(_model.state.desiredRate[i], _model.state.gyro.adc[i]) * tpaFactor; + _model.state.output.ch[i] = _model.state.innerPid[i].update(_model.state.setpoint.rate[i], _model.state.gyro.adc[i]) * tpaFactor; //_model.state.debug[i] = lrintf(_model.state.innerPid[i].fTerm * 1000); } - _model.state.output.ch[AXIS_THRUST] = _model.state.desiredRate[AXIS_THRUST]; + _model.state.output.ch[AXIS_THRUST] = _model.state.setpoint.rate[AXIS_THRUST]; if(_model.config.debug.mode == DEBUG_ITERM_RELAX) { diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index bf6a8fa3..451b13d7 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -304,6 +304,12 @@ struct AttitudeState Quaternion quaternion; }; +struct SetpointState +{ + VectorFloat angle; + float rate[AXES]; +}; + // working data struct ModelState { @@ -312,38 +318,19 @@ struct ModelState MagState mag; BaroState baro; - AttitudeState attitude; - - VectorFloat gyroPose; - Quaternion gyroPoseQ; - VectorFloat accelPose; - VectorFloat accelPose2; - Quaternion accelPoseQ; - VectorFloat pose; - Quaternion poseQ; + InputState input; + FailsafeState failsafe; + AttitudeState attitude; RotationMatrixFloat boardAlignment; - VectorFloat velocity; - VectorFloat desiredVelocity; - - VectorFloat desiredAngle; - Quaternion desiredAngleQ; - - float desiredRate[AXES]; - + SetpointState setpoint; Control::Pid innerPid[AXES]; Control::Pid outerPid[AXES]; - InputState input; - FailsafeState failsafe; - MixerState mixer; OutputState output; - // other state - Kalman kalman[AXES]; - int32_t loopRate; Timer loopTimer; @@ -380,6 +367,16 @@ struct ModelState Timer serialTimer; Target::Queue appQueue; + + // other state + Kalman kalman[AXES]; + VectorFloat gyroPose; + Quaternion gyroPoseQ; + VectorFloat accelPose; + VectorFloat accelPose2; + Quaternion accelPoseQ; + VectorFloat pose; + Quaternion poseQ; }; } From b15ae408d0b8cdfbb96af8bd895a38973a673b32 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Tue, 29 Oct 2024 23:17:35 +0100 Subject: [PATCH 35/68] mode state refactor --- lib/Espfc/src/Actuator.h | 40 ++++++++++++++--------------- lib/Espfc/src/Blackbox/Blackbox.cpp | 14 +++++----- lib/Espfc/src/Cli.h | 4 +-- lib/Espfc/src/Model.h | 26 +++++++++---------- lib/Espfc/src/ModelState.h | 27 +++++++++---------- lib/Espfc/src/Msp/MspProcessor.h | 4 +-- lib/Espfc/src/Wireless.h | 2 +- test/test_fc/test_fc.cpp | 12 ++++----- 8 files changed, 65 insertions(+), 64 deletions(-) diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h index c46be816..2182d9ca 100644 --- a/lib/Espfc/src/Actuator.h +++ b/lib/Espfc/src/Actuator.h @@ -13,19 +13,19 @@ class Actuator int begin() { - _model.state.modeMask = 0; - _model.state.modeMaskPrev = 0; - _model.state.modeMaskPresent = 0; - _model.state.modeMaskSwitch = 0; + _model.state.mode.mask = 0; + _model.state.mode.maskPrev = 0; + _model.state.mode.maskPresent = 0; + _model.state.mode.maskSwitch = 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.mode.maskPresent |= 1 << c.id; } - _model.state.airmodeAllowed = false; - _model.state.rescueConfigMode = RESCUE_CONFIG_PENDING; + _model.state.mode.airmodeAllowed = false; + _model.state.mode.rescueConfigMode = RESCUE_CONFIG_PENDING; return 1; } @@ -104,7 +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); + _model.setArmingDisabled(ARMING_DISABLED_REBOOT_REQUIRED, _model.state.mode.rescueConfigMode == RESCUE_CONFIG_ACTIVE); } void updateModeMask() @@ -141,7 +141,7 @@ class Actuator for(size_t i = 0; i < MODE_COUNT; i++) { bool newVal = newMask & (1 << i); - bool oldVal = _model.state.modeMask & (1 << i); + bool oldVal = _model.state.mode.mask & (1 << i); if(newVal == oldVal) continue; // mode unchanged if(newVal && !canActivateMode((FlightMode)i)) { @@ -161,7 +161,7 @@ class Actuator case MODE_ANGLE: return _model.accelActive(); case MODE_AIRMODE: - return _model.state.airmodeAllowed; + return _model.state.mode.airmodeAllowed; default: return true; } @@ -174,12 +174,12 @@ class Actuator bool armed = _model.isModeActive(MODE_ARMED); if(armed) { - _model.state.disarmReason = DISARM_REASON_SYSTEM; - _model.state.rescueConfigMode = RESCUE_CONFIG_DISABLED; + _model.state.mode.disarmReason = DISARM_REASON_SYSTEM; + _model.state.mode.rescueConfigMode = RESCUE_CONFIG_DISABLED; } - else if(!armed && _model.state.disarmReason == DISARM_REASON_SYSTEM) + else if(!armed && _model.state.mode.disarmReason == DISARM_REASON_SYSTEM) { - _model.state.disarmReason = DISARM_REASON_SWITCH; + _model.state.mode.disarmReason = DISARM_REASON_SWITCH; } } } @@ -189,11 +189,11 @@ class Actuator bool armed = _model.isModeActive(MODE_ARMED); if(!armed) { - _model.state.airmodeAllowed = false; + _model.state.mode.airmodeAllowed = false; } - if(armed && !_model.state.airmodeAllowed && _model.state.input.us[AXIS_THRUST] > 1400) // activate airmode in the air + if(armed && !_model.state.mode.airmodeAllowed && _model.state.input.us[AXIS_THRUST] > 1400) // activate airmode in the air { - _model.state.airmodeAllowed = true; + _model.state.mode.airmodeAllowed = true; } } @@ -237,17 +237,17 @@ class Actuator void updateRescueConfig() { - switch(_model.state.rescueConfigMode) + switch(_model.state.mode.rescueConfigMode) { case RESCUE_CONFIG_PENDING: // if some rc frames are received, disable to prevent activate later if(_model.state.input.frameCount > 100) { - _model.state.rescueConfigMode = RESCUE_CONFIG_DISABLED; + _model.state.mode.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; + _model.state.mode.rescueConfigMode = RESCUE_CONFIG_ACTIVE; } break; case RESCUE_CONFIG_ACTIVE: diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index 75f23150..82fc8e30 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -191,12 +191,12 @@ int Blackbox::begin() rpmFilterConfigMutable()->rpm_filter_weights[1] = _model.config.gyro.rpmFilter.weights[1]; rpmFilterConfigMutable()->rpm_filter_weights[2] = _model.config.gyro.rpmFilter.weights[2]; - 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); + updateModeFlag(&rcModeActivationPresent, BOXARM, _model.state.mode.maskPresent & 1 << MODE_ARMED); + updateModeFlag(&rcModeActivationPresent, BOXANGLE, _model.state.mode.maskPresent & 1 << MODE_ANGLE); + updateModeFlag(&rcModeActivationPresent, BOXAIRMODE, _model.state.mode.maskPresent & 1 << MODE_AIRMODE); + updateModeFlag(&rcModeActivationPresent, BOXFAILSAFE, _model.state.mode.maskPresent & 1 << MODE_FAILSAFE); + updateModeFlag(&rcModeActivationPresent, BOXBLACKBOX, _model.state.mode.maskPresent & 1 << MODE_BLACKBOX); + updateModeFlag(&rcModeActivationPresent, BOXBLACKBOXERASE, _model.state.mode.maskPresent & 1 << MODE_BLACKBOX_ERASE); blackboxInit(); @@ -301,7 +301,7 @@ void FAST_CODE_ATTR Blackbox::updateArmed() { DISABLE_ARMING_FLAG(ARMED); flightLogEventData_t eventData; - eventData.disarm.reason = _model.state.disarmReason; + eventData.disarm.reason = _model.state.mode.disarmReason; blackboxLogEvent(FLIGHT_LOG_EVENT_DISARM, &eventData); stop = _model.state.loopTimer.last + 500000; // schedule stop in 500ms } diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 7e112c05..345a0c9a 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -1300,14 +1300,14 @@ class Cli s.print(F("arming flags:")); for(size_t i = 0; i < armingDisableNamesLength; i++) { - if(_model.state.armingDisabledFlags & (1 << i)) { + if(_model.state.mode.armingDisabledFlags & (1 << i)) { s.print(' '); s.print(armingDisableNames[i]); } } s.println(); s.print(F(" rescue mode: ")); - s.print(_model.state.rescueConfigMode); + s.print(_model.state.mode.rescueConfigMode); s.println(); s.print(F(" uptime: ")); diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 47912c96..27cb4416 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -40,39 +40,39 @@ class Model bool isModeActive(FlightMode mode) const { - return state.modeMask & (1 << mode); + return state.mode.mask & (1 << mode); } bool hasChanged(FlightMode mode) const { - return (state.modeMask & (1 << mode)) != (state.modeMaskPrev & (1 << mode)); + return (state.mode.mask & (1 << mode)) != (state.mode.maskPrev & (1 << mode)); } void clearMode(FlightMode mode) { - state.modeMaskPrev |= state.modeMask & (1 << mode); - state.modeMask &= ~(1 << mode); + state.mode.maskPrev |= state.mode.mask & (1 << mode); + state.mode.mask &= ~(1 << mode); } void updateModes(uint32_t mask) { - state.modeMaskPrev = state.modeMask; - state.modeMask = mask; + state.mode.maskPrev = state.mode.mask; + state.mode.mask = mask; } bool isSwitchActive(FlightMode mode) const { - return state.modeMaskSwitch & (1 << mode); + return state.mode.maskSwitch & (1 << mode); } void updateSwitchActive(uint32_t mask) { - state.modeMaskSwitch = mask; + state.mode.maskSwitch = mask; } void disarm(DisarmReason r) { - state.disarmReason = r; + state.mode.disarmReason = r; clearMode(MODE_ARMED); clearMode(MODE_AIRMODE); state.appQueue.send(Event(EVENT_DISARM)); @@ -172,19 +172,19 @@ class Model #if defined(ESPFC_DEV_PRESET_UNSAFE_ARMING) return false; #else - return state.armingDisabledFlags != 0; + return state.mode.armingDisabledFlags != 0; #endif } void setArmingDisabled(ArmingDisabledFlags flag, bool value) { - if(value) state.armingDisabledFlags |= flag; - else state.armingDisabledFlags &= ~flag; + if(value) state.mode.armingDisabledFlags |= flag; + else state.mode.armingDisabledFlags &= ~flag; } bool getArmingDisabled(ArmingDisabledFlags flag) { - return state.armingDisabledFlags & flag; + return state.mode.armingDisabledFlags & flag; } void setOutputSaturated(bool val) diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 451b13d7..43671b3c 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -310,7 +310,19 @@ struct SetpointState float rate[AXES]; }; -// working data +struct ModeState +{ + uint32_t mask; + uint32_t maskPrev; + uint32_t maskSwitch; + uint32_t maskPresent; + uint32_t disarmReason; + uint32_t armingDisabledFlags; + RescueConfigMode rescueConfigMode; + bool airmodeAllowed; +}; + +// runtime data struct ModelState { GyroState gyro; @@ -337,16 +349,9 @@ struct ModelState Timer actuatorTimer; Timer telemetryTimer; + ModeState mode; Stats stats; - uint32_t modeMask; - uint32_t modeMaskPrev; - uint32_t modeMaskSwitch; - uint32_t modeMaskPresent; - uint32_t disarmReason; - - bool airmodeAllowed; - int16_t debug[8]; BuzzerState buzzer; @@ -359,10 +364,6 @@ struct ModelState int16_t i2cErrorCount; int16_t i2cErrorDelta; - uint32_t armingDisabledFlags; - - RescueConfigMode rescueConfigMode; - SerialPortState serial[SERIAL_UART_COUNT]; Timer serialTimer; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 651b1ba4..6827e90d 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -243,7 +243,7 @@ class MspProcessor r.writeU16(_model.state.i2cErrorCount); // i2c error count // acc, baro, mag, gps, sonar, gyro r.writeU16(_model.accelActive() | _model.baroActive() << 1 | _model.magActive() << 2 | 0 << 3 | 0 << 4 | _model.gyroActive() << 5); - r.writeU32(_model.state.modeMask); // flight mode flags + r.writeU32(_model.state.mode.mask); // flight mode flags r.writeU8(0); // pid profile r.writeU16(lrintf(_model.state.stats.getCpuLoad())); if (m.cmd == MSP_STATUS_EX) { @@ -259,7 +259,7 @@ class MspProcessor // Write arming disable flags r.writeU8(ARMING_DISABLED_FLAGS_COUNT); // 1 byte, flag count - r.writeU32(_model.state.armingDisabledFlags); // 4 bytes, flags + r.writeU32(_model.state.mode.armingDisabledFlags); // 4 bytes, flags r.writeU8(0); // reboot required break; diff --git a/lib/Espfc/src/Wireless.h b/lib/Espfc/src/Wireless.h index cf222afe..96e9e550 100644 --- a/lib/Espfc/src/Wireless.h +++ b/lib/Espfc/src/Wireless.h @@ -100,7 +100,7 @@ class Wireless switch(_status) { case STOPPED: - if(_model.state.rescueConfigMode == RESCUE_CONFIG_ACTIVE && _model.isFeatureActive(FEATURE_SOFTSERIAL)) + if(_model.state.mode.rescueConfigMode == RESCUE_CONFIG_ACTIVE && _model.isFeatureActive(FEATURE_SOFTSERIAL)) { connect(); _status = STARTED; diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index ac19b1c3..e8d7d6fa 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -454,11 +454,11 @@ void test_actuator_arming_gyro_motor_calbration() Actuator actuator(model); actuator.begin(); - TEST_ASSERT_EQUAL_UINT32(0, model.state.armingDisabledFlags); + TEST_ASSERT_EQUAL_UINT32(0, model.state.mode.armingDisabledFlags); actuator.updateArmingDisabled(); - TEST_ASSERT_EQUAL_UINT32(ARMING_DISABLED_NO_GYRO | ARMING_DISABLED_MOTOR_PROTOCOL, model.state.armingDisabledFlags); + TEST_ASSERT_EQUAL_UINT32(ARMING_DISABLED_NO_GYRO | ARMING_DISABLED_MOTOR_PROTOCOL, model.state.mode.armingDisabledFlags); } void test_actuator_arming_failsafe() @@ -476,11 +476,11 @@ void test_actuator_arming_failsafe() Actuator actuator(model); actuator.begin(); - TEST_ASSERT_EQUAL_UINT32(0, model.state.armingDisabledFlags); + TEST_ASSERT_EQUAL_UINT32(0, model.state.mode.armingDisabledFlags); actuator.updateArmingDisabled(); - TEST_ASSERT_EQUAL_UINT32(ARMING_DISABLED_RX_FAILSAFE | ARMING_DISABLED_FAILSAFE | ARMING_DISABLED_CALIBRATING, model.state.armingDisabledFlags); + TEST_ASSERT_EQUAL_UINT32(ARMING_DISABLED_RX_FAILSAFE | ARMING_DISABLED_FAILSAFE | ARMING_DISABLED_CALIBRATING, model.state.mode.armingDisabledFlags); } void test_actuator_arming_throttle() @@ -496,11 +496,11 @@ void test_actuator_arming_throttle() Actuator actuator(model); actuator.begin(); - TEST_ASSERT_EQUAL_UINT32(0, model.state.armingDisabledFlags); + TEST_ASSERT_EQUAL_UINT32(0, model.state.mode.armingDisabledFlags); actuator.updateArmingDisabled(); - TEST_ASSERT_EQUAL_UINT32(ARMING_DISABLED_THROTTLE, model.state.armingDisabledFlags); + TEST_ASSERT_EQUAL_UINT32(ARMING_DISABLED_THROTTLE, model.state.mode.armingDisabledFlags); } void test_mixer_throttle_limit_none() From 33232920e89d138db75559ce4055f49877b5184e Mon Sep 17 00:00:00 2001 From: rtlopez Date: Wed, 30 Oct 2024 00:09:43 +0100 Subject: [PATCH 36/68] dyn notch count + axis consts --- lib/Espfc/src/Actuator.h | 2 +- lib/Espfc/src/Blackbox/Blackbox.cpp | 6 +++--- lib/Espfc/src/Cli.h | 2 +- lib/Espfc/src/Controller.cpp | 4 ++-- lib/Espfc/src/Filter.h | 13 +++++++----- lib/Espfc/src/Math/FFTAnalyzer.h | 2 +- lib/Espfc/src/Model.h | 8 ++++---- lib/Espfc/src/ModelConfig.h | 4 +--- lib/Espfc/src/ModelState.h | 13 ++++++------ lib/Espfc/src/Msp/MspProcessor.h | 14 ++++++------- lib/Espfc/src/Output/Mixers.h | 2 +- lib/Espfc/src/Sensor/AccelSensor.h | 4 ++-- lib/Espfc/src/Sensor/GyroSensor.cpp | 31 +++++++++++++---------------- lib/Espfc/src/Sensor/GyroSensor.h | 1 + lib/Espfc/src/Sensor/MagSensor.h | 6 +++--- 15 files changed, 56 insertions(+), 56 deletions(-) diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h index 2182d9ca..67657422 100644 --- a/lib/Espfc/src/Actuator.h +++ b/lib/Espfc/src/Actuator.h @@ -68,7 +68,7 @@ class Actuator float min = _model.config.scaler[i].minScale * 0.01f; float max = _model.config.scaler[i].maxScale * 0.01f; float scale = Math::map3(v, -1.f, 0.f, 1.f, min, min < 0 ? 0.f : 1.f, max); - for(size_t x = 0; x < AXES; x++) + for(size_t x = 0; x < AXIS_COUNT_RPYT; x++) { if( (x == AXIS_ROLL && (mode & ACT_AXIS_ROLL)) || diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index 82fc8e30..d37ecc73 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -124,7 +124,7 @@ int Blackbox::begin() gyroConfigMutable()->gyro_soft_notch_hz_2 = _model.config.gyro.notch2Filter.freq; gyroConfigMutable()->gyro_sync_denom = 1; - dynNotchConfigMutable()->dyn_notch_count = _model.config.gyro.dynamicFilter.width; + dynNotchConfigMutable()->dyn_notch_count = _model.config.gyro.dynamicFilter.count; dynNotchConfigMutable()->dyn_notch_q = _model.config.gyro.dynamicFilter.q; dynNotchConfigMutable()->dyn_notch_min_hz = _model.config.gyro.dynamicFilter.min_freq; dynNotchConfigMutable()->dyn_notch_max_hz = _model.config.gyro.dynamicFilter.max_freq; @@ -235,7 +235,7 @@ int FAST_CODE_ATTR Blackbox::update() void FAST_CODE_ATTR Blackbox::updateData() { - for(size_t i = 0; i < 3; i++) + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { gyro.gyroADCf[i] = Math::toDeg(_model.state.gyro.adc[i]); gyro.gyroADC[i] = Math::toDeg(_model.state.gyro.scaled[i]); @@ -265,7 +265,7 @@ void FAST_CODE_ATTR Blackbox::updateData() } if(_model.config.debug.mode != DEBUG_NONE && _model.config.debug.mode != DEBUG_BLACKBOX_OUTPUT) { - for(size_t i = 0; i < 8; i++) + for(size_t i = 0; i < DEBUG_VALUE_COUNT; i++) { debug[i] = _model.state.debug[i]; } diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 345a0c9a..a1d46322 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -449,7 +449,7 @@ class Cli Param(PSTR("gyro_dyn_lpf_min"), &c.gyro.dynLpfFilter.cutoff), Param(PSTR("gyro_dyn_lpf_max"), &c.gyro.dynLpfFilter.freq), Param(PSTR("gyro_dyn_notch_q"), &c.gyro.dynamicFilter.q), - Param(PSTR("gyro_dyn_notch_count"), &c.gyro.dynamicFilter.width), + Param(PSTR("gyro_dyn_notch_count"), &c.gyro.dynamicFilter.count), Param(PSTR("gyro_dyn_notch_min"), &c.gyro.dynamicFilter.min_freq), Param(PSTR("gyro_dyn_notch_max"), &c.gyro.dynamicFilter.max_freq), Param(PSTR("gyro_rpm_harmonics"), &c.gyro.rpmFilter.harmonics), diff --git a/lib/Espfc/src/Controller.cpp b/lib/Espfc/src/Controller.cpp index d8bb07ce..e661042a 100644 --- a/lib/Espfc/src/Controller.cpp +++ b/lib/Espfc/src/Controller.cpp @@ -131,7 +131,7 @@ void FAST_CODE_ATTR Controller::outerLoop() if(_model.config.debug.mode == DEBUG_ANGLERATE) { - for(size_t i = 0; i < 3; ++i) + for(size_t i = 0; i < AXIS_COUNT_RPY; ++i) { _model.state.debug[i] = lrintf(Math::toDeg(_model.state.setpoint.rate[i])); } @@ -170,7 +170,7 @@ void Controller::resetIterm() || (!_model.isAirModeActive() && _model.config.iterm.lowThrottleZeroIterm && _model.isThrottleLow()) // on low throttle (not in air mode) ) { - for(size_t i = 0; i < AXES; i++) + for(size_t i = 0; i < AXIS_COUNT_RPYT; i++) { _model.state.innerPid[i].iTerm = 0; _model.state.outerPid[i].iTerm = 0; diff --git a/lib/Espfc/src/Filter.h b/lib/Espfc/src/Filter.h index 11b3be23..cf4b01c8 100644 --- a/lib/Espfc/src/Filter.h +++ b/lib/Espfc/src/Filter.h @@ -1,6 +1,7 @@ #pragma once #include +#include namespace Espfc { @@ -36,14 +37,16 @@ class FilterConfig int16_t cutoff; }; +constexpr size_t DYN_NOTCH_COUNT_MAX = 6; + class DynamicFilterConfig { public: DynamicFilterConfig() {} - DynamicFilterConfig(int8_t w, int16_t qf, int16_t lf, int16_t hf): width(w), q(qf), min_freq(lf), max_freq(hf) {} - int8_t width; - int16_t q; - int16_t min_freq; - int16_t max_freq; + DynamicFilterConfig(int8_t c, int16_t qf, int16_t lf, int16_t hf): count(c), q(qf), min_freq(lf), max_freq(hf) {} + uint8_t count = 4; + int16_t q = 300; + int16_t min_freq = 80; + int16_t max_freq = 400; static const int MIN_FREQ = 1000; }; diff --git a/lib/Espfc/src/Math/FFTAnalyzer.h b/lib/Espfc/src/Math/FFTAnalyzer.h index dab9c6e4..31749b20 100644 --- a/lib/Espfc/src/Math/FFTAnalyzer.h +++ b/lib/Espfc/src/Math/FFTAnalyzer.h @@ -31,7 +31,7 @@ class FFTAnalyzer _rate = rate; _freq_min = config.min_freq; _freq_max = std::min(config.max_freq, nyquistLimit); - _peak_count = std::min((size_t)config.width, PEAKS_MAX); + _peak_count = std::min((size_t)config.count, PEAKS_MAX); _idx = axis * SAMPLES / 3; _bin_width = (float)_rate / SAMPLES; // no need to dived by 2 as we next process `SAMPLES / 2` results diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 27cb4416..a95dcb5c 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -190,7 +190,7 @@ class Model void setOutputSaturated(bool val) { state.output.saturated = val; - for(size_t i = 0; i < 3; i++) + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { state.innerPid[i].outputSaturated = val; state.outerPid[i].outputSaturated = val; @@ -416,9 +416,9 @@ class Model 1 << (BUZZER_ARMING - 1) | 1 << (BUZZER_BAT_LOW - 1); - if(config.gyro.dynamicFilter.width > 6) + if(config.gyro.dynamicFilter.count > DYN_NOTCH_COUNT_MAX) { - config.gyro.dynamicFilter.width = 6; + config.gyro.dynamicFilter.count = DYN_NOTCH_COUNT_MAX; } } @@ -456,7 +456,7 @@ class Model { if(isActive(FEATURE_DYNAMIC_FILTER)) { - for(size_t p = 0; p < (size_t)config.gyro.dynamicFilter.width; p++) + for(size_t p = 0; p < (size_t)config.gyro.dynamicFilter.count; p++) { state.gyro.dynNotchFilter[p][i].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 380), gyroFilterRate); } diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 0d15099b..31b6df03 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -229,8 +229,6 @@ enum InputFilterType : uint8_t { }; constexpr size_t MODEL_NAME_LEN = 16; -constexpr size_t AXES = 4; -constexpr size_t AXES_RPY = 3; constexpr size_t INPUT_CHANNELS = AXIS_COUNT; constexpr size_t OUTPUT_CHANNELS = ESC_CHANNEL_COUNT; static_assert(ESC_CHANNEL_COUNT == ESPFC_OUTPUT_COUNT, "ESC_CHANNEL_COUNT and ESPFC_OUTPUT_COUNT must be equal"); @@ -580,7 +578,7 @@ struct GyroConfig FilterConfig notch1Filter{FILTER_NOTCH, 0, 0}; FilterConfig notch2Filter{FILTER_NOTCH, 0, 0}; FilterConfig dynLpfFilter{FILTER_PT1, 425, 170}; - DynamicFilterConfig dynamicFilter{4, 300, 80, 400}; + DynamicFilterConfig dynamicFilter; RpmFilterConfig rpmFilter; }; diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 43671b3c..412ef0b8 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -20,6 +20,7 @@ namespace Espfc { +constexpr size_t DEBUG_VALUE_COUNT = 8; constexpr size_t CLI_BUFF_SIZE = 128; constexpr size_t CLI_ARGS_SIZE = 12; @@ -272,7 +273,7 @@ struct GyroState Filter filter3[AXIS_COUNT_RPY]; Filter notch1Filter[AXIS_COUNT_RPY]; Filter notch2Filter[AXIS_COUNT_RPY]; - Filter dynNotchFilter[6][AXIS_COUNT_RPY]; + Filter dynNotchFilter[DYN_NOTCH_COUNT_MAX][AXIS_COUNT_RPY]; Filter rpmFilter[RPM_FILTER_MOTOR_MAX][RPM_FILTER_HARMONICS_MAX][AXIS_COUNT_RPY]; Filter rpmFreqFilter[RPM_FILTER_MOTOR_MAX]; @@ -307,7 +308,7 @@ struct AttitudeState struct SetpointState { VectorFloat angle; - float rate[AXES]; + float rate[AXIS_COUNT_RPYT]; }; struct ModeState @@ -337,8 +338,8 @@ struct ModelState RotationMatrixFloat boardAlignment; SetpointState setpoint; - Control::Pid innerPid[AXES]; - Control::Pid outerPid[AXES]; + Control::Pid innerPid[AXIS_COUNT_RPYT]; + Control::Pid outerPid[AXIS_COUNT_RPYT]; MixerState mixer; OutputState output; @@ -352,7 +353,7 @@ struct ModelState ModeState mode; Stats stats; - int16_t debug[8]; + int16_t debug[DEBUG_VALUE_COUNT]; BuzzerState buzzer; @@ -370,7 +371,7 @@ struct ModelState Target::Queue appQueue; // other state - Kalman kalman[AXES]; + Kalman kalman[AXIS_COUNT_RPYT]; VectorFloat gyroPose; Quaternion gyroPoseQ; VectorFloat accelPose; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 6827e90d..00a53bba 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -906,7 +906,7 @@ class MspProcessor case MSP_RC_TUNING: r.writeU8(_model.config.input.rate[AXIS_ROLL]); r.writeU8(_model.config.input.expo[AXIS_ROLL]); - for(size_t i = 0; i < 3; i++) + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { r.writeU8(_model.config.input.superRate[i]); } @@ -947,7 +947,7 @@ class MspProcessor } _model.config.input.expo[AXIS_ROLL] = expo; - for(size_t i = 0; i < 3; i++) + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { _model.config.input.superRate[i] = m.readU8(); } @@ -1085,7 +1085,7 @@ class MspProcessor r.writeU16(_model.config.dterm.dynLpfFilter.freq); // dyn lpf dterm max // gyro analyse r.writeU8(3); // deprecated dyn notch range - r.writeU8(_model.config.gyro.dynamicFilter.width); // dyn_notch_width_percent + r.writeU8(_model.config.gyro.dynamicFilter.count); // dyn_notch_width_percent r.writeU16(_model.config.gyro.dynamicFilter.q); // dyn_notch_q r.writeU16(_model.config.gyro.dynamicFilter.min_freq); // dyn_notch_min_hz // rpm filter @@ -1131,7 +1131,7 @@ class MspProcessor } if (m.remain() >= 8) { m.readU8(); // deprecated dyn_notch_range - _model.config.gyro.dynamicFilter.width = m.readU8(); // dyn_notch_width_percent + _model.config.gyro.dynamicFilter.count = m.readU8(); // dyn_notch_width_percent _model.config.gyro.dynamicFilter.q = m.readU16(); // dyn_notch_q _model.config.gyro.dynamicFilter.min_freq = m.readU16(); // dyn_notch_min_hz _model.config.gyro.rpmFilter.harmonics = m.readU8(); // gyro_rpm_notch_harmonics @@ -1277,15 +1277,15 @@ class MspProcessor break; case MSP_RAW_IMU: - for (int i = 0; i < 3; i++) + for (int i = 0; i < AXIS_COUNT_RPY; i++) { r.writeU16(lrintf(_model.state.accel.adc[i] * ACCEL_G_INV * 2048.f)); } - for (int i = 0; i < 3; i++) + for (int i = 0; i < AXIS_COUNT_RPY; i++) { r.writeU16(lrintf(Math::toDeg(_model.state.gyro.adc[i]))); } - for (int i = 0; i < 3; i++) + for (int i = 0; i < AXIS_COUNT_RPY; i++) { r.writeU16(lrintf(_model.state.mag.adc[i] * 1090)); } diff --git a/lib/Espfc/src/Output/Mixers.h b/lib/Espfc/src/Output/Mixers.h index 17c0fd0b..a42605b4 100644 --- a/lib/Espfc/src/Output/Mixers.h +++ b/lib/Espfc/src/Output/Mixers.h @@ -55,7 +55,7 @@ enum ThrottleLimitType { THROTTLE_LIMIT_TYPE_MAX, }; -static const size_t MIXER_RULE_MAX = 64; +constexpr size_t MIXER_RULE_MAX = 64; class MixerEntry { public: diff --git a/lib/Espfc/src/Sensor/AccelSensor.h b/lib/Espfc/src/Sensor/AccelSensor.h index 9b4da449..f9ea1bca 100644 --- a/lib/Espfc/src/Sensor/AccelSensor.h +++ b/lib/Espfc/src/Sensor/AccelSensor.h @@ -22,7 +22,7 @@ class AccelSensor: public BaseSensor _model.state.accel.scale = 16.f * ACCEL_G / 32768.f; - for(size_t i = 0; i < 3; i++) + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { _filter[i].begin(FilterConfig(FILTER_FIR2, 1), _model.state.accel.timer.rate); } @@ -67,7 +67,7 @@ class AccelSensor: public BaseSensor align(_model.state.accel.adc, _model.config.gyro.align); _model.state.accel.adc = _model.state.boardAlignment.apply(_model.state.accel.adc); - for(size_t i = 0; i < 3; i++) + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { if(_model.config.debug.mode == DEBUG_ACCELEROMETER) { diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index 6e6b941e..4413f093 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -30,7 +30,8 @@ int GyroSensor::begin() _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.gyro.dynamicFilter.width > 0 && _model.state.loopTimer.rate >= DynamicFilterConfig::MIN_FREQ; + _dyn_notch_count = std::min((size_t)_model.config.gyro.dynamicFilter.count, DYN_NOTCH_COUNT_MAX); + _dyn_notch_enabled = _model.isActive(FEATURE_DYNAMIC_FILTER) && _dyn_notch_count > 0 && _model.state.loopTimer.rate >= DynamicFilterConfig::MIN_FREQ; _dyn_notch_debug = _model.config.debug.mode == DEBUG_FFT_FREQ || _model.config.debug.mode == DEBUG_FFT_TIME; _rpm_enabled = _model.config.gyro.rpmFilter.harmonics > 0 && _model.config.output.dshotTelemetry; @@ -44,7 +45,7 @@ int GyroSensor::begin() { _rpm_weights[i] = Math::clamp(0.01f * _model.config.gyro.rpmFilter.weights[i], 0.0f, 1.0f); } - for (size_t i = 0; i < 3; i++) + for (size_t i = 0; i < AXIS_COUNT_RPY; i++) { #ifdef ESPFC_DSP _fft[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.gyro.dynamicFilter, i); @@ -95,7 +96,7 @@ int FAST_CODE_ATTR GyroSensor::filter() _model.state.gyro.scaled = _model.state.gyro.adc; // must be after calibration - for (size_t i = 0; i < 3; ++i) + for (size_t i = 0; i < AXIS_COUNT_RPY; ++i) { _model.setDebug(DEBUG_GYRO_RAW, i, _model.state.gyro.raw[i]); _model.setDebug(DEBUG_GYRO_SCALED, i, lrintf(Math::toDeg(_model.state.gyro.scaled[i]))); @@ -133,13 +134,13 @@ int FAST_CODE_ATTR GyroSensor::filter() if (_dyn_notch_enabled) { - for (size_t p = 0; p < (size_t)_model.config.gyro.dynamicFilter.width; p++) + for (size_t p = 0; p < _dyn_notch_count; p++) { _model.state.gyro.adc = Utils::applyFilter(_model.state.gyro.dynNotchFilter[p], _model.state.gyro.adc); } } - for (size_t i = 0; i < 3; ++i) + for (size_t i = 0; i < AXIS_COUNT_RPY; ++i) { _model.setDebug(DEBUG_GYRO_FILTERED, i, lrintf(Math::toDeg(_model.state.gyro.adc[i]))); } @@ -175,7 +176,7 @@ void FAST_CODE_ATTR GyroSensor::rpmFilterUpdate() weight *= freqMargin * _rpm_fade_inv; } _model.state.gyro.rpmFilter[_rpm_motor_index][n][0].reconfigure(freq, freq, _rpm_q, weight); - for (size_t i = 1; i < 3; ++i) + for (size_t i = 1; i < AXIS_COUNT_RPY; ++i) { // copy coefs from roll to pitch and yaw _model.state.gyro.rpmFilter[_rpm_motor_index][n][i].reconfigure(_model.state.gyro.rpmFilter[_rpm_motor_index][n][0]); @@ -204,7 +205,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() bool feed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0; bool update = _model.state.gyro.dynamicFilterTimer.check(); - for (size_t i = 0; i < 3; ++i) + for (size_t i = 0; i < AXIS_COUNT_RPY; ++i) { #ifdef ESPFC_DSP (void)update; @@ -214,8 +215,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() int status = _fft[i].update(_model.state.gyro.dynNotch[i]); if (_model.config.debug.mode == DEBUG_FFT_TIME) { - if (i == 0) - _model.state.debug[0] = status; + if (i == 0) _model.state.debug[0] = status; _model.state.debug[i + 1] = micros() - startTime; } if (_model.config.debug.mode == DEBUG_FFT_FREQ && i == _model.config.debug.axis) @@ -227,7 +227,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() } if (_dyn_notch_enabled && status) { - for (size_t p = 0; p < (size_t)_model.config.gyro.dynamicFilter.width; p++) + for (size_t p = 0; p < _dyn_notch_count; p++) { float freq = _fft[i].peaks[p].freq; if (freq >= _model.config.gyro.dynamicFilter.min_freq && freq <= _model.config.gyro.dynamicFilter.max_freq) @@ -245,22 +245,19 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() float freq = _freqAnalyzer[i].freq; if (_model.config.debug.mode == DEBUG_FFT_TIME) { - if (i == 0) - _model.state.debug[0] = update; + if (i == 0) _model.state.debug[0] = update; _model.state.debug[i + 1] = micros() - startTime; } if (_model.config.debug.mode == DEBUG_FFT_FREQ) { - if (update) - _model.state.debug[i] = lrintf(freq); - if (i == _model.config.debug.axis) - _model.state.debug[3] = lrintf(Math::toDeg(_model.state.gyro.dynNotch[i])); + if (update) _model.state.debug[i] = lrintf(freq); + if (i == _model.config.debug.axis) _model.state.debug[3] = lrintf(Math::toDeg(_model.state.gyro.dynNotch[i])); } if (_dyn_notch_enabled && update) { if (freq >= _model.config.gyro.dynamicFilter.min_freq && freq <= _model.config.gyro.dynamicFilter.max_freq) { - for (size_t p = 0; p < (size_t)_model.config.gyro.dynamicFilter.width; p++) + for (size_t p = 0; p < _dyn_notch_count; p++) { size_t x = (p + i) % 3; int harmonic = (p / 3) + 1; diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index 43bdcfcc..57f7a613 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -35,6 +35,7 @@ class GyroSensor: public BaseSensor Math::Sma _sma; Math::Sma _dyn_notch_sma; size_t _dyn_notch_denom; + size_t _dyn_notch_count; bool _dyn_notch_enabled; bool _dyn_notch_debug; bool _rpm_enabled; diff --git a/lib/Espfc/src/Sensor/MagSensor.h b/lib/Espfc/src/Sensor/MagSensor.h index 1f3c90e5..01f00c78 100644 --- a/lib/Espfc/src/Sensor/MagSensor.h +++ b/lib/Espfc/src/Sensor/MagSensor.h @@ -119,7 +119,7 @@ class MagSensor: public BaseSensor void updateCalibration() { - for(int i = 0; i < 3; i++) + for(int i = 0; i < AXIS_COUNT_RPY; i++) { if(_model.state.mag.adc[i] < _model.state.mag.calibrationMin[i]) _model.state.mag.calibrationMin.set(i, _model.state.mag.adc[i]); if(_model.state.mag.adc[i] > _model.state.mag.calibrationMax[i]) _model.state.mag.calibrationMax.set(i, _model.state.mag.adc[i]); @@ -132,7 +132,7 @@ class MagSensor: public BaseSensor // verify calibration data and find biggest range float maxRange = -1; - for(int i = 0; i < 3; i++) + for(int i = 0; i < AXIS_COUNT_RPY; i++) { if(_model.state.mag.calibrationMin[i] > -EPSILON) return; if(_model.state.mag.calibrationMax[i] < EPSILON) return; @@ -148,7 +148,7 @@ class MagSensor: public BaseSensor VectorFloat scale(1.f, 1.f, 1.f); VectorFloat offset(0.f, 0.f, 0.f); - for (int i = 0; i < 3; i++) + for (int i = 0; i < AXIS_COUNT_RPY; i++) { const float range = (_model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]); const float bias = (_model.state.mag.calibrationMax[i] + _model.state.mag.calibrationMin[i]) * 0.5f; From a3be921ab0d62db01aba89241e10e75975ac6f71 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Wed, 30 Oct 2024 23:40:44 +0100 Subject: [PATCH 37/68] move Controller to Control namespace --- lib/Espfc/src/{ => Control}/Controller.cpp | 6 +++++- lib/Espfc/src/{ => Control}/Controller.h | 7 ++++--- lib/Espfc/src/Espfc.h | 4 ++-- test/test_fc/test_fc.cpp | 3 ++- 4 files changed, 13 insertions(+), 7 deletions(-) rename lib/Espfc/src/{ => Control}/Controller.cpp (99%) rename lib/Espfc/src/{ => Control}/Controller.h (87%) diff --git a/lib/Espfc/src/Controller.cpp b/lib/Espfc/src/Control/Controller.cpp similarity index 99% rename from lib/Espfc/src/Controller.cpp rename to lib/Espfc/src/Control/Controller.cpp index e661042a..d57f9841 100644 --- a/lib/Espfc/src/Controller.cpp +++ b/lib/Espfc/src/Control/Controller.cpp @@ -1,8 +1,10 @@ -#include "Controller.h" +#include "Control/Controller.h" #include "Math/Utils.h" namespace Espfc { +namespace Control { + Controller::Controller(Model& model): _model(model) {} int Controller::begin() @@ -185,3 +187,5 @@ float Controller::calculateSetpointRate(int axis, float input) } } + +} diff --git a/lib/Espfc/src/Controller.h b/lib/Espfc/src/Control/Controller.h similarity index 87% rename from lib/Espfc/src/Controller.h rename to lib/Espfc/src/Control/Controller.h index e24c6782..87432151 100644 --- a/lib/Espfc/src/Controller.h +++ b/lib/Espfc/src/Control/Controller.h @@ -1,11 +1,12 @@ -#ifndef _ESPFC_CONTROLLER_H_ -#define _ESPFC_CONTROLLER_H_ +#pragma once #include "Model.h" #include "Control/Rates.h" namespace Espfc { +namespace Control { + class Controller { public: @@ -30,4 +31,4 @@ class Controller } -#endif +} diff --git a/lib/Espfc/src/Espfc.h b/lib/Espfc/src/Espfc.h index 43fa0e7d..da90dabf 100644 --- a/lib/Espfc/src/Espfc.h +++ b/lib/Espfc/src/Espfc.h @@ -2,7 +2,7 @@ #include "Model.h" #include "Hardware.h" -#include "Controller.h" +#include "Control/Controller.h" #include "Input.h" #include "Actuator.h" #include "SensorManager.h" @@ -32,7 +32,7 @@ class Espfc private: Model _model; Hardware _hardware; - Controller _controller; + Control::Controller _controller; TelemetryManager _telemetry; Input _input; Actuator _actuator; diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index e8d7d6fa..3ea650e2 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -3,12 +3,13 @@ #include #include "Timer.h" #include "Model.h" -#include "Controller.h" +#include "Control/Controller.h" #include "Actuator.h" #include "Output/Mixer.h" using namespace fakeit; using namespace Espfc; +using namespace Espfc::Control; /*void setUp(void) { From cbedb0ee45f3c1f3e054ebec7ecfe91f698e3eec Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 31 Oct 2024 00:08:38 +0100 Subject: [PATCH 38/68] move Fusion to Control namespace --- lib/Espfc/src/{ => Control}/Fusion.cpp | 6 +++++- lib/Espfc/src/{ => Control}/Fusion.h | 8 ++++++-- lib/Espfc/src/SensorManager.h | 4 ++-- 3 files changed, 13 insertions(+), 5 deletions(-) rename lib/Espfc/src/{ => Control}/Fusion.cpp (99%) rename lib/Espfc/src/{ => Control}/Fusion.h (89%) diff --git a/lib/Espfc/src/Fusion.cpp b/lib/Espfc/src/Control/Fusion.cpp similarity index 99% rename from lib/Espfc/src/Fusion.cpp rename to lib/Espfc/src/Control/Fusion.cpp index 5c2eae12..ab057c2e 100644 --- a/lib/Espfc/src/Fusion.cpp +++ b/lib/Espfc/src/Control/Fusion.cpp @@ -1,8 +1,10 @@ -#include "Fusion.h" +#include "Control/Fusion.h" #include "Utils/MemoryHelper.h" namespace Espfc { +namespace Control { + Fusion::Fusion(Model& model): _model(model), _first(true) {} int Fusion::begin() @@ -330,3 +332,5 @@ void FAST_CODE_ATTR Fusion::mahonyFusion() } } + +} diff --git a/lib/Espfc/src/Fusion.h b/lib/Espfc/src/Control/Fusion.h similarity index 89% rename from lib/Espfc/src/Fusion.h rename to lib/Espfc/src/Control/Fusion.h index dc2ead12..61aedc83 100644 --- a/lib/Espfc/src/Fusion.h +++ b/lib/Espfc/src/Control/Fusion.h @@ -1,11 +1,13 @@ #pragma once #include "Model.h" -#include "Madgwick.h" -#include "Mahony.h" +#include +#include namespace Espfc { +namespace Control { + class Fusion { public: @@ -35,3 +37,5 @@ class Fusion }; } + +} diff --git a/lib/Espfc/src/SensorManager.h b/lib/Espfc/src/SensorManager.h index 5e5de836..a1fbceef 100644 --- a/lib/Espfc/src/SensorManager.h +++ b/lib/Espfc/src/SensorManager.h @@ -1,7 +1,7 @@ #pragma once #include "Model.h" -#include "Fusion.h" +#include "Control/Fusion.h" #include "Sensor/GyroSensor.h" #include "Sensor/AccelSensor.h" #include "Sensor/MagSensor.h" @@ -32,7 +32,7 @@ class SensorManager Sensor::MagSensor _mag; Sensor::BaroSensor _baro; Sensor::VoltageSensor _voltage; - Fusion _fusion; + Control::Fusion _fusion; bool _fusionUpdate; }; From d8bb20beea9231381f901b4b6c514077e59b9a05 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 31 Oct 2024 00:26:05 +0100 Subject: [PATCH 39/68] move Actuator to Control namespace and split --- lib/Espfc/src/Actuator.h | 265 ----------------------------- lib/Espfc/src/Control/Actuator.cpp | 254 +++++++++++++++++++++++++++ lib/Espfc/src/Control/Actuator.h | 36 ++++ lib/Espfc/src/Espfc.h | 4 +- test/test_fc/test_fc.cpp | 2 +- 5 files changed, 293 insertions(+), 268 deletions(-) delete mode 100644 lib/Espfc/src/Actuator.h create mode 100644 lib/Espfc/src/Control/Actuator.cpp create mode 100644 lib/Espfc/src/Control/Actuator.h diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h deleted file mode 100644 index 67657422..00000000 --- a/lib/Espfc/src/Actuator.h +++ /dev/null @@ -1,265 +0,0 @@ -#ifndef _ESPFC_ACTUATOR_H_ -#define _ESPFC_ACTUATOR_H_ - -#include "Model.h" -#include "Math/Utils.h" - -namespace Espfc { - -class Actuator -{ - public: - Actuator(Model& model): _model(model) {} - - int begin() - { - _model.state.mode.mask = 0; - _model.state.mode.maskPrev = 0; - _model.state.mode.maskPresent = 0; - _model.state.mode.maskSwitch = 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.mode.maskPresent |= 1 << c.id; - } - _model.state.mode.airmodeAllowed = false; - _model.state.mode.rescueConfigMode = RESCUE_CONFIG_PENDING; - return 1; - } - - int update() - { - uint32_t startTime = micros(); - Stats::Measure(_model.state.stats, COUNTER_ACTUATOR); - updateArmingDisabled(); - updateModeMask(); - updateArmed(); - updateAirMode(); - updateScaler(); - updateBuzzer(); - updateDynLpf(); - updateRescueConfig(); - - if(_model.config.debug.mode == DEBUG_PIDLOOP) - { - _model.state.debug[4] = micros() - startTime; - } - - return 1; - } - - #ifndef UNIT_TEST - private: - #endif - - void updateScaler() - { - for(size_t i = 0; i < SCALER_COUNT; i++) - { - uint32_t mode = _model.config.scaler[i].dimension; - if(!mode) continue; - - short c = _model.config.scaler[i].channel; - if(c < AXIS_AUX_1) continue; - - float v = _model.state.input.ch[c]; - float min = _model.config.scaler[i].minScale * 0.01f; - float max = _model.config.scaler[i].maxScale * 0.01f; - float scale = Math::map3(v, -1.f, 0.f, 1.f, min, min < 0 ? 0.f : 1.f, max); - for(size_t x = 0; x < AXIS_COUNT_RPYT; x++) - { - if( - (x == AXIS_ROLL && (mode & ACT_AXIS_ROLL)) || - (x == AXIS_PITCH && (mode & ACT_AXIS_PITCH)) || - (x == AXIS_YAW && (mode & ACT_AXIS_YAW)) || - (x == AXIS_THRUST && (mode & ACT_AXIS_THRUST)) - ) - { - - if(mode & ACT_INNER_P) _model.state.innerPid[x].pScale = scale; - if(mode & ACT_INNER_I) _model.state.innerPid[x].iScale = scale; - if(mode & ACT_INNER_D) _model.state.innerPid[x].dScale = scale; - if(mode & ACT_INNER_F) _model.state.innerPid[x].fScale = scale; - - if(mode & ACT_OUTER_P) _model.state.outerPid[x].pScale = scale; - if(mode & ACT_OUTER_I) _model.state.outerPid[x].iScale = scale; - if(mode & ACT_OUTER_D) _model.state.outerPid[x].dScale = scale; - if(mode & ACT_OUTER_F) _model.state.outerPid[x].fScale = scale; - - } - } - } - } - - void updateArmingDisabled() - { - int errors = _model.state.i2cErrorDelta; - _model.state.i2cErrorDelta = 0; - - _model.setArmingDisabled(ARMING_DISABLED_NO_GYRO, !_model.state.gyro.present || errors); - _model.setArmingDisabled(ARMING_DISABLED_FAILSAFE, _model.state.failsafe.phase != FC_FAILSAFE_IDLE); - _model.setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE, _model.state.input.rxLoss || _model.state.input.rxFailSafe); - _model.setArmingDisabled(ARMING_DISABLED_THROTTLE, !_model.isThrottleLow()); - _model.setArmingDisabled(ARMING_DISABLED_CALIBRATING, _model.calibrationActive()); - _model.setArmingDisabled(ARMING_DISABLED_MOTOR_PROTOCOL, _model.config.output.protocol == ESC_PROTOCOL_DISABLED); - _model.setArmingDisabled(ARMING_DISABLED_REBOOT_REQUIRED, _model.state.mode.rescueConfigMode == RESCUE_CONFIG_ACTIVE); - } - - void updateModeMask() - { - uint32_t newMask = 0; - for(size_t i = 0; i < ACTUATOR_CONDITIONS; i++) - { - ActuatorCondition * c = &_model.config.conditions[i]; - if(!(c->min < c->max)) continue; // inactive - - int16_t min = c->min; // * 25 + 900; - int16_t max = c->max; // * 25 + 900; - size_t ch = c->ch; // + AXIS_AUX_1; - if(ch < AXIS_AUX_1 || ch >= AXIS_COUNT) continue; // invalid channel - - int16_t val = _model.state.input.us[ch]; - if(val > min && val < max) - { - newMask |= 1 << c->id; - } - } - - _model.updateSwitchActive(newMask); - - _model.setArmingDisabled(ARMING_DISABLED_FAILSAFE, _model.state.failsafe.phase != FC_FAILSAFE_IDLE); - _model.setArmingDisabled(ARMING_DISABLED_BOXFAILSAFE, _model.isSwitchActive(MODE_FAILSAFE)); - _model.setArmingDisabled(ARMING_DISABLED_ARM_SWITCH, _model.armingDisabled() && _model.isSwitchActive(MODE_ARMED)); - - if(_model.state.failsafe.phase != FC_FAILSAFE_IDLE) - { - newMask |= (1 << MODE_FAILSAFE); - } - - for(size_t i = 0; i < MODE_COUNT; i++) - { - bool newVal = newMask & (1 << i); - bool oldVal = _model.state.mode.mask & (1 << i); - if(newVal == oldVal) continue; // mode unchanged - if(newVal && !canActivateMode((FlightMode)i)) - { - newMask &= ~(1 << i); // block activation, clear bit - } - } - - _model.updateModes(newMask); - } - - bool canActivateMode(FlightMode mode) - { - switch(mode) - { - case MODE_ARMED: - return !_model.armingDisabled() && _model.isThrottleLow(); - case MODE_ANGLE: - return _model.accelActive(); - case MODE_AIRMODE: - return _model.state.mode.airmodeAllowed; - default: - return true; - } - } - - void updateArmed() - { - if((_model.hasChanged(MODE_ARMED))) - { - bool armed = _model.isModeActive(MODE_ARMED); - if(armed) - { - _model.state.mode.disarmReason = DISARM_REASON_SYSTEM; - _model.state.mode.rescueConfigMode = RESCUE_CONFIG_DISABLED; - } - else if(!armed && _model.state.mode.disarmReason == DISARM_REASON_SYSTEM) - { - _model.state.mode.disarmReason = DISARM_REASON_SWITCH; - } - } - } - - void updateAirMode() - { - bool armed = _model.isModeActive(MODE_ARMED); - if(!armed) - { - _model.state.mode.airmodeAllowed = false; - } - if(armed && !_model.state.mode.airmodeAllowed && _model.state.input.us[AXIS_THRUST] > 1400) // activate airmode in the air - { - _model.state.mode.airmodeAllowed = true; - } - } - - void updateBuzzer() - { - if(_model.isModeActive(MODE_FAILSAFE)) - { - _model.state.buzzer.play(BUZZER_RX_LOST); - } - if(_model.state.battery.warn(_model.config.vbat.cellWarning)) - { - _model.state.buzzer.play(BUZZER_BAT_LOW); - } - if(_model.isModeActive(MODE_BUZZER)) - { - _model.state.buzzer.play(BUZZER_RX_SET); - } - if((_model.hasChanged(MODE_ARMED))) - { - _model.state.buzzer.push(_model.isModeActive(MODE_ARMED) ? BUZZER_ARMING : BUZZER_DISARMING); - } - } - - void updateDynLpf() - { - return; // temporary disable - int scale = Math::clamp((int)_model.state.input.us[AXIS_THRUST], 1000, 2000); - if(_model.config.gyro.dynLpfFilter.cutoff > 0) { - int gyroFreq = Math::map(scale, 1000, 2000, _model.config.gyro.dynLpfFilter.cutoff, _model.config.gyro.dynLpfFilter.freq); - for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { - _model.state.gyro.filter[i].reconfigure(gyroFreq); - } - } - if(_model.config.dterm.dynLpfFilter.cutoff > 0) { - int dtermFreq = Math::map(scale, 1000, 2000, _model.config.dterm.dynLpfFilter.cutoff, _model.config.dterm.dynLpfFilter.freq); - for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { - _model.state.innerPid[i].dtermFilter.reconfigure(dtermFreq); - } - } - } - - void updateRescueConfig() - { - switch(_model.state.mode.rescueConfigMode) - { - case RESCUE_CONFIG_PENDING: - // if some rc frames are received, disable to prevent activate later - if(_model.state.input.frameCount > 100) - { - _model.state.mode.rescueConfigMode = RESCUE_CONFIG_DISABLED; - } - if(_model.state.failsafe.phase != FC_FAILSAFE_IDLE && _model.config.rescueConfigDelay > 0 && millis() > _model.config.rescueConfigDelay * 1000) - { - _model.state.mode.rescueConfigMode = RESCUE_CONFIG_ACTIVE; - } - break; - case RESCUE_CONFIG_ACTIVE: - case RESCUE_CONFIG_DISABLED: - // nothing to do here - break; - } - } - - Model& _model; -}; - -} - -#endif diff --git a/lib/Espfc/src/Control/Actuator.cpp b/lib/Espfc/src/Control/Actuator.cpp new file mode 100644 index 00000000..0cff63f7 --- /dev/null +++ b/lib/Espfc/src/Control/Actuator.cpp @@ -0,0 +1,254 @@ +#include "Control/Actuator.h" +#include "Math/Utils.h" + +namespace Espfc { + +namespace Control { + +Actuator::Actuator(Model& model): _model(model) {} + +int Actuator::begin() +{ + _model.state.mode.mask = 0; + _model.state.mode.maskPrev = 0; + _model.state.mode.maskPresent = 0; + _model.state.mode.maskSwitch = 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.mode.maskPresent |= 1 << c.id; + } + _model.state.mode.airmodeAllowed = false; + _model.state.mode.rescueConfigMode = RESCUE_CONFIG_PENDING; + return 1; +} + +int Actuator::update() +{ + uint32_t startTime = micros(); + Stats::Measure(_model.state.stats, COUNTER_ACTUATOR); + updateArmingDisabled(); + updateModeMask(); + updateArmed(); + updateAirMode(); + updateScaler(); + updateBuzzer(); + updateDynLpf(); + updateRescueConfig(); + + if(_model.config.debug.mode == DEBUG_PIDLOOP) + { + _model.state.debug[4] = micros() - startTime; + } + + return 1; +} + +void Actuator::updateScaler() +{ + for(size_t i = 0; i < SCALER_COUNT; i++) + { + uint32_t mode = _model.config.scaler[i].dimension; + if(!mode) continue; + + short c = _model.config.scaler[i].channel; + if(c < AXIS_AUX_1) continue; + + float v = _model.state.input.ch[c]; + float min = _model.config.scaler[i].minScale * 0.01f; + float max = _model.config.scaler[i].maxScale * 0.01f; + float scale = Math::map3(v, -1.f, 0.f, 1.f, min, min < 0 ? 0.f : 1.f, max); + for(size_t x = 0; x < AXIS_COUNT_RPYT; x++) + { + if( + (x == AXIS_ROLL && (mode & ACT_AXIS_ROLL)) || + (x == AXIS_PITCH && (mode & ACT_AXIS_PITCH)) || + (x == AXIS_YAW && (mode & ACT_AXIS_YAW)) || + (x == AXIS_THRUST && (mode & ACT_AXIS_THRUST)) + ) + { + + if(mode & ACT_INNER_P) _model.state.innerPid[x].pScale = scale; + if(mode & ACT_INNER_I) _model.state.innerPid[x].iScale = scale; + if(mode & ACT_INNER_D) _model.state.innerPid[x].dScale = scale; + if(mode & ACT_INNER_F) _model.state.innerPid[x].fScale = scale; + + if(mode & ACT_OUTER_P) _model.state.outerPid[x].pScale = scale; + if(mode & ACT_OUTER_I) _model.state.outerPid[x].iScale = scale; + if(mode & ACT_OUTER_D) _model.state.outerPid[x].dScale = scale; + if(mode & ACT_OUTER_F) _model.state.outerPid[x].fScale = scale; + + } + } + } +} + +void Actuator::updateArmingDisabled() +{ + int errors = _model.state.i2cErrorDelta; + _model.state.i2cErrorDelta = 0; + + _model.setArmingDisabled(ARMING_DISABLED_NO_GYRO, !_model.state.gyro.present || errors); + _model.setArmingDisabled(ARMING_DISABLED_FAILSAFE, _model.state.failsafe.phase != FC_FAILSAFE_IDLE); + _model.setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE, _model.state.input.rxLoss || _model.state.input.rxFailSafe); + _model.setArmingDisabled(ARMING_DISABLED_THROTTLE, !_model.isThrottleLow()); + _model.setArmingDisabled(ARMING_DISABLED_CALIBRATING, _model.calibrationActive()); + _model.setArmingDisabled(ARMING_DISABLED_MOTOR_PROTOCOL, _model.config.output.protocol == ESC_PROTOCOL_DISABLED); + _model.setArmingDisabled(ARMING_DISABLED_REBOOT_REQUIRED, _model.state.mode.rescueConfigMode == RESCUE_CONFIG_ACTIVE); +} + +void Actuator::updateModeMask() +{ + uint32_t newMask = 0; + for(size_t i = 0; i < ACTUATOR_CONDITIONS; i++) + { + ActuatorCondition * c = &_model.config.conditions[i]; + if(!(c->min < c->max)) continue; // inactive + + int16_t min = c->min; // * 25 + 900; + int16_t max = c->max; // * 25 + 900; + size_t ch = c->ch; // + AXIS_AUX_1; + if(ch < AXIS_AUX_1 || ch >= AXIS_COUNT) continue; // invalid channel + + int16_t val = _model.state.input.us[ch]; + if(val > min && val < max) + { + newMask |= 1 << c->id; + } + } + + _model.updateSwitchActive(newMask); + + _model.setArmingDisabled(ARMING_DISABLED_FAILSAFE, _model.state.failsafe.phase != FC_FAILSAFE_IDLE); + _model.setArmingDisabled(ARMING_DISABLED_BOXFAILSAFE, _model.isSwitchActive(MODE_FAILSAFE)); + _model.setArmingDisabled(ARMING_DISABLED_ARM_SWITCH, _model.armingDisabled() && _model.isSwitchActive(MODE_ARMED)); + + if(_model.state.failsafe.phase != FC_FAILSAFE_IDLE) + { + newMask |= (1 << MODE_FAILSAFE); + } + + for(size_t i = 0; i < MODE_COUNT; i++) + { + bool newVal = newMask & (1 << i); + bool oldVal = _model.state.mode.mask & (1 << i); + if(newVal == oldVal) continue; // mode unchanged + if(newVal && !canActivateMode((FlightMode)i)) + { + newMask &= ~(1 << i); // block activation, clear bit + } + } + + _model.updateModes(newMask); +} + +bool Actuator::canActivateMode(FlightMode mode) +{ + switch(mode) + { + case MODE_ARMED: + return !_model.armingDisabled() && _model.isThrottleLow(); + case MODE_ANGLE: + return _model.accelActive(); + case MODE_AIRMODE: + return _model.state.mode.airmodeAllowed; + default: + return true; + } +} + +void Actuator::updateArmed() +{ + if((_model.hasChanged(MODE_ARMED))) + { + bool armed = _model.isModeActive(MODE_ARMED); + if(armed) + { + _model.state.mode.disarmReason = DISARM_REASON_SYSTEM; + _model.state.mode.rescueConfigMode = RESCUE_CONFIG_DISABLED; + } + else if(!armed && _model.state.mode.disarmReason == DISARM_REASON_SYSTEM) + { + _model.state.mode.disarmReason = DISARM_REASON_SWITCH; + } + } +} + +void Actuator::updateAirMode() +{ + bool armed = _model.isModeActive(MODE_ARMED); + if(!armed) + { + _model.state.mode.airmodeAllowed = false; + } + if(armed && !_model.state.mode.airmodeAllowed && _model.state.input.us[AXIS_THRUST] > 1400) // activate airmode in the air + { + _model.state.mode.airmodeAllowed = true; + } +} + +void Actuator::updateBuzzer() +{ + if(_model.isModeActive(MODE_FAILSAFE)) + { + _model.state.buzzer.play(BUZZER_RX_LOST); + } + if(_model.state.battery.warn(_model.config.vbat.cellWarning)) + { + _model.state.buzzer.play(BUZZER_BAT_LOW); + } + if(_model.isModeActive(MODE_BUZZER)) + { + _model.state.buzzer.play(BUZZER_RX_SET); + } + if((_model.hasChanged(MODE_ARMED))) + { + _model.state.buzzer.push(_model.isModeActive(MODE_ARMED) ? BUZZER_ARMING : BUZZER_DISARMING); + } +} + +void Actuator::updateDynLpf() +{ + return; // temporary disable + int scale = Math::clamp((int)_model.state.input.us[AXIS_THRUST], 1000, 2000); + if(_model.config.gyro.dynLpfFilter.cutoff > 0) { + int gyroFreq = Math::map(scale, 1000, 2000, _model.config.gyro.dynLpfFilter.cutoff, _model.config.gyro.dynLpfFilter.freq); + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { + _model.state.gyro.filter[i].reconfigure(gyroFreq); + } + } + if(_model.config.dterm.dynLpfFilter.cutoff > 0) { + int dtermFreq = Math::map(scale, 1000, 2000, _model.config.dterm.dynLpfFilter.cutoff, _model.config.dterm.dynLpfFilter.freq); + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { + _model.state.innerPid[i].dtermFilter.reconfigure(dtermFreq); + } + } +} + +void Actuator::updateRescueConfig() +{ + switch(_model.state.mode.rescueConfigMode) + { + case RESCUE_CONFIG_PENDING: + // if some rc frames are received, disable to prevent activate later + if(_model.state.input.frameCount > 100) + { + _model.state.mode.rescueConfigMode = RESCUE_CONFIG_DISABLED; + } + if(_model.state.failsafe.phase != FC_FAILSAFE_IDLE && _model.config.rescueConfigDelay > 0 && millis() > _model.config.rescueConfigDelay * 1000) + { + _model.state.mode.rescueConfigMode = RESCUE_CONFIG_ACTIVE; + } + break; + case RESCUE_CONFIG_ACTIVE: + case RESCUE_CONFIG_DISABLED: + // nothing to do here + break; + } +} + +} + +} diff --git a/lib/Espfc/src/Control/Actuator.h b/lib/Espfc/src/Control/Actuator.h new file mode 100644 index 00000000..5d80c973 --- /dev/null +++ b/lib/Espfc/src/Control/Actuator.h @@ -0,0 +1,36 @@ +#pragma once + +#include "Model.h" + +namespace Espfc { + +namespace Control { + +class Actuator +{ + public: + Actuator(Model& model); + + int begin(); + int update(); + + #ifndef UNIT_TEST + private: + #endif + + void updateScaler(); + void updateArmingDisabled(); + void updateModeMask(); + bool canActivateMode(FlightMode mode); + void updateArmed(); + void updateAirMode(); + void updateBuzzer(); + void updateDynLpf(); + void updateRescueConfig(); + + Model& _model; +}; + +} + +} diff --git a/lib/Espfc/src/Espfc.h b/lib/Espfc/src/Espfc.h index da90dabf..59700fca 100644 --- a/lib/Espfc/src/Espfc.h +++ b/lib/Espfc/src/Espfc.h @@ -4,7 +4,7 @@ #include "Hardware.h" #include "Control/Controller.h" #include "Input.h" -#include "Actuator.h" +#include "Control/Actuator.h" #include "SensorManager.h" #include "TelemetryManager.h" #include "SerialManager.h" @@ -35,7 +35,7 @@ class Espfc Control::Controller _controller; TelemetryManager _telemetry; Input _input; - Actuator _actuator; + Control::Actuator _actuator; SensorManager _sensor; Output::Mixer _mixer; Blackbox::Blackbox _blackbox; diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index 3ea650e2..2dca3cfe 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -4,7 +4,7 @@ #include "Timer.h" #include "Model.h" #include "Control/Controller.h" -#include "Actuator.h" +#include "Control/Actuator.h" #include "Output/Mixer.h" using namespace fakeit; From e816b9104f247cb8a067141608b948ab162cd13b Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 31 Oct 2024 00:38:25 +0100 Subject: [PATCH 40/68] AccelSensor split --- lib/Espfc/src/Sensor/AccelSensor.cpp | 112 +++++++++++++++++++++++++++ lib/Espfc/src/Sensor/AccelSensor.h | 112 +++------------------------ 2 files changed, 121 insertions(+), 103 deletions(-) create mode 100644 lib/Espfc/src/Sensor/AccelSensor.cpp diff --git a/lib/Espfc/src/Sensor/AccelSensor.cpp b/lib/Espfc/src/Sensor/AccelSensor.cpp new file mode 100644 index 00000000..432f8f0a --- /dev/null +++ b/lib/Espfc/src/Sensor/AccelSensor.cpp @@ -0,0 +1,112 @@ +#include "Sensor/AccelSensor.h" +#include "Utils/FilterHelper.h" + +namespace Espfc { + +namespace Sensor { + +AccelSensor::AccelSensor(Model& model): _model(model) {} + +int AccelSensor::begin() +{ + _model.state.accel.adc.z = ACCEL_G; + + _gyro = _model.state.gyro.dev; + if(!_gyro) return 0; + + _model.state.accel.scale = 16.f * ACCEL_G / 32768.f; + + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) + { + _filter[i].begin(FilterConfig(FILTER_FIR2, 1), _model.state.accel.timer.rate); + } + + _model.state.accel.biasAlpha = _model.state.accel.timer.rate > 0 ? 5.0f / _model.state.accel.timer.rate : 0; + _model.state.accel.calibrationState = CALIBRATION_IDLE; + + _model.logger.info().log(F("ACCEL INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_gyro->getAddress()).log(_model.state.accel.timer.rate).log(_model.state.accel.timer.interval).logln(_model.state.accel.present); + + return 1; +} + +int FAST_CODE_ATTR AccelSensor::update() +{ + int status = read(); + + if (status) filter(); + + return status; +} + +int FAST_CODE_ATTR AccelSensor::read() +{ + if(!_model.accelActive()) return 0; + + //if(!_model.state.accel.timer.check()) return 0; + + Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_READ); + _gyro->readAccel(_model.state.accel.raw); + + return 1; +} + +int FAST_CODE_ATTR AccelSensor::filter() +{ + if(!_model.accelActive()) return 0; + + Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_FILTER); + + _model.state.accel.adc = (VectorFloat)_model.state.accel.raw * _model.state.accel.scale; + + align(_model.state.accel.adc, _model.config.gyro.align); + _model.state.accel.adc = _model.state.boardAlignment.apply(_model.state.accel.adc); + + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) + { + if(_model.config.debug.mode == DEBUG_ACCELEROMETER) + { + _model.state.debug[i] = _model.state.accel.raw[i]; + } + _model.state.accel.adc.set(i, _filter[i].update(_model.state.accel.adc[i])); + _model.state.accel.adc.set(i, _model.state.accel.filter[i].update(_model.state.accel.adc[i])); + } + + calibrate(); + + return 1; +} + +void FAST_CODE_ATTR AccelSensor::calibrate() +{ + switch(_model.state.accel.calibrationState) + { + case CALIBRATION_IDLE: + _model.state.accel.adc -= _model.state.accel.bias; + break; + case CALIBRATION_START: + _model.state.accel.bias = VectorFloat(0, 0, ACCEL_G); + _model.state.accel.biasSamples = 2 * _model.state.accel.timer.rate; + _model.state.accel.calibrationState = CALIBRATION_UPDATE; + break; + case CALIBRATION_UPDATE: + _model.state.accel.bias += (_model.state.accel.adc - _model.state.accel.bias) * _model.state.accel.biasAlpha; + _model.state.accel.biasSamples--; + if(_model.state.accel.biasSamples <= 0) _model.state.accel.calibrationState = CALIBRATION_APPLY; + break; + case CALIBRATION_APPLY: + _model.state.accel.bias.z -= ACCEL_G; + _model.state.accel.calibrationState = CALIBRATION_SAVE; + break; + case CALIBRATION_SAVE: + _model.finishCalibration(); + _model.state.accel.calibrationState = CALIBRATION_IDLE; + break; + default: + _model.state.accel.calibrationState = CALIBRATION_IDLE; + break; + } +} + +} + +} diff --git a/lib/Espfc/src/Sensor/AccelSensor.h b/lib/Espfc/src/Sensor/AccelSensor.h index f9ea1bca..c67301f2 100644 --- a/lib/Espfc/src/Sensor/AccelSensor.h +++ b/lib/Espfc/src/Sensor/AccelSensor.h @@ -1,6 +1,6 @@ -#ifndef _ESPFC_SENSOR_ACCEL_SENSOR_H_ -#define _ESPFC_SENSOR_ACCEL_SENSOR_H_ +#pragma once +#include "Model.h" #include "BaseSensor.h" #include "Device/GyroDevice.h" @@ -11,115 +11,21 @@ namespace Sensor { class AccelSensor: public BaseSensor { public: - AccelSensor(Model& model): _model(model) {} + AccelSensor(Model& model); - int begin() - { - _model.state.accel.adc.z = ACCEL_G; - - _gyro = _model.state.gyro.dev; - if(!_gyro) return 0; - - _model.state.accel.scale = 16.f * ACCEL_G / 32768.f; - - for(size_t i = 0; i < AXIS_COUNT_RPY; i++) - { - _filter[i].begin(FilterConfig(FILTER_FIR2, 1), _model.state.accel.timer.rate); - } - - _model.state.accel.biasAlpha = _model.state.accel.timer.rate > 0 ? 5.0f / _model.state.accel.timer.rate : 0; - _model.state.accel.calibrationState = CALIBRATION_IDLE; - - _model.logger.info().log(F("ACCEL INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_gyro->getAddress()).log(_model.state.accel.timer.rate).log(_model.state.accel.timer.interval).logln(_model.state.accel.present); - - return 1; - } - - int update() - { - int status = read(); - - if (status) filter(); - - return status; - } - - int read() - { - if(!_model.accelActive()) return 0; - - //if(!_model.state.accel.timer.check()) return 0; - - Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_READ); - _gyro->readAccel(_model.state.accel.raw); - - return 1; - } - - int filter() - { - if(!_model.accelActive()) return 0; - - Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_FILTER); - - _model.state.accel.adc = (VectorFloat)_model.state.accel.raw * _model.state.accel.scale; - - align(_model.state.accel.adc, _model.config.gyro.align); - _model.state.accel.adc = _model.state.boardAlignment.apply(_model.state.accel.adc); - - for(size_t i = 0; i < AXIS_COUNT_RPY; i++) - { - if(_model.config.debug.mode == DEBUG_ACCELEROMETER) - { - _model.state.debug[i] = _model.state.accel.raw[i]; - } - _model.state.accel.adc.set(i, _filter[i].update(_model.state.accel.adc[i])); - _model.state.accel.adc.set(i, _model.state.accel.filter[i].update(_model.state.accel.adc[i])); - } - - calibrate(); - - return 1; - } + int begin(); + int update(); + int read(); + int filter(); private: - void calibrate() - { - switch(_model.state.accel.calibrationState) - { - case CALIBRATION_IDLE: - _model.state.accel.adc -= _model.state.accel.bias; - break; - case CALIBRATION_START: - _model.state.accel.bias = VectorFloat(0, 0, ACCEL_G); - _model.state.accel.biasSamples = 2 * _model.state.accel.timer.rate; - _model.state.accel.calibrationState = CALIBRATION_UPDATE; - break; - case CALIBRATION_UPDATE: - _model.state.accel.bias += (_model.state.accel.adc - _model.state.accel.bias) * _model.state.accel.biasAlpha; - _model.state.accel.biasSamples--; - if(_model.state.accel.biasSamples <= 0) _model.state.accel.calibrationState = CALIBRATION_APPLY; - break; - case CALIBRATION_APPLY: - _model.state.accel.bias.z -= ACCEL_G; - _model.state.accel.calibrationState = CALIBRATION_SAVE; - break; - case CALIBRATION_SAVE: - _model.finishCalibration(); - _model.state.accel.calibrationState = CALIBRATION_IDLE; - break; - default: - _model.state.accel.calibrationState = CALIBRATION_IDLE; - break; - } - } + void calibrate(); Model& _model; Device::GyroDevice * _gyro; - Filter _filter[3]; + Filter _filter[AXIS_COUNT_RPY]; }; } } -#endif \ No newline at end of file From 6abae4a601b714807638ac43b92de104dc46c7b0 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 31 Oct 2024 22:54:42 +0100 Subject: [PATCH 41/68] split baro, mag and voltage sensors --- lib/Espfc/src/Sensor/BaroSensor.cpp | 125 +++++++++++++++++++ lib/Espfc/src/Sensor/BaroSensor.h | 121 ++---------------- lib/Espfc/src/Sensor/MagSensor.cpp | 163 +++++++++++++++++++++++++ lib/Espfc/src/Sensor/MagSensor.h | 162 ++---------------------- lib/Espfc/src/Sensor/VoltageSensor.cpp | 115 +++++++++++++++++ lib/Espfc/src/Sensor/VoltageSensor.h | 114 ++--------------- 6 files changed, 429 insertions(+), 371 deletions(-) create mode 100644 lib/Espfc/src/Sensor/BaroSensor.cpp create mode 100644 lib/Espfc/src/Sensor/MagSensor.cpp create mode 100644 lib/Espfc/src/Sensor/VoltageSensor.cpp diff --git a/lib/Espfc/src/Sensor/BaroSensor.cpp b/lib/Espfc/src/Sensor/BaroSensor.cpp new file mode 100644 index 00000000..cba071bd --- /dev/null +++ b/lib/Espfc/src/Sensor/BaroSensor.cpp @@ -0,0 +1,125 @@ +#include "BaroSensor.h" + +namespace Espfc { + +namespace Sensor { + +BaroSensor::BaroSensor(Model& model): _model(model), _state(BARO_STATE_INIT), _counter(0) {} + +int BaroSensor::begin() +{ + _model.state.baro.rate = 0; + if(!_model.baroActive()) return 0; + _baro = _model.state.baro.dev; + if(!_baro) return 0; + + _baro->setMode(BARO_MODE_TEMP); + int delay = _baro->getDelay(BARO_MODE_TEMP) + _baro->getDelay(BARO_MODE_PRESS); + int toGyroRate = (delay / _model.state.gyro.timer.interval) + 1; // number of gyro readings per cycle + int interval = _model.state.gyro.timer.interval * toGyroRate; + _model.state.baro.rate = 1000000 / interval; + + _model.state.baro.altitudeBiasSamples = 2 * _model.state.baro.rate; + + // TODO: move filters to BaroState + _temperatureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baro.rate * 0.1f), _model.state.baro.rate); + _pressureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baro.rate * 0.1f), _model.state.baro.rate); + _altitudeFilter.begin(_model.config.baro.filter, _model.state.baro.rate); + + _model.logger.info().log(F("BARO INIT")).log(FPSTR(Device::BaroDevice::getName(_baro->getType()))).log(_baro->getAddress()).log(toGyroRate).log(_model.state.baro.rate).logln(_model.config.baro.filter.freq); + + return 1; +} + +int BaroSensor::update() +{ + int status = read(); + + return status; +} + +int BaroSensor::read() +{ + if(!_baro || !_model.baroActive()) return 0; + + if(_wait > micros()) return 0; + + Stats::Measure measure(_model.state.stats, COUNTER_BARO); + + if(_model.config.debug.mode == DEBUG_BARO) + { + _model.state.debug[0] = _state; + } + + switch(_state) + { + case BARO_STATE_INIT: + _baro->setMode(BARO_MODE_TEMP); + _state = BARO_STATE_TEMP_GET; + _wait = micros() + _baro->getDelay(BARO_MODE_TEMP); + return 0; + case BARO_STATE_TEMP_GET: + readTemperature(); + _baro->setMode(BARO_MODE_PRESS); + _state = BARO_STATE_PRESS_GET; + _wait = micros() + _baro->getDelay(BARO_MODE_PRESS); + _counter = 1; + return 1; + case BARO_STATE_PRESS_GET: + readPressure(); + updateAltitude(); + if(--_counter > 0) + { + _baro->setMode(BARO_MODE_PRESS); + _state = BARO_STATE_PRESS_GET; + _wait = micros() + _baro->getDelay(BARO_MODE_PRESS); + } + else + { + _baro->setMode(BARO_MODE_TEMP); + _state = BARO_STATE_TEMP_GET; + _wait = micros() + _baro->getDelay(BARO_MODE_TEMP); + } + return 1; + break; + default: + _state = BARO_STATE_INIT; + break; + } + + return 0; +} + +void BaroSensor::readTemperature() +{ + _model.state.baro.temperatureRaw = _baro->readTemperature(); + _model.state.baro.temperature = _temperatureFilter.update(_model.state.baro.temperatureRaw); +} + +void BaroSensor::readPressure() +{ + _model.state.baro.pressureRaw = _baro->readPressure(); + _model.state.baro.pressure = _pressureFilter.update(_model.state.baro.pressureRaw); +} + +void BaroSensor::updateAltitude() +{ + _model.state.baro.altitudeRaw = _altitudeFilter.update(Math::toAltitude(_model.state.baro.pressure)); + if(_model.state.baro.altitudeBiasSamples > 0) + { + _model.state.baro.altitudeBiasSamples--; + _model.state.baro.altitudeBias += (_model.state.baro.altitudeRaw - _model.state.baro.altitudeBias) * 0.2f; + } + _model.state.baro.altitude = _model.state.baro.altitudeRaw - _model.state.baro.altitudeBias; + + if(_model.config.debug.mode == DEBUG_BARO) + { + _model.state.debug[1] = lrintf(_model.state.baro.pressureRaw * 0.1f); // hPa x 10 + _model.state.debug[2] = lrintf(_model.state.baro.temperatureRaw * 100.f); // deg C x 100 + _model.state.debug[3] = lrintf(_model.state.baro.altitudeRaw * 10.f); + } +} + +} + +} diff --git a/lib/Espfc/src/Sensor/BaroSensor.h b/lib/Espfc/src/Sensor/BaroSensor.h index dbeda6df..d5f82019 100644 --- a/lib/Espfc/src/Sensor/BaroSensor.h +++ b/lib/Espfc/src/Sensor/BaroSensor.h @@ -2,6 +2,7 @@ #define _ESPFC_SENSOR_BARO_SENSOR_H_ #include "BaseSensor.h" +#include "Model.h" #include "Device/BaroDevice.h" #include "Filter.h" @@ -19,122 +20,16 @@ class BaroSensor: public BaseSensor BARO_STATE_PRESS_GET, }; - BaroSensor(Model& model): _model(model), _state(BARO_STATE_INIT), _counter(0) {} + BaroSensor(Model& model); - int begin() - { - _model.state.baro.rate = 0; - if(!_model.baroActive()) return 0; - _baro = _model.state.baro.dev; - if(!_baro) return 0; - - _baro->setMode(BARO_MODE_TEMP); - int delay = _baro->getDelay(BARO_MODE_TEMP) + _baro->getDelay(BARO_MODE_PRESS); - int toGyroRate = (delay / _model.state.gyro.timer.interval) + 1; // number of gyro readings per cycle - int interval = _model.state.gyro.timer.interval * toGyroRate; - _model.state.baro.rate = 1000000 / interval; - - _model.state.baro.altitudeBiasSamples = 2 * _model.state.baro.rate; - - // TODO: move filters to BaroState - _temperatureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baro.rate * 0.1f), _model.state.baro.rate); - _pressureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baro.rate * 0.1f), _model.state.baro.rate); - _altitudeFilter.begin(_model.config.baro.filter, _model.state.baro.rate); - - _model.logger.info().log(F("BARO INIT")).log(FPSTR(Device::BaroDevice::getName(_baro->getType()))).log(_baro->getAddress()).log(toGyroRate).log(_model.state.baro.rate).logln(_model.config.baro.filter.freq); - - return 1; - } - - int update() - { - int status = read(); - - return status; - } - - int read() - { - if(!_baro || !_model.baroActive()) return 0; - - if(_wait > micros()) return 0; - - Stats::Measure measure(_model.state.stats, COUNTER_BARO); - - if(_model.config.debug.mode == DEBUG_BARO) - { - _model.state.debug[0] = _state; - } - - switch(_state) - { - case BARO_STATE_INIT: - _baro->setMode(BARO_MODE_TEMP); - _state = BARO_STATE_TEMP_GET; - _wait = micros() + _baro->getDelay(BARO_MODE_TEMP); - return 0; - case BARO_STATE_TEMP_GET: - readTemperature(); - _baro->setMode(BARO_MODE_PRESS); - _state = BARO_STATE_PRESS_GET; - _wait = micros() + _baro->getDelay(BARO_MODE_PRESS); - _counter = 1; - return 1; - case BARO_STATE_PRESS_GET: - readPressure(); - updateAltitude(); - if(--_counter > 0) - { - _baro->setMode(BARO_MODE_PRESS); - _state = BARO_STATE_PRESS_GET; - _wait = micros() + _baro->getDelay(BARO_MODE_PRESS); - } - else - { - _baro->setMode(BARO_MODE_TEMP); - _state = BARO_STATE_TEMP_GET; - _wait = micros() + _baro->getDelay(BARO_MODE_TEMP); - } - return 1; - break; - default: - _state = BARO_STATE_INIT; - break; - } - - return 0; - } + int begin(); + int update(); + int read(); private: - void readTemperature() - { - _model.state.baro.temperatureRaw = _baro->readTemperature(); - _model.state.baro.temperature = _temperatureFilter.update(_model.state.baro.temperatureRaw); - } - - void readPressure() - { - _model.state.baro.pressureRaw = _baro->readPressure(); - _model.state.baro.pressure = _pressureFilter.update(_model.state.baro.pressureRaw); - } - - void updateAltitude() - { - _model.state.baro.altitudeRaw = _altitudeFilter.update(Math::toAltitude(_model.state.baro.pressure)); - if(_model.state.baro.altitudeBiasSamples > 0) - { - _model.state.baro.altitudeBiasSamples--; - _model.state.baro.altitudeBias += (_model.state.baro.altitudeRaw - _model.state.baro.altitudeBias) * 0.2f; - } - _model.state.baro.altitude = _model.state.baro.altitudeRaw - _model.state.baro.altitudeBias; - - if(_model.config.debug.mode == DEBUG_BARO) - { - _model.state.debug[1] = lrintf(_model.state.baro.pressureRaw * 0.1f); // hPa x 10 - _model.state.debug[2] = lrintf(_model.state.baro.temperatureRaw * 100.f); // deg C x 100 - _model.state.debug[3] = lrintf(_model.state.baro.altitudeRaw * 10.f); - } - } + void readTemperature(); + void readPressure(); + void updateAltitude(); Model& _model; Device::BaroDevice * _baro; diff --git a/lib/Espfc/src/Sensor/MagSensor.cpp b/lib/Espfc/src/Sensor/MagSensor.cpp new file mode 100644 index 00000000..f98cf639 --- /dev/null +++ b/lib/Espfc/src/Sensor/MagSensor.cpp @@ -0,0 +1,163 @@ +#include "MagSensor.h" +#include + +#ifdef abs +#undef abs +#endif + +namespace Espfc { + +namespace Sensor { + +MagSensor::MagSensor(Model& model): _model(model) {} + +int MagSensor::begin() +{ + if(!_model.magActive()) return 0; + + _mag = _model.state.mag.dev; + if(!_mag) return 0; + + if(_model.state.mag.timer.rate < 5) return 0; + + // by default use eeprom calibration settings + _model.state.mag.calibrationState = CALIBRATION_IDLE; + _model.state.mag.calibrationValid = true; + + _model.logger.info().log(F("MAG INIT")).log(FPSTR(Device::MagDevice::getName(_mag->getType()))).log(_mag->getAddress()).logln(_model.state.mag.timer.rate); + + return 1; +} + +int MagSensor::update() +{ + int status = read(); + + if (status) filter(); + + return status; +} + +int MagSensor::read() +{ + if(!_mag || !_model.magActive() || !_model.state.mag.timer.check()) return 0; + + Stats::Measure measure(_model.state.stats, COUNTER_MAG_READ); + _mag->readMag(_model.state.mag.raw); + + return 1; +} + +int MagSensor::filter() +{ + if(!_mag || !_model.magActive()) return 0; + + Stats::Measure measure(_model.state.stats, COUNTER_MAG_FILTER); + + _model.state.mag.adc = _mag->convert(_model.state.mag.raw); + + align(_model.state.mag.adc, _model.config.mag.align); + _model.state.mag.adc = _model.state.boardAlignment.apply(_model.state.mag.adc); + + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) + { + _model.state.mag.adc.set(i, _model.state.mag.filter[i].update(_model.state.mag.adc[i])); + } + + calibrate(); + + return 1; +} + +void MagSensor::calibrate() +{ + switch(_model.state.mag.calibrationState) + { + case CALIBRATION_IDLE: + if(_model.state.mag.calibrationValid) + { + _model.state.mag.adc -= _model.state.mag.calibrationOffset; + _model.state.mag.adc *= _model.state.mag.calibrationScale; + } + break; + case CALIBRATION_START: + resetCalibration(); + _model.state.mag.calibrationSamples = 30 * _model.state.mag.timer.rate; + _model.state.mag.calibrationState = CALIBRATION_UPDATE; + break; + case CALIBRATION_UPDATE: + updateCalibration(); + _model.state.mag.calibrationSamples--; + if(_model.state.mag.calibrationSamples <= 0) _model.state.mag.calibrationState = CALIBRATION_APPLY; + break; + case CALIBRATION_APPLY: + applyCalibration(); + _model.state.mag.calibrationState = CALIBRATION_SAVE; + break; + case CALIBRATION_SAVE: + _model.finishCalibration(); + _model.state.mag.calibrationState = CALIBRATION_IDLE; + break; + default: + _model.state.mag.calibrationState = CALIBRATION_IDLE; + break; + } +} + +void MagSensor::resetCalibration() +{ + _model.state.mag.calibrationMin = VectorFloat(); + _model.state.mag.calibrationMax = VectorFloat(); + _model.state.mag.calibrationValid = false; +} + +void MagSensor::updateCalibration() +{ + for(int i = 0; i < AXIS_COUNT_RPY; i++) + { + if(_model.state.mag.adc[i] < _model.state.mag.calibrationMin[i]) _model.state.mag.calibrationMin.set(i, _model.state.mag.adc[i]); + if(_model.state.mag.adc[i] > _model.state.mag.calibrationMax[i]) _model.state.mag.calibrationMax.set(i, _model.state.mag.adc[i]); + } +} + +void MagSensor::applyCalibration() +{ + const float EPSILON = 0.001f; + + // verify calibration data and find biggest range + float maxRange = -1; + for(int i = 0; i < AXIS_COUNT_RPY; i++) + { + if(_model.state.mag.calibrationMin[i] > -EPSILON) return; + if(_model.state.mag.calibrationMax[i] < EPSILON) return; + if((_model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]) > maxRange) + { + maxRange = _model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]; + } + } + + // probably incomplete data, must be positive + if(maxRange <= 0) return; + + VectorFloat scale(1.f, 1.f, 1.f); + VectorFloat offset(0.f, 0.f, 0.f); + + for (int i = 0; i < AXIS_COUNT_RPY; i++) + { + const float range = (_model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]); + const float bias = (_model.state.mag.calibrationMax[i] + _model.state.mag.calibrationMin[i]) * 0.5f; + + if(std::abs(range) < EPSILON) return; // incomplete data + + scale.set(i, maxRange / range); // makes everything the same range + offset.set(i, bias); // level bias + } + + _model.state.mag.calibrationScale = scale; + _model.state.mag.calibrationOffset = offset; + _model.state.mag.calibrationValid = true; +} + +} + +} diff --git a/lib/Espfc/src/Sensor/MagSensor.h b/lib/Espfc/src/Sensor/MagSensor.h index 3fcea156..0c424171 100644 --- a/lib/Espfc/src/Sensor/MagSensor.h +++ b/lib/Espfc/src/Sensor/MagSensor.h @@ -1,11 +1,9 @@ -#ifndef _ESPFC_SENSOR_MAG_SENSOR_H_ -#define _ESPFC_SENSOR_MAG_SENSOR_H_ +#pragma once +#include "Model.h" #include "BaseSensor.h" #include "Device/MagDevice.h" -#include - namespace Espfc { namespace Sensor { @@ -13,155 +11,18 @@ namespace Sensor { class MagSensor: public BaseSensor { public: - MagSensor(Model& model): _model(model) {} - - int begin() - { - if(!_model.magActive()) return 0; - - _mag = _model.state.mag.dev; - if(!_mag) return 0; - - if(_model.state.mag.timer.rate < 5) return 0; - - // by default use eeprom calibration settings - _model.state.mag.calibrationState = CALIBRATION_IDLE; - _model.state.mag.calibrationValid = true; - - _model.logger.info().log(F("MAG INIT")).log(FPSTR(Device::MagDevice::getName(_mag->getType()))).log(_mag->getAddress()).logln(_model.state.mag.timer.rate); - - return 1; - } - - int update() - { - int status = read(); - - if (status) filter(); - - return status; - } - - int read() - { - if(!_mag || !_model.magActive() || !_model.state.mag.timer.check()) return 0; - - Stats::Measure measure(_model.state.stats, COUNTER_MAG_READ); - _mag->readMag(_model.state.mag.raw); - - return 1; - } - - int filter() - { - if(!_mag || !_model.magActive()) return 0; - - Stats::Measure measure(_model.state.stats, COUNTER_MAG_FILTER); + MagSensor(Model& model); - _model.state.mag.adc = _mag->convert(_model.state.mag.raw); - - align(_model.state.mag.adc, _model.config.mag.align); - _model.state.mag.adc = _model.state.boardAlignment.apply(_model.state.mag.adc); - - for(size_t i = 0; i < AXIS_COUNT_RPY; i++) - { - _model.state.mag.adc.set(i, _model.state.mag.filter[i].update(_model.state.mag.adc[i])); - } - - calibrate(); - - return 1; - } + int begin(); + int update(); + int read(); + int filter(); private: - void calibrate() - { - switch(_model.state.mag.calibrationState) - { - case CALIBRATION_IDLE: - if(_model.state.mag.calibrationValid) - { - _model.state.mag.adc -= _model.state.mag.calibrationOffset; - _model.state.mag.adc *= _model.state.mag.calibrationScale; - } - break; - case CALIBRATION_START: - resetCalibration(); - _model.state.mag.calibrationSamples = 30 * _model.state.mag.timer.rate; - _model.state.mag.calibrationState = CALIBRATION_UPDATE; - break; - case CALIBRATION_UPDATE: - updateCalibration(); - _model.state.mag.calibrationSamples--; - if(_model.state.mag.calibrationSamples <= 0) _model.state.mag.calibrationState = CALIBRATION_APPLY; - break; - case CALIBRATION_APPLY: - applyCalibration(); - _model.state.mag.calibrationState = CALIBRATION_SAVE; - break; - case CALIBRATION_SAVE: - _model.finishCalibration(); - _model.state.mag.calibrationState = CALIBRATION_IDLE; - break; - default: - _model.state.mag.calibrationState = CALIBRATION_IDLE; - break; - } - } - - void resetCalibration() - { - _model.state.mag.calibrationMin = VectorFloat(); - _model.state.mag.calibrationMax = VectorFloat(); - _model.state.mag.calibrationValid = false; - } - - void updateCalibration() - { - for(int i = 0; i < AXIS_COUNT_RPY; i++) - { - if(_model.state.mag.adc[i] < _model.state.mag.calibrationMin[i]) _model.state.mag.calibrationMin.set(i, _model.state.mag.adc[i]); - if(_model.state.mag.adc[i] > _model.state.mag.calibrationMax[i]) _model.state.mag.calibrationMax.set(i, _model.state.mag.adc[i]); - } - } - - void applyCalibration() - { - const float EPSILON = 0.001f; - - // verify calibration data and find biggest range - float maxRange = -1; - for(int i = 0; i < AXIS_COUNT_RPY; i++) - { - if(_model.state.mag.calibrationMin[i] > -EPSILON) return; - if(_model.state.mag.calibrationMax[i] < EPSILON) return; - if((_model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]) > maxRange) - { - maxRange = _model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]; - } - } - - // probably incomplete data, must be positive - if(maxRange <= 0) return; - - VectorFloat scale(1.f, 1.f, 1.f); - VectorFloat offset(0.f, 0.f, 0.f); - - for (int i = 0; i < AXIS_COUNT_RPY; i++) - { - const float range = (_model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]); - const float bias = (_model.state.mag.calibrationMax[i] + _model.state.mag.calibrationMin[i]) * 0.5f; - - if(abs(range) < EPSILON) return; // incomplete data - - scale.set(i, maxRange / range); // makes everything the same range - offset.set(i, bias); // level bias - } - - _model.state.mag.calibrationScale = scale; - _model.state.mag.calibrationOffset = offset; - _model.state.mag.calibrationValid = true; - } + void calibrate(); + void resetCalibration(); + void updateCalibration(); + void applyCalibration(); Model& _model; Device::MagDevice * _mag; @@ -170,4 +31,3 @@ class MagSensor: public BaseSensor } } -#endif \ No newline at end of file diff --git a/lib/Espfc/src/Sensor/VoltageSensor.cpp b/lib/Espfc/src/Sensor/VoltageSensor.cpp new file mode 100644 index 00000000..6718ba01 --- /dev/null +++ b/lib/Espfc/src/Sensor/VoltageSensor.cpp @@ -0,0 +1,115 @@ +#include "VoltageSensor.h" + +#include + +namespace Espfc { + +namespace Sensor { + +VoltageSensor::VoltageSensor(Model &model) : _model(model) {} + +int VoltageSensor::begin() +{ + _model.state.battery.timer.setRate(100); + _model.state.battery.samples = 50; + + _vFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate); + _vFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate); + + _iFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate); + _iFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate); + + _state = VBAT; + + return 1; +} + +int VoltageSensor::update() +{ + if (!_model.state.battery.timer.check()) return 0; + + Stats::Measure measure(_model.state.stats, COUNTER_BATTERY); + + switch (_state) + { + case VBAT: + _state = IBAT; + return readVbat(); + case IBAT: + _state = VBAT; + return readIbat(); + } + + return 0; +} + +int VoltageSensor::readVbat() +{ +#ifdef ESPFC_ADC_0 + if (_model.config.vbat.source != 1 || _model.config.pin[PIN_INPUT_ADC_0] == -1) return 0; + // wemos d1 mini has divider 3.2:1 (220k:100k) + // additionaly I've used divider 5.7:1 (4k7:1k) + // total should equals ~18.24:1, 73:4 resDiv:resMult should be ideal, + // but ~52:1 is real, did I miss something? + _model.state.battery.rawVoltage = analogRead(_model.config.pin[PIN_INPUT_ADC_0]); + float volts = _vFilterFast.update(_model.state.battery.rawVoltage * ESPFC_ADC_SCALE); + + volts *= _model.config.vbat.scale * 0.1f; + volts *= _model.config.vbat.resMult; + volts /= _model.config.vbat.resDiv; + + _model.state.battery.voltageUnfiltered = volts; + _model.state.battery.voltage = _vFilter.update(_model.state.battery.voltageUnfiltered); + + // cell count detection + if (_model.state.battery.samples > 0) + { + _model.state.battery.cells = std::ceil(_model.state.battery.voltage / 4.2f); + _model.state.battery.samples--; + } + + _model.state.battery.cellVoltage = _model.state.battery.voltage / constrain(_model.state.battery.cells, 1, 6); + _model.state.battery.percentage = Math::clamp(Math::map(_model.state.battery.cellVoltage, 3.4f, 4.2f, 0.0f, 100.0f), 0.0f, 100.0f); + + if (_model.config.debug.mode == DEBUG_BATTERY) + { + _model.state.debug[0] = constrain(lrintf(_model.state.battery.voltageUnfiltered * 100.0f), 0, 32000); + _model.state.debug[1] = constrain(lrintf(_model.state.battery.voltage * 100.0f), 0, 32000); + } + return 1; +#else + return 0; +#endif +} + +int VoltageSensor::readIbat() +{ +#ifdef ESPFC_ADC_1 + if (_model.config.ibat.source != 1 && _model.config.pin[PIN_INPUT_ADC_1] == -1) return 0; + + _model.state.battery.rawCurrent = analogRead(_model.config.pin[PIN_INPUT_ADC_1]); + float volts = _iFilterFast.update(_model.state.battery.rawCurrent * ESPFC_ADC_SCALE); + float milivolts = volts * 1000.0f; + + volts += _model.config.ibat.offset * 0.001f; + volts *= _model.config.ibat.scale * 0.1f; + + _model.state.battery.currentUnfiltered = volts; + _model.state.battery.current = _iFilter.update(_model.state.battery.currentUnfiltered); + + if (_model.config.debug.mode == DEBUG_CURRENT_SENSOR) + { + _model.state.debug[0] = lrintf(milivolts); + _model.state.debug[1] = constrain(lrintf(_model.state.battery.currentUnfiltered * 100.0f), 0, 32000); + _model.state.debug[2] = _model.state.battery.rawCurrent; + } + + return 1; +#else + return 0; +#endif +} + +} + +} diff --git a/lib/Espfc/src/Sensor/VoltageSensor.h b/lib/Espfc/src/Sensor/VoltageSensor.h index 3e4af7ad..8c8688b2 100644 --- a/lib/Espfc/src/Sensor/VoltageSensor.h +++ b/lib/Espfc/src/Sensor/VoltageSensor.h @@ -1,9 +1,8 @@ -#ifndef _ESPFC_SENSOR_VOLTAGE_SENSOR_H_ -#define _ESPFC_SENSOR_VOLTAGE_SENSOR_H_ +#pragma once #include "Model.h" #include "BaseSensor.h" -#include +#include "Filter.h" namespace Espfc { @@ -16,108 +15,11 @@ class VoltageSensor: public BaseSensor VBAT, IBAT }; - VoltageSensor(Model& model): _model(model) {} - - int begin() - { - _model.state.battery.timer.setRate(100); - _model.state.battery.samples = 50; - - _vFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate); - _vFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate); - - _iFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate); - _iFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate); - - _state = VBAT; - - return 1; - } - - int update() - { - if(!_model.state.battery.timer.check()) return 0; - - Stats::Measure measure(_model.state.stats, COUNTER_BATTERY); - - switch(_state) { - case VBAT: - _state = IBAT; - return readVbat(); - case IBAT: - _state = VBAT; - return readIbat(); - } - - return 0; - } - - int readVbat() - { -#ifdef ESPFC_ADC_0 - if(_model.config.vbat.source != 1 || _model.config.pin[PIN_INPUT_ADC_0] == -1) return 0; - // wemos d1 mini has divider 3.2:1 (220k:100k) - // additionaly I've used divider 5.7:1 (4k7:1k) - // total should equals ~18.24:1, 73:4 resDiv:resMult should be ideal, - // but ~52:1 is real, did I miss something? - _model.state.battery.rawVoltage = analogRead(_model.config.pin[PIN_INPUT_ADC_0]); - float volts = _vFilterFast.update(_model.state.battery.rawVoltage * ESPFC_ADC_SCALE); - - volts *= _model.config.vbat.scale * 0.1f; - volts *= _model.config.vbat.resMult; - volts /= _model.config.vbat.resDiv; - - _model.state.battery.voltageUnfiltered = volts; - _model.state.battery.voltage = _vFilter.update(_model.state.battery.voltageUnfiltered); - - // cell count detection - if(_model.state.battery.samples > 0) - { - _model.state.battery.cells = std::ceil(_model.state.battery.voltage / 4.2f); - _model.state.battery.samples--; - } - - _model.state.battery.cellVoltage = _model.state.battery.voltage / constrain(_model.state.battery.cells, 1, 6); - _model.state.battery.percentage = Math::clamp(Math::map(_model.state.battery.cellVoltage, 3.4f, 4.2f, 0.0f, 100.0f), 0.0f, 100.0f); - - if(_model.config.debug.mode == DEBUG_BATTERY) - { - _model.state.debug[0] = constrain(lrintf(_model.state.battery.voltageUnfiltered * 100.0f), 0, 32000); - _model.state.debug[1] = constrain(lrintf(_model.state.battery.voltage * 100.0f), 0, 32000); - } - return 1; -#else - return 0; -#endif - } - - int readIbat() - { -#ifdef ESPFC_ADC_1 - if(_model.config.ibat.source != 1 && _model.config.pin[PIN_INPUT_ADC_1] == -1) return 0; - - _model.state.battery.rawCurrent = analogRead(_model.config.pin[PIN_INPUT_ADC_1]); - float volts = _iFilterFast.update(_model.state.battery.rawCurrent * ESPFC_ADC_SCALE); - float milivolts = volts * 1000.0f; - - volts += _model.config.ibat.offset * 0.001f; - volts *= _model.config.ibat.scale * 0.1f; - - _model.state.battery.currentUnfiltered = volts; - _model.state.battery.current = _iFilter.update(_model.state.battery.currentUnfiltered); - - if(_model.config.debug.mode == DEBUG_CURRENT_SENSOR) - { - _model.state.debug[0] = lrintf(milivolts); - _model.state.debug[1] = constrain(lrintf(_model.state.battery.currentUnfiltered * 100.0f), 0, 32000); - _model.state.debug[2] = _model.state.battery.rawCurrent; - } - - return 1; -#else - return 0; -#endif - } + VoltageSensor(Model& model); + int begin(); + int update(); + int readVbat(); + int readIbat(); private: Model& _model; @@ -131,5 +33,3 @@ class VoltageSensor: public BaseSensor } } - -#endif \ No newline at end of file From 59afc642b3f62fab661e8d65df50880f9eb48c91 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 2 Nov 2024 22:01:01 +0100 Subject: [PATCH 42/68] split hardware unit --- lib/Espfc/src/Hal/Gpio.cpp | 10 ++ lib/Espfc/src/Hal/Gpio.h | 13 ++- lib/Espfc/src/Hardware.cpp | 191 ++++++++++++++++++++++++++++++++++ lib/Espfc/src/Hardware.h | 204 ++----------------------------------- 4 files changed, 218 insertions(+), 200 deletions(-) create mode 100644 lib/Espfc/src/Hardware.cpp diff --git a/lib/Espfc/src/Hal/Gpio.cpp b/lib/Espfc/src/Hal/Gpio.cpp index 26bba426..806497cb 100644 --- a/lib/Espfc/src/Hal/Gpio.cpp +++ b/lib/Espfc/src/Hal/Gpio.cpp @@ -59,6 +59,16 @@ pin_status_t FAST_CODE_ATTR Gpio::digitalRead(uint8_t pin) #endif } +void FAST_CODE_ATTR Gpio::pinMode(uint8_t pin, pin_mode_t mode) +{ +#if defined(UNIT_TEST) + return; + // do nothing +#else + ::pinMode(pin, mode); +#endif +} + } } diff --git a/lib/Espfc/src/Hal/Gpio.h b/lib/Espfc/src/Hal/Gpio.h index 5f24b992..a0ad7e9a 100644 --- a/lib/Espfc/src/Hal/Gpio.h +++ b/lib/Espfc/src/Hal/Gpio.h @@ -3,9 +3,11 @@ #include #if defined(ARCH_RP2040) -typedef PinStatus pin_status_t; + typedef PinStatus pin_status_t; + typedef PinMode pin_mode_t; #else -typedef uint8_t pin_status_t; + typedef uint8_t pin_status_t; + typedef uint8_t pin_mode_t; #endif namespace Espfc { @@ -14,9 +16,10 @@ namespace Hal { class Gpio { - public: - static void digitalWrite(uint8_t pin, pin_status_t val); - static pin_status_t digitalRead(uint8_t pin); +public: + static void digitalWrite(uint8_t pin, pin_status_t val); + static pin_status_t digitalRead(uint8_t pin); + static void pinMode(uint8_t pin, pin_mode_t mode); }; } diff --git a/lib/Espfc/src/Hardware.cpp b/lib/Espfc/src/Hardware.cpp new file mode 100644 index 00000000..021c8184 --- /dev/null +++ b/lib/Espfc/src/Hardware.cpp @@ -0,0 +1,191 @@ +#include "Hardware.h" +#include "Device/GyroDevice.h" +#include "Device/GyroMPU6050.h" +#include "Device/GyroMPU6500.h" +#include "Device/GyroMPU9250.h" +#include "Device/GyroLSM6DSO.h" +#include "Device/GyroICM20602.h" +#include "Device/GyroBMI160.h" +#include "Device/MagHMC5338L.h" +#include "Device/MagQMC5338L.h" +#include "Device/MagAK8963.h" +#include "Device/BaroDevice.h" +#include "Device/BaroBMP085.h" +#include "Device/BaroBMP280.h" +#include "Device/BaroSPL06.h" +#if defined(ESPFC_WIFI_ALT) +#include +#elif defined(ESPFC_WIFI) +#include +#endif + +namespace { +#if defined(ESPFC_SPI_0) + #if defined(ESP32C3) || defined(ESP32S3) || defined(ESP32S2) + static SPIClass SPI1(HSPI); + #elif defined(ESP32) + static SPIClass SPI1(VSPI); + #endif + static Espfc::Device::BusSPI spiBus(ESPFC_SPI_0_DEV); +#endif +#if defined(ESPFC_I2C_0) + static Espfc::Device::BusI2C i2cBus(WireInstance); +#endif + static Espfc::Device::BusSlave gyroSlaveBus; + static Espfc::Device::GyroMPU6050 mpu6050; + static Espfc::Device::GyroMPU6500 mpu6500; + static Espfc::Device::GyroMPU9250 mpu9250; + static Espfc::Device::GyroLSM6DSO lsm6dso; + static Espfc::Device::GyroICM20602 icm20602; + static Espfc::Device::GyroBMI160 bmi160; + static Espfc::Device::MagHMC5338L hmc5883l; + static Espfc::Device::MagQMC5338L qmc5883l; + static Espfc::Device::MagAK8963 ak8963; + static Espfc::Device::BaroBMP085 bmp085; + static Espfc::Device::BaroBMP280 bmp280; + static Espfc::Device::BaroSPL06 spl06; +} + +namespace Espfc { + +Hardware::Hardware(Model& model): _model(model) {} + +int Hardware::begin() +{ + initBus(); + detectGyro(); + detectMag(); + detectBaro(); + return 1; +} + +void Hardware::onI2CError() +{ + _model.state.i2cErrorCount++; + _model.state.i2cErrorDelta++; +} + +void Hardware::initBus() +{ +#if defined(ESPFC_SPI_0) + int spiResult = spiBus.begin(_model.config.pin[PIN_SPI_0_SCK], _model.config.pin[PIN_SPI_0_MOSI], _model.config.pin[PIN_SPI_0_MISO]); + _model.logger.info().log(F("SPI SETUP")).log(_model.config.pin[PIN_SPI_0_SCK]).log(_model.config.pin[PIN_SPI_0_MOSI]).log(_model.config.pin[PIN_SPI_0_MISO]).logln(spiResult); +#endif +#if defined(ESPFC_I2C_0) + int i2cResult = i2cBus.begin(_model.config.pin[PIN_I2C_0_SDA], _model.config.pin[PIN_I2C_0_SCL], _model.config.i2cSpeed * 1000ul); + i2cBus.onError = std::bind(&Hardware::onI2CError, this); + _model.logger.info().log(F("I2C SETUP")).log(_model.config.pin[PIN_I2C_0_SDA]).log(_model.config.pin[PIN_I2C_0_SCL]).log(_model.config.i2cSpeed).logln(i2cResult); +#endif +} + +void Hardware::detectGyro() +{ + if(_model.config.gyro.dev == GYRO_NONE) return; + + Device::GyroDevice * detectedGyro = nullptr; +#if defined(ESPFC_SPI_0) + if(_model.config.pin[PIN_SPI_CS0] != -1) + { + Hal::Gpio::digitalWrite(_model.config.pin[PIN_SPI_CS0], HIGH); + Hal::Gpio::pinMode(_model.config.pin[PIN_SPI_CS0], OUTPUT); + if(!detectedGyro && detectDevice(mpu9250, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &mpu9250; + if(!detectedGyro && detectDevice(mpu6500, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &mpu6500; + if(!detectedGyro && detectDevice(icm20602, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &icm20602; + if(!detectedGyro && detectDevice(bmi160, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &bmi160; + if(!detectedGyro && detectDevice(lsm6dso, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &lsm6dso; + if(detectedGyro) gyroSlaveBus.begin(&spiBus, detectedGyro->getAddress()); + } +#endif +#if defined(ESPFC_I2C_0) + if(!detectedGyro && _model.config.pin[PIN_I2C_0_SDA] != -1 && _model.config.pin[PIN_I2C_0_SCL] != -1) + { + if(!detectedGyro && detectDevice(mpu9250, i2cBus)) detectedGyro = &mpu9250; + if(!detectedGyro && detectDevice(mpu6500, i2cBus)) detectedGyro = &mpu6500; + if(!detectedGyro && detectDevice(icm20602, i2cBus)) detectedGyro = &icm20602; + if(!detectedGyro && detectDevice(bmi160, i2cBus)) detectedGyro = &bmi160; + if(!detectedGyro && detectDevice(mpu6050, i2cBus)) detectedGyro = &mpu6050; + if(!detectedGyro && detectDevice(lsm6dso, i2cBus)) detectedGyro = &lsm6dso; + if(detectedGyro) gyroSlaveBus.begin(&i2cBus, detectedGyro->getAddress()); + } +#endif + if(!detectedGyro) return; + + detectedGyro->setDLPFMode(_model.config.gyro.dlpf); + _model.state.gyro.dev = detectedGyro; + _model.state.gyro.present = (bool)detectedGyro; + _model.state.accel.present = _model.state.gyro.present && _model.config.accel.dev != GYRO_NONE; + _model.state.gyro.clock = detectedGyro->getRate(); +} + +void Hardware::detectMag() +{ + if(_model.config.mag.dev == MAG_NONE) return; + + 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) + { + if(!detectedMag && detectDevice(ak8963, i2cBus)) detectedMag = &ak8963; + if(!detectedMag && detectDevice(hmc5883l, i2cBus)) detectedMag = &hmc5883l; + if(!detectedMag && detectDevice(qmc5883l, i2cBus)) detectedMag = &qmc5883l; + } +#endif + if(gyroSlaveBus.getBus()) + { + if(!detectedMag && detectDevice(ak8963, gyroSlaveBus)) detectedMag = &ak8963; + if(!detectedMag && detectDevice(hmc5883l, gyroSlaveBus)) detectedMag = &hmc5883l; + if(!detectedMag && detectDevice(qmc5883l, gyroSlaveBus)) detectedMag = &qmc5883l; + + } + _model.state.mag.dev = detectedMag; + _model.state.mag.present = (bool)detectedMag; + _model.state.mag.rate = detectedMag ? detectedMag->getRate() : 0; +} + +void Hardware::detectBaro() +{ + if(_model.config.baro.dev == BARO_NONE) return; + + Device::BaroDevice * detectedBaro = nullptr; +#if defined(ESPFC_SPI_0) + if(_model.config.pin[PIN_SPI_CS1] != -1) + { + Hal::Gpio::digitalWrite(_model.config.pin[PIN_SPI_CS1], HIGH); + Hal::Gpio::pinMode(_model.config.pin[PIN_SPI_CS1], OUTPUT); + if(!detectedBaro && detectDevice(bmp280, spiBus, _model.config.pin[PIN_SPI_CS1])) detectedBaro = &bmp280; + if(!detectedBaro && detectDevice(bmp085, spiBus, _model.config.pin[PIN_SPI_CS1])) detectedBaro = &bmp085; + if(!detectedBaro && detectDevice(spl06, spiBus, _model.config.pin[PIN_SPI_CS1])) detectedBaro = &spl06; + } +#endif +#if defined(ESPFC_I2C_0) + if(_model.config.pin[PIN_I2C_0_SDA] != -1 && _model.config.pin[PIN_I2C_0_SCL] != -1) + { + if(!detectedBaro && detectDevice(bmp280, i2cBus)) detectedBaro = &bmp280; + if(!detectedBaro && detectDevice(bmp085, i2cBus)) detectedBaro = &bmp085; + if(!detectedBaro && detectDevice(spl06, i2cBus)) detectedBaro = &spl06; + } +#endif + if(gyroSlaveBus.getBus()) + { + if(!detectedBaro && detectDevice(bmp280, gyroSlaveBus)) detectedBaro = &bmp280; + if(!detectedBaro && detectDevice(bmp085, gyroSlaveBus)) detectedBaro = &bmp085; + if(!detectedBaro && detectDevice(spl06, gyroSlaveBus)) detectedBaro = &spl06; + } + + _model.state.baro.dev = detectedBaro; + _model.state.baro.present = (bool)detectedBaro; +} + +void Hardware::restart(const Model& model) +{ + if(model.state.mixer.escMotor) model.state.mixer.escMotor->end(); + if(model.state.mixer.escServo) model.state.mixer.escServo->end(); +#ifdef ESPFC_SERIAL_SOFT_0_WIFI + WiFi.disconnect(); + WiFi.softAPdisconnect(); +#endif + delay(100); + targetReset(); +} + +} diff --git a/lib/Espfc/src/Hardware.h b/lib/Espfc/src/Hardware.h index e32c0f3f..c8539488 100644 --- a/lib/Espfc/src/Hardware.h +++ b/lib/Espfc/src/Hardware.h @@ -1,14 +1,6 @@ -#ifndef _ESPFC_HARDWARE_H_ -#define _ESPFC_HARDWARE_H_ +#pragma once -#include #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" #endif @@ -16,181 +8,20 @@ #include "Device/BusSPI.h" #endif #include "Device/BusSlave.h" -#include "Device/GyroDevice.h" -#include "Device/GyroMPU6050.h" -#include "Device/GyroMPU6500.h" -#include "Device/GyroMPU9250.h" -#include "Device/GyroLSM6DSO.h" -#include "Device/GyroICM20602.h" -#include "Device/GyroBMI160.h" -#include "Device/MagHMC5338L.h" -#include "Device/MagQMC5338L.h" -#include "Device/MagAK8963.h" -#include "Device/BaroDevice.h" -#include "Device/BaroBMP085.h" -#include "Device/BaroBMP280.h" -#include "Device/BaroSPL06.h" - - -namespace { -#if defined(ESPFC_SPI_0) - #if defined(ESP32C3) || defined(ESP32S3) || defined(ESP32S2) - static SPIClass SPI1(HSPI); - #elif defined(ESP32) - static SPIClass SPI1(VSPI); - #endif - static Espfc::Device::BusSPI spiBus(ESPFC_SPI_0_DEV); -#endif -#if defined(ESPFC_I2C_0) - static Espfc::Device::BusI2C i2cBus(WireInstance); -#endif - static Espfc::Device::BusSlave gyroSlaveBus; - static Espfc::Device::GyroMPU6050 mpu6050; - static Espfc::Device::GyroMPU6500 mpu6500; - static Espfc::Device::GyroMPU9250 mpu9250; - static Espfc::Device::GyroLSM6DSO lsm6dso; - static Espfc::Device::GyroICM20602 icm20602; - static Espfc::Device::GyroBMI160 bmi160; - static Espfc::Device::MagHMC5338L hmc5883l; - static Espfc::Device::MagQMC5338L qmc5883l; - static Espfc::Device::MagAK8963 ak8963; - static Espfc::Device::BaroBMP085 bmp085; - static Espfc::Device::BaroBMP280 bmp280; - static Espfc::Device::BaroSPL06 spl06; -} namespace Espfc { class Hardware { public: - Hardware(Model& model): _model(model) {} - - int begin() - { - initBus(); - detectGyro(); - detectMag(); - detectBaro(); - return 1; - } - - void onI2CError() - { - _model.state.i2cErrorCount++; - _model.state.i2cErrorDelta++; - } - - void initBus() - { -#if defined(ESPFC_SPI_0) - int spiResult = spiBus.begin(_model.config.pin[PIN_SPI_0_SCK], _model.config.pin[PIN_SPI_0_MOSI], _model.config.pin[PIN_SPI_0_MISO]); - _model.logger.info().log(F("SPI SETUP")).log(_model.config.pin[PIN_SPI_0_SCK]).log(_model.config.pin[PIN_SPI_0_MOSI]).log(_model.config.pin[PIN_SPI_0_MISO]).logln(spiResult); -#endif -#if defined(ESPFC_I2C_0) - int i2cResult = i2cBus.begin(_model.config.pin[PIN_I2C_0_SDA], _model.config.pin[PIN_I2C_0_SCL], _model.config.i2cSpeed * 1000ul); - i2cBus.onError = std::bind(&Hardware::onI2CError, this); - _model.logger.info().log(F("I2C SETUP")).log(_model.config.pin[PIN_I2C_0_SDA]).log(_model.config.pin[PIN_I2C_0_SCL]).log(_model.config.i2cSpeed).logln(i2cResult); -#endif - } - - void detectGyro() - { - if(_model.config.gyro.dev == GYRO_NONE) return; - - Device::GyroDevice * detectedGyro = nullptr; -#if defined(ESPFC_SPI_0) - if(_model.config.pin[PIN_SPI_CS0] != -1) - { - digitalWrite(_model.config.pin[PIN_SPI_CS0], HIGH); - pinMode(_model.config.pin[PIN_SPI_CS0], OUTPUT); - if(!detectedGyro && detectDevice(mpu9250, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &mpu9250; - if(!detectedGyro && detectDevice(mpu6500, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &mpu6500; - if(!detectedGyro && detectDevice(icm20602, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &icm20602; - if(!detectedGyro && detectDevice(bmi160, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &bmi160; - if(!detectedGyro && detectDevice(lsm6dso, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &lsm6dso; - if(detectedGyro) gyroSlaveBus.begin(&spiBus, detectedGyro->getAddress()); - } -#endif -#if defined(ESPFC_I2C_0) - if(!detectedGyro && _model.config.pin[PIN_I2C_0_SDA] != -1 && _model.config.pin[PIN_I2C_0_SCL] != -1) - { - if(!detectedGyro && detectDevice(mpu9250, i2cBus)) detectedGyro = &mpu9250; - if(!detectedGyro && detectDevice(mpu6500, i2cBus)) detectedGyro = &mpu6500; - if(!detectedGyro && detectDevice(icm20602, i2cBus)) detectedGyro = &icm20602; - if(!detectedGyro && detectDevice(bmi160, i2cBus)) detectedGyro = &bmi160; - if(!detectedGyro && detectDevice(mpu6050, i2cBus)) detectedGyro = &mpu6050; - if(!detectedGyro && detectDevice(lsm6dso, i2cBus)) detectedGyro = &lsm6dso; - if(detectedGyro) gyroSlaveBus.begin(&i2cBus, detectedGyro->getAddress()); - } -#endif - if(!detectedGyro) return; - - detectedGyro->setDLPFMode(_model.config.gyro.dlpf); - _model.state.gyro.dev = detectedGyro; - _model.state.gyro.present = (bool)detectedGyro; - _model.state.accel.present = _model.state.gyro.present && _model.config.accel.dev != GYRO_NONE; - _model.state.gyro.clock = detectedGyro->getRate(); - } - - void detectMag() - { - if(_model.config.mag.dev == MAG_NONE) return; - - 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) - { - if(!detectedMag && detectDevice(ak8963, i2cBus)) detectedMag = &ak8963; - if(!detectedMag && detectDevice(hmc5883l, i2cBus)) detectedMag = &hmc5883l; - if(!detectedMag && detectDevice(qmc5883l, i2cBus)) detectedMag = &qmc5883l; - } -#endif - if(gyroSlaveBus.getBus()) - { - if(!detectedMag && detectDevice(ak8963, gyroSlaveBus)) detectedMag = &ak8963; - if(!detectedMag && detectDevice(hmc5883l, gyroSlaveBus)) detectedMag = &hmc5883l; - if(!detectedMag && detectDevice(qmc5883l, gyroSlaveBus)) detectedMag = &qmc5883l; - - } - _model.state.mag.dev = detectedMag; - _model.state.mag.present = (bool)detectedMag; - _model.state.mag.rate = detectedMag ? detectedMag->getRate() : 0; - } - - void detectBaro() - { - if(_model.config.baro.dev == BARO_NONE) return; - - Device::BaroDevice * detectedBaro = nullptr; -#if defined(ESPFC_SPI_0) - if(_model.config.pin[PIN_SPI_CS1] != -1) - { - digitalWrite(_model.config.pin[PIN_SPI_CS1], HIGH); - pinMode(_model.config.pin[PIN_SPI_CS1], OUTPUT); - if(!detectedBaro && detectDevice(bmp280, spiBus, _model.config.pin[PIN_SPI_CS1])) detectedBaro = &bmp280; - if(!detectedBaro && detectDevice(bmp085, spiBus, _model.config.pin[PIN_SPI_CS1])) detectedBaro = &bmp085; - if(!detectedBaro && detectDevice(spl06, spiBus, _model.config.pin[PIN_SPI_CS1])) detectedBaro = &spl06; - } -#endif -#if defined(ESPFC_I2C_0) - if(_model.config.pin[PIN_I2C_0_SDA] != -1 && _model.config.pin[PIN_I2C_0_SCL] != -1) - { - if(!detectedBaro && detectDevice(bmp280, i2cBus)) detectedBaro = &bmp280; - if(!detectedBaro && detectDevice(bmp085, i2cBus)) detectedBaro = &bmp085; - if(!detectedBaro && detectDevice(spl06, i2cBus)) detectedBaro = &spl06; - } -#endif - if(gyroSlaveBus.getBus()) - { - if(!detectedBaro && detectDevice(bmp280, gyroSlaveBus)) detectedBaro = &bmp280; - if(!detectedBaro && detectDevice(bmp085, gyroSlaveBus)) detectedBaro = &bmp085; - if(!detectedBaro && detectDevice(spl06, gyroSlaveBus)) detectedBaro = &spl06; - } + Hardware(Model& model); + int begin(); + void onI2CError(); - _model.state.baro.dev = detectedBaro; - _model.state.baro.present = (bool)detectedBaro; - } + void initBus(); + void detectGyro(); + void detectMag(); + void detectBaro(); #if defined(ESPFC_SPI_0) template @@ -223,27 +54,10 @@ class Hardware return status; } - int update() - { - return 1; - } - - static void restart(const Model& model) - { - if(model.state.mixer.escMotor) model.state.mixer.escMotor->end(); - if(model.state.mixer.escServo) model.state.mixer.escServo->end(); -#ifdef ESPFC_SERIAL_SOFT_0_WIFI - WiFi.disconnect(); - WiFi.softAPdisconnect(); -#endif - delay(100); - targetReset(); - } + static void restart(const Model& model); private: Model& _model; }; } - -#endif From dfc83484397017f923590c33b9ff0f7aca7bbc7b Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 2 Nov 2024 23:39:52 +0100 Subject: [PATCH 43/68] split buzzer --- lib/Espfc/src/Buzzer.cpp | 156 +++++++++++++++++++++++++++++++++++ lib/Espfc/src/Buzzer.h | 172 ++++----------------------------------- 2 files changed, 171 insertions(+), 157 deletions(-) create mode 100644 lib/Espfc/src/Buzzer.cpp diff --git a/lib/Espfc/src/Buzzer.cpp b/lib/Espfc/src/Buzzer.cpp new file mode 100644 index 00000000..eb453c86 --- /dev/null +++ b/lib/Espfc/src/Buzzer.cpp @@ -0,0 +1,156 @@ +#ifndef UNIT_TEST +#include +#endif +#include "Buzzer.h" +#include "Hal/Gpio.h" + +namespace Espfc { + +Buzzer::Buzzer(Model& model): _model(model), _status(BUZZER_STATUS_IDLE), _wait(0), _scheme(NULL), _e(BUZZER_SILENCE) {} + +int Buzzer::begin() +{ +#ifndef UNIT_TEST + if(_model.config.pin[PIN_BUZZER] == -1) return 0; + Hal::Gpio::pinMode(_model.config.pin[PIN_BUZZER], OUTPUT); + Hal::Gpio::digitalWrite(_model.config.pin[PIN_BUZZER], (pin_status_t)_model.config.buzzer.inverted); +#endif + _model.state.buzzer.timer.setRate(100); + + return 1; +} + +int Buzzer::update() +{ + //_model.state.debug[0] = _e; + //_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; + + switch(_status) + { + case BUZZER_STATUS_IDLE: // wait for command + if(!_model.state.buzzer.empty()) + { + _e = _model.state.buzzer.pop(); + _scheme = schemes()[_e]; + _status = BUZZER_STATUS_TEST; + } + break; + case BUZZER_STATUS_TEST: // test for end or continue + if(!_scheme || *_scheme == 0) + { + _play(false, 10, BUZZER_STATUS_IDLE); + _scheme = NULL; + _e = BUZZER_SILENCE; + } + else + { + _play(true, *_scheme, BUZZER_STATUS_ON); // start playing + } + break; + case BUZZER_STATUS_ON: // end playing with pause, same length as on + _play(false, (*_scheme) / 2, BUZZER_STATUS_OFF); + break; + case BUZZER_STATUS_OFF: // move to next cycle + _scheme++; + _status = BUZZER_STATUS_TEST; + break; + } + + return 1; +} + +void Buzzer::_play(bool v, int time, BuzzerPlayStatus s) +{ + _write(v); + _delay(time); + _status = s; +} + +void Buzzer::_write(bool v) +{ +#ifndef UNIT_TEST + Hal::Gpio::digitalWrite(_model.config.pin[PIN_BUZZER], (pin_status_t)(_model.config.buzzer.inverted ? !v : v)); +#endif +} + +void Buzzer::_delay(int time) +{ + _wait = millis() + time * 10; +} + +const uint8_t** Buzzer::schemes() +{ + static const uint8_t beeperSilence[] = { 0 }; + static const uint8_t beeperGyroCalibrated[] = { 10, 10, 10, 0 }; + static const uint8_t beeperRxLost[] = { 30, 0 }; + static const uint8_t beeperDisarming[] = { 10, 10, 0 }; + static const uint8_t beeperArming[] = { 20, 0 }; + static const uint8_t beeperSystemInit[] = { 10, 0 }; + static const uint8_t beeperBatteryLow[] = { 30, 0 }; + static const uint8_t beeperBatteryCritical[] = { 50, 0 }; + + static const uint8_t* beeperSchemes[] = { + //BUZZER_SILENCE + beeperSilence, + //BUZZER_GYRO_CALIBRATED, + beeperGyroCalibrated, + //BUZZER_RX_LOST, // Beeps when TX is turned off or signal lost (repeat until TX is okay) + beeperRxLost, + //BUZZER_RX_LOST_LANDING, // Beeps SOS when armed and TX is turned off or signal lost (autolanding/autodisarm) + beeperSilence, + //BUZZER_DISARMING, // Beep when disarming the board + beeperDisarming, + //BUZZER_ARMING, // Beep when arming the board + beeperArming, + //BUZZER_ARMING_GPS_FIX, // Beep a special tone when arming the board and GPS has fix + beeperSilence, + //BUZZER_BAT_CRIT_LOW, // Longer warning beeps when battery is critically low (repeats) + beeperBatteryCritical, + //BUZZER_BAT_LOW, // Warning beeps when battery is getting low (repeats) + beeperBatteryLow, + //BUZZER_GPS_STATUS, // FIXME **** Disable beeper when connected to USB **** + beeperSilence, + //BUZZER_RX_SET, // Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled + beeperRxLost, + //BUZZER_ACC_CALIBRATION, // ACC inflight calibration completed confirmation + beeperSilence, + //BUZZER_ACC_CALIBRATION_FAIL, // ACC inflight calibration failed + beeperSilence, + //BUZZER_READY_BEEP, // Ring a tone when GPS is locked and ready + beeperSilence, + //BUZZER_MULTI_BEEPS, // Internal value used by 'beeperConfirmationBeeps()'. + beeperSilence, + //BUZZER_DISARM_REPEAT, // Beeps sounded while stick held in disarm position + beeperSilence, + //BUZZER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased) + beeperSilence, + //BUZZER_SYSTEM_INIT, // Initialisation beeps when board is powered on + beeperSystemInit, + //BUZZER_USB, // Some boards have beeper powered USB connected + beeperSilence, + //BUZZER_BLACKBOX_ERASE, // Beep when blackbox erase completes + beeperSilence, + //BUZZER_CRASH_FLIP_MODE, // Crash flip mode is active + beeperSilence, + //BUZZER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated + beeperSilence, + //BUZZER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop + beeperSilence, + //BUZZER_ALL, // Turn ON or OFF all beeper conditions + beeperSilence, + //BUZZER_PREFERENCE, // Save preferred beeper configuration + beeperSilence + // BUZZER_ALL and BUZZER_PREFERENCE must remain at the bottom of this enum + }; + + return beeperSchemes; +} + +} diff --git a/lib/Espfc/src/Buzzer.h b/lib/Espfc/src/Buzzer.h index 7b184fc6..30331458 100644 --- a/lib/Espfc/src/Buzzer.h +++ b/lib/Espfc/src/Buzzer.h @@ -1,9 +1,5 @@ -#ifndef _ESPFC_BUZZER_H_ -#define _ESPFC_BUZZER_H_ +#pragma once -#ifndef UNIT_TEST -#include -#endif #include "Model.h" namespace Espfc { @@ -18,162 +14,24 @@ enum BuzzerPlayStatus class Buzzer { - public: - Buzzer(Model& model): _model(model), _status(BUZZER_STATUS_IDLE), _wait(0), _scheme(NULL), _e(BUZZER_SILENCE) {} +public: + Buzzer(Model& model); + int begin(); + int update(); - int begin() - { -#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 - _model.state.buzzer.timer.setRate(100); +private: + void _play(bool v, int time, BuzzerPlayStatus s); + void _write(bool v); + void _delay(int time); - return 1; - } + static const uint8_t** schemes(); - int update() - { - //_model.state.debug[0] = _e; - //_model.state.debug[1] = _status; - //_model.state.debug[2] = (int16_t)(millis() - _wait); + Model& _model; -#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; - - switch(_status) - { - case BUZZER_STATUS_IDLE: // wait for command - if(!_model.state.buzzer.empty()) - { - _e = _model.state.buzzer.pop(); - _scheme = schemes()[_e]; - _status = BUZZER_STATUS_TEST; - } - break; - case BUZZER_STATUS_TEST: // test for end or continue - if(!_scheme || *_scheme == 0) - { - _play(false, 10, BUZZER_STATUS_IDLE); - _scheme = NULL; - _e = BUZZER_SILENCE; - } - else - { - _play(true, *_scheme, BUZZER_STATUS_ON); // start playing - } - break; - case BUZZER_STATUS_ON: // end playing with pause, same length as on - _play(false, (*_scheme) / 2, BUZZER_STATUS_OFF); - break; - case BUZZER_STATUS_OFF: // move to next cycle - _scheme++; - _status = BUZZER_STATUS_TEST; - break; - } - - return 1; - } - - void _play(bool v, int time, BuzzerPlayStatus s) - { - _write(v); - _delay(time); - _status = s; - } - - void _write(bool v) - { -#ifndef UNIT_TEST - digitalWrite(_model.config.pin[PIN_BUZZER], _model.config.buzzer.inverted ? !v : v); -#endif - } - - void _delay(int time) - { - _wait = millis() + time * 10; - } - - static const uint8_t** schemes() - { - static const uint8_t beeperSilence[] = { 0 }; - static const uint8_t beeperGyroCalibrated[] = { 10, 10, 10, 0 }; - static const uint8_t beeperRxLost[] = { 30, 0 }; - static const uint8_t beeperDisarming[] = { 10, 10, 0 }; - static const uint8_t beeperArming[] = { 20, 0 }; - static const uint8_t beeperSystemInit[] = { 10, 0 }; - static const uint8_t beeperBatteryLow[] = { 30, 0 }; - static const uint8_t beeperBatteryCritical[] = { 50, 0 }; - - static const uint8_t* beeperSchemes[] = { - //BUZZER_SILENCE - beeperSilence, - //BUZZER_GYRO_CALIBRATED, - beeperGyroCalibrated, - //BUZZER_RX_LOST, // Beeps when TX is turned off or signal lost (repeat until TX is okay) - beeperRxLost, - //BUZZER_RX_LOST_LANDING, // Beeps SOS when armed and TX is turned off or signal lost (autolanding/autodisarm) - beeperSilence, - //BUZZER_DISARMING, // Beep when disarming the board - beeperDisarming, - //BUZZER_ARMING, // Beep when arming the board - beeperArming, - //BUZZER_ARMING_GPS_FIX, // Beep a special tone when arming the board and GPS has fix - beeperSilence, - //BUZZER_BAT_CRIT_LOW, // Longer warning beeps when battery is critically low (repeats) - beeperBatteryCritical, - //BUZZER_BAT_LOW, // Warning beeps when battery is getting low (repeats) - beeperBatteryLow, - //BUZZER_GPS_STATUS, // FIXME **** Disable beeper when connected to USB **** - beeperSilence, - //BUZZER_RX_SET, // Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled - beeperRxLost, - //BUZZER_ACC_CALIBRATION, // ACC inflight calibration completed confirmation - beeperSilence, - //BUZZER_ACC_CALIBRATION_FAIL, // ACC inflight calibration failed - beeperSilence, - //BUZZER_READY_BEEP, // Ring a tone when GPS is locked and ready - beeperSilence, - //BUZZER_MULTI_BEEPS, // Internal value used by 'beeperConfirmationBeeps()'. - beeperSilence, - //BUZZER_DISARM_REPEAT, // Beeps sounded while stick held in disarm position - beeperSilence, - //BUZZER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased) - beeperSilence, - //BUZZER_SYSTEM_INIT, // Initialisation beeps when board is powered on - beeperSystemInit, - //BUZZER_USB, // Some boards have beeper powered USB connected - beeperSilence, - //BUZZER_BLACKBOX_ERASE, // Beep when blackbox erase completes - beeperSilence, - //BUZZER_CRASH_FLIP_MODE, // Crash flip mode is active - beeperSilence, - //BUZZER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated - beeperSilence, - //BUZZER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop - beeperSilence, - //BUZZER_ALL, // Turn ON or OFF all beeper conditions - beeperSilence, - //BUZZER_PREFERENCE, // Save preferred beeper configuration - beeperSilence - // BUZZER_ALL and BUZZER_PREFERENCE must remain at the bottom of this enum - }; - - return beeperSchemes; - } - - Model& _model; - - BuzzerPlayStatus _status; - uint32_t _wait; - const uint8_t * _scheme; - BuzzerEvent _e; + BuzzerPlayStatus _status; + uint32_t _wait; + const uint8_t * _scheme; + BuzzerEvent _e; }; } - -#endif From 6156bde13f5ddf23a838863b98b74fcbaade47a8 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 2 Nov 2024 23:51:47 +0100 Subject: [PATCH 44/68] remove unittest defines --- lib/Espfc/src/Buzzer.cpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/lib/Espfc/src/Buzzer.cpp b/lib/Espfc/src/Buzzer.cpp index eb453c86..81c71a4e 100644 --- a/lib/Espfc/src/Buzzer.cpp +++ b/lib/Espfc/src/Buzzer.cpp @@ -1,6 +1,4 @@ -#ifndef UNIT_TEST -#include -#endif +//#include #include "Buzzer.h" #include "Hal/Gpio.h" @@ -10,11 +8,9 @@ Buzzer::Buzzer(Model& model): _model(model), _status(BUZZER_STATUS_IDLE), _wait( int Buzzer::begin() { -#ifndef UNIT_TEST if(_model.config.pin[PIN_BUZZER] == -1) return 0; Hal::Gpio::pinMode(_model.config.pin[PIN_BUZZER], OUTPUT); Hal::Gpio::digitalWrite(_model.config.pin[PIN_BUZZER], (pin_status_t)_model.config.buzzer.inverted); -#endif _model.state.buzzer.timer.setRate(100); return 1; @@ -26,9 +22,7 @@ int Buzzer::update() //_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; @@ -75,9 +69,7 @@ void Buzzer::_play(bool v, int time, BuzzerPlayStatus s) void Buzzer::_write(bool v) { -#ifndef UNIT_TEST Hal::Gpio::digitalWrite(_model.config.pin[PIN_BUZZER], (pin_status_t)(_model.config.buzzer.inverted ? !v : v)); -#endif } void Buzzer::_delay(int time) From 10dcd767dcacab060e854d62e42b20f52f399015 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 3 Nov 2024 00:06:10 +0100 Subject: [PATCH 45/68] move filter to utils namespace --- lib/Espfc/src/Control/Controller.h | 2 +- lib/Espfc/src/Control/Pid.h | 14 +++++++------- lib/Espfc/src/Input.h | 2 +- lib/Espfc/src/Math/FFTAnalyzer.h | 2 +- lib/Espfc/src/Math/FreqAnalyzer.h | 4 ++-- lib/Espfc/src/ModelConfig.h | 2 +- lib/Espfc/src/ModelState.h | 26 +++++++++++++------------- lib/Espfc/src/Sensor/AccelSensor.h | 2 +- lib/Espfc/src/Sensor/BaroSensor.h | 8 ++++---- lib/Espfc/src/Sensor/VoltageSensor.h | 10 +++++----- lib/Espfc/src/{ => Utils}/Filter.cpp | 6 +++++- lib/Espfc/src/{ => Utils}/Filter.h | 4 ++++ lib/Espfc/src/Utils/FilterHelper.h | 2 +- test/test_math/test_math.cpp | 3 ++- 14 files changed, 48 insertions(+), 39 deletions(-) rename lib/Espfc/src/{ => Utils}/Filter.cpp (99%) rename lib/Espfc/src/{ => Utils}/Filter.h (99%) diff --git a/lib/Espfc/src/Control/Controller.h b/lib/Espfc/src/Control/Controller.h index 87432151..1f386e1c 100644 --- a/lib/Espfc/src/Control/Controller.h +++ b/lib/Espfc/src/Control/Controller.h @@ -26,7 +26,7 @@ class Controller private: Model& _model; Rates _rates; - Filter _speedFilter; + Utils::Filter _speedFilter; }; } diff --git a/lib/Espfc/src/Control/Pid.h b/lib/Espfc/src/Control/Pid.h index 2b71a520..453a0f5e 100644 --- a/lib/Espfc/src/Control/Pid.h +++ b/lib/Espfc/src/Control/Pid.h @@ -1,7 +1,7 @@ #pragma once #include -#include "Filter.h" +#include "Utils/Filter.h" #include "Math/Utils.h" namespace Espfc { @@ -64,12 +64,12 @@ class Pid float dTerm; float fTerm; - Filter dtermFilter; - Filter dtermFilter2; - Filter dtermNotchFilter; - Filter ptermFilter; - Filter ftermFilter; - Filter itermRelaxFilter; + Utils::Filter dtermFilter; + Utils::Filter dtermFilter2; + Utils::Filter dtermNotchFilter; + Utils::Filter ptermFilter; + Utils::Filter ftermFilter; + Utils::Filter itermRelaxFilter; float prevMeasurement; float prevError; diff --git a/lib/Espfc/src/Input.h b/lib/Espfc/src/Input.h index 68730a1b..4f014cca 100644 --- a/lib/Espfc/src/Input.h +++ b/lib/Espfc/src/Input.h @@ -58,7 +58,7 @@ class Input Model& _model; TelemetryManager& _telemetry; Device::InputDevice * _device; - Filter _filter[INPUT_CHANNELS]; + Utils::Filter _filter[INPUT_CHANNELS]; float _step; Device::InputPPM _ppm; Device::InputSBUS _sbus; diff --git a/lib/Espfc/src/Math/FFTAnalyzer.h b/lib/Espfc/src/Math/FFTAnalyzer.h index 31749b20..e9293a8f 100644 --- a/lib/Espfc/src/Math/FFTAnalyzer.h +++ b/lib/Espfc/src/Math/FFTAnalyzer.h @@ -4,7 +4,7 @@ // https://github.com/espressif/esp-dsp/blob/5f2bfe1f3ee7c9b024350557445b32baf6407a08/examples/fft4real/main/dsps_fft4real_main.c #include "Math/Utils.h" -#include "Filter.h" +#include "Utils/Filter.h" #include "dsps_fft4r.h" #include "dsps_wind_hann.h" #include diff --git a/lib/Espfc/src/Math/FreqAnalyzer.h b/lib/Espfc/src/Math/FreqAnalyzer.h index a76e156d..7b5be7fe 100644 --- a/lib/Espfc/src/Math/FreqAnalyzer.h +++ b/lib/Espfc/src/Math/FreqAnalyzer.h @@ -2,7 +2,7 @@ #define _ESPFC_MATH_FREQ_ANALYZER_H_ #include "Math/Utils.h" -#include "Filter.h" +#include "Utils/Filter.h" namespace Espfc { @@ -57,7 +57,7 @@ class FreqAnalyzer float noise; private: - Filter _bpf; + Utils::Filter _bpf; float _rate; float _freq_min; diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 2ab13759..ffebb324 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -3,7 +3,7 @@ #include "Target/Target.h" #include "EscDriver.h" -#include "Filter.h" +#include "Utils/Filter.h" #include "Device/BusDevice.h" #include "Device/GyroDevice.h" #include "Device/MagDevice.h" diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 412ef0b8..4575c6ea 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -11,7 +11,7 @@ #include "helper_3dmath.h" #include "Control/Pid.h" #include "Kalman.h" -#include "Filter.h" +#include "Utils/Filter.h" #include "Stats.h" #include "Timer.h" #include "Device/SerialDevice.h" @@ -194,7 +194,7 @@ struct InputState float us[INPUT_CHANNELS]; float ch[INPUT_CHANNELS]; - Filter filter[AXIS_COUNT_RPYT]; + Utils::Filter filter[AXIS_COUNT_RPYT]; Timer timer; }; @@ -218,7 +218,7 @@ struct MagState VectorInt16 raw; VectorFloat adc; - Filter filter[3]; + Utils::Filter filter[3]; Timer timer; int calibrationSamples; @@ -268,14 +268,14 @@ struct GyroState int calibrationState; int calibrationRate; - Filter filter[AXIS_COUNT_RPY]; - Filter filter2[AXIS_COUNT_RPY]; - Filter filter3[AXIS_COUNT_RPY]; - Filter notch1Filter[AXIS_COUNT_RPY]; - Filter notch2Filter[AXIS_COUNT_RPY]; - Filter dynNotchFilter[DYN_NOTCH_COUNT_MAX][AXIS_COUNT_RPY]; - Filter rpmFilter[RPM_FILTER_MOTOR_MAX][RPM_FILTER_HARMONICS_MAX][AXIS_COUNT_RPY]; - Filter rpmFreqFilter[RPM_FILTER_MOTOR_MAX]; + Utils::Filter filter[AXIS_COUNT_RPY]; + Utils::Filter filter2[AXIS_COUNT_RPY]; + Utils::Filter filter3[AXIS_COUNT_RPY]; + Utils::Filter notch1Filter[AXIS_COUNT_RPY]; + Utils::Filter notch2Filter[AXIS_COUNT_RPY]; + Utils::Filter dynNotchFilter[DYN_NOTCH_COUNT_MAX][AXIS_COUNT_RPY]; + Utils::Filter rpmFilter[RPM_FILTER_MOTOR_MAX][RPM_FILTER_HARMONICS_MAX][AXIS_COUNT_RPY]; + Utils::Filter rpmFreqFilter[RPM_FILTER_MOTOR_MAX]; Timer timer; Timer dynamicFilterTimer; @@ -287,7 +287,7 @@ struct AccelState VectorInt16 raw; VectorFloat adc; VectorFloat prev; - Filter filter[AXIS_COUNT_RPY]; + Utils::Filter filter[AXIS_COUNT_RPY]; Timer timer; float scale; @@ -300,7 +300,7 @@ struct AccelState struct AttitudeState { VectorFloat rate; - Filter filter[AXIS_COUNT_RPY]; + Utils::Filter filter[AXIS_COUNT_RPY]; VectorFloat euler; Quaternion quaternion; }; diff --git a/lib/Espfc/src/Sensor/AccelSensor.h b/lib/Espfc/src/Sensor/AccelSensor.h index c67301f2..970d698c 100644 --- a/lib/Espfc/src/Sensor/AccelSensor.h +++ b/lib/Espfc/src/Sensor/AccelSensor.h @@ -23,7 +23,7 @@ class AccelSensor: public BaseSensor Model& _model; Device::GyroDevice * _gyro; - Filter _filter[AXIS_COUNT_RPY]; + Utils::Filter _filter[AXIS_COUNT_RPY]; }; } diff --git a/lib/Espfc/src/Sensor/BaroSensor.h b/lib/Espfc/src/Sensor/BaroSensor.h index d5f82019..adb31b13 100644 --- a/lib/Espfc/src/Sensor/BaroSensor.h +++ b/lib/Espfc/src/Sensor/BaroSensor.h @@ -4,7 +4,7 @@ #include "BaseSensor.h" #include "Model.h" #include "Device/BaroDevice.h" -#include "Filter.h" +#include "Utils/Filter.h" namespace Espfc { @@ -34,9 +34,9 @@ class BaroSensor: public BaseSensor Model& _model; Device::BaroDevice * _baro; BaroState _state; - Filter _temperatureFilter; - Filter _pressureFilter; - Filter _altitudeFilter; + Utils::Filter _temperatureFilter; + Utils::Filter _pressureFilter; + Utils::Filter _altitudeFilter; uint32_t _wait; int32_t _counter; }; diff --git a/lib/Espfc/src/Sensor/VoltageSensor.h b/lib/Espfc/src/Sensor/VoltageSensor.h index 8c8688b2..cc93c4df 100644 --- a/lib/Espfc/src/Sensor/VoltageSensor.h +++ b/lib/Espfc/src/Sensor/VoltageSensor.h @@ -2,7 +2,7 @@ #include "Model.h" #include "BaseSensor.h" -#include "Filter.h" +#include "Utils/Filter.h" namespace Espfc { @@ -23,10 +23,10 @@ class VoltageSensor: public BaseSensor private: Model& _model; - Filter _vFilterFast; - Filter _vFilter; - Filter _iFilterFast; - Filter _iFilter; + Utils::Filter _vFilterFast; + Utils::Filter _vFilter; + Utils::Filter _iFilterFast; + Utils::Filter _iFilter; State _state; }; diff --git a/lib/Espfc/src/Filter.cpp b/lib/Espfc/src/Utils/Filter.cpp similarity index 99% rename from lib/Espfc/src/Filter.cpp rename to lib/Espfc/src/Utils/Filter.cpp index 0746dfb5..7a10f11d 100644 --- a/lib/Espfc/src/Filter.cpp +++ b/lib/Espfc/src/Utils/Filter.cpp @@ -1,5 +1,5 @@ #include -#include "Filter.h" +#include "Utils/Filter.h" #include "Math/Utils.h" #include "Utils/MemoryHelper.h" @@ -28,6 +28,8 @@ FilterConfig FAST_CODE_ATTR FilterConfig::sanitize(int rate) const return FilterConfig(t, f, c); } +namespace Utils { + void FilterStatePt1::reset() { v = 0.f; @@ -480,3 +482,5 @@ float FAST_CODE_ATTR Filter::getNotchQ(float freq, float cutoff) } } + +} diff --git a/lib/Espfc/src/Filter.h b/lib/Espfc/src/Utils/Filter.h similarity index 99% rename from lib/Espfc/src/Filter.h rename to lib/Espfc/src/Utils/Filter.h index cf4b01c8..e37dc71a 100644 --- a/lib/Espfc/src/Filter.h +++ b/lib/Espfc/src/Utils/Filter.h @@ -50,6 +50,8 @@ class DynamicFilterConfig { static const int MIN_FREQ = 1000; }; +namespace Utils { + class FilterStatePt1 { public: void reset(); @@ -165,3 +167,5 @@ class Filter }; } + +} \ No newline at end of file diff --git a/lib/Espfc/src/Utils/FilterHelper.h b/lib/Espfc/src/Utils/FilterHelper.h index 76834c9a..a90b54bc 100644 --- a/lib/Espfc/src/Utils/FilterHelper.h +++ b/lib/Espfc/src/Utils/FilterHelper.h @@ -1,6 +1,6 @@ #pragma once -#include "Filter.h" +#include "Utils/Filter.h" #include namespace Espfc { diff --git a/test/test_math/test_math.cpp b/test/test_math/test_math.cpp index 23c150c0..60ebc4e7 100644 --- a/test/test_math/test_math.cpp +++ b/test/test_math/test_math.cpp @@ -5,7 +5,7 @@ #include "msp/msp_protocol.h" #include "Math/Utils.h" #include "Math/Bits.h" -#include "Filter.h" +#include "Utils/Filter.h" #include "Control/Pid.h" #include "Target/QueueAtomic.h" #include "Utils/RingBuf.h" @@ -21,6 +21,7 @@ using namespace Espfc; using namespace Espfc::Control; +using namespace Espfc::Utils; void test_math_map() { From eb7f04842c02474b55b8cde34074c33f62dc0949 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 3 Nov 2024 22:10:31 +0100 Subject: [PATCH 46/68] move timer to utils, drop timer dependency in escdriver --- lib/EscDriver/src/EscDriverEsp32.cpp | 1 - lib/EscDriver/src/EscDriverEsp32.h | 2 -- lib/Espfc/src/Control/Rates.cpp | 4 ++++ lib/Espfc/src/Control/Rates.h | 6 +++++- lib/Espfc/src/ModelState.h | 26 +++++++++++++------------- lib/Espfc/src/Stats.h | 4 ++-- lib/Espfc/src/{ => Utils}/Timer.cpp | 6 +++++- lib/Espfc/src/{ => Utils}/Timer.h | 4 ++++ test/test_fc/test_fc.cpp | 7 +++++-- 9 files changed, 38 insertions(+), 22 deletions(-) rename lib/Espfc/src/{ => Utils}/Timer.cpp (96%) rename lib/Espfc/src/{ => Utils}/Timer.h (95%) diff --git a/lib/EscDriver/src/EscDriverEsp32.cpp b/lib/EscDriver/src/EscDriverEsp32.cpp index 503e22d3..f4c08092 100644 --- a/lib/EscDriver/src/EscDriverEsp32.cpp +++ b/lib/EscDriver/src/EscDriverEsp32.cpp @@ -94,7 +94,6 @@ int EscDriverEsp32::begin(const EscConfig& conf) _interval = TO_INTERVAL_US(_rate); _digital = isDigital(_protocol); _dshot_tlm = conf.dshotTelemetry && (_protocol == ESC_PROTOCOL_DSHOT300 || _protocol == ESC_PROTOCOL_DSHOT600); - _timer.setInterval(1000000); return 1; } diff --git a/lib/EscDriver/src/EscDriverEsp32.h b/lib/EscDriver/src/EscDriverEsp32.h index 97f0e3df..e2b6e3f1 100644 --- a/lib/EscDriver/src/EscDriverEsp32.h +++ b/lib/EscDriver/src/EscDriverEsp32.h @@ -5,7 +5,6 @@ #include "EscDriver.h" #include -#include "Timer.h" class EscDriverEsp32: public EscDriverBase { @@ -120,7 +119,6 @@ class EscDriverEsp32: public EscDriverBase static bool _tx_end_installed; static EscDriverEsp32* instances[]; - Espfc::Timer _timer; }; #endif diff --git a/lib/Espfc/src/Control/Rates.cpp b/lib/Espfc/src/Control/Rates.cpp index b2191ba2..585333e7 100644 --- a/lib/Espfc/src/Control/Rates.cpp +++ b/lib/Espfc/src/Control/Rates.cpp @@ -3,6 +3,8 @@ namespace Espfc { +namespace Control { + constexpr float SETPOINT_RATE_LIMIT = 1998.0f; constexpr float RC_RATE_INCREMENTAL = 14.54f; @@ -111,3 +113,5 @@ float FAST_CODE_ATTR Rates::quick(const int axis, float rcCommandf, const float } } + +} diff --git a/lib/Espfc/src/Control/Rates.h b/lib/Espfc/src/Control/Rates.h index 3831dd65..868bbf54 100644 --- a/lib/Espfc/src/Control/Rates.h +++ b/lib/Espfc/src/Control/Rates.h @@ -14,6 +14,8 @@ enum RateType { RATES_TYPE_QUICK, }; +namespace Control { + class Rates { public: @@ -50,4 +52,6 @@ class Rates int16_t rateLimit[3]; }; -} // namespace Espfc +} + +} diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 4575c6ea..fa190203 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -12,8 +12,8 @@ #include "Control/Pid.h" #include "Kalman.h" #include "Utils/Filter.h" +#include "Utils/Timer.h" #include "Stats.h" -#include "Timer.h" #include "Device/SerialDevice.h" #include "Math/FreqAnalyzer.h" #include "Msp/Msp.h" @@ -80,7 +80,7 @@ class BuzzerState return idx >= BUZZER_MAX_EVENTS; } - Timer timer; + Utils::Timer timer; BuzzerEvent events[BUZZER_MAX_EVENTS]; size_t idx; int32_t beeperMask; @@ -105,7 +105,7 @@ class BatteryState float percentage; int8_t cells; int8_t samples; - Timer timer; + Utils::Timer timer; }; enum CalibrationState { @@ -196,12 +196,12 @@ struct InputState Utils::Filter filter[AXIS_COUNT_RPYT]; - Timer timer; + Utils::Timer timer; }; struct MixerState { - Timer timer; + Utils::Timer timer; float minThrottle; float maxThrottle; bool digitalOutput; @@ -219,7 +219,7 @@ struct MagState VectorInt16 raw; VectorFloat adc; Utils::Filter filter[3]; - Timer timer; + Utils::Timer timer; int calibrationSamples; int calibrationState; @@ -277,8 +277,8 @@ struct GyroState Utils::Filter rpmFilter[RPM_FILTER_MOTOR_MAX][RPM_FILTER_HARMONICS_MAX][AXIS_COUNT_RPY]; Utils::Filter rpmFreqFilter[RPM_FILTER_MOTOR_MAX]; - Timer timer; - Timer dynamicFilterTimer; + Utils::Timer timer; + Utils::Timer dynamicFilterTimer; }; struct AccelState @@ -288,7 +288,7 @@ struct AccelState VectorFloat adc; VectorFloat prev; Utils::Filter filter[AXIS_COUNT_RPY]; - Timer timer; + Utils::Timer timer; float scale; VectorFloat bias; @@ -345,10 +345,10 @@ struct ModelState OutputState output; int32_t loopRate; - Timer loopTimer; + Utils::Timer loopTimer; - Timer actuatorTimer; - Timer telemetryTimer; + Utils::Timer actuatorTimer; + Utils::Timer telemetryTimer; ModeState mode; Stats stats; @@ -366,7 +366,7 @@ struct ModelState int16_t i2cErrorDelta; SerialPortState serial[SERIAL_UART_COUNT]; - Timer serialTimer; + Utils::Timer serialTimer; Target::Queue appQueue; diff --git a/lib/Espfc/src/Stats.h b/lib/Espfc/src/Stats.h index 20f3d102..0c1994b9 100644 --- a/lib/Espfc/src/Stats.h +++ b/lib/Espfc/src/Stats.h @@ -1,7 +1,7 @@ #ifndef _ESPFC_STATS_H_ #define _ESPFC_STATS_H_ -#include "Timer.h" +#include "Utils/Timer.h" namespace Espfc { @@ -201,7 +201,7 @@ class Stats } } - Timer timer; + Utils::Timer timer; private: uint32_t _start[COUNTER_COUNT]; diff --git a/lib/Espfc/src/Timer.cpp b/lib/Espfc/src/Utils/Timer.cpp similarity index 96% rename from lib/Espfc/src/Timer.cpp rename to lib/Espfc/src/Utils/Timer.cpp index 71696458..c82fd599 100644 --- a/lib/Espfc/src/Timer.cpp +++ b/lib/Espfc/src/Utils/Timer.cpp @@ -1,9 +1,11 @@ #include -#include "Timer.h" +#include "Utils/Timer.h" #include "Utils/MemoryHelper.h" namespace Espfc { +namespace Utils { + Timer::Timer(): interval(0), last(0), next(0), iteration(0), delta(0) { } @@ -68,3 +70,5 @@ bool FAST_CODE_ATTR Timer::syncTo(const Timer& t, uint32_t slot) } } + +} diff --git a/lib/Espfc/src/Timer.h b/lib/Espfc/src/Utils/Timer.h similarity index 95% rename from lib/Espfc/src/Timer.h rename to lib/Espfc/src/Utils/Timer.h index 189587d4..3de7a958 100644 --- a/lib/Espfc/src/Timer.h +++ b/lib/Espfc/src/Utils/Timer.h @@ -4,6 +4,8 @@ namespace Espfc { +namespace Utils { + class Timer { public: @@ -29,3 +31,5 @@ class Timer }; } + +} diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index 2dca3cfe..cfac4ebd 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -1,7 +1,7 @@ #include #include #include -#include "Timer.h" +#include "Utils/Timer.h" #include "Model.h" #include "Control/Controller.h" #include "Control/Actuator.h" @@ -9,7 +9,10 @@ using namespace fakeit; using namespace Espfc; -using namespace Espfc::Control; +using Espfc::Control::Rates; +using Espfc::Control::Controller; +using Espfc::Control::Actuator; +using Espfc::Utils::Timer; /*void setUp(void) { From 5c53a61d430fbd32e54f4b2473d8aabab0e6cf4f Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 3 Nov 2024 22:25:31 +0100 Subject: [PATCH 47/68] move stats to utils namespace --- lib/Espfc/src/Blackbox/Blackbox.cpp | 2 +- lib/Espfc/src/Control/Actuator.cpp | 2 +- lib/Espfc/src/Control/Controller.cpp | 4 +- lib/Espfc/src/Control/Fusion.cpp | 2 +- lib/Espfc/src/Espfc.cpp | 4 +- lib/Espfc/src/Input.cpp | 6 +- lib/Espfc/src/ModelState.h | 4 +- lib/Espfc/src/Output/Mixer.cpp | 6 +- lib/Espfc/src/Sensor/AccelSensor.cpp | 4 +- lib/Espfc/src/Sensor/BaroSensor.cpp | 2 +- lib/Espfc/src/Sensor/GyroSensor.cpp | 8 +- lib/Espfc/src/Sensor/MagSensor.cpp | 4 +- lib/Espfc/src/Sensor/VoltageSensor.cpp | 2 +- lib/Espfc/src/SerialManager.cpp | 2 +- lib/Espfc/src/Stats.h | 219 ------------------------- lib/Espfc/src/TelemetryManager.h | 2 +- lib/Espfc/src/Utils/Stats.cpp | 158 ++++++++++++++++++ lib/Espfc/src/Utils/Stats.h | 98 +++++++++++ lib/Espfc/src/Wireless.h | 2 +- 19 files changed, 284 insertions(+), 247 deletions(-) delete mode 100644 lib/Espfc/src/Stats.h create mode 100644 lib/Espfc/src/Utils/Stats.cpp create mode 100644 lib/Espfc/src/Utils/Stats.h diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index d37ecc73..fd58058f 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -208,7 +208,7 @@ int FAST_CODE_ATTR Blackbox::update() if(!_model.blackboxEnabled()) return 0; if(_model.config.blackbox.dev == BLACKBOX_DEV_SERIAL && !_serial) return 0; - Stats::Measure measure(_model.state.stats, COUNTER_BLACKBOX); + Utils::Stats::Measure measure(_model.state.stats, COUNTER_BLACKBOX); uint32_t startTime = micros(); updateArmed(); diff --git a/lib/Espfc/src/Control/Actuator.cpp b/lib/Espfc/src/Control/Actuator.cpp index 0cff63f7..f476b6bc 100644 --- a/lib/Espfc/src/Control/Actuator.cpp +++ b/lib/Espfc/src/Control/Actuator.cpp @@ -28,7 +28,7 @@ int Actuator::begin() int Actuator::update() { uint32_t startTime = micros(); - Stats::Measure(_model.state.stats, COUNTER_ACTUATOR); + Utils::Stats::Measure(_model.state.stats, COUNTER_ACTUATOR); updateArmingDisabled(); updateModeMask(); updateArmed(); diff --git a/lib/Espfc/src/Control/Controller.cpp b/lib/Espfc/src/Control/Controller.cpp index d57f9841..2dd6eb9a 100644 --- a/lib/Espfc/src/Control/Controller.cpp +++ b/lib/Espfc/src/Control/Controller.cpp @@ -24,7 +24,7 @@ int FAST_CODE_ATTR Controller::update() } { - Stats::Measure(_model.state.stats, COUNTER_OUTER_PID); + Utils::Stats::Measure(_model.state.stats, COUNTER_OUTER_PID); resetIterm(); if(_model.config.mixer.type == FC_MIXER_GIMBAL) { @@ -37,7 +37,7 @@ int FAST_CODE_ATTR Controller::update() } { - Stats::Measure(_model.state.stats, COUNTER_INNER_PID); + Utils::Stats::Measure(_model.state.stats, COUNTER_INNER_PID); if(_model.config.mixer.type == FC_MIXER_GIMBAL) { innerLoopRobot(); diff --git a/lib/Espfc/src/Control/Fusion.cpp b/lib/Espfc/src/Control/Fusion.cpp index ab057c2e..06715bf8 100644 --- a/lib/Espfc/src/Control/Fusion.cpp +++ b/lib/Espfc/src/Control/Fusion.cpp @@ -32,7 +32,7 @@ void Fusion::restoreGain() int FAST_CODE_ATTR Fusion::update() { - Stats::Measure measure(_model.state.stats, COUNTER_IMU_FUSION); + Utils::Stats::Measure measure(_model.state.stats, COUNTER_IMU_FUSION); if(_model.accelActive()) { diff --git a/lib/Espfc/src/Espfc.cpp b/lib/Espfc/src/Espfc.cpp index 1714b307..7d996f2d 100644 --- a/lib/Espfc/src/Espfc.cpp +++ b/lib/Espfc/src/Espfc.cpp @@ -44,7 +44,7 @@ int FAST_CODE_ATTR Espfc::update(bool externalTrigger) { if(!_model.state.gyro.timer.check()) return 0; } - Stats::Measure measure(_model.state.stats, COUNTER_CPU_0); + Utils::Stats::Measure measure(_model.state.stats, COUNTER_CPU_0); #if defined(ESPFC_MULTI_CORE) @@ -102,7 +102,7 @@ int FAST_CODE_ATTR Espfc::updateOther() } Event e = _model.state.appQueue.receive(); - Stats::Measure measure(_model.state.stats, COUNTER_CPU_1); + Utils::Stats::Measure measure(_model.state.stats, COUNTER_CPU_1); switch(e.type) { diff --git a/lib/Espfc/src/Input.cpp b/lib/Espfc/src/Input.cpp index 38f04616..b1fba036 100644 --- a/lib/Espfc/src/Input.cpp +++ b/lib/Espfc/src/Input.cpp @@ -98,7 +98,7 @@ int FAST_CODE_ATTR Input::update() InputStatus FAST_CODE_ATTR Input::readInputs() { - Stats::Measure readMeasure(_model.state.stats, COUNTER_INPUT_READ); + Utils::Stats::Measure readMeasure(_model.state.stats, COUNTER_INPUT_READ); uint32_t startTime = micros(); InputStatus status = _device->update(); @@ -183,7 +183,7 @@ void FAST_CODE_ATTR Input::processInputs() bool FAST_CODE_ATTR Input::failsafe(InputStatus status) { - Stats::Measure readMeasure(_model.state.stats, COUNTER_FAILSAFE); + Utils::Stats::Measure readMeasure(_model.state.stats, COUNTER_FAILSAFE); if(_model.isSwitchActive(MODE_FAILSAFE)) { @@ -251,7 +251,7 @@ void FAST_CODE_ATTR Input::failsafeStage2() void FAST_CODE_ATTR Input::filterInputs(InputStatus status) { - Stats::Measure filterMeasure(_model.state.stats, COUNTER_INPUT_FILTER); + Utils::Stats::Measure filterMeasure(_model.state.stats, COUNTER_INPUT_FILTER); uint32_t startTime = micros(); const bool newFrame = status != INPUT_IDLE; diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index fa190203..d930b7c4 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -13,7 +13,7 @@ #include "Kalman.h" #include "Utils/Filter.h" #include "Utils/Timer.h" -#include "Stats.h" +#include "Utils/Stats.h" #include "Device/SerialDevice.h" #include "Math/FreqAnalyzer.h" #include "Msp/Msp.h" @@ -351,7 +351,7 @@ struct ModelState Utils::Timer telemetryTimer; ModeState mode; - Stats stats; + Utils::Stats stats; int16_t debug[DEBUG_VALUE_COUNT]; diff --git a/lib/Espfc/src/Output/Mixer.cpp b/lib/Espfc/src/Output/Mixer.cpp index 561353f1..e4bdda59 100644 --- a/lib/Espfc/src/Output/Mixer.cpp +++ b/lib/Espfc/src/Output/Mixer.cpp @@ -108,7 +108,7 @@ int FAST_CODE_ATTR Mixer::update() void FAST_CODE_ATTR Mixer::updateMixer(const MixerConfig& mixer, float * outputs) { - Stats::Measure mixerMeasure(_model.state.stats, COUNTER_MIXER); + Utils::Stats::Measure mixerMeasure(_model.state.stats, COUNTER_MIXER); float sources[MIXER_SOURCE_MAX]; sources[MIXER_SOURCE_NULL] = 0; @@ -236,7 +236,7 @@ float FAST_CODE_ATTR Mixer::limitOutput(float output, const OutputChannelConfig& void FAST_CODE_ATTR Mixer::writeOutput(const MixerConfig& mixer, float * out) { - Stats::Measure mixerMeasure(_model.state.stats, COUNTER_MIXER_WRITE); + Utils::Stats::Measure mixerMeasure(_model.state.stats, COUNTER_MIXER_WRITE); bool stop = _stop(); for(size_t i = 0; i < OUTPUT_CHANNELS; i++) @@ -280,7 +280,7 @@ void FAST_CODE_ATTR Mixer::writeOutput(const MixerConfig& mixer, float * out) void FAST_CODE_ATTR Mixer::readTelemetry() { - Stats::Measure mixerMeasure(_model.state.stats, COUNTER_MIXER_READ); + Utils::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++) diff --git a/lib/Espfc/src/Sensor/AccelSensor.cpp b/lib/Espfc/src/Sensor/AccelSensor.cpp index 432f8f0a..72b78828 100644 --- a/lib/Espfc/src/Sensor/AccelSensor.cpp +++ b/lib/Espfc/src/Sensor/AccelSensor.cpp @@ -44,7 +44,7 @@ int FAST_CODE_ATTR AccelSensor::read() //if(!_model.state.accel.timer.check()) return 0; - Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_READ); + Utils::Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_READ); _gyro->readAccel(_model.state.accel.raw); return 1; @@ -54,7 +54,7 @@ int FAST_CODE_ATTR AccelSensor::filter() { if(!_model.accelActive()) return 0; - Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_FILTER); + Utils::Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_FILTER); _model.state.accel.adc = (VectorFloat)_model.state.accel.raw * _model.state.accel.scale; diff --git a/lib/Espfc/src/Sensor/BaroSensor.cpp b/lib/Espfc/src/Sensor/BaroSensor.cpp index cba071bd..5751ac6a 100644 --- a/lib/Espfc/src/Sensor/BaroSensor.cpp +++ b/lib/Espfc/src/Sensor/BaroSensor.cpp @@ -44,7 +44,7 @@ int BaroSensor::read() if(_wait > micros()) return 0; - Stats::Measure measure(_model.state.stats, COUNTER_BARO); + Utils::Stats::Measure measure(_model.state.stats, COUNTER_BARO); if(_model.config.debug.mode == DEBUG_BARO) { diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index 68447dca..22e4941d 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -63,7 +63,7 @@ int FAST_CODE_ATTR GyroSensor::read() { if (!_model.gyroActive()) return 0; - Stats::Measure measure(_model.state.stats, COUNTER_GYRO_READ); + Utils::Stats::Measure measure(_model.state.stats, COUNTER_GYRO_READ); _gyro->readGyro(_model.state.gyro.raw); @@ -87,7 +87,7 @@ int FAST_CODE_ATTR GyroSensor::filter() { if (!_model.gyroActive()) return 0; - Stats::Measure measure(_model.state.stats, COUNTER_GYRO_FILTER); + Utils::Stats::Measure measure(_model.state.stats, COUNTER_GYRO_FILTER); _model.state.gyro.adc = _model.state.gyro.sampled; @@ -162,7 +162,7 @@ void FAST_CODE_ATTR GyroSensor::rpmFilterUpdate() { if (!_rpm_enabled) return; - Stats::Measure measure(_model.state.stats, COUNTER_RPM_UPDATE); + Utils::Stats::Measure measure(_model.state.stats, COUNTER_RPM_UPDATE); const float motorFreq = _model.state.output.telemetry.freq[_rpm_motor_index]; for (size_t n = 0; n < _model.config.gyro.rpmFilter.harmonics; n++) @@ -198,7 +198,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() if (_dyn_notch_enabled || _dyn_notch_debug) { - Stats::Measure measure(_model.state.stats, COUNTER_GYRO_FFT); + Utils::Stats::Measure measure(_model.state.stats, COUNTER_GYRO_FFT); const float q = _model.config.gyro.dynamicFilter.q * 0.01; bool feed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0; diff --git a/lib/Espfc/src/Sensor/MagSensor.cpp b/lib/Espfc/src/Sensor/MagSensor.cpp index f98cf639..75a23084 100644 --- a/lib/Espfc/src/Sensor/MagSensor.cpp +++ b/lib/Espfc/src/Sensor/MagSensor.cpp @@ -42,7 +42,7 @@ int MagSensor::read() { if(!_mag || !_model.magActive() || !_model.state.mag.timer.check()) return 0; - Stats::Measure measure(_model.state.stats, COUNTER_MAG_READ); + Utils::Stats::Measure measure(_model.state.stats, COUNTER_MAG_READ); _mag->readMag(_model.state.mag.raw); return 1; @@ -52,7 +52,7 @@ int MagSensor::filter() { if(!_mag || !_model.magActive()) return 0; - Stats::Measure measure(_model.state.stats, COUNTER_MAG_FILTER); + Utils::Stats::Measure measure(_model.state.stats, COUNTER_MAG_FILTER); _model.state.mag.adc = _mag->convert(_model.state.mag.raw); diff --git a/lib/Espfc/src/Sensor/VoltageSensor.cpp b/lib/Espfc/src/Sensor/VoltageSensor.cpp index 6718ba01..3b3f9ed7 100644 --- a/lib/Espfc/src/Sensor/VoltageSensor.cpp +++ b/lib/Espfc/src/Sensor/VoltageSensor.cpp @@ -28,7 +28,7 @@ int VoltageSensor::update() { if (!_model.state.battery.timer.check()) return 0; - Stats::Measure measure(_model.state.stats, COUNTER_BATTERY); + Utils::Stats::Measure measure(_model.state.stats, COUNTER_BATTERY); switch (_state) { diff --git a/lib/Espfc/src/SerialManager.cpp b/lib/Espfc/src/SerialManager.cpp index 80704906..c5a5fe87 100644 --- a/lib/Espfc/src/SerialManager.cpp +++ b/lib/Espfc/src/SerialManager.cpp @@ -147,7 +147,7 @@ int FAST_CODE_ATTR SerialManager::update() { if(!serialRx) { - Stats::Measure measure(_model.state.stats, COUNTER_SERIAL); + Utils::Stats::Measure measure(_model.state.stats, COUNTER_SERIAL); size_t len = stream->available(); if(len > 0) { diff --git a/lib/Espfc/src/Stats.h b/lib/Espfc/src/Stats.h deleted file mode 100644 index 0c1994b9..00000000 --- a/lib/Espfc/src/Stats.h +++ /dev/null @@ -1,219 +0,0 @@ -#ifndef _ESPFC_STATS_H_ -#define _ESPFC_STATS_H_ - -#include "Utils/Timer.h" - -namespace Espfc { - -enum StatCounter { - COUNTER_GYRO_READ, - COUNTER_GYRO_FILTER, - COUNTER_GYRO_FFT, - COUNTER_RPM_UPDATE, - COUNTER_ACCEL_READ, - COUNTER_ACCEL_FILTER, - COUNTER_MAG_READ, - COUNTER_MAG_FILTER, - COUNTER_BARO, - COUNTER_IMU_FUSION, - COUNTER_IMU_FUSION2, - 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, - COUNTER_BATTERY, - COUNTER_CPU_0, - COUNTER_CPU_1, - COUNTER_COUNT -}; - -class Stats -{ - public: - class Measure - { - public: - inline Measure(Stats& stats, StatCounter counter): _stats(stats), _counter(counter) - { - _stats.start(_counter); - } - inline ~Measure() - { - _stats.end(_counter); - } - private: - Stats& _stats; - StatCounter _counter; - }; - - Stats(): _loop_last(0), _loop_time(0) - { - for(size_t i = 0; i < COUNTER_COUNT; i++) - { - _start[i] = 0; - _avg[i] = 0; - _sum[i] = 0; - _count[i] = 0; - _freq[i] = 0; - } - } - - inline void start(StatCounter c) IRAM_ATTR - { - _start[c] = micros(); - } - - inline void end(StatCounter c) IRAM_ATTR - { - uint32_t diff = micros() - _start[c]; - _sum[c] += diff; - _count[c]++; - } - - void loopTick() - { - uint32_t now = micros(); - uint32_t diff = now - _loop_last; - //_loop_time = diff; - _loop_time += (((int32_t)diff - _loop_time + 4) >> 3); - _loop_last = now; - } - - uint32_t loopTime() const - { - return _loop_time; - } - - void update() - { - if(!timer.check()) return; - for(size_t i = 0; i < COUNTER_COUNT; i++) - { - _avg[i] = (float)(_sum[i] + (_count[i] >> 1)) / timer.delta; - _freq[i] = (float)_count[i] * 1e6 / timer.delta; - _real[i] = _count[i] > 0 ? ((float)(_sum[i] + (_count[i] >> 1)) / _count[i]) : 0.0f; - _sum[i] = 0; - _count[i] = 0; - } - } - - float getLoad(StatCounter c) const - { - return _avg[c] * 100.f; - } - - /** - * @brief Get the Time of counter normalized to 1 ms - */ - float getTime(StatCounter c) const - { - return _avg[c] * 1000.0f; - } - - float getReal(StatCounter c) const - { - return _real[c]; - } - - float getFreq(StatCounter c) const - { - return _freq[c]; - } - - float getTotalLoad() const - { - float ret = 0; - for(size_t i = 0; i < COUNTER_COUNT; i++) - { - if(i == COUNTER_CPU_0 || i == COUNTER_CPU_1) continue; - ret += getLoad((StatCounter)i); - } - return ret; - } - - float getTotalTime() const - { - float ret = 0; - for(size_t i = 0; i < COUNTER_COUNT; i++) - { - if(i == COUNTER_CPU_0 || i == COUNTER_CPU_1) continue; - ret += getTime((StatCounter)i); - } - return ret; - } - - float getCpuLoad() const - { - 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 - { - return getTime(COUNTER_CPU_0) + getTime(COUNTER_CPU_1); - } - - const char * getName(StatCounter c) const - { - 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_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"); - } - } - - Utils::Timer timer; - - private: - uint32_t _start[COUNTER_COUNT]; - uint32_t _sum[COUNTER_COUNT]; - uint32_t _count[COUNTER_COUNT]; - float _avg[COUNTER_COUNT]; - float _freq[COUNTER_COUNT]; - float _real[COUNTER_COUNT]; - uint32_t _loop_last; - int32_t _loop_time; -}; - -} - -#endif diff --git a/lib/Espfc/src/TelemetryManager.h b/lib/Espfc/src/TelemetryManager.h index 84fe58c5..f7968e60 100644 --- a/lib/Espfc/src/TelemetryManager.h +++ b/lib/Espfc/src/TelemetryManager.h @@ -20,7 +20,7 @@ class TelemetryManager int process(Device::SerialDevice& s, TelemetryProtocol protocol) const { - Stats::Measure measure(_model.state.stats, COUNTER_TELEMETRY); + Utils::Stats::Measure measure(_model.state.stats, COUNTER_TELEMETRY); switch(protocol) { diff --git a/lib/Espfc/src/Utils/Stats.cpp b/lib/Espfc/src/Utils/Stats.cpp new file mode 100644 index 00000000..2df91755 --- /dev/null +++ b/lib/Espfc/src/Utils/Stats.cpp @@ -0,0 +1,158 @@ +#include +#include "Utils/Stats.h" +#include "Utils/MemoryHelper.h" +#include + +namespace Espfc { + +namespace Utils { + +Stats::Stats(): _loop_last(0), _loop_time(0) +{ + for(size_t i = 0; i < COUNTER_COUNT; i++) + { + _start[i] = 0; + _avg[i] = 0; + _sum[i] = 0; + _count[i] = 0; + _freq[i] = 0; + } +} + +void FAST_CODE_ATTR Stats::start(StatCounter c) +{ + _start[c] = micros(); +} + +void FAST_CODE_ATTR Stats::end(StatCounter c) +{ + uint32_t diff = micros() - _start[c]; + _sum[c] += diff; + _count[c]++; +} + +void Stats::loopTick() +{ + uint32_t now = micros(); + uint32_t diff = now - _loop_last; + //_loop_time = diff; + _loop_time += (((int32_t)diff - _loop_time + 4) >> 3); + _loop_last = now; +} + +uint32_t Stats::loopTime() const +{ + return _loop_time; +} + +void Stats::update() +{ + if(!timer.check()) return; + for(size_t i = 0; i < COUNTER_COUNT; i++) + { + _avg[i] = (float)(_sum[i] + (_count[i] >> 1)) / timer.delta; + _freq[i] = (float)_count[i] * 1e6 / timer.delta; + _real[i] = _count[i] > 0 ? ((float)(_sum[i] + (_count[i] >> 1)) / _count[i]) : 0.0f; + _sum[i] = 0; + _count[i] = 0; + } +} + +float Stats::getLoad(StatCounter c) const +{ + return _avg[c] * 100.f; +} + +/** + * @brief Get the Time of counter normalized to 1 ms + */ +float Stats::getTime(StatCounter c) const +{ + return _avg[c] * 1000.0f; +} + +float Stats::getReal(StatCounter c) const +{ + return _real[c]; +} + +float Stats::getFreq(StatCounter c) const +{ + return _freq[c]; +} + +float Stats::getTotalLoad() const +{ + float ret = 0; + for(size_t i = 0; i < COUNTER_COUNT; i++) + { + if(i == COUNTER_CPU_0 || i == COUNTER_CPU_1) continue; + ret += getLoad((StatCounter)i); + } + return ret; +} + +float Stats::getTotalTime() const +{ + float ret = 0; + for(size_t i = 0; i < COUNTER_COUNT; i++) + { + if(i == COUNTER_CPU_0 || i == COUNTER_CPU_1) continue; + ret += getTime((StatCounter)i); + } + return ret; +} + +float Stats::getCpuLoad() const +{ + 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 Stats::getCpuTime() const +{ + return getTime(COUNTER_CPU_0) + getTime(COUNTER_CPU_1); +} + +const char * Stats::getName(StatCounter c) const +{ + 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_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/Utils/Stats.h b/lib/Espfc/src/Utils/Stats.h new file mode 100644 index 00000000..26666f48 --- /dev/null +++ b/lib/Espfc/src/Utils/Stats.h @@ -0,0 +1,98 @@ +#pragma once + +#include "Utils/Timer.h" +#include + +namespace Espfc { + +enum StatCounter { + COUNTER_GYRO_READ, + COUNTER_GYRO_FILTER, + COUNTER_GYRO_FFT, + COUNTER_RPM_UPDATE, + COUNTER_ACCEL_READ, + COUNTER_ACCEL_FILTER, + COUNTER_MAG_READ, + COUNTER_MAG_FILTER, + COUNTER_BARO, + COUNTER_IMU_FUSION, + COUNTER_IMU_FUSION2, + 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, + COUNTER_BATTERY, + COUNTER_CPU_0, + COUNTER_CPU_1, + COUNTER_COUNT +}; + +namespace Utils { + +class Stats +{ + public: + class Measure + { + public: + inline Measure(Stats& stats, StatCounter counter): _stats(stats), _counter(counter) + { + _stats.start(_counter); + } + inline ~Measure() + { + _stats.end(_counter); + } + private: + Stats& _stats; + StatCounter _counter; + }; + + Stats(); + + void start(StatCounter c); + void end(StatCounter c); + + void update(); + + void loopTick(); + uint32_t loopTime() const; + float getLoad(StatCounter c) const; + + /** + * @brief Get the Time of counter normalized to 1 ms + */ + float getTime(StatCounter c) const; + float getReal(StatCounter c) const; + float getFreq(StatCounter c) const; + float getTotalLoad() const; + float getTotalTime() const; + float getCpuLoad() const; + float getCpuTime() const; + const char * getName(StatCounter c) const; + + Utils::Timer timer; + + private: + uint32_t _start[COUNTER_COUNT]; + uint32_t _sum[COUNTER_COUNT]; + uint32_t _count[COUNTER_COUNT]; + float _avg[COUNTER_COUNT]; + float _freq[COUNTER_COUNT]; + float _real[COUNTER_COUNT]; + uint32_t _loop_last; + int32_t _loop_time; +}; + +} + +} diff --git a/lib/Espfc/src/Wireless.h b/lib/Espfc/src/Wireless.h index 96e9e550..0092c6c8 100644 --- a/lib/Espfc/src/Wireless.h +++ b/lib/Espfc/src/Wireless.h @@ -95,7 +95,7 @@ class Wireless int update() { - Stats::Measure measure(_model.state.stats, COUNTER_WIFI); + Utils::Stats::Measure measure(_model.state.stats, COUNTER_WIFI); switch(_status) { From 7e5f8f9a87893e561bd492bf7aadf64f35d54430 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 4 Nov 2024 20:22:54 +0100 Subject: [PATCH 48/68] move msp to connect --- lib/Espfc/src/{Msp => Connect}/Msp.h | 2 +- lib/Espfc/src/{Msp => Connect}/MspParser.h | 2 +- lib/Espfc/src/{Msp => Connect}/MspProcessor.h | 6 ++-- lib/Espfc/src/Device/InputCRSF.cpp | 2 +- lib/Espfc/src/ModelState.h | 6 ++-- lib/Espfc/src/Rc/Crsf.cpp | 30 +++++++++---------- lib/Espfc/src/Rc/Crsf.h | 6 ++-- lib/Espfc/src/SerialManager.cpp | 4 +-- lib/Espfc/src/SerialManager.h | 4 +-- lib/Espfc/src/Telemetry/TelemetryCRSF.h | 2 +- lib/Espfc/src/TelemetryManager.h | 10 +++---- test/test_input_crsf/test_input_crsf.cpp | 10 +++---- test/test_msp/test_msp.cpp | 6 ++-- 13 files changed, 45 insertions(+), 45 deletions(-) rename lib/Espfc/src/{Msp => Connect}/Msp.h (99%) rename lib/Espfc/src/{Msp => Connect}/MspParser.h (99%) rename lib/Espfc/src/{Msp => Connect}/MspProcessor.h (99%) diff --git a/lib/Espfc/src/Msp/Msp.h b/lib/Espfc/src/Connect/Msp.h similarity index 99% rename from lib/Espfc/src/Msp/Msp.h rename to lib/Espfc/src/Connect/Msp.h index 7328a4e7..2f1d9326 100644 --- a/lib/Espfc/src/Msp/Msp.h +++ b/lib/Espfc/src/Connect/Msp.h @@ -13,7 +13,7 @@ extern "C" { namespace Espfc { -namespace Msp { +namespace Connect { static const size_t MSP_BUF_SIZE = 192; static const size_t MSP_BUF_OUT_SIZE = 240; diff --git a/lib/Espfc/src/Msp/MspParser.h b/lib/Espfc/src/Connect/MspParser.h similarity index 99% rename from lib/Espfc/src/Msp/MspParser.h rename to lib/Espfc/src/Connect/MspParser.h index 8ab11be9..c43116c4 100644 --- a/lib/Espfc/src/Msp/MspParser.h +++ b/lib/Espfc/src/Connect/MspParser.h @@ -6,7 +6,7 @@ namespace Espfc { -namespace Msp { +namespace Connect { class MspParser { diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Connect/MspProcessor.h similarity index 99% rename from lib/Espfc/src/Msp/MspProcessor.h rename to lib/Espfc/src/Connect/MspProcessor.h index 00a53bba..f90549f6 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Connect/MspProcessor.h @@ -1,10 +1,10 @@ #ifndef _ESPFC_MSP_PROCESSOR_H_ #define _ESPFC_MSP_PROCESSOR_H_ -#include "Msp/Msp.h" +#include "Connect/Msp.h" #include "Model.h" #include "Hardware.h" -#include "Msp/MspParser.h" +#include "Connect/MspParser.h" #include "platform.h" #if defined(ESPFC_MULTI_CORE) && defined(ESPFC_FREE_RTOS) #include @@ -155,7 +155,7 @@ static uint16_t toIbatCurrent(float current) namespace Espfc { -namespace Msp { +namespace Connect { class MspProcessor { diff --git a/lib/Espfc/src/Device/InputCRSF.cpp b/lib/Espfc/src/Device/InputCRSF.cpp index ff355474..39597397 100644 --- a/lib/Espfc/src/Device/InputCRSF.cpp +++ b/lib/Espfc/src/Device/InputCRSF.cpp @@ -166,7 +166,7 @@ void FAST_CODE_ATTR InputCRSF::applyMspReq(const CrsfMessage& msg) if(!_telemetry) return; uint8_t origin; - Msp::MspMessage m; + Connect::MspMessage m; Crsf::decodeMsp(msg, m, origin); diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index d930b7c4..73cc3f97 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -16,7 +16,7 @@ #include "Utils/Stats.h" #include "Device/SerialDevice.h" #include "Math/FreqAnalyzer.h" -#include "Msp/Msp.h" +#include "Connect/Msp.h" namespace Espfc { @@ -38,8 +38,8 @@ class CliCmd class SerialPortState { public: - Msp::MspMessage mspRequest; - Msp::MspResponse mspResponse; + Connect::MspMessage mspRequest; + Connect::MspResponse mspResponse; CliCmd cliCmd; Device::SerialDevice * stream; }; diff --git a/lib/Espfc/src/Rc/Crsf.cpp b/lib/Espfc/src/Rc/Crsf.cpp index e93e041f..c18551bb 100644 --- a/lib/Espfc/src/Rc/Crsf.cpp +++ b/lib/Espfc/src/Rc/Crsf.cpp @@ -92,7 +92,7 @@ void Crsf::encodeRcData(CrsfMessage& msg, const CrsfData& data) msg.payload[sizeof(data)] = crc(msg); } -int Crsf::encodeMsp(CrsfMessage& msg, const Msp::MspResponse& resp, uint8_t origin) +int Crsf::encodeMsp(CrsfMessage& msg, const Connect::MspResponse& resp, uint8_t origin) { uint8_t buff[CRSF_PAYLOAD_SIZE_MAX]; size_t size = resp.serialize(buff, CRSF_PAYLOAD_SIZE_MAX); @@ -101,7 +101,7 @@ int Crsf::encodeMsp(CrsfMessage& msg, const Msp::MspResponse& resp, uint8_t orig uint8_t status = 0; status |= (1 << 4); // start bit - status |= ((resp.version == Msp::MSP_V1 ? 1 : 2) << 5); + status |= ((resp.version == Connect::MSP_V1 ? 1 : 2) << 5); msg.prepare(Rc::CRSF_FRAMETYPE_MSP_RESP); msg.writeU8(origin); @@ -113,7 +113,7 @@ int Crsf::encodeMsp(CrsfMessage& msg, const Msp::MspResponse& resp, uint8_t orig return msg.size; } -int Crsf::decodeMsp(const CrsfMessage& msg, Msp::MspMessage& m, uint8_t& origin) +int Crsf::decodeMsp(const CrsfMessage& msg, Connect::MspMessage& m, uint8_t& origin) { //uint8_t dst = msg.payload[0]; origin = msg.payload[1]; @@ -128,32 +128,32 @@ int Crsf::decodeMsp(const CrsfMessage& msg, Msp::MspMessage& m, uint8_t& origin) { if(version == 1) { - const Msp::MspHeaderV1 * hdr = reinterpret_cast(msg.payload + 3); - size_t framePayloadSize = msg.size - 5 - sizeof(Msp::MspHeaderV1); + const Connect::MspHeaderV1 * hdr = reinterpret_cast(msg.payload + 3); + size_t framePayloadSize = msg.size - 5 - sizeof(Connect::MspHeaderV1); if(framePayloadSize >= hdr->size) { m.expected = hdr->size; m.received = hdr->size; m.cmd = hdr->cmd; - m.state = Msp::MSP_STATE_RECEIVED; - m.dir = Msp::MSP_TYPE_CMD; - m.version = Msp::MSP_V1; - std::copy_n(msg.payload + 3 + sizeof(Msp::MspHeaderV1), m.received, m.buffer); + m.state = Connect::MSP_STATE_RECEIVED; + m.dir = Connect::MSP_TYPE_CMD; + m.version = Connect::MSP_V1; + std::copy_n(msg.payload + 3 + sizeof(Connect::MspHeaderV1), m.received, m.buffer); } } else if(version == 2) { - const Msp::MspHeaderV2 * hdr = reinterpret_cast(msg.payload + 3); - size_t framePayloadSize = msg.size - 5 - sizeof(Msp::MspHeaderV2); + const Connect::MspHeaderV2 * hdr = reinterpret_cast(msg.payload + 3); + size_t framePayloadSize = msg.size - 5 - sizeof(Connect::MspHeaderV2); if(framePayloadSize >= hdr->size) { m.expected = hdr->size; m.received = hdr->size; m.cmd = hdr->cmd; - m.state = Msp::MSP_STATE_RECEIVED; - m.dir = Msp::MSP_TYPE_CMD; - m.version = Msp::MSP_V1; - std::copy_n(msg.payload + 3 + sizeof(Msp::MspHeaderV2), m.received, m.buffer); + m.state = Connect::MSP_STATE_RECEIVED; + m.dir = Connect::MSP_TYPE_CMD; + m.version = Connect::MSP_V1; + std::copy_n(msg.payload + 3 + sizeof(Connect::MspHeaderV2), m.received, m.buffer); } } } diff --git a/lib/Espfc/src/Rc/Crsf.h b/lib/Espfc/src/Rc/Crsf.h index 78d7ed13..0c4eba2f 100644 --- a/lib/Espfc/src/Rc/Crsf.h +++ b/lib/Espfc/src/Rc/Crsf.h @@ -3,7 +3,7 @@ #include #include #include "Math/Crc.h" -#include "Msp/Msp.h" +#include "Connect/Msp.h" namespace Espfc { @@ -182,8 +182,8 @@ class Crsf static void decodeRcDataShift8(uint16_t* channels, const CrsfData* frame); //static void decodeRcDataShift32(uint16_t* channels, const CrsfData* frame); static void encodeRcData(CrsfMessage& frame, const CrsfData& data); - static int encodeMsp(CrsfMessage& msg, const Msp::MspResponse& res, uint8_t origin); - static int decodeMsp(const CrsfMessage& msg, Msp::MspMessage& m, uint8_t& origin); + static int encodeMsp(CrsfMessage& msg, const Connect::MspResponse& res, uint8_t origin); + static int decodeMsp(const CrsfMessage& msg, Connect::MspMessage& m, uint8_t& origin); static uint16_t convert(int v); static uint8_t crc(const CrsfMessage& frame); }; diff --git a/lib/Espfc/src/SerialManager.cpp b/lib/Espfc/src/SerialManager.cpp index c5a5fe87..6828f408 100644 --- a/lib/Espfc/src/SerialManager.cpp +++ b/lib/Espfc/src/SerialManager.cpp @@ -167,8 +167,8 @@ int FAST_CODE_ATTR SerialManager::update() _msp.processCommand(ss.mspRequest, ss.mspResponse, *stream); _msp.sendResponse(ss.mspResponse, *stream); _msp.postCommand(); - ss.mspRequest = Msp::MspMessage(); - ss.mspResponse = Msp::MspResponse(); + ss.mspRequest = Connect::MspMessage(); + ss.mspResponse = Connect::MspResponse(); } } else diff --git a/lib/Espfc/src/SerialManager.h b/lib/Espfc/src/SerialManager.h index b025f8c9..3bc121e0 100644 --- a/lib/Espfc/src/SerialManager.h +++ b/lib/Espfc/src/SerialManager.h @@ -2,7 +2,7 @@ #include "Model.h" #include "Device/SerialDevice.h" -#include "Msp/MspProcessor.h" +#include "Connect/MspProcessor.h" #include "Cli.h" #include "TelemetryManager.h" #ifdef ESPFC_SERIAL_SOFT_0_WIFI @@ -29,7 +29,7 @@ class SerialManager } Model& _model; - Msp::MspProcessor _msp; + Connect::MspProcessor _msp; Cli _cli; #ifdef ESPFC_SERIAL_SOFT_0_WIFI Wireless _wireless; diff --git a/lib/Espfc/src/Telemetry/TelemetryCRSF.h b/lib/Espfc/src/Telemetry/TelemetryCRSF.h index c724a7c5..834fca96 100644 --- a/lib/Espfc/src/Telemetry/TelemetryCRSF.h +++ b/lib/Espfc/src/Telemetry/TelemetryCRSF.h @@ -73,7 +73,7 @@ class TelemetryCRSF return 1; } - int sendMsp(Device::SerialDevice& s, Msp::MspResponse r, uint8_t origin) const + int sendMsp(Device::SerialDevice& s, Connect::MspResponse r, uint8_t origin) const { Rc::CrsfMessage msg; diff --git a/lib/Espfc/src/TelemetryManager.h b/lib/Espfc/src/TelemetryManager.h index f7968e60..219ff615 100644 --- a/lib/Espfc/src/TelemetryManager.h +++ b/lib/Espfc/src/TelemetryManager.h @@ -3,8 +3,8 @@ #include "Model.h" #include "Telemetry/TelemetryText.h" #include "Telemetry/TelemetryCRSF.h" -#include "Msp/Msp.h" -#include "Msp/MspProcessor.h" +#include "Connect/Msp.h" +#include "Connect/MspProcessor.h" namespace Espfc { @@ -35,9 +35,9 @@ class TelemetryManager return 1; } - int processMsp(Device::SerialDevice& s, TelemetryProtocol protocol, Msp::MspMessage m, uint8_t origin) + int processMsp(Device::SerialDevice& s, TelemetryProtocol protocol, Connect::MspMessage m, uint8_t origin) { - Msp::MspResponse r; + Connect::MspResponse r; // not valid msp message, stop processing if(!m.isReady() || !m.isCmd()) return 0; @@ -60,7 +60,7 @@ class TelemetryManager private: Model& _model; - Msp::MspProcessor _msp; + Connect::MspProcessor _msp; Telemetry::TelemetryText _text; Telemetry::TelemetryCRSF _crsf; }; diff --git a/test/test_input_crsf/test_input_crsf.cpp b/test/test_input_crsf/test_input_crsf.cpp index 501001ff..3d25b11c 100644 --- a/test/test_input_crsf/test_input_crsf.cpp +++ b/test/test_input_crsf/test_input_crsf.cpp @@ -304,8 +304,8 @@ void test_crsf_encode_msp_v1() CrsfMessage frame; memset(&frame, 0, sizeof(frame)); - Msp::MspResponse resp; - resp.version = Msp::MSP_V1; + Connect::MspResponse resp; + resp.version = Connect::MSP_V1; resp.cmd = MSP_API_VERSION; resp.result = 0; resp.writeU8(1); @@ -342,8 +342,8 @@ void test_crsf_encode_msp_v2() CrsfMessage frame; memset(&frame, 0, sizeof(frame)); - Msp::MspResponse resp; - resp.version = Msp::MSP_V2; + Connect::MspResponse resp; + resp.version = Connect::MSP_V2; resp.cmd = MSP_API_VERSION; resp.result = 0; resp.writeU8(1); @@ -386,7 +386,7 @@ void test_crsf_decode_msp_v1() CrsfMessage frame; std::copy_n(data, sizeof(data), (uint8_t*)&frame); - Msp::MspMessage m; + Connect::MspMessage m; uint8_t origin = 0; Crsf::decodeMsp(frame, m, origin); diff --git a/test/test_msp/test_msp.cpp b/test/test_msp/test_msp.cpp index aee123b3..1d705dc2 100644 --- a/test/test_msp/test_msp.cpp +++ b/test/test_msp/test_msp.cpp @@ -5,11 +5,12 @@ #include #include #include -#include "Msp/Msp.h" -#include "Msp/MspParser.h" +#include "Connect/Msp.h" +#include "Connect/MspParser.h" using namespace fakeit; using namespace Espfc; +using namespace Espfc::Connect; /*void setUp(void) { @@ -20,7 +21,6 @@ using namespace Espfc; // // clean stuff up here // } -using namespace Espfc::Msp; #define MSP_V2_FLAG 0 From 3be19d00a5ae8eed758f363c9253671dfddcd34b Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 4 Nov 2024 20:25:53 +0100 Subject: [PATCH 49/68] move cli to connect ns --- lib/Espfc/src/{ => Connect}/Cli.h | 4 ++++ lib/Espfc/src/SerialManager.h | 4 ++-- 2 files changed, 6 insertions(+), 2 deletions(-) rename lib/Espfc/src/{ => Connect}/Cli.h (99%) diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Connect/Cli.h similarity index 99% rename from lib/Espfc/src/Cli.h rename to lib/Espfc/src/Connect/Cli.h index 9ad1a0dc..a51d80f6 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Connect/Cli.h @@ -27,6 +27,8 @@ namespace Espfc { +namespace Connect { + class Cli { public: @@ -1560,4 +1562,6 @@ class Cli } +} + #endif diff --git a/lib/Espfc/src/SerialManager.h b/lib/Espfc/src/SerialManager.h index 3bc121e0..7e7f6c6d 100644 --- a/lib/Espfc/src/SerialManager.h +++ b/lib/Espfc/src/SerialManager.h @@ -3,7 +3,7 @@ #include "Model.h" #include "Device/SerialDevice.h" #include "Connect/MspProcessor.h" -#include "Cli.h" +#include "Connect/Cli.h" #include "TelemetryManager.h" #ifdef ESPFC_SERIAL_SOFT_0_WIFI #include "Wireless.h" @@ -30,7 +30,7 @@ class SerialManager Model& _model; Connect::MspProcessor _msp; - Cli _cli; + Connect::Cli _cli; #ifdef ESPFC_SERIAL_SOFT_0_WIFI Wireless _wireless; #endif From 6ff81a9b276e597da1475de096a128ff505cc954 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 4 Nov 2024 21:02:06 +0100 Subject: [PATCH 50/68] move and split storage --- lib/Espfc/src/Model.h | 6 +- lib/Espfc/src/Storage.h | 105 -------------------------------- lib/Espfc/src/Utils/Storage.cpp | 69 +++++++++++++++++++++ lib/Espfc/src/Utils/Storage.h | 43 +++++++++++++ 4 files changed, 115 insertions(+), 108 deletions(-) delete mode 100644 lib/Espfc/src/Storage.h create mode 100644 lib/Espfc/src/Utils/Storage.cpp create mode 100644 lib/Espfc/src/Utils/Storage.h diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index a95dcb5c..7d89e944 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -7,7 +7,7 @@ #include "Debug_Espfc.h" #include "ModelConfig.h" #include "ModelState.h" -#include "Storage.h" +#include "Utils/Storage.h" #include "Logger.h" #include "Math/Utils.h" @@ -279,7 +279,7 @@ class Model { preSave(); #ifndef UNIT_TEST - _storageResult = _storage.write(config); + _storageResult = _storage.save(config); logStorageResult(); #endif } @@ -617,7 +617,7 @@ class Model private: #ifndef UNIT_TEST - Storage _storage; + Utils::Storage _storage; #endif StorageResult _storageResult; }; diff --git a/lib/Espfc/src/Storage.h b/lib/Espfc/src/Storage.h deleted file mode 100644 index f7a279ca..00000000 --- a/lib/Espfc/src/Storage.h +++ /dev/null @@ -1,105 +0,0 @@ -#ifndef _ESPFC_STORAGE_H_ -#define _ESPFC_STORAGE_H_ - -namespace Espfc { - -enum StorageResult -{ - STORAGE_NONE, - STORAGE_LOAD_SUCCESS, - STORAGE_SAVE_SUCCESS, - STORAGE_SAVE_ERROR, - STORAGE_ERR_BAD_MAGIC, - STORAGE_ERR_BAD_VERSION, - STORAGE_ERR_BAD_SIZE, -}; - -} - -#ifndef UNIT_TEST - -#include -#include "ModelConfig.h" -#include "EEPROM.h" -#include "Logger.h" - -namespace Espfc { - -class Storage -{ - public: - int begin() - { - EEPROM.begin(EEPROM_SIZE); - static_assert(sizeof(ModelConfig) <= EEPROM_SIZE, "ModelConfig Size too big"); - return 1; - } - - StorageResult load(ModelConfig& config) - { - //return STORAGE_ERR_BAD_MAGIC; - - int addr = 0; - uint8_t magic = EEPROM.read(addr++); - if(EEPROM_MAGIC != magic) - { - return STORAGE_ERR_BAD_MAGIC; - } - - uint8_t version = EEPROM.read(addr++); - if(EEPROM_VERSION != version) - { - return STORAGE_ERR_BAD_VERSION; - } - - uint16_t size = 0; - size = EEPROM.read(addr++); - size |= EEPROM.read(addr++) << 8; - if(size != sizeof(ModelConfig)) - { - return STORAGE_ERR_BAD_SIZE; - } - - uint8_t * begin = reinterpret_cast(&config); - uint8_t * end = begin + size; - for(uint8_t * it = begin; it < end; ++it) - { - *it = EEPROM.read(addr++); - } - return STORAGE_LOAD_SUCCESS; - } - - StorageResult write(const ModelConfig& config) - { - int addr = 0; - uint16_t size = sizeof(ModelConfig); - EEPROM.write(addr++, EEPROM_MAGIC); - EEPROM.write(addr++, EEPROM_VERSION); - EEPROM.write(addr++, size & 0xFF); - EEPROM.write(addr++, size >> 8); - const uint8_t * begin = reinterpret_cast(&config); - const uint8_t * end = begin + sizeof(ModelConfig); - for(const uint8_t * it = begin; it < end; ++it) - { - EEPROM.write(addr++, *it); - } - if(EEPROM.commit()) - { - return STORAGE_SAVE_SUCCESS; - } - return STORAGE_SAVE_ERROR; - } - - private: - static const uint8_t EEPROM_MAGIC = 0xA5; - static const uint8_t EEPROM_VERSION = 0x01; - static const size_t EEPROM_SIZE = 2048; -#if defined(NO_GLOBAL_INSTANCES) || defined(NO_GLOBAL_EEPROM) - EEPROMClass EEPROM; -#endif -}; - -} -#endif - -#endif \ No newline at end of file diff --git a/lib/Espfc/src/Utils/Storage.cpp b/lib/Espfc/src/Utils/Storage.cpp new file mode 100644 index 00000000..c82fe0e5 --- /dev/null +++ b/lib/Espfc/src/Utils/Storage.cpp @@ -0,0 +1,69 @@ +#ifndef UNIT_TEST + +#include "Utils/Storage.h" +#include "ModelConfig.h" +#include +#include + +#if defined(NO_GLOBAL_INSTANCES) || defined(NO_GLOBAL_EEPROM) +static EEPROMClass EEPROM; +#endif + +namespace Espfc { + +namespace Utils { + +int Storage::begin() +{ + EEPROM.begin(EEPROM_SIZE); + static_assert(sizeof(ModelConfig) <= EEPROM_SIZE, "ModelConfig Size too big"); + return 1; +} + +StorageResult Storage::load(ModelConfig& config) const +{ + //return STORAGE_ERR_BAD_MAGIC; + + int addr = 0; + uint8_t magic = EEPROM.read(addr++); + if(EEPROM_MAGIC != magic) + { + return STORAGE_ERR_BAD_MAGIC; + } + + uint8_t version = EEPROM.read(addr++); + if(EEPROM_VERSION != version) + { + return STORAGE_ERR_BAD_VERSION; + } + + uint16_t size = 0; + size = EEPROM.read(addr++); + size |= EEPROM.read(addr++) << 8; + if(size != sizeof(ModelConfig)) + { + return STORAGE_ERR_BAD_SIZE; + } + + EEPROM.get(addr, config); + return STORAGE_LOAD_SUCCESS; +} + +StorageResult Storage::save(const ModelConfig& config) +{ + int addr = 0; + uint16_t size = sizeof(ModelConfig); + EEPROM.write(addr++, EEPROM_MAGIC); + EEPROM.write(addr++, EEPROM_VERSION); + EEPROM.write(addr++, size & 0xFF); + EEPROM.write(addr++, (size >> 8) & 0xFF); + EEPROM.put(addr, config); + if(!EEPROM.commit()) return STORAGE_SAVE_ERROR; + return STORAGE_SAVE_SUCCESS; +} + +} + +} + +#endif diff --git a/lib/Espfc/src/Utils/Storage.h b/lib/Espfc/src/Utils/Storage.h new file mode 100644 index 00000000..c2b90f19 --- /dev/null +++ b/lib/Espfc/src/Utils/Storage.h @@ -0,0 +1,43 @@ +#pragma once + +namespace Espfc { + +enum StorageResult +{ + STORAGE_NONE, + STORAGE_LOAD_SUCCESS, + STORAGE_SAVE_SUCCESS, + STORAGE_SAVE_ERROR, + STORAGE_ERR_BAD_MAGIC, + STORAGE_ERR_BAD_VERSION, + STORAGE_ERR_BAD_SIZE, +}; + +} + +#ifndef UNIT_TEST + +#include "ModelConfig.h" + +namespace Espfc { + +namespace Utils { + +class Storage +{ + public: + int begin(); + StorageResult load(ModelConfig& config) const; + StorageResult save(const ModelConfig& config); + + private: + static const uint8_t EEPROM_MAGIC = 0xA5; + static const uint8_t EEPROM_VERSION = 0x01; + static const size_t EEPROM_SIZE = 2048; +}; + +} + +} + +#endif From c779041f6816b97d50c9a73072ffe41c6219e62f Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 4 Nov 2024 23:20:43 +0100 Subject: [PATCH 51/68] move logger to utils + remove deprecated Model::isActive() --- lib/Espfc/src/Blackbox/Blackbox.cpp | 2 +- lib/Espfc/src/Connect/Cli.h | 8 +++----- lib/Espfc/src/Connect/MspProcessor.h | 6 +++--- lib/Espfc/src/Control/Controller.cpp | 6 +++--- lib/Espfc/src/Debug_Espfc.h | 6 ++---- lib/Espfc/src/Model.h | 24 ++++-------------------- lib/Espfc/src/Output/Mixer.cpp | 4 ++-- lib/Espfc/src/Sensor/GyroSensor.cpp | 2 +- lib/Espfc/src/{ => Utils}/Logger.h | 8 ++++---- 9 files changed, 23 insertions(+), 43 deletions(-) rename lib/Espfc/src/{ => Utils}/Logger.h (95%) diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index fd58058f..ed7651ee 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -290,7 +290,7 @@ void FAST_CODE_ATTR Blackbox::updateArmed() stop = 0; } - bool armed = _model.isActive(MODE_ARMED); + bool armed = _model.isModeActive(MODE_ARMED); if(armed == ARMING_FLAG(ARMED)) return; if(armed) { diff --git a/lib/Espfc/src/Connect/Cli.h b/lib/Espfc/src/Connect/Cli.h index a51d80f6..746433c1 100644 --- a/lib/Espfc/src/Connect/Cli.h +++ b/lib/Espfc/src/Connect/Cli.h @@ -1,13 +1,11 @@ #ifndef _ESPFC_CLI_H_ #define _ESPFC_CLI_H_ -#include -#include #include +#include #include "Model.h" #include "Hardware.h" -#include "Logger.h" #include "Device/GyroDevice.h" #include "Hal/Pgm.h" @@ -1078,12 +1076,12 @@ class Cli } else if(strcmp_P(cmd.args[1], PSTR("gyro")) == 0) { - if(!_model.isActive(MODE_ARMED)) _model.calibrateGyro(); + if(!_model.isModeActive(MODE_ARMED)) _model.calibrateGyro(); s.println(F("OK")); } else if(strcmp_P(cmd.args[1], PSTR("mag")) == 0) { - if(!_model.isActive(MODE_ARMED)) _model.calibrateMag(); + if(!_model.isModeActive(MODE_ARMED)) _model.calibrateMag(); s.println(F("OK")); } else if(strcmp_P(cmd.args[1], PSTR("reset_accel")) == 0 || strcmp_P(cmd.args[1], PSTR("reset_all")) == 0) diff --git a/lib/Espfc/src/Connect/MspProcessor.h b/lib/Espfc/src/Connect/MspProcessor.h index f90549f6..7e176df3 100644 --- a/lib/Espfc/src/Connect/MspProcessor.h +++ b/lib/Espfc/src/Connect/MspProcessor.h @@ -1392,11 +1392,11 @@ class MspProcessor break; case MSP_ACC_CALIBRATION: - if(!_model.isActive(MODE_ARMED)) _model.calibrateGyro(); + if(!_model.isModeActive(MODE_ARMED)) _model.calibrateGyro(); break; case MSP_MAG_CALIBRATION: - if(!_model.isActive(MODE_ARMED)) _model.calibrateMag(); + if(!_model.isModeActive(MODE_ARMED)) _model.calibrateMag(); break; case MSP_VTX_CONFIG: @@ -1463,7 +1463,7 @@ class MspProcessor break; case MSP_RESET_CONF: - if(!_model.isActive(MODE_ARMED)) + if(!_model.isModeActive(MODE_ARMED)) { _model.reset(); } diff --git a/lib/Espfc/src/Control/Controller.cpp b/lib/Espfc/src/Control/Controller.cpp index 2dd6eb9a..f3b3004e 100644 --- a/lib/Espfc/src/Control/Controller.cpp +++ b/lib/Espfc/src/Control/Controller.cpp @@ -63,7 +63,7 @@ void Controller::outerLoopRobot() const float speed = _speedFilter.update(_model.state.output.ch[AXIS_PITCH] * speedScale + _model.state.gyro.adc[AXIS_PITCH] * gyroScale); float angle = 0; - if(true || _model.isActive(MODE_ANGLE)) + if(true || _model.isModeActive(MODE_ANGLE)) { angle = _model.state.input.ch[AXIS_PITCH] * Math::toRad(_model.config.level.angleLimit); } @@ -110,7 +110,7 @@ void Controller::innerLoopRobot() void FAST_CODE_ATTR Controller::outerLoop() { - if(_model.isActive(MODE_ANGLE)) + if(_model.isModeActive(MODE_ANGLE)) { _model.state.setpoint.angle = VectorFloat( _model.state.input.ch[AXIS_ROLL] * Math::toRad(_model.config.level.angleLimit), @@ -168,7 +168,7 @@ float Controller::getTpaFactor() const void Controller::resetIterm() { - if(!_model.isActive(MODE_ARMED) // when not armed + if(!_model.isModeActive(MODE_ARMED) // when not armed || (!_model.isAirModeActive() && _model.config.iterm.lowThrottleZeroIterm && _model.isThrottleLow()) // on low throttle (not in air mode) ) { diff --git a/lib/Espfc/src/Debug_Espfc.h b/lib/Espfc/src/Debug_Espfc.h index f90b3a91..13823805 100644 --- a/lib/Espfc/src/Debug_Espfc.h +++ b/lib/Espfc/src/Debug_Espfc.h @@ -6,7 +6,7 @@ #ifdef ESPFC_DEBUG_PIN #define PIN_DEBUG(v) ::Espfc::Hal::Gpio::digitalWrite(ESPFC_DEBUG_PIN, v) - #define PIN_DEBUG_INIT() ::pinMode(ESPFC_DEBUG_PIN, OUTPUT) + #define PIN_DEBUG_INIT() ::Espfc::Hal::Gpio::pinMode(ESPFC_DEBUG_PIN, OUTPUT) #else #define PIN_DEBUG(v) #define PIN_DEBUG_INIT() @@ -24,13 +24,11 @@ static inline void initDebugStream(Stream * p) { _debugStream = p; } #define LOG_SERIAL_DEBUG(v) if(_debugStream) { _debugStream->print(v); } #define LOG_SERIAL_DEBUG_HEX(v) if(_debugStream) { _debugStream->print(v, HEX); } -template +template void D(T t) { if(!_debugStream) return; _debugStream->println(t); - //_debugStream->print('\r'); - //_debugStream->print('\n'); } template diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 7d89e944..0c120e3f 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -8,7 +8,7 @@ #include "ModelConfig.h" #include "ModelState.h" #include "Utils/Storage.h" -#include "Logger.h" +#include "Utils/Logger.h" #include "Math/Utils.h" namespace Espfc { @@ -30,14 +30,6 @@ class Model //config.brobot(); } - /** - * @deprecated use isModeActive - */ - bool isActive(FlightMode mode) const - { - return isModeActive(mode); - } - bool isModeActive(FlightMode mode) const { return state.mode.mask & (1 << mode); @@ -78,14 +70,6 @@ class Model state.appQueue.send(Event(EVENT_DISARM)); } - /** - * @deprecated use isFeatureActive - */ - bool isActive(Feature feature) const - { - return isFeatureActive(feature); - } - bool isFeatureActive(Feature feature) const { return config.featureMask & feature; @@ -93,7 +77,7 @@ class Model bool isAirModeActive() const { - return isModeActive(MODE_AIRMODE);// || isActive(FEATURE_AIRMODE); + return isModeActive(MODE_AIRMODE);// || isFeatureActive(FEATURE_AIRMODE); } bool isThrottleLow() const @@ -454,7 +438,7 @@ class Model // configure filters for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { - if(isActive(FEATURE_DYNAMIC_FILTER)) + if(isFeatureActive(FEATURE_DYNAMIC_FILTER)) { for(size_t p = 0; p < (size_t)config.gyro.dynamicFilter.count; p++) { @@ -595,7 +579,7 @@ class Model ModelState state; ModelConfig config; - Logger logger; + Utils::Logger logger; void logStorageResult() { diff --git a/lib/Espfc/src/Output/Mixer.cpp b/lib/Espfc/src/Output/Mixer.cpp index e4bdda59..014ca231 100644 --- a/lib/Espfc/src/Output/Mixer.cpp +++ b/lib/Espfc/src/Output/Mixer.cpp @@ -374,8 +374,8 @@ float inline Mixer::erpmToRpm(float erpm) bool Mixer::_stop(void) { - if(!_model.isActive(MODE_ARMED)) return true; - if(_model.isActive(FEATURE_MOTOR_STOP) && _model.isThrottleLow()) return true; + if(!_model.isModeActive(MODE_ARMED)) return true; + if(_model.isFeatureActive(FEATURE_MOTOR_STOP) && _model.isThrottleLow()) return true; return false; } diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index 22e4941d..722e32b3 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -31,7 +31,7 @@ int GyroSensor::begin() _dyn_notch_denom = std::max((uint32_t)1, _model.state.loopTimer.rate / 1000); _dyn_notch_sma.begin(_dyn_notch_denom); _dyn_notch_count = std::min((size_t)_model.config.gyro.dynamicFilter.count, DYN_NOTCH_COUNT_MAX); - _dyn_notch_enabled = _model.isActive(FEATURE_DYNAMIC_FILTER) && _dyn_notch_count > 0 && _model.state.loopTimer.rate >= DynamicFilterConfig::MIN_FREQ; + _dyn_notch_enabled = _model.isFeatureActive(FEATURE_DYNAMIC_FILTER) && _dyn_notch_count > 0 && _model.state.loopTimer.rate >= DynamicFilterConfig::MIN_FREQ; _dyn_notch_debug = _model.config.debug.mode == DEBUG_FFT_FREQ || _model.config.debug.mode == DEBUG_FFT_TIME; _rpm_enabled = _model.config.gyro.rpmFilter.harmonics > 0 && _model.config.output.dshotTelemetry; diff --git a/lib/Espfc/src/Logger.h b/lib/Espfc/src/Utils/Logger.h similarity index 95% rename from lib/Espfc/src/Logger.h rename to lib/Espfc/src/Utils/Logger.h index 3e79b810..f037eb49 100644 --- a/lib/Espfc/src/Logger.h +++ b/lib/Espfc/src/Utils/Logger.h @@ -1,11 +1,11 @@ -#ifndef _ESPFC_LOGGER_H_ -#define _ESPFC_LOGGER_H_ +#pragma once -#include #include "Debug_Espfc.h" namespace Espfc { +namespace Utils { + class Logger { public: @@ -114,4 +114,4 @@ class Logger } -#endif +} From 8a784bc2c2c9729eee3b2a1539fd4ecf01f11820 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Tue, 5 Nov 2024 21:52:00 +0100 Subject: [PATCH 52/68] split telemetry and wireless --- lib/Espfc/src/TelemetryManager.cpp | 47 +++++++++++++ lib/Espfc/src/TelemetryManager.h | 45 ++---------- lib/Espfc/src/Wireless.cpp | 107 ++++++++++++++++++++++++++++ lib/Espfc/src/Wireless.h | 109 +++-------------------------- 4 files changed, 168 insertions(+), 140 deletions(-) create mode 100644 lib/Espfc/src/TelemetryManager.cpp create mode 100644 lib/Espfc/src/Wireless.cpp diff --git a/lib/Espfc/src/TelemetryManager.cpp b/lib/Espfc/src/TelemetryManager.cpp new file mode 100644 index 00000000..6ebce6a5 --- /dev/null +++ b/lib/Espfc/src/TelemetryManager.cpp @@ -0,0 +1,47 @@ +#include "TelemetryManager.h" + +namespace Espfc { + +TelemetryManager::TelemetryManager(Model& model): _model(model), _msp(model), _text(model), _crsf(model) {} + +int TelemetryManager::process(Device::SerialDevice& s, TelemetryProtocol protocol) const +{ + Utils::Stats::Measure measure(_model.state.stats, COUNTER_TELEMETRY); + + switch(protocol) + { + case TELEMETRY_PROTOCOL_TEXT: + _text.process(s); + break; + case TELEMETRY_PROTOCOL_CRSF: + _crsf.process(s); + break; + } + + return 1; +} + +int TelemetryManager::processMsp(Device::SerialDevice& s, TelemetryProtocol protocol, Connect::MspMessage m, uint8_t origin) +{ + Connect::MspResponse r; + + // not valid msp message, stop processing + if(!m.isReady() || !m.isCmd()) return 0; + + _msp.processCommand(m, r, s); + + switch(protocol) + { + case TELEMETRY_PROTOCOL_CRSF: + _crsf.sendMsp(s, r, origin); + break; + default: + break; + } + + _msp.postCommand(); + + return 1; +} + +} diff --git a/lib/Espfc/src/TelemetryManager.h b/lib/Espfc/src/TelemetryManager.h index 219ff615..881ed9c3 100644 --- a/lib/Espfc/src/TelemetryManager.h +++ b/lib/Espfc/src/TelemetryManager.h @@ -1,6 +1,7 @@ #pragma once #include "Model.h" +#include "Device/SerialDevice.h" #include "Telemetry/TelemetryText.h" #include "Telemetry/TelemetryCRSF.h" #include "Connect/Msp.h" @@ -16,47 +17,9 @@ enum TelemetryProtocol { class TelemetryManager { public: - TelemetryManager(Model& model): _model(model), _msp(model), _text(model), _crsf(model) {} - - int process(Device::SerialDevice& s, TelemetryProtocol protocol) const - { - Utils::Stats::Measure measure(_model.state.stats, COUNTER_TELEMETRY); - - switch(protocol) - { - case TELEMETRY_PROTOCOL_TEXT: - _text.process(s); - break; - case TELEMETRY_PROTOCOL_CRSF: - _crsf.process(s); - break; - } - - return 1; - } - - int processMsp(Device::SerialDevice& s, TelemetryProtocol protocol, Connect::MspMessage m, uint8_t origin) - { - Connect::MspResponse r; - - // not valid msp message, stop processing - if(!m.isReady() || !m.isCmd()) return 0; - - _msp.processCommand(m, r, s); - - switch(protocol) - { - case TELEMETRY_PROTOCOL_CRSF: - _crsf.sendMsp(s, r, origin); - break; - default: - break; - } - - _msp.postCommand(); - - return 1; - } + TelemetryManager(Model& model); + int process(Device::SerialDevice& s, TelemetryProtocol protocol) const; + int processMsp(Device::SerialDevice& s, TelemetryProtocol protocol, Connect::MspMessage m, uint8_t origin); private: Model& _model; diff --git a/lib/Espfc/src/Wireless.cpp b/lib/Espfc/src/Wireless.cpp new file mode 100644 index 00000000..a027e6c6 --- /dev/null +++ b/lib/Espfc/src/Wireless.cpp @@ -0,0 +1,107 @@ +#include "Wireless.h" + +#ifdef ESPFC_SERIAL_SOFT_0_WIFI + +namespace Espfc { + +Wireless::Wireless(Model& model): _model(model), _status(STOPPED), _server(1111), _adapter(_client) {} + +int Wireless::begin() +{ + WiFi.persistent(false); +#ifdef ESPFC_ESPNOW + if(_model.isFeatureActive(FEATURE_RX_SPI)) + { + startAp(); + } +#endif + return 1; +} + +void Wireless::startAp() +{ + bool status = WiFi.softAP("ESP-FC"); + _model.logger.info().log(F("WIFI AP START")).logln(status); +} + +int Wireless::connect() +{ +#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) + { + 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 Wireless::wifiEventConnected(const String& ssid, int channel) +{ + _model.logger.info().log(F("WIFI STA CONN")).log(ssid).logln(channel); +} + +void Wireless::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 Wireless::wifiEventGotIp(const IPAddress& ip) +{ + _model.logger.info().log(F("WIFI STA IP")).logln(ip.toString()); +} + +void Wireless::wifiEventDisconnected() +{ + _model.logger.info().logln(F("WIFI STA DISCONNECT")); +} + +int Wireless::update() +{ + Utils::Stats::Measure measure(_model.state.stats, COUNTER_WIFI); + + switch(_status) + { + case STOPPED: + if(_model.state.mode.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 diff --git a/lib/Espfc/src/Wireless.h b/lib/Espfc/src/Wireless.h index 0092c6c8..136810a0 100644 --- a/lib/Espfc/src/Wireless.h +++ b/lib/Espfc/src/Wireless.h @@ -1,5 +1,4 @@ -#ifndef _ESPFC_WIRELESS_H_ -#define _ESPFC_WIRELESS_H_ +#pragma once #include "Model.h" #include "Device/SerialDeviceAdapter.h" @@ -20,103 +19,17 @@ class Wireless STARTED, }; public: - Wireless(Model& model): _model(model), _status(STOPPED), _server(1111), _adapter(_client) {} + Wireless(Model& model); - int begin() - { - WiFi.persistent(false); -#ifdef ESPFC_ESPNOW - if(_model.isFeatureActive(FEATURE_RX_SPI)) - { - startAp(); - } -#endif - return 1; - } - - void startAp() - { - bool status = WiFi.softAP("ESP-FC"); - _model.logger.info().log(F("WIFI AP START")).logln(status); - } - - int connect() - { -#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) - { - 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); - } + int begin(); + int update(); - 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")); - } - - int update() - { - Utils::Stats::Measure measure(_model.state.stats, COUNTER_WIFI); - - switch(_status) - { - case STOPPED: - if(_model.state.mode.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; - } + void startAp(); + int connect(); + void wifiEventConnected(const String& ssid, int channel); + void wifiEventApConnected(const uint8_t* mac); + void wifiEventGotIp(const IPAddress& ip); + void wifiEventDisconnected(); private: Model& _model; @@ -132,5 +45,3 @@ class Wireless } #endif - -#endif \ No newline at end of file From f2675d54de9cd293771e44e2a17379cf120d0655 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Tue, 5 Nov 2024 22:34:49 +0100 Subject: [PATCH 53/68] cleanup headers --- lib/EspWire/src/EspWire.cpp | 1 - lib/EspWire/src/EspWire.h | 4 +--- lib/EspWire/src/esp_twi.cpp | 6 +----- lib/EspWire/src/esp_twi.h | 1 - lib/Espfc/src/Buzzer.cpp | 1 - lib/Espfc/src/Device/FlashDevice.h | 4 +--- lib/Espfc/src/ModelState.h | 2 -- lib/betaflight/src/platform_cpp.cpp | 7 +++---- 8 files changed, 6 insertions(+), 20 deletions(-) diff --git a/lib/EspWire/src/EspWire.cpp b/lib/EspWire/src/EspWire.cpp index f6353bd9..aa14bdfc 100644 --- a/lib/EspWire/src/EspWire.cpp +++ b/lib/EspWire/src/EspWire.cpp @@ -26,7 +26,6 @@ extern "C" { #include #include } - #include "esp_twi.h" #include "EspWire.h" diff --git a/lib/EspWire/src/EspWire.h b/lib/EspWire/src/EspWire.h index 6e908b68..d5711954 100644 --- a/lib/EspWire/src/EspWire.h +++ b/lib/EspWire/src/EspWire.h @@ -25,9 +25,7 @@ #define EspTwoWire_h #include -#ifndef UNIT_TEST -#include -#endif +#include #define ESPWIRE_BUFFER_LENGTH 64 diff --git a/lib/EspWire/src/esp_twi.cpp b/lib/EspWire/src/esp_twi.cpp index 8ada32fc..e6e9c7ea 100644 --- a/lib/EspWire/src/esp_twi.cpp +++ b/lib/EspWire/src/esp_twi.cpp @@ -18,15 +18,11 @@ License along with this library; if not, write to the Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA */ +#include #include "esp_twi.h" #if defined(ESP32C3) #include #endif -#ifndef UNIT_TEST -#include -#include -#else -#endif uint16_t esp_twi_dcount = 18; static unsigned char esp_twi_sda, esp_twi_scl; diff --git a/lib/EspWire/src/esp_twi.h b/lib/EspWire/src/esp_twi.h index 97f2908d..77755f20 100644 --- a/lib/EspWire/src/esp_twi.h +++ b/lib/EspWire/src/esp_twi.h @@ -20,7 +20,6 @@ */ #ifndef ESP_TWI_H #define ESP_TWI_H -#include #ifdef __cplusplus extern "C" { diff --git a/lib/Espfc/src/Buzzer.cpp b/lib/Espfc/src/Buzzer.cpp index 81c71a4e..0035fe8d 100644 --- a/lib/Espfc/src/Buzzer.cpp +++ b/lib/Espfc/src/Buzzer.cpp @@ -1,4 +1,3 @@ -//#include #include "Buzzer.h" #include "Hal/Gpio.h" diff --git a/lib/Espfc/src/Device/FlashDevice.h b/lib/Espfc/src/Device/FlashDevice.h index 1df5d874..160f7901 100644 --- a/lib/Espfc/src/Device/FlashDevice.h +++ b/lib/Espfc/src/Device/FlashDevice.h @@ -1,9 +1,7 @@ #pragma once -#include -#include #include -#include "esp_partition.h" +#include namespace Espfc { diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 73cc3f97..950efc69 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -1,8 +1,6 @@ #ifndef _ESPFC_MODEL_STATE_H_ #define _ESPFC_MODEL_STATE_H_ -#include - #ifndef UNIT_TEST #include #endif diff --git a/lib/betaflight/src/platform_cpp.cpp b/lib/betaflight/src/platform_cpp.cpp index 7ab4ebe0..c9b16657 100644 --- a/lib/betaflight/src/platform_cpp.cpp +++ b/lib/betaflight/src/platform_cpp.cpp @@ -1,5 +1,4 @@ -#include -#include +#include "platform.h" #include "Device/SerialDevice.h" #include "EscDriver.h" #include "Hal/Gpio.h" @@ -14,11 +13,11 @@ void IOConfigGPIO(IO_t pin, uint8_t mode) { switch(mode) { case IOCFG_IPU: - ::pinMode(pin, INPUT_PULLUP); + Espfc::Hal::Gpio::pinMode(pin, INPUT_PULLUP); break; case IOCFG_OUT_PP: case IOCFG_AF_PP: - ::pinMode(pin, OUTPUT); + Espfc::Hal::Gpio::pinMode(pin, OUTPUT); break; } } From 147940f6ca4618fec1e020629c2eca933a031389 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 7 Nov 2024 23:46:18 +0100 Subject: [PATCH 54/68] split and move sma and freq analyzers --- lib/Espfc/src/Math/FFTAnalyzer.h | 153 --------------------------- lib/Espfc/src/Math/FreqAnalyzer.h | 79 -------------- lib/Espfc/src/Math/Sma.h | 48 --------- lib/Espfc/src/ModelState.h | 1 - lib/Espfc/src/Sensor/GyroSensor.cpp | 4 + lib/Espfc/src/Sensor/GyroSensor.h | 14 +-- lib/Espfc/src/Utils/FFTAnalyzer.hpp | 59 +++++++++++ lib/Espfc/src/Utils/FFTAnalyzer.ipp | 119 +++++++++++++++++++++ lib/Espfc/src/Utils/FreqAnalyzer.cpp | 52 +++++++++ lib/Espfc/src/Utils/FreqAnalyzer.hpp | 38 +++++++ lib/Espfc/src/Utils/Sma.hpp | 27 +++++ lib/Espfc/src/Utils/Sma.ipp | 38 +++++++ 12 files changed, 344 insertions(+), 288 deletions(-) delete mode 100644 lib/Espfc/src/Math/FFTAnalyzer.h delete mode 100644 lib/Espfc/src/Math/FreqAnalyzer.h delete mode 100644 lib/Espfc/src/Math/Sma.h create mode 100644 lib/Espfc/src/Utils/FFTAnalyzer.hpp create mode 100644 lib/Espfc/src/Utils/FFTAnalyzer.ipp create mode 100644 lib/Espfc/src/Utils/FreqAnalyzer.cpp create mode 100644 lib/Espfc/src/Utils/FreqAnalyzer.hpp create mode 100644 lib/Espfc/src/Utils/Sma.hpp create mode 100644 lib/Espfc/src/Utils/Sma.ipp diff --git a/lib/Espfc/src/Math/FFTAnalyzer.h b/lib/Espfc/src/Math/FFTAnalyzer.h deleted file mode 100644 index e9293a8f..00000000 --- a/lib/Espfc/src/Math/FFTAnalyzer.h +++ /dev/null @@ -1,153 +0,0 @@ -#ifndef _ESPFC_MATH_FFT_ANALYZER_H_ -#define _ESPFC_MATH_FFT_ANALYZER_H_ - -// https://github.com/espressif/esp-dsp/blob/5f2bfe1f3ee7c9b024350557445b32baf6407a08/examples/fft4real/main/dsps_fft4real_main.c - -#include "Math/Utils.h" -#include "Utils/Filter.h" -#include "dsps_fft4r.h" -#include "dsps_wind_hann.h" -#include - -namespace Espfc { - -namespace Math { - -enum FFTPhase { - PHASE_COLLECT, - PHASE_FFT, - PHASE_PEAKS -}; - -template -class FFTAnalyzer -{ -public: - FFTAnalyzer(): _idx(0), _phase(PHASE_COLLECT) {} - - int begin(int16_t rate, const DynamicFilterConfig& config, size_t axis) - { - int16_t nyquistLimit = rate / 2; - _rate = rate; - _freq_min = config.min_freq; - _freq_max = std::min(config.max_freq, nyquistLimit); - _peak_count = std::min((size_t)config.count, PEAKS_MAX); - - _idx = axis * SAMPLES / 3; - _bin_width = (float)_rate / SAMPLES; // no need to dived by 2 as we next process `SAMPLES / 2` results - - _begin = (_freq_min / _bin_width) + 1; - _end = std::min(BINS - 1, (size_t)(_freq_max / _bin_width)) - 1; - - // init fft tables - dsps_fft4r_init_fc32(nullptr, BINS); - - // Generate hann window - dsps_wind_hann_f32(_win, SAMPLES); - - clearPeaks(); - - for(size_t i = 0; i < SAMPLES; i++) _in[i] = 0; - //std::fill(_in, _in + SAMPLES, 0); - - return 1; - } - - // calculate fft and find noise peaks - int update(float v) - { - _in[_idx] = v; - - if(++_idx >= SAMPLES) - { - _idx = 0; - _phase = PHASE_FFT; - } - - switch(_phase) - { - case PHASE_COLLECT: - return 0; - - case PHASE_FFT: // 32us - // apply window function - for (size_t j = 0; j < SAMPLES; j++) - { - _out[j] = _in[j] * _win[j]; // real - } - - // FFT Radix-4 - dsps_fft4r_fc32(_out, BINS); - - // Bit reverse - dsps_bit_rev4r_fc32(_out, BINS); - - // Convert one complex vector with length SAMPLES/2 to one real spectrum vector with length SAMPLES/2 - dsps_cplx2real_fc32(_out, BINS); - - _phase = PHASE_PEAKS; - return 0; - - case PHASE_PEAKS: // 12us + 22us sqrt() - // calculate magnitude - for (size_t j = 0; j < BINS; j++) - { - size_t k = j * 2; - //_out[j] = _out[k] * _out[k] + _out[k + 1] * _out[k + 1]; - _out[j] = sqrt(_out[k] * _out[k] + _out[k + 1] * _out[k + 1]); - } - - clearPeaks(); - - Math::peakDetect(_out, _begin, _end, _bin_width, peaks, _peak_count); - - // sort peaks by freq - Math::peakSort(peaks, _peak_count); - - _phase = PHASE_COLLECT; - return 1; - - default: - _phase = PHASE_COLLECT; - return 0; - } - } - - static const size_t PEAKS_MAX = 8; - Peak peaks[PEAKS_MAX]; - -private: - void clearPeaks() - { - for(size_t i = 0; i < PEAKS_MAX; i++) peaks[i] = Peak(); - //std::fill(peaks, peaks + PEAKS_MAX, Peak()); - } - - static const size_t BINS = SAMPLES >> 1; - - int16_t _rate; - int16_t _freq_min; - int16_t _freq_max; - int16_t _peak_count; - - size_t _idx; - FFTPhase _phase; - size_t _begin; - size_t _end; - float _bin_width; - - // fft input - __attribute__((aligned(16))) float _in[SAMPLES]; - - // fft output - __attribute__((aligned(16))) float _out[SAMPLES]; - - // Window coefficients - __attribute__((aligned(16))) float _win[SAMPLES]; -}; - -} - -} - -#endif diff --git a/lib/Espfc/src/Math/FreqAnalyzer.h b/lib/Espfc/src/Math/FreqAnalyzer.h deleted file mode 100644 index 7b5be7fe..00000000 --- a/lib/Espfc/src/Math/FreqAnalyzer.h +++ /dev/null @@ -1,79 +0,0 @@ -#ifndef _ESPFC_MATH_FREQ_ANALYZER_H_ -#define _ESPFC_MATH_FREQ_ANALYZER_H_ - -#include "Math/Utils.h" -#include "Utils/Filter.h" - -namespace Espfc { - -namespace Math { - -class FreqAnalyzer -{ - public: - FreqAnalyzer() {} - - int begin(int rate, DynamicFilterConfig config) - { - _rate = rate; - _freq_min = config.min_freq; - _freq_max = config.max_freq; - _pitch_freq_raise = _pitch_freq_fall = (_freq_min + _freq_max) * 0.5f; - _pitch_count_raise = _pitch_count_fall = 0; - _bpf.begin(FilterConfig(FILTER_BPF, 180, 100), _rate); // 100 - 180 - 260 [Hz] - return 1; - } - - // calculate noise pitch freq - void update(float v) - { - // pitch detection - noise = _bpf.update(v); - bool sign = noise > 0.f; - float k = 0.33f; - - // detect rising zero crossing - if(sign && !_sign_prev) { - float f = Math::clamp(_rate / std::max(_pitch_count_raise, 1), _freq_min, _freq_max); - _pitch_freq_raise += k * (f - _pitch_freq_raise); - _pitch_count_raise = 0; - } - - // detect falling zero crossing - if(!sign && _sign_prev) { - float f = Math::clamp(_rate / std::max(_pitch_count_fall, 1), _freq_min, _freq_max); - _pitch_freq_fall += k * (f - _pitch_freq_fall); - _pitch_count_fall = 0; - } - - _sign_prev = sign; - _pitch_count_raise++; - _pitch_count_fall++; - - freq = (_pitch_freq_raise + _pitch_freq_fall) * 0.5f; - } - - float freq; - float noise; - - private: - Utils::Filter _bpf; - float _rate; - - float _freq_min; - float _freq_max; - - int _pitch_count_raise; - int _pitch_count_fall; - - float _pitch_freq_raise; - float _pitch_freq_fall; - - bool _sign_prev; -}; - -} - -} - -#endif diff --git a/lib/Espfc/src/Math/Sma.h b/lib/Espfc/src/Math/Sma.h deleted file mode 100644 index 0e5c0910..00000000 --- a/lib/Espfc/src/Math/Sma.h +++ /dev/null @@ -1,48 +0,0 @@ -#ifndef _ESPFC_MATH_SMA_H_ -#define _ESPFC_MATH_SMA_H_ - -namespace Espfc { - -namespace Math { - -template -class Sma -{ -public: - Sma(): _idx(0), _count(MaxSize) - { - begin(MaxSize); - } - - void begin(size_t count) - { - _count = std::min(count, MaxSize); - _inv_count = 1.f / _count; - } - - SampleType update(const SampleType& input) - { - if(_count > 1) - { - _sum -= _samples[_idx]; - _sum += input; - _samples[_idx] = input; - if (++_idx >= _count) _idx = 0; - return _sum * _inv_count; - } - return input; - } - -private: - SampleType _samples[MaxSize]; - SampleType _sum; - size_t _idx; - size_t _count; - float _inv_count; -}; - -} - -} - -#endif \ No newline at end of file diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 950efc69..4990a04c 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -13,7 +13,6 @@ #include "Utils/Timer.h" #include "Utils/Stats.h" #include "Device/SerialDevice.h" -#include "Math/FreqAnalyzer.h" #include "Connect/Msp.h" namespace Espfc { diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index 722e32b3..f20ebac5 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -1,6 +1,10 @@ #include "GyroSensor.h" #include "Utils/FilterHelper.h" +#include "Utils/Sma.ipp" +#ifdef ESPFC_DSP +#include "Utils/FFTAnalyzer.ipp" +#endif #define ESPFC_FUZZY_ACCEL_ZERO 0.05 #define ESPFC_FUZZY_GYRO_ZERO 0.20 diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index 57f7a613..ea388573 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -3,11 +3,11 @@ #include "BaseSensor.h" #include "Model.h" #include "Device/GyroDevice.h" -#include "Math/Sma.h" +#include "Utils/Sma.hpp" #ifdef ESPFC_DSP -#include "Math/FFTAnalyzer.h" +#include "Utils/FFTAnalyzer.hpp" #else -#include "Math/FreqAnalyzer.h" +#include "Utils/FreqAnalyzer.hpp" #endif #define ESPFC_FUZZY_ACCEL_ZERO 0.05 @@ -32,8 +32,8 @@ class GyroSensor: public BaseSensor private: void calibrate(); - Math::Sma _sma; - Math::Sma _dyn_notch_sma; + Utils::Sma _sma; + Utils::Sma _dyn_notch_sma; size_t _dyn_notch_denom; size_t _dyn_notch_count; bool _dyn_notch_enabled; @@ -50,9 +50,9 @@ class GyroSensor: public BaseSensor Device::GyroDevice * _gyro; #ifdef ESPFC_DSP - Math::FFTAnalyzer<128> _fft[3]; + Utils::FFTAnalyzer<128> _fft[3]; #else - Math::FreqAnalyzer _freqAnalyzer[3]; + Utils::FreqAnalyzer _freqAnalyzer[3]; #endif }; diff --git a/lib/Espfc/src/Utils/FFTAnalyzer.hpp b/lib/Espfc/src/Utils/FFTAnalyzer.hpp new file mode 100644 index 00000000..7f004bcc --- /dev/null +++ b/lib/Espfc/src/Utils/FFTAnalyzer.hpp @@ -0,0 +1,59 @@ +#pragma once + +// https://github.com/espressif/esp-dsp/blob/5f2bfe1f3ee7c9b024350557445b32baf6407a08/examples/fft4real/main/dsps_fft4real_main.c + +#include +#include +#include "Utils/Filter.h" +#include "Math/Utils.h" + +namespace Espfc { + +namespace Utils { + +enum FFTPhase { + PHASE_COLLECT, + PHASE_FFT, + PHASE_PEAKS +}; + +template +class FFTAnalyzer +{ +public: + FFTAnalyzer(); + int begin(int16_t rate, const DynamicFilterConfig& config, size_t axis); + int update(float v); + + static const size_t PEAKS_MAX = 8; + Math::Peak peaks[PEAKS_MAX]; + +private: + void clearPeaks(); + + static const size_t BINS = SAMPLES >> 1; + + int16_t _rate; + int16_t _freq_min; + int16_t _freq_max; + int16_t _peak_count; + + size_t _idx; + FFTPhase _phase; + size_t _begin; + size_t _end; + float _bin_width; + + // fft input + __attribute__((aligned(16))) float _in[SAMPLES]; + + // fft output + __attribute__((aligned(16))) float _out[SAMPLES]; + + // Window coefficients + __attribute__((aligned(16))) float _win[SAMPLES]; +}; + +} + +} diff --git a/lib/Espfc/src/Utils/FFTAnalyzer.ipp b/lib/Espfc/src/Utils/FFTAnalyzer.ipp new file mode 100644 index 00000000..7ae71cc4 --- /dev/null +++ b/lib/Espfc/src/Utils/FFTAnalyzer.ipp @@ -0,0 +1,119 @@ +#pragma once + +#ifdef ESPFC_DSP + +// https://github.com/espressif/esp-dsp/blob/5f2bfe1f3ee7c9b024350557445b32baf6407a08/examples/fft4real/main/dsps_fft4real_main.c +#include "Utils/FFTAnalyzer.hpp" +#include "dsps_fft4r.h" +#include "dsps_wind_hann.h" +#include + +namespace Espfc { + +namespace Utils { + +template +FFTAnalyzer::FFTAnalyzer(): _idx(0), _phase(PHASE_COLLECT) {} + +template +int FFTAnalyzer::begin(int16_t rate, const DynamicFilterConfig& config, size_t axis) +{ + int16_t nyquistLimit = rate / 2; + _rate = rate; + _freq_min = config.min_freq; + _freq_max = std::min(config.max_freq, nyquistLimit); + _peak_count = std::min((size_t)config.count, PEAKS_MAX); + + _idx = axis * SAMPLES / 3; + _bin_width = (float)_rate / SAMPLES; // no need to dived by 2 as we next process `SAMPLES / 2` results + + _begin = (_freq_min / _bin_width) + 1; + _end = std::min(BINS - 1, (size_t)(_freq_max / _bin_width)) - 1; + + // init fft tables + dsps_fft4r_init_fc32(nullptr, BINS); + + // Generate hann window + dsps_wind_hann_f32(_win, SAMPLES); + + clearPeaks(); + + for(size_t i = 0; i < SAMPLES; i++) _in[i] = 0; + //std::fill(_in, _in + SAMPLES, 0); + + return 1; +} + +template +// calculate fft and find noise peaks +int FFTAnalyzer::update(float v) +{ + _in[_idx] = v; + + if(++_idx >= SAMPLES) + { + _idx = 0; + _phase = PHASE_FFT; + } + + switch(_phase) + { + case PHASE_COLLECT: + return 0; + + case PHASE_FFT: // 32us + // apply window function + for (size_t j = 0; j < SAMPLES; j++) + { + _out[j] = _in[j] * _win[j]; // real + } + + // FFT Radix-4 + dsps_fft4r_fc32(_out, BINS); + + // Bit reverse + dsps_bit_rev4r_fc32(_out, BINS); + + // Convert one complex vector with length SAMPLES/2 to one real spectrum vector with length SAMPLES/2 + dsps_cplx2real_fc32(_out, BINS); + + _phase = PHASE_PEAKS; + return 0; + + case PHASE_PEAKS: // 12us + 22us sqrt() + // calculate magnitude + for (size_t j = 0; j < BINS; j++) + { + size_t k = j * 2; + //_out[j] = _out[k] * _out[k] + _out[k + 1] * _out[k + 1]; + _out[j] = sqrt(_out[k] * _out[k] + _out[k + 1] * _out[k + 1]); + } + + clearPeaks(); + + Math::peakDetect(_out, _begin, _end, _bin_width, peaks, _peak_count); + + // sort peaks by freq + Math::peakSort(peaks, _peak_count); + + _phase = PHASE_COLLECT; + return 1; + + default: + _phase = PHASE_COLLECT; + return 0; + } +} + +template +void FFTAnalyzer::clearPeaks() +{ + for(size_t i = 0; i < PEAKS_MAX; i++) peaks[i] = Math::Peak(); + //std::fill(peaks, peaks + PEAKS_MAX, Peak()); +} + +} + +} + +#endif diff --git a/lib/Espfc/src/Utils/FreqAnalyzer.cpp b/lib/Espfc/src/Utils/FreqAnalyzer.cpp new file mode 100644 index 00000000..f7f5d141 --- /dev/null +++ b/lib/Espfc/src/Utils/FreqAnalyzer.cpp @@ -0,0 +1,52 @@ +#include "Utils/FreqAnalyzer.hpp" +#include "Math/Utils.h" + +namespace Espfc { + +namespace Utils { + +FreqAnalyzer::FreqAnalyzer() {} + +int FreqAnalyzer::begin(int rate, DynamicFilterConfig config) +{ + _rate = rate; + _freq_min = config.min_freq; + _freq_max = config.max_freq; + _pitch_freq_raise = _pitch_freq_fall = (_freq_min + _freq_max) * 0.5f; + _pitch_count_raise = _pitch_count_fall = 0; + _bpf.begin(FilterConfig(FILTER_BPF, 180, 100), _rate); // 100 - 180 - 260 [Hz] + return 1; +} + +// calculate noise pitch freq +void FreqAnalyzer::update(float v) +{ + // pitch detection + noise = _bpf.update(v); + bool sign = noise > 0.f; + float k = 0.33f; + + // detect rising zero crossing + if(sign && !_sign_prev) { + float f = Math::clamp(_rate / std::max(_pitch_count_raise, 1), _freq_min, _freq_max); + _pitch_freq_raise += k * (f - _pitch_freq_raise); + _pitch_count_raise = 0; + } + + // detect falling zero crossing + if(!sign && _sign_prev) { + float f = Math::clamp(_rate / std::max(_pitch_count_fall, 1), _freq_min, _freq_max); + _pitch_freq_fall += k * (f - _pitch_freq_fall); + _pitch_count_fall = 0; + } + + _sign_prev = sign; + _pitch_count_raise++; + _pitch_count_fall++; + + freq = (_pitch_freq_raise + _pitch_freq_fall) * 0.5f; +} + +} + +} diff --git a/lib/Espfc/src/Utils/FreqAnalyzer.hpp b/lib/Espfc/src/Utils/FreqAnalyzer.hpp new file mode 100644 index 00000000..1d3e8112 --- /dev/null +++ b/lib/Espfc/src/Utils/FreqAnalyzer.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include "Utils/Filter.h" + +namespace Espfc { + +namespace Utils { + +class FreqAnalyzer +{ + public: + FreqAnalyzer(); + + int begin(int rate, DynamicFilterConfig config); + void update(float v); + + float freq; + float noise; + + private: + Utils::Filter _bpf; + float _rate; + + float _freq_min; + float _freq_max; + + int _pitch_count_raise; + int _pitch_count_fall; + + float _pitch_freq_raise; + float _pitch_freq_fall; + + bool _sign_prev; +}; + +} + +} diff --git a/lib/Espfc/src/Utils/Sma.hpp b/lib/Espfc/src/Utils/Sma.hpp new file mode 100644 index 00000000..6f682f04 --- /dev/null +++ b/lib/Espfc/src/Utils/Sma.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include + +namespace Espfc { + +namespace Utils { + +template +class Sma +{ +public: + Sma(); + void begin(size_t count); + SampleType update(const SampleType& input); + +private: + SampleType _samples[MaxSize]; + SampleType _sum; + size_t _idx; + size_t _count; + float _inv_count; +}; + +} + +} diff --git a/lib/Espfc/src/Utils/Sma.ipp b/lib/Espfc/src/Utils/Sma.ipp new file mode 100644 index 00000000..1a3517fe --- /dev/null +++ b/lib/Espfc/src/Utils/Sma.ipp @@ -0,0 +1,38 @@ +#pragma once + +#include "Utils/Sma.hpp" + +namespace Espfc { + +namespace Utils { + +template +Sma::Sma(): _idx(0), _count(MaxSize) +{ + begin(MaxSize); +} + +template +void Sma::begin(size_t count) +{ + _count = std::min(count, MaxSize); + _inv_count = 1.f / _count; +} + +template +SampleType Sma::update(const SampleType& input) +{ + if(_count > 1) + { + _sum -= _samples[_idx]; + _sum += input; + _samples[_idx] = input; + if (++_idx >= _count) _idx = 0; + return _sum * _inv_count; + } + return input; +} + +} + +} From 7941be5934820cfcc549e53165e7f67580297c25 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Fri, 8 Nov 2024 22:40:36 +0100 Subject: [PATCH 55/68] move Math/Bits.h to Utils --- lib/Espfc/src/Device/BusDevice.h | 17 ++-- lib/Espfc/src/Hal/Pgm.h | 10 +-- lib/Espfc/src/{Math/Bits.h => Utils/Bits.hpp} | 4 +- test/test_math/test_math.cpp | 84 +++++++++---------- 4 files changed, 59 insertions(+), 56 deletions(-) rename lib/Espfc/src/{Math/Bits.h => Utils/Bits.hpp} (98%) diff --git a/lib/Espfc/src/Device/BusDevice.h b/lib/Espfc/src/Device/BusDevice.h index dd3c3e69..1f3a38cb 100644 --- a/lib/Espfc/src/Device/BusDevice.h +++ b/lib/Espfc/src/Device/BusDevice.h @@ -3,7 +3,8 @@ #include #include -#include "Math/Bits.h" +#include "Hal/Pgm.h" +#include "Utils/Bits.hpp" #define ESPFC_BUS_TIMEOUT 100 @@ -57,7 +58,7 @@ class BusDevice { uint8_t b; uint8_t count = readByte(devAddr, regAddr, &b); - *data = Math::getBit(b, bitNum); + *data = Utils::getBit(b, bitNum); return count; } @@ -66,7 +67,7 @@ class BusDevice uint8_t count, b; if ((count = readByte(devAddr, regAddr, &b)) != 0) { - *data = Math::getBitsMsb(b, bitStart, length); + *data = Utils::getBitsMsb(b, bitStart, length); } return count; } @@ -76,7 +77,7 @@ class BusDevice uint8_t count, b; if ((count = readByte(devAddr, regAddr, &b)) != 0) { - *data = Math::getBitsLsb(b, bitStart, length); + *data = Utils::getBitsLsb(b, bitStart, length); } return count; } @@ -85,7 +86,7 @@ class BusDevice { uint8_t b; readByte(devAddr, regAddr, &b); - b = Math::setBit(b, bitNum, data); + b = Utils::setBit(b, bitNum, data); return writeByte(devAddr, regAddr, b); } @@ -94,7 +95,7 @@ class BusDevice uint8_t b = 0; if (readByte(devAddr, regAddr, &b) != 0) { - b = Math::setBitsMsb(b, bitStart, length, data); + b = Utils::setBitsMsb(b, bitStart, length, data); return writeByte(devAddr, regAddr, b); } else { return false; @@ -106,7 +107,7 @@ class BusDevice uint8_t b = 0; if (readByte(devAddr, regAddr, &b) != 0) { - b = Math::setBitsLsb(b, bitStart, length, data); + b = Utils::setBitsLsb(b, bitStart, length, data); return writeByte(devAddr, regAddr, b); } else { return false; @@ -118,7 +119,7 @@ class BusDevice uint8_t b = 0; if (readByte(devAddr, regAddr, &b) != 0) { - b = Math::setMasked(b, mask, data); + b = Utils::setMasked(b, mask, data); return writeByte(devAddr, regAddr, b); } else { return false; diff --git a/lib/Espfc/src/Hal/Pgm.h b/lib/Espfc/src/Hal/Pgm.h index 624a3acf..f613b19f 100644 --- a/lib/Espfc/src/Hal/Pgm.h +++ b/lib/Espfc/src/Hal/Pgm.h @@ -1,6 +1,10 @@ #pragma once -#ifdef UNIT_TEST +#ifndef UNIT_TEST + +#include + +#else #ifndef PSTR #define PSTR(s) (s) @@ -18,7 +22,3 @@ #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) #endif // UNIT_TEST - -//#undef max -//#undef min - diff --git a/lib/Espfc/src/Math/Bits.h b/lib/Espfc/src/Utils/Bits.hpp similarity index 98% rename from lib/Espfc/src/Math/Bits.h rename to lib/Espfc/src/Utils/Bits.hpp index 5eca168d..c350a528 100644 --- a/lib/Espfc/src/Math/Bits.h +++ b/lib/Espfc/src/Utils/Bits.hpp @@ -1,8 +1,10 @@ #pragma once +#include + namespace Espfc { -namespace Math { +namespace Utils { inline bool getBit(uint8_t data, uint8_t bitNum) { diff --git a/test/test_math/test_math.cpp b/test/test_math/test_math.cpp index 60ebc4e7..72116c24 100644 --- a/test/test_math/test_math.cpp +++ b/test/test_math/test_math.cpp @@ -4,7 +4,7 @@ #include #include "msp/msp_protocol.h" #include "Math/Utils.h" -#include "Math/Bits.h" +#include "Utils/Bits.hpp" #include "Utils/Filter.h" #include "Control/Pid.h" #include "Target/QueueAtomic.h" @@ -61,70 +61,70 @@ void test_math_deadband() 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(0, Utils::getBit(0, 0)); + TEST_ASSERT_EQUAL(1, Utils::getBit(1, 0)); + TEST_ASSERT_EQUAL(0, Utils::getBit(1, 1)); + TEST_ASSERT_EQUAL(1, Utils::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_UINT8(0, Utils::setBit(0, 0, 0)); + TEST_ASSERT_EQUAL_UINT8(1, Utils::setBit(0, 0, 1)); + TEST_ASSERT_EQUAL_UINT8(2, Utils::setBit(0, 1, 1)); + TEST_ASSERT_EQUAL_UINT8(3, Utils::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, Utils::setMasked(0, 1, 0)); + TEST_ASSERT_EQUAL_UINT8( 1, Utils::setMasked(0, 1, 1)); + TEST_ASSERT_EQUAL_UINT8(0x38, Utils::setMasked(0x00, 0x38, 0xff)); + TEST_ASSERT_EQUAL_UINT8(0xc7, Utils::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, Utils::getMaskLsb(0, 1)); + TEST_ASSERT_EQUAL_UINT8( 2, Utils::getMaskLsb(1, 1)); + TEST_ASSERT_EQUAL_UINT8( 4, Utils::getMaskLsb(2, 1)); + TEST_ASSERT_EQUAL_UINT8(12, Utils::getMaskLsb(2, 2)); + TEST_ASSERT_EQUAL_UINT8(56, Utils::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, Utils::getMaskMsb(0, 1)); + TEST_ASSERT_EQUAL_UINT8( 2, Utils::getMaskMsb(1, 1)); + TEST_ASSERT_EQUAL_UINT8( 4, Utils::getMaskMsb(2, 1)); + TEST_ASSERT_EQUAL_UINT8( 6, Utils::getMaskMsb(2, 2)); + TEST_ASSERT_EQUAL_UINT8(14, Utils::getMaskMsb(3, 3)); + TEST_ASSERT_EQUAL_UINT8(30, Utils::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(0, Utils::getBitsLsb(0x00, 1, 1)); + TEST_ASSERT_EQUAL_UINT8(0, Utils::getBitsLsb(0x55, 1, 1)); + TEST_ASSERT_EQUAL_UINT8(1, Utils::getBitsLsb(0x55, 2, 2)); + TEST_ASSERT_EQUAL_UINT8(1, Utils::getBitsLsb(0x55, 4, 2)); + TEST_ASSERT_EQUAL_UINT8(5, Utils::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( 8, Utils::setBitsLsb(0x00, 3, 4, 1)); + TEST_ASSERT_EQUAL_UINT8( 80, Utils::setBitsLsb(0x00, 3, 4, 10)); + TEST_ASSERT_EQUAL_UINT8( 16, Utils::setBitsLsb(0x00, 4, 2, 1)); + TEST_ASSERT_EQUAL_UINT8(160, Utils::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( 0, Utils::getBitsMsb(0x00, 1, 1)); + TEST_ASSERT_EQUAL_UINT8( 0, Utils::getBitsMsb(0x55, 1, 1)); + TEST_ASSERT_EQUAL_UINT8( 2, Utils::getBitsMsb(0x55, 2, 2)); + TEST_ASSERT_EQUAL_UINT8( 2, Utils::getBitsMsb(0x55, 4, 2)); + TEST_ASSERT_EQUAL_UINT8(10, Utils::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( 1, Utils::setBitsMsb(0x00, 3, 4, 1)); + TEST_ASSERT_EQUAL_UINT8(10, Utils::setBitsMsb(0x00, 3, 4, 10)); + TEST_ASSERT_EQUAL_UINT8( 8, Utils::setBitsMsb(0x00, 6, 4, 1)); + TEST_ASSERT_EQUAL_UINT8(80, Utils::setBitsMsb(0x00, 6, 4, 10)); } void test_math_clock_align() From 45c26ebce450dd47d4189634e83c9767b675bc92 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Fri, 8 Nov 2024 22:52:53 +0100 Subject: [PATCH 56/68] move Crc to Utils --- lib/Espfc/src/Connect/Msp.h | 10 +++++----- lib/Espfc/src/Connect/MspParser.h | 10 +++++----- lib/Espfc/src/Rc/Crsf.cpp | 6 +++--- lib/Espfc/src/Rc/Crsf.h | 6 +++--- lib/Espfc/src/{Math => Utils}/Crc.cpp | 6 +++--- lib/Espfc/src/{Math/Crc.h => Utils/Crc.hpp} | 4 ++-- 6 files changed, 21 insertions(+), 21 deletions(-) rename lib/Espfc/src/{Math => Utils}/Crc.cpp (92%) rename lib/Espfc/src/{Math/Crc.h => Utils/Crc.hpp} (74%) diff --git a/lib/Espfc/src/Connect/Msp.h b/lib/Espfc/src/Connect/Msp.h index 2f1d9326..855a9063 100644 --- a/lib/Espfc/src/Connect/Msp.h +++ b/lib/Espfc/src/Connect/Msp.h @@ -3,7 +3,7 @@ #include #include "Hal/Pgm.h" -#include "Math/Crc.h" +#include "Utils/Crc.hpp" extern "C" { #include "msp/msp_protocol.h" @@ -205,11 +205,11 @@ class MspResponse buff[3] = len; buff[4] = cmd; - uint8_t checksum = Math::crc8_xor(0, &buff[3], 2); + uint8_t checksum = Utils::crc8_xor(0, &buff[3], 2); size_t i = 5; for(size_t j = 0; j < len; j++) { - checksum = Math::crc8_xor(checksum, data[j]); + checksum = Utils::crc8_xor(checksum, data[j]); buff[i++] = data[j]; } buff[i] = checksum; @@ -231,11 +231,11 @@ class MspResponse buff[6] = len & 0xff; buff[7] = (len >> 8) & 0xff; - uint8_t checksum = Math::crc8_dvb_s2(0, &buff[3], 5); + uint8_t checksum = Utils::crc8_dvb_s2(0, &buff[3], 5); size_t i = 8; for(size_t j = 0; j < len; j++) { - checksum = Math::crc8_dvb_s2(checksum, data[j]); + checksum = Utils::crc8_dvb_s2(checksum, data[j]); buff[i++] = data[j]; } buff[i] = checksum; diff --git a/lib/Espfc/src/Connect/MspParser.h b/lib/Espfc/src/Connect/MspParser.h index c43116c4..f156fef3 100644 --- a/lib/Espfc/src/Connect/MspParser.h +++ b/lib/Espfc/src/Connect/MspParser.h @@ -2,7 +2,7 @@ #define _ESPFC_MSP_PARSER_H_ #include "Msp.h" -#include "Math/Crc.h" +#include "Utils/Crc.hpp" namespace Espfc { @@ -73,7 +73,7 @@ class MspParser case MSP_STATE_HEADER_V1: msg.buffer[msg.received++] = c; - msg.checksum = Math::crc8_xor(msg.checksum, c); + msg.checksum = Utils::crc8_xor(msg.checksum, c); if(msg.received == sizeof(MspHeaderV1)) { const MspHeaderV1 * hdr = reinterpret_cast(msg.buffer); @@ -90,7 +90,7 @@ class MspParser case MSP_STATE_PAYLOAD_V1: msg.buffer[msg.received++] = c; - msg.checksum = Math::crc8_xor(msg.checksum, c); + msg.checksum = Utils::crc8_xor(msg.checksum, c); if(msg.received == msg.expected) { msg.state = MSP_STATE_CHECKSUM_V1; @@ -103,7 +103,7 @@ class MspParser case MSP_STATE_HEADER_V2: msg.buffer[msg.received++] = c; - msg.checksum2 = Math::crc8_dvb_s2(msg.checksum2, c); + msg.checksum2 = Utils::crc8_dvb_s2(msg.checksum2, c); if(msg.received == sizeof(MspHeaderV2)) { const MspHeaderV2 * hdr = reinterpret_cast(msg.buffer); @@ -121,7 +121,7 @@ class MspParser case MSP_STATE_PAYLOAD_V2: msg.buffer[msg.received++] = c; - msg.checksum2 = Math::crc8_dvb_s2(msg.checksum2, c); + msg.checksum2 = Utils::crc8_dvb_s2(msg.checksum2, c); if(msg.received == msg.expected) { msg.state = MSP_STATE_CHECKSUM_V2; diff --git a/lib/Espfc/src/Rc/Crsf.cpp b/lib/Espfc/src/Rc/Crsf.cpp index c18551bb..e4fef0c8 100644 --- a/lib/Espfc/src/Rc/Crsf.cpp +++ b/lib/Espfc/src/Rc/Crsf.cpp @@ -1,7 +1,7 @@ #include #include "Crsf.h" #include "Math/Utils.h" -#include "Math/Crc.h" +#include "Utils/Crc.hpp" #include "Utils/MemoryHelper.h" #include @@ -185,9 +185,9 @@ uint16_t Crsf::convert(int v) uint8_t Crsf::crc(const CrsfMessage& msg) { // CRC includes type and payload - uint8_t crc = Math::crc8_dvb_s2(0, msg.type); + uint8_t crc = Utils::crc8_dvb_s2(0, msg.type); for (int i = 0; i < msg.size - 2; i++) { // size includes type and crc - crc = Math::crc8_dvb_s2(crc, msg.payload[i]); + crc = Utils::crc8_dvb_s2(crc, msg.payload[i]); } return crc; } diff --git a/lib/Espfc/src/Rc/Crsf.h b/lib/Espfc/src/Rc/Crsf.h index 0c4eba2f..e8bc2d20 100644 --- a/lib/Espfc/src/Rc/Crsf.h +++ b/lib/Espfc/src/Rc/Crsf.h @@ -2,7 +2,7 @@ #include #include -#include "Math/Crc.h" +#include "Utils/Crc.hpp" #include "Connect/Msp.h" namespace Espfc { @@ -170,8 +170,8 @@ struct CrsfMessage uint8_t crc() const { - uint8_t crc = Math::crc8_dvb_s2(0, type); - return Math::crc8_dvb_s2(crc, payload, size - 2); // size includes type and crc + uint8_t crc = Utils::crc8_dvb_s2(0, type); + return Utils::crc8_dvb_s2(crc, payload, size - 2); // size includes type and crc } } __attribute__ ((__packed__)); diff --git a/lib/Espfc/src/Math/Crc.cpp b/lib/Espfc/src/Utils/Crc.cpp similarity index 92% rename from lib/Espfc/src/Math/Crc.cpp rename to lib/Espfc/src/Utils/Crc.cpp index 5c8ced3c..d3f734ed 100644 --- a/lib/Espfc/src/Math/Crc.cpp +++ b/lib/Espfc/src/Utils/Crc.cpp @@ -1,9 +1,9 @@ -#include "Crc.h" +#include "Utils/Crc.hpp" #include "Utils/MemoryHelper.h" namespace Espfc { -namespace Math { +namespace Utils { uint8_t FAST_CODE_ATTR crc8_dvb_s2(uint8_t crc, const uint8_t a) { @@ -36,7 +36,7 @@ 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) +uint8_t FAST_CODE_ATTR crc8_xor(uint8_t checksum, const uint8_t *data, size_t len) { while (len-- > 0) { diff --git a/lib/Espfc/src/Math/Crc.h b/lib/Espfc/src/Utils/Crc.hpp similarity index 74% rename from lib/Espfc/src/Math/Crc.h rename to lib/Espfc/src/Utils/Crc.hpp index 809ce180..a6abdb5e 100644 --- a/lib/Espfc/src/Math/Crc.h +++ b/lib/Espfc/src/Utils/Crc.hpp @@ -5,12 +5,12 @@ namespace Espfc { -namespace Math { +namespace Utils { 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); +uint8_t crc8_xor(uint8_t checksum, const uint8_t *data, size_t len); } From 78695cbc9f2c4d657fd586739505dc0b3fb8b269 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Fri, 8 Nov 2024 23:06:49 +0100 Subject: [PATCH 57/68] move Math to Utils --- lib/Espfc/src/Blackbox/Blackbox.cpp | 8 +- lib/Espfc/src/Blackbox/BlackboxBridge.cpp | 2 +- lib/Espfc/src/Blackbox/BlackboxFlashfs.cpp | 4 +- lib/Espfc/src/Connect/Cli.h | 10 +- lib/Espfc/src/Connect/MspProcessor.h | 12 +-- lib/Espfc/src/Control/Actuator.cpp | 10 +- lib/Espfc/src/Control/Controller.cpp | 28 ++--- lib/Espfc/src/Control/Fusion.cpp | 6 +- lib/Espfc/src/Control/Pid.cpp | 8 +- lib/Espfc/src/Control/Pid.h | 10 +- lib/Espfc/src/Control/Rates.cpp | 4 +- lib/Espfc/src/Control/Rates.h | 4 +- lib/Espfc/src/Device/InputSBUS.cpp | 4 +- lib/Espfc/src/Input.cpp | 18 ++-- lib/Espfc/src/Input.h | 2 +- lib/Espfc/src/Model.h | 22 ++-- lib/Espfc/src/Output/Mixer.cpp | 16 +-- lib/Espfc/src/Rc/Crsf.cpp | 6 +- lib/Espfc/src/Sensor/BaroSensor.cpp | 2 +- lib/Espfc/src/Sensor/GyroSensor.cpp | 22 ++-- lib/Espfc/src/Sensor/VoltageSensor.cpp | 2 +- lib/Espfc/src/Telemetry/TelemetryCRSF.h | 24 ++--- lib/Espfc/src/Utils/FFTAnalyzer.hpp | 4 +- lib/Espfc/src/Utils/FFTAnalyzer.ipp | 6 +- lib/Espfc/src/Utils/Filter.cpp | 14 +-- lib/Espfc/src/Utils/FreqAnalyzer.cpp | 6 +- .../src/{Math/Utils.h => Utils/Math.hpp} | 3 +- test/test_math/test_math.cpp | 100 +++++++++--------- 28 files changed, 179 insertions(+), 178 deletions(-) rename lib/Espfc/src/{Math/Utils.h => Utils/Math.hpp} (99%) diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index ed7651ee..991b2761 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -1,7 +1,7 @@ #include "Blackbox.h" #include "Hardware.h" #include "EscDriver.h" -#include "Math/Utils.h" +#include "Utils/Math.hpp" #include "BlackboxBridge.h" namespace Espfc { @@ -237,8 +237,8 @@ void FAST_CODE_ATTR Blackbox::updateData() { for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { - gyro.gyroADCf[i] = Math::toDeg(_model.state.gyro.adc[i]); - gyro.gyroADC[i] = Math::toDeg(_model.state.gyro.scaled[i]); + gyro.gyroADCf[i] = Utils::toDeg(_model.state.gyro.adc[i]); + gyro.gyroADC[i] = Utils::toDeg(_model.state.gyro.scaled[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; @@ -257,7 +257,7 @@ void FAST_CODE_ATTR Blackbox::updateData() rcCommand[AXIS_THRUST] = _model.state.input.buffer[AXIS_THRUST]; for(size_t i = 0; i < 4; i++) { - motor[i] = Math::clamp(_model.state.output.us[i], (int16_t)1000, (int16_t)2000); + motor[i] = Utils::clamp(_model.state.output.us[i], (int16_t)1000, (int16_t)2000); if(_model.state.mixer.digitalOutput) { motor[i] = PWM_TO_DSHOT(motor[i]); diff --git a/lib/Espfc/src/Blackbox/BlackboxBridge.cpp b/lib/Espfc/src/Blackbox/BlackboxBridge.cpp index 823d6dca..15fff807 100644 --- a/lib/Espfc/src/Blackbox/BlackboxBridge.cpp +++ b/lib/Espfc/src/Blackbox/BlackboxBridge.cpp @@ -65,7 +65,7 @@ bool sensors(uint32_t mask) float pidGetPreviousSetpoint(int axis) { - return Espfc::Math::toDeg(_model_ptr->state.setpoint.rate[axis]); + return Espfc::Utils::toDeg(_model_ptr->state.setpoint.rate[axis]); } float mixerGetThrottle(void) diff --git a/lib/Espfc/src/Blackbox/BlackboxFlashfs.cpp b/lib/Espfc/src/Blackbox/BlackboxFlashfs.cpp index 8e810a08..04596ed5 100644 --- a/lib/Espfc/src/Blackbox/BlackboxFlashfs.cpp +++ b/lib/Espfc/src/Blackbox/BlackboxFlashfs.cpp @@ -5,7 +5,7 @@ #include "Device/FlashDevice.h" #include "Utils/RingBuf.h" -#include "Math/Utils.h" +#include "Utils/Math.hpp" static const uint32_t FLASHFS_ERASED_VAL = 0xffffffff; @@ -101,7 +101,7 @@ bool IRAM_ATTR flashfsFlushAsync(bool force) 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); + //uint32_t newAddress = force ? (flashfs.address + size) : Espfc::Utils::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; diff --git a/lib/Espfc/src/Connect/Cli.h b/lib/Espfc/src/Connect/Cli.h index 746433c1..fb2ddc1d 100644 --- a/lib/Espfc/src/Connect/Cli.h +++ b/lib/Espfc/src/Connect/Cli.h @@ -1046,9 +1046,9 @@ class Cli s.print(_model.config.gyro.bias[0]); s.print(' '); s.print(_model.config.gyro.bias[1]); s.print(' '); s.print(_model.config.gyro.bias[2]); s.print(F(" [")); - s.print(Math::toDeg(_model.state.gyro.bias[0])); s.print(' '); - s.print(Math::toDeg(_model.state.gyro.bias[1])); s.print(' '); - s.print(Math::toDeg(_model.state.gyro.bias[2])); s.println(F("]")); + s.print(Utils::toDeg(_model.state.gyro.bias[0])); s.print(' '); + s.print(Utils::toDeg(_model.state.gyro.bias[1])); s.print(' '); + s.print(Utils::toDeg(_model.state.gyro.bias[2])); s.println(F("]")); s.print(F("accel offset: ")); s.print(_model.config.accel.bias[0]); s.print(' '); @@ -1205,7 +1205,7 @@ class Cli float v = _model.state.input.ch[c]; float min = _model.config.scaler[i].minScale * 0.01f; float max = _model.config.scaler[i].maxScale * 0.01f; - float scale = Math::map3(v, -1.f, 0.f, 1.f, min, min < 0 ? 0.f : 1.f, max); + float scale = Utils::map3(v, -1.f, 0.f, 1.f, min, min < 0 ? 0.f : 1.f, max); s.print(F("scaler: ")); s.print(i); s.print(' '); @@ -1450,7 +1450,7 @@ class Cli { size = String(cmd.args[3]).toInt(); } - size = Math::clamp(size, 8u, 128 * 1024u); + size = Utils::clamp(size, 8u, 128 * 1024u); size_t chunk_size = 256; uint8_t* data = new uint8_t[chunk_size]; diff --git a/lib/Espfc/src/Connect/MspProcessor.h b/lib/Espfc/src/Connect/MspProcessor.h index 7e176df3..9e70258f 100644 --- a/lib/Espfc/src/Connect/MspProcessor.h +++ b/lib/Espfc/src/Connect/MspProcessor.h @@ -679,9 +679,9 @@ class MspProcessor break; case MSP_ATTITUDE: - r.writeU16(lrintf(Math::toDeg(_model.state.attitude.euler.x) * 10.f)); // roll [decidegrees] - r.writeU16(lrintf(Math::toDeg(_model.state.attitude.euler.y) * 10.f)); // pitch [decidegrees] - r.writeU16(lrintf(Math::toDeg(-_model.state.attitude.euler.z))); // yaw [degrees] + r.writeU16(lrintf(Utils::toDeg(_model.state.attitude.euler.x) * 10.f)); // roll [decidegrees] + r.writeU16(lrintf(Utils::toDeg(_model.state.attitude.euler.y) * 10.f)); // pitch [decidegrees] + r.writeU16(lrintf(Utils::toDeg(-_model.state.attitude.euler.z))); // yaw [degrees] break; case MSP_ALTITUDE: @@ -951,10 +951,10 @@ class MspProcessor { _model.config.input.superRate[i] = m.readU8(); } - _model.config.controller.tpaScale = Math::clamp(m.readU8(), (uint8_t)0, (uint8_t)90); // dyn thr pid + _model.config.controller.tpaScale = Utils::clamp(m.readU8(), (uint8_t)0, (uint8_t)90); // dyn thr pid m.readU8(); // thrMid8 m.readU8(); // thr expo - _model.config.controller.tpaBreakpoint = Math::clamp(m.readU16(), (uint16_t)1000, (uint16_t)2000); // tpa breakpoint + _model.config.controller.tpaBreakpoint = Utils::clamp(m.readU16(), (uint16_t)1000, (uint16_t)2000); // tpa breakpoint if(m.remain() >= 1) { _model.config.input.expo[AXIS_YAW] = m.readU8(); // yaw expo @@ -1283,7 +1283,7 @@ class MspProcessor } for (int i = 0; i < AXIS_COUNT_RPY; i++) { - r.writeU16(lrintf(Math::toDeg(_model.state.gyro.adc[i]))); + r.writeU16(lrintf(Utils::toDeg(_model.state.gyro.adc[i]))); } for (int i = 0; i < AXIS_COUNT_RPY; i++) { diff --git a/lib/Espfc/src/Control/Actuator.cpp b/lib/Espfc/src/Control/Actuator.cpp index f476b6bc..41fc7015 100644 --- a/lib/Espfc/src/Control/Actuator.cpp +++ b/lib/Espfc/src/Control/Actuator.cpp @@ -1,5 +1,5 @@ #include "Control/Actuator.h" -#include "Math/Utils.h" +#include "Utils/Math.hpp" namespace Espfc { @@ -59,7 +59,7 @@ void Actuator::updateScaler() float v = _model.state.input.ch[c]; float min = _model.config.scaler[i].minScale * 0.01f; float max = _model.config.scaler[i].maxScale * 0.01f; - float scale = Math::map3(v, -1.f, 0.f, 1.f, min, min < 0 ? 0.f : 1.f, max); + float scale = Utils::map3(v, -1.f, 0.f, 1.f, min, min < 0 ? 0.f : 1.f, max); for(size_t x = 0; x < AXIS_COUNT_RPYT; x++) { if( @@ -212,15 +212,15 @@ void Actuator::updateBuzzer() void Actuator::updateDynLpf() { return; // temporary disable - int scale = Math::clamp((int)_model.state.input.us[AXIS_THRUST], 1000, 2000); + int scale = Utils::clamp((int)_model.state.input.us[AXIS_THRUST], 1000, 2000); if(_model.config.gyro.dynLpfFilter.cutoff > 0) { - int gyroFreq = Math::map(scale, 1000, 2000, _model.config.gyro.dynLpfFilter.cutoff, _model.config.gyro.dynLpfFilter.freq); + int gyroFreq = Utils::map(scale, 1000, 2000, _model.config.gyro.dynLpfFilter.cutoff, _model.config.gyro.dynLpfFilter.freq); for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { _model.state.gyro.filter[i].reconfigure(gyroFreq); } } if(_model.config.dterm.dynLpfFilter.cutoff > 0) { - int dtermFreq = Math::map(scale, 1000, 2000, _model.config.dterm.dynLpfFilter.cutoff, _model.config.dterm.dynLpfFilter.freq); + int dtermFreq = Utils::map(scale, 1000, 2000, _model.config.dterm.dynLpfFilter.cutoff, _model.config.dterm.dynLpfFilter.freq); for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { _model.state.innerPid[i].dtermFilter.reconfigure(dtermFreq); } diff --git a/lib/Espfc/src/Control/Controller.cpp b/lib/Espfc/src/Control/Controller.cpp index f3b3004e..6288fe3c 100644 --- a/lib/Espfc/src/Control/Controller.cpp +++ b/lib/Espfc/src/Control/Controller.cpp @@ -1,5 +1,5 @@ #include "Control/Controller.h" -#include "Math/Utils.h" +#include "Utils/Math.hpp" namespace Espfc { @@ -65,19 +65,19 @@ void Controller::outerLoopRobot() if(true || _model.isModeActive(MODE_ANGLE)) { - angle = _model.state.input.ch[AXIS_PITCH] * Math::toRad(_model.config.level.angleLimit); + angle = _model.state.input.ch[AXIS_PITCH] * Utils::toRad(_model.config.level.angleLimit); } else { - angle = _model.state.outerPid[AXIS_PITCH].update(_model.state.input.ch[AXIS_PITCH], speed) * Math::toRad(_model.config.level.rateLimit); + angle = _model.state.outerPid[AXIS_PITCH].update(_model.state.input.ch[AXIS_PITCH], speed) * Utils::toRad(_model.config.level.rateLimit); } _model.state.setpoint.angle.set(AXIS_PITCH, angle); - _model.state.setpoint.rate[AXIS_YAW] = _model.state.input.ch[AXIS_YAW] * Math::toRad(_model.config.level.rateLimit); + _model.state.setpoint.rate[AXIS_YAW] = _model.state.input.ch[AXIS_YAW] * Utils::toRad(_model.config.level.rateLimit); if(_model.config.debug.mode == DEBUG_ANGLERATE) { _model.state.debug[0] = speed * 1000; - _model.state.debug[1] = lrintf(Math::toDeg(angle) * 10); + _model.state.debug[1] = lrintf(Utils::toDeg(angle) * 10); } } @@ -88,7 +88,7 @@ void Controller::innerLoopRobot() //const float angle = acos(v.z); const float angle = std::max(abs(_model.state.attitude.euler[AXIS_PITCH]), abs(_model.state.attitude.euler[AXIS_ROLL])); - const bool stabilize = angle < Math::toRad(_model.config.level.angleLimit); + const bool stabilize = angle < Utils::toRad(_model.config.level.angleLimit); if(stabilize) { _model.state.output.ch[AXIS_PITCH] = _model.state.innerPid[AXIS_PITCH].update(_model.state.setpoint.angle[AXIS_PITCH], _model.state.attitude.euler[AXIS_PITCH]); @@ -103,7 +103,7 @@ void Controller::innerLoopRobot() if(_model.config.debug.mode == DEBUG_ANGLERATE) { - _model.state.debug[2] = lrintf(Math::toDeg(_model.state.attitude.euler[AXIS_PITCH]) * 10); + _model.state.debug[2] = lrintf(Utils::toDeg(_model.state.attitude.euler[AXIS_PITCH]) * 10); _model.state.debug[3] = lrintf(_model.state.output.ch[AXIS_PITCH] * 1000); } } @@ -113,8 +113,8 @@ void FAST_CODE_ATTR Controller::outerLoop() if(_model.isModeActive(MODE_ANGLE)) { _model.state.setpoint.angle = VectorFloat( - _model.state.input.ch[AXIS_ROLL] * Math::toRad(_model.config.level.angleLimit), - _model.state.input.ch[AXIS_PITCH] * Math::toRad(_model.config.level.angleLimit), + _model.state.input.ch[AXIS_ROLL] * Utils::toRad(_model.config.level.angleLimit), + _model.state.input.ch[AXIS_PITCH] * Utils::toRad(_model.config.level.angleLimit), _model.state.attitude.euler[AXIS_YAW] ); _model.state.setpoint.rate[AXIS_ROLL] = _model.state.outerPid[AXIS_ROLL].update(_model.state.setpoint.angle[AXIS_ROLL], _model.state.attitude.euler[AXIS_ROLL]); @@ -135,7 +135,7 @@ void FAST_CODE_ATTR Controller::outerLoop() { for(size_t i = 0; i < AXIS_COUNT_RPY; ++i) { - _model.state.debug[i] = lrintf(Math::toDeg(_model.state.setpoint.rate[i])); + _model.state.debug[i] = lrintf(Utils::toDeg(_model.state.setpoint.rate[i])); } } } @@ -152,9 +152,9 @@ void FAST_CODE_ATTR Controller::innerLoop() if(_model.config.debug.mode == DEBUG_ITERM_RELAX) { - _model.state.debug[0] = lrintf(Math::toDeg(_model.state.innerPid[0].itermRelaxBase)); + _model.state.debug[0] = lrintf(Utils::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[2] = lrintf(Utils::toDeg(_model.state.innerPid[0].iTermError)); _model.state.debug[3] = lrintf(_model.state.innerPid[0].iTerm * 1000.0f); } } @@ -162,8 +162,8 @@ void FAST_CODE_ATTR Controller::innerLoop() float Controller::getTpaFactor() const { if(_model.config.controller.tpaScale == 0) return 1.f; - float t = Math::clamp(_model.state.input.us[AXIS_THRUST], (float)_model.config.controller.tpaBreakpoint, 2000.f); - return Math::map(t, (float)_model.config.controller.tpaBreakpoint, 2000.f, 1.f, 1.f - ((float)_model.config.controller.tpaScale * 0.01f)); + float t = Utils::clamp(_model.state.input.us[AXIS_THRUST], (float)_model.config.controller.tpaBreakpoint, 2000.f); + return Utils::map(t, (float)_model.config.controller.tpaBreakpoint, 2000.f, 1.f, 1.f - ((float)_model.config.controller.tpaScale * 0.01f)); } void Controller::resetIterm() diff --git a/lib/Espfc/src/Control/Fusion.cpp b/lib/Espfc/src/Control/Fusion.cpp index 06715bf8..5bcde08a 100644 --- a/lib/Espfc/src/Control/Fusion.cpp +++ b/lib/Espfc/src/Control/Fusion.cpp @@ -74,9 +74,9 @@ int FAST_CODE_ATTR Fusion::update() if(_model.config.debug.mode == DEBUG_ALTITUDE) { - _model.state.debug[0] = lrintf(Math::toDeg(_model.state.attitude.euler[0]) * 10); - _model.state.debug[1] = lrintf(Math::toDeg(_model.state.attitude.euler[1]) * 10); - _model.state.debug[2] = lrintf(Math::toDeg(_model.state.attitude.euler[2]) * 10); + _model.state.debug[0] = lrintf(Utils::toDeg(_model.state.attitude.euler[0]) * 10); + _model.state.debug[1] = lrintf(Utils::toDeg(_model.state.attitude.euler[1]) * 10); + _model.state.debug[2] = lrintf(Utils::toDeg(_model.state.attitude.euler[2]) * 10); } return 1; } diff --git a/lib/Espfc/src/Control/Pid.cpp b/lib/Espfc/src/Control/Pid.cpp index 9a0c522f..8fb0f74a 100644 --- a/lib/Espfc/src/Control/Pid.cpp +++ b/lib/Espfc/src/Control/Pid.cpp @@ -1,5 +1,5 @@ #include "Pid.h" -#include "Math/Utils.h" +#include "Utils/Math.hpp" #include "Utils/MemoryHelper.h" namespace Espfc { @@ -41,11 +41,11 @@ float FAST_CODE_ATTR Pid::update(float setpoint, float measurement) 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) + itermRelaxFactor = std::max(0.0f, 1.0f - std::abs(Utils::toDeg(itermRelaxBase)) * 0.025f); // (itermRelaxBase / 40) if(!incrementOnly || increasing) iTermError *= itermRelaxFactor; } iTerm += Ki * iScale * iTermError * dt; - iTerm = Math::clamp(iTerm, -iLimit, iLimit); + iTerm = Utils::clamp(iTerm, -iLimit, iLimit); } } else @@ -82,7 +82,7 @@ float FAST_CODE_ATTR Pid::update(float setpoint, float measurement) prevError = error; prevSetpoint = setpoint; - return Math::clamp(pTerm + iTerm + dTerm + fTerm, -oLimit, oLimit); + return Utils::clamp(pTerm + iTerm + dTerm + fTerm, -oLimit, oLimit); } } diff --git a/lib/Espfc/src/Control/Pid.h b/lib/Espfc/src/Control/Pid.h index 453a0f5e..dd7d3936 100644 --- a/lib/Espfc/src/Control/Pid.h +++ b/lib/Espfc/src/Control/Pid.h @@ -2,7 +2,7 @@ #include #include "Utils/Filter.h" -#include "Math/Utils.h" +#include "Utils/Math.hpp" namespace Espfc { @@ -12,10 +12,10 @@ constexpr float ITERM_SCALE_BETAFLIGHT = 0.244381f; constexpr float DTERM_SCALE_BETAFLIGHT = 0.000529f; constexpr float FTERM_SCALE_BETAFLIGHT = 0.00013754f; -constexpr float PTERM_SCALE = PTERM_SCALE_BETAFLIGHT * Math::toDeg(1.0f) * 0.001f; // ~ 0.00183 = 0.032029f * 57.29 / 1000 -constexpr float ITERM_SCALE = ITERM_SCALE_BETAFLIGHT * Math::toDeg(1.0f) * 0.001f; // ~ 0.014f -constexpr float DTERM_SCALE = DTERM_SCALE_BETAFLIGHT * Math::toDeg(1.0f) * 0.001f; // ~ 0.0000303f -constexpr float FTERM_SCALE = FTERM_SCALE_BETAFLIGHT * Math::toDeg(1.0f) * 0.001f; // ~ 0.00000788f +constexpr float PTERM_SCALE = PTERM_SCALE_BETAFLIGHT * Utils::toDeg(1.0f) * 0.001f; // ~ 0.00183 = 0.032029f * 57.29 / 1000 +constexpr float ITERM_SCALE = ITERM_SCALE_BETAFLIGHT * Utils::toDeg(1.0f) * 0.001f; // ~ 0.014f +constexpr float DTERM_SCALE = DTERM_SCALE_BETAFLIGHT * Utils::toDeg(1.0f) * 0.001f; // ~ 0.0000303f +constexpr float FTERM_SCALE = FTERM_SCALE_BETAFLIGHT * Utils::toDeg(1.0f) * 0.001f; // ~ 0.00000788f constexpr float LEVEL_PTERM_SCALE = 0.1f; // 1/10 constexpr float LEVEL_ITERM_SCALE = 0.1f; // 1/10 diff --git a/lib/Espfc/src/Control/Rates.cpp b/lib/Espfc/src/Control/Rates.cpp index 585333e7..405b8f93 100644 --- a/lib/Espfc/src/Control/Rates.cpp +++ b/lib/Espfc/src/Control/Rates.cpp @@ -22,7 +22,7 @@ void Rates::begin(const InputConfig& config) float FAST_CODE_ATTR Rates::getSetpoint(const int axis, float input) const { - input = Math::clamp(input, -0.995f, 0.995f); // limit input + input = Utils::clamp(input, -0.995f, 0.995f); // limit input const float inputAbs = fabsf(input); float result = 0; switch(rateType) @@ -38,7 +38,7 @@ float FAST_CODE_ATTR Rates::getSetpoint(const int axis, float input) const case RATES_TYPE_QUICK: result = quick(axis, input, inputAbs); } - return Math::toRad(Math::clamp(result, -(float)rateLimit[axis], (float)rateLimit[axis])); + return Utils::toRad(Utils::clamp(result, -(float)rateLimit[axis], (float)rateLimit[axis])); } float FAST_CODE_ATTR Rates::betaflight(const int axis, float rcCommandf, const float rcCommandfAbs) const diff --git a/lib/Espfc/src/Control/Rates.h b/lib/Espfc/src/Control/Rates.h index 868bbf54..26579aa1 100644 --- a/lib/Espfc/src/Control/Rates.h +++ b/lib/Espfc/src/Control/Rates.h @@ -1,6 +1,6 @@ #pragma once -#include "Math/Utils.h" +#include "Utils/Math.hpp" #include "ModelConfig.h" namespace Espfc @@ -41,7 +41,7 @@ class Rates inline float constrainf(float x, float l, float h) const { - return Math::clamp(x, l, h); + return Utils::clamp(x, l, h); } private: diff --git a/lib/Espfc/src/Device/InputSBUS.cpp b/lib/Espfc/src/Device/InputSBUS.cpp index af87b5c7..0d1d1234 100644 --- a/lib/Espfc/src/Device/InputSBUS.cpp +++ b/lib/Espfc/src/Device/InputSBUS.cpp @@ -1,5 +1,5 @@ #include "InputSBUS.h" -#include "Math/Utils.h" +#include "Utils/Math.hpp" #include "Utils/MemoryHelper.h" namespace Espfc { @@ -140,7 +140,7 @@ void FAST_CODE_ATTR InputSBUS::apply() uint16_t FAST_CODE_ATTR InputSBUS::convert(int v) { - return Math::clamp(((v * 5) / 8) + 880, 800, 2200); + return Utils::clamp(((v * 5) / 8) + 880, 800, 2200); } } diff --git a/lib/Espfc/src/Input.cpp b/lib/Espfc/src/Input.cpp index b1fba036..bef0f9da 100644 --- a/lib/Espfc/src/Input.cpp +++ b/lib/Espfc/src/Input.cpp @@ -1,6 +1,6 @@ #include "Input.h" -#include "Math/Utils.h" +#include "Utils/Math.hpp" #include "Utils/MemoryHelper.h" namespace Espfc { @@ -18,7 +18,7 @@ int Input::begin() switch(_model.config.input.interpolationMode) { case INPUT_INTERPOLATION_AUTO: - _model.state.input.interpolationDelta = Math::clamp(_model.state.input.frameDelta, (uint32_t)4000, (uint32_t)40000) * 0.000001f; // estimate real interval + _model.state.input.interpolationDelta = Utils::clamp(_model.state.input.frameDelta, (uint32_t)4000, (uint32_t)40000) * 0.000001f; // estimate real interval break; case INPUT_INTERPOLATION_MANUAL: _model.state.input.interpolationDelta = _model.config.input.interpolationInterval * 0.001f; // manual interval @@ -66,12 +66,12 @@ void FAST_CODE_ATTR Input::setInput(Axis i, float v, bool newFrame, bool noFilte { const float nv = noFilter ? v : _model.state.input.filter[i].update(v); _model.state.input.us[i] = nv; - _model.state.input.ch[i] = Math::map(nv, ich.min, ich.max, -1.f, 1.f); + _model.state.input.ch[i] = Utils::map(nv, ich.min, ich.max, -1.f, 1.f); } else if(newFrame) { _model.state.input.us[i] = v; - _model.state.input.ch[i] = Math::map(v, ich.min, ich.max, -1.f, 1.f); + _model.state.input.ch[i] = Utils::map(v, ich.min, ich.max, -1.f, 1.f); } } @@ -150,8 +150,8 @@ void FAST_CODE_ATTR Input::processInputs() 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); + //float t = Utils::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 = Utils::mapi(v, ich.min, ich.max, PWM_RANGE_MIN, PWM_RANGE_MAX); // filter if required t = _filter[c].update(t); @@ -160,7 +160,7 @@ void FAST_CODE_ATTR Input::processInputs() // apply deadband if(c < AXIS_THRUST) { - v = Math::deadband(v - PWM_RANGE_MID, (int)_model.config.input.deadband) + PWM_RANGE_MID; + v = Utils::deadband(v - PWM_RANGE_MID, (int)_model.config.input.deadband) + PWM_RANGE_MID; } // check if inputs are valid, apply failsafe value otherwise @@ -205,7 +205,7 @@ bool FAST_CODE_ATTR Input::failsafe(InputStatus status) // stage 2 timeout _model.state.input.lossTime = micros() - _model.state.input.frameTime; - if(_model.state.input.lossTime > Math::clamp((uint32_t)_model.config.failsafe.delay, (uint32_t)2u, (uint32_t)200u) * TENTH_TO_US) + if(_model.state.input.lossTime > Utils::clamp((uint32_t)_model.config.failsafe.delay, (uint32_t)2u, (uint32_t)200u) * TENTH_TO_US) { failsafeStage2(); return true; @@ -296,7 +296,7 @@ void FAST_CODE_ATTR Input::updateFrameRate() if (_model.config.input.interpolationMode == INPUT_INTERPOLATION_AUTO && _model.config.input.filterType == INPUT_INTERPOLATION) { - _model.state.input.interpolationDelta = Math::clamp(_model.state.input.frameDelta, (uint32_t)4000, (uint32_t)40000) * 0.000001f; // estimate real interval + _model.state.input.interpolationDelta = Utils::clamp(_model.state.input.frameDelta, (uint32_t)4000, (uint32_t)40000) * 0.000001f; // estimate real interval _model.state.input.interpolationStep = _model.state.loopTimer.intervalf / _model.state.input.interpolationDelta; } diff --git a/lib/Espfc/src/Input.h b/lib/Espfc/src/Input.h index 4f014cca..95d549ff 100644 --- a/lib/Espfc/src/Input.h +++ b/lib/Espfc/src/Input.h @@ -1,7 +1,7 @@ #pragma once #include "Model.h" -#include "Math/Utils.h" +#include "Utils/Math.hpp" #include "Device/InputDevice.h" #include "Device/InputPPM.h" #include "Device/InputSBUS.h" diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 0c120e3f..254bf6d6 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -9,7 +9,7 @@ #include "ModelState.h" #include "Utils/Storage.h" #include "Utils/Logger.h" -#include "Math/Utils.h" +#include "Utils/Math.hpp" namespace Espfc { @@ -136,7 +136,7 @@ class Model { //save(); state.buzzer.push(BUZZER_GYRO_CALIBRATED); - logger.info().log(F("GYRO BIAS")).log(Math::toDeg(state.gyro.bias.x)).log(Math::toDeg(state.gyro.bias.y)).logln(Math::toDeg(state.gyro.bias.z)); + logger.info().log(F("GYRO BIAS")).log(Utils::toDeg(state.gyro.bias.x)).log(Utils::toDeg(state.gyro.bias.y)).logln(Utils::toDeg(state.gyro.bias.z)); } if(state.accel.calibrationState == CALIBRATION_SAVE) { @@ -243,7 +243,7 @@ class Model size_t channel = config.input.rssiChannel; if(channel < 4 || channel > state.input.channelCount) return 0; float value = state.input.ch[channel - 1]; - return Math::clamp(lrintf(Math::map(value, -1.0f, 1.0f, 0.0f, 1023.0f)), 0l, 1023l); + return Utils::clamp(lrintf(Utils::map(value, -1.0f, 1.0f, 0.0f, 1023.0f)), 0l, 1023l); } int load() @@ -285,11 +285,11 @@ class Model // for spi gyro allow full speed mode if (state.gyro.dev && state.gyro.dev->getBus()->isSPI()) { - state.gyro.rate = Math::alignToClock(state.gyro.clock, ESPFC_GYRO_SPI_RATE_MAX); + state.gyro.rate = Utils::alignToClock(state.gyro.clock, ESPFC_GYRO_SPI_RATE_MAX); } else { - state.gyro.rate = Math::alignToClock(state.gyro.clock, ESPFC_GYRO_I2C_RATE_MAX); + state.gyro.rate = Utils::alignToClock(state.gyro.clock, ESPFC_GYRO_I2C_RATE_MAX); // first usage if(_storageResult == STORAGE_ERR_BAD_MAGIC || _storageResult == STORAGE_ERR_BAD_SIZE || _storageResult == STORAGE_ERR_BAD_VERSION) { @@ -413,11 +413,11 @@ class Model // init timers // sample rate = clock / ( divider + 1) state.gyro.timer.setRate(state.gyro.rate); - int accelRate = Math::alignToClock(state.gyro.timer.rate, 500); + int accelRate = Utils::alignToClock(state.gyro.timer.rate, 500); state.accel.timer.setRate(state.gyro.timer.rate, state.gyro.timer.rate / accelRate); state.loopTimer.setRate(state.gyro.timer.rate, config.loopSync); state.mixer.timer.setRate(state.loopTimer.rate, config.mixerSync); - int inputRate = Math::alignToClock(state.gyro.timer.rate, 1000); + int inputRate = Utils::alignToClock(state.gyro.timer.rate, 1000); state.input.timer.setRate(state.gyro.timer.rate, state.gyro.timer.rate / inputRate); state.actuatorTimer.setRate(50); state.gyro.dynamicFilterTimer.setRate(50); @@ -428,7 +428,7 @@ class Model state.mag.timer.setRate(state.mag.rate); } - state.boardAlignment.init(VectorFloat(Math::toRad(config.boardAlignment[0]), Math::toRad(config.boardAlignment[1]), Math::toRad(config.boardAlignment[2]))); + state.boardAlignment.init(VectorFloat(Utils::toRad(config.boardAlignment[0]), Utils::toRad(config.boardAlignment[1]), Utils::toRad(config.boardAlignment[2]))); const uint32_t gyroPreFilterRate = state.gyro.timer.rate; const uint32_t gyroFilterRate = state.loopTimer.rate; @@ -464,7 +464,7 @@ class Model state.gyro.rpmFreqFilter[m].begin(FilterConfig(FILTER_PT1, config.gyro.rpmFilter.freqLpf), gyroFilterRate); for(size_t n = 0; n < config.gyro.rpmFilter.harmonics; n++) { - int center = Math::mapi(m * RPM_FILTER_HARMONICS_MAX + n, 0, RPM_FILTER_MOTOR_MAX * config.gyro.rpmFilter.harmonics, config.gyro.rpmFilter.minFreq, gyroFilterRate / 2); + int center = Utils::mapi(m * RPM_FILTER_HARMONICS_MAX + n, 0, RPM_FILTER_MOTOR_MAX * config.gyro.rpmFilter.harmonics, config.gyro.rpmFilter.minFreq, gyroFilterRate / 2); state.gyro.rpmFilter[m][n][i].begin(FilterConfig(FILTER_NOTCH_DF1, center, center * 0.98f), gyroFilterRate); } } @@ -539,8 +539,8 @@ class Model pid.Ki = (float)pc.I * LEVEL_ITERM_SCALE; pid.Kd = (float)pc.D * LEVEL_DTERM_SCALE; pid.Kf = (float)pc.F * LEVEL_FTERM_SCALE; - pid.iLimit = Math::toRad(config.level.rateLimit) * 0.1f; - pid.oLimit = Math::toRad(config.level.rateLimit); + pid.iLimit = Utils::toRad(config.level.rateLimit) * 0.1f; + pid.oLimit = Utils::toRad(config.level.rateLimit); pid.rate = state.loopTimer.rate; pid.ptermFilter.begin(config.level.ptermFilter, pidFilterRate); //pid.iLimit = 0.3f; // ROBOT diff --git a/lib/Espfc/src/Output/Mixer.cpp b/lib/Espfc/src/Output/Mixer.cpp index 014ca231..739d8eae 100644 --- a/lib/Espfc/src/Output/Mixer.cpp +++ b/lib/Espfc/src/Output/Mixer.cpp @@ -167,7 +167,7 @@ void FAST_CODE_ATTR Mixer::updateMixer(const MixerConfig& mixer, float * outputs } else { - thrust = Math::clamp(thrust, -1.f + range, 1.f - range); + thrust = Utils::clamp(thrust, -1.f + range, 1.f - range); } } @@ -210,7 +210,7 @@ float FAST_CODE_ATTR Mixer::limitThrust(float thrust, ThrottleLimitType type, in 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); + return Utils::clamp(thrust, -1.f, (limit * 0.02f) - 1.0f); default: break; } @@ -225,12 +225,12 @@ float FAST_CODE_ATTR Mixer::limitOutput(float output, const OutputChannelConfig& if(occ.servo) { const float factor = limit * 0.01f; - return Math::clamp(output, -factor, factor); + return Utils::clamp(output, -factor, factor); } else { const float factor = limit * 0.02f; // *2 - return Math::clamp(output + 1.f, 0.f, factor) - 1.0f; + return Utils::clamp(output + 1.f, 0.f, factor) - 1.0f; } } @@ -250,13 +250,13 @@ void FAST_CODE_ATTR Mixer::writeOutput(const MixerConfig& mixer, float * out) { 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.output.us[i] = Math::clamp(tmp, och.min, och.max); + const int16_t tmp = lrintf(Utils::map3(out[i], -1.f, 0.f, 1.f, och.reverse ? 2000 : 1000, och.neutral, och.reverse ? 1000 : 2000)); + _model.state.output.us[i] = Utils::clamp(tmp, och.min, och.max); } else { - float v = Math::clamp(out[i], -1.f, 1.f); - _model.state.output.us[i] = lrintf(Math::map(v, -1.f, 1.f, _model.state.mixer.minThrottle, _model.state.mixer.maxThrottle)); + float v = Utils::clamp(out[i], -1.f, 1.f); + _model.state.output.us[i] = lrintf(Utils::map(v, -1.f, 1.f, _model.state.mixer.minThrottle, _model.state.mixer.maxThrottle)); } } } diff --git a/lib/Espfc/src/Rc/Crsf.cpp b/lib/Espfc/src/Rc/Crsf.cpp index e4fef0c8..147cc267 100644 --- a/lib/Espfc/src/Rc/Crsf.cpp +++ b/lib/Espfc/src/Rc/Crsf.cpp @@ -1,6 +1,6 @@ #include #include "Crsf.h" -#include "Math/Utils.h" +#include "Utils/Math.hpp" #include "Utils/Crc.hpp" #include "Utils/MemoryHelper.h" #include @@ -178,8 +178,8 @@ uint16_t Crsf::convert(int v) */ 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); + //return Utils::map(v, 172, 1811, 988, 2012); + //return Utils::mapi(v, 172, 1811, 988, 2012); } uint8_t Crsf::crc(const CrsfMessage& msg) diff --git a/lib/Espfc/src/Sensor/BaroSensor.cpp b/lib/Espfc/src/Sensor/BaroSensor.cpp index 5751ac6a..ffc0851b 100644 --- a/lib/Espfc/src/Sensor/BaroSensor.cpp +++ b/lib/Espfc/src/Sensor/BaroSensor.cpp @@ -104,7 +104,7 @@ void BaroSensor::readPressure() void BaroSensor::updateAltitude() { - _model.state.baro.altitudeRaw = _altitudeFilter.update(Math::toAltitude(_model.state.baro.pressure)); + _model.state.baro.altitudeRaw = _altitudeFilter.update(Utils::toAltitude(_model.state.baro.pressure)); if(_model.state.baro.altitudeBiasSamples > 0) { _model.state.baro.altitudeBiasSamples--; diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index f20ebac5..648f314a 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -25,7 +25,7 @@ int GyroSensor::begin() _gyro->setDLPFMode(_model.config.gyro.dlpf); _gyro->setRate(_gyro->getRate()); - _model.state.gyro.scale = Math::toRad(2000.f) / 32768.f; + _model.state.gyro.scale = Utils::toRad(2000.f) / 32768.f; _model.state.gyro.calibrationState = CALIBRATION_START; // calibrate gyro on start _model.state.gyro.calibrationRate = _model.state.loopTimer.rate; @@ -47,7 +47,7 @@ int GyroSensor::begin() for (size_t i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) { - _rpm_weights[i] = Math::clamp(0.01f * _model.config.gyro.rpmFilter.weights[i], 0.0f, 1.0f); + _rpm_weights[i] = Utils::clamp(0.01f * _model.config.gyro.rpmFilter.weights[i], 0.0f, 1.0f); } for (size_t i = 0; i < AXIS_COUNT_RPY; i++) { @@ -102,14 +102,14 @@ int FAST_CODE_ATTR GyroSensor::filter() for (size_t i = 0; i < AXIS_COUNT_RPY; ++i) { _model.setDebug(DEBUG_GYRO_RAW, i, _model.state.gyro.raw[i]); - _model.setDebug(DEBUG_GYRO_SCALED, i, lrintf(Math::toDeg(_model.state.gyro.scaled[i]))); + _model.setDebug(DEBUG_GYRO_SCALED, i, lrintf(Utils::toDeg(_model.state.gyro.scaled[i]))); } - _model.setDebug(DEBUG_GYRO_SAMPLE, 0, lrintf(Math::toDeg(_model.state.gyro.adc[_model.config.debug.axis]))); + _model.setDebug(DEBUG_GYRO_SAMPLE, 0, lrintf(Utils::toDeg(_model.state.gyro.adc[_model.config.debug.axis]))); _model.state.gyro.adc = Utils::applyFilter(_model.state.gyro.filter2, _model.state.gyro.adc); - _model.setDebug(DEBUG_GYRO_SAMPLE, 1, lrintf(Math::toDeg(_model.state.gyro.adc[_model.config.debug.axis]))); + _model.setDebug(DEBUG_GYRO_SAMPLE, 1, lrintf(Utils::toDeg(_model.state.gyro.adc[_model.config.debug.axis]))); if (_rpm_enabled) { @@ -122,13 +122,13 @@ int FAST_CODE_ATTR GyroSensor::filter() } } - _model.setDebug(DEBUG_GYRO_SAMPLE, 2, lrintf(Math::toDeg(_model.state.gyro.adc[_model.config.debug.axis]))); + _model.setDebug(DEBUG_GYRO_SAMPLE, 2, lrintf(Utils::toDeg(_model.state.gyro.adc[_model.config.debug.axis]))); _model.state.gyro.adc = Utils::applyFilter(_model.state.gyro.notch1Filter, _model.state.gyro.adc); _model.state.gyro.adc = Utils::applyFilter(_model.state.gyro.notch2Filter, _model.state.gyro.adc); _model.state.gyro.adc = Utils::applyFilter(_model.state.gyro.filter, _model.state.gyro.adc); - _model.setDebug(DEBUG_GYRO_SAMPLE, 3, lrintf(Math::toDeg(_model.state.gyro.adc[_model.config.debug.axis]))); + _model.setDebug(DEBUG_GYRO_SAMPLE, 3, lrintf(Utils::toDeg(_model.state.gyro.adc[_model.config.debug.axis]))); if (_dyn_notch_enabled || _dyn_notch_debug) { @@ -145,7 +145,7 @@ int FAST_CODE_ATTR GyroSensor::filter() for (size_t i = 0; i < AXIS_COUNT_RPY; ++i) { - _model.setDebug(DEBUG_GYRO_FILTERED, i, lrintf(Math::toDeg(_model.state.gyro.adc[i]))); + _model.setDebug(DEBUG_GYRO_FILTERED, i, lrintf(Utils::toDeg(_model.state.gyro.adc[i]))); } if (_model.accelActive()) @@ -171,7 +171,7 @@ void FAST_CODE_ATTR GyroSensor::rpmFilterUpdate() const float motorFreq = _model.state.output.telemetry.freq[_rpm_motor_index]; for (size_t n = 0; n < _model.config.gyro.rpmFilter.harmonics; n++) { - const float freq = Math::clamp(motorFreq * (n + 1), _rpm_min_freq, _rpm_max_freq); + const float freq = Utils::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.gyro.rpmFilter.fade) @@ -254,7 +254,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() if (_model.config.debug.mode == DEBUG_FFT_FREQ) { if (update) _model.state.debug[i] = lrintf(freq); - if (i == _model.config.debug.axis) _model.state.debug[3] = lrintf(Math::toDeg(_model.state.gyro.dynNotch[i])); + if (i == _model.config.debug.axis) _model.state.debug[3] = lrintf(Utils::toDeg(_model.state.gyro.dynNotch[i])); } if (_dyn_notch_enabled && update) { @@ -264,7 +264,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() { size_t x = (p + i) % 3; int harmonic = (p / 3) + 1; - int16_t f = Math::clamp((int16_t)lrintf(freq * harmonic), _model.config.gyro.dynamicFilter.min_freq, _model.config.gyro.dynamicFilter.max_freq); + int16_t f = Utils::clamp((int16_t)lrintf(freq * harmonic), _model.config.gyro.dynamicFilter.min_freq, _model.config.gyro.dynamicFilter.max_freq); _model.state.gyro.dynNotchFilter[p][x].reconfigure(f, f, q); } } diff --git a/lib/Espfc/src/Sensor/VoltageSensor.cpp b/lib/Espfc/src/Sensor/VoltageSensor.cpp index 3b3f9ed7..4e5eb948 100644 --- a/lib/Espfc/src/Sensor/VoltageSensor.cpp +++ b/lib/Espfc/src/Sensor/VoltageSensor.cpp @@ -69,7 +69,7 @@ int VoltageSensor::readVbat() } _model.state.battery.cellVoltage = _model.state.battery.voltage / constrain(_model.state.battery.cells, 1, 6); - _model.state.battery.percentage = Math::clamp(Math::map(_model.state.battery.cellVoltage, 3.4f, 4.2f, 0.0f, 100.0f), 0.0f, 100.0f); + _model.state.battery.percentage = Utils::clamp(Utils::map(_model.state.battery.cellVoltage, 3.4f, 4.2f, 0.0f, 100.0f), 0.0f, 100.0f); if (_model.config.debug.mode == DEBUG_BATTERY) { diff --git a/lib/Espfc/src/Telemetry/TelemetryCRSF.h b/lib/Espfc/src/Telemetry/TelemetryCRSF.h index 834fca96..728ba984 100644 --- a/lib/Espfc/src/Telemetry/TelemetryCRSF.h +++ b/lib/Espfc/src/Telemetry/TelemetryCRSF.h @@ -3,7 +3,7 @@ #include "Model.h" #include "Device/SerialDevice.h" #include "Rc/Crsf.h" -#include "Math/Utils.h" +#include "Utils/Math.hpp" // https://github.com/crsf-wg/crsf/wiki/CRSF_FRAMETYPE_MSP_REQ - not correct // https://github.com/betaflight/betaflight/blob/2525be9a3369fa666d8ce1485ec5ad344326b085/src/main/telemetry/crsf.c#L664 @@ -91,8 +91,8 @@ class TelemetryCRSF int16_t toAngle(float angle) const { - if(angle < -Math::pi()) angle += Math::twoPi(); - if(angle > Math::pi()) angle -= Math::twoPi(); + if(angle < -Utils::pi()) angle += Utils::twoPi(); + if(angle > Utils::pi()) angle -= Utils::twoPi(); return lrintf(angle * 1000); } @@ -104,9 +104,9 @@ class TelemetryCRSF int16_t p = toAngle(_model.state.attitude.euler.y); int16_t y = toAngle(_model.state.attitude.euler.z); - msg.writeU16(Math::toBigEndian16(r)); - msg.writeU16(Math::toBigEndian16(p)); - msg.writeU16(Math::toBigEndian16(y)); + msg.writeU16(Utils::toBigEndian16(r)); + msg.writeU16(Utils::toBigEndian16(p)); + msg.writeU16(Utils::toBigEndian16(y)); msg.finalize(); } @@ -115,13 +115,13 @@ class TelemetryCRSF { msg.prepare(Rc::CRSF_FRAMETYPE_BATTERY_SENSOR); - uint16_t voltage = Math::clamp(lrintf(_model.state.battery.voltage * 10.0f), 0l, 32000l); - uint16_t current = Math::clamp(lrintf(_model.state.battery.current * 10.0f), 0l, 32000l); + uint16_t voltage = Utils::clamp(lrintf(_model.state.battery.voltage * 10.0f), 0l, 32000l); + uint16_t current = Utils::clamp(lrintf(_model.state.battery.current * 10.0f), 0l, 32000l); uint32_t mahDrawn = 0; uint8_t remainPerc = lrintf(_model.state.battery.percentage); - msg.writeU16(Math::toBigEndian16(voltage)); - msg.writeU16(Math::toBigEndian16(current)); + msg.writeU16(Utils::toBigEndian16(voltage)); + msg.writeU16(Utils::toBigEndian16(current)); msg.writeU8(mahDrawn >> 16); msg.writeU8(mahDrawn >> 8); msg.writeU8(mahDrawn); @@ -148,7 +148,7 @@ class TelemetryCRSF { msg.prepare(Rc::CRSF_FRAMETYPE_VARIO_SENSOR); - msg.writeU16(Math::toBigEndian16(0)); + msg.writeU16(Utils::toBigEndian16(0)); msg.finalize(); } @@ -157,7 +157,7 @@ class TelemetryCRSF { msg.prepare(Rc::CRSF_FRAMETYPE_HEARTBEAT); - msg.writeU16(Math::toBigEndian16(Rc::CRSF_ADDRESS_FLIGHT_CONTROLLER)); + msg.writeU16(Utils::toBigEndian16(Rc::CRSF_ADDRESS_FLIGHT_CONTROLLER)); msg.finalize(); } diff --git a/lib/Espfc/src/Utils/FFTAnalyzer.hpp b/lib/Espfc/src/Utils/FFTAnalyzer.hpp index 7f004bcc..5e3528be 100644 --- a/lib/Espfc/src/Utils/FFTAnalyzer.hpp +++ b/lib/Espfc/src/Utils/FFTAnalyzer.hpp @@ -5,7 +5,7 @@ #include #include #include "Utils/Filter.h" -#include "Math/Utils.h" +#include "Utils/Math.hpp" namespace Espfc { @@ -26,7 +26,7 @@ class FFTAnalyzer int update(float v); static const size_t PEAKS_MAX = 8; - Math::Peak peaks[PEAKS_MAX]; + Utils::Peak peaks[PEAKS_MAX]; private: void clearPeaks(); diff --git a/lib/Espfc/src/Utils/FFTAnalyzer.ipp b/lib/Espfc/src/Utils/FFTAnalyzer.ipp index 7ae71cc4..d5219867 100644 --- a/lib/Espfc/src/Utils/FFTAnalyzer.ipp +++ b/lib/Espfc/src/Utils/FFTAnalyzer.ipp @@ -91,10 +91,10 @@ int FFTAnalyzer::update(float v) clearPeaks(); - Math::peakDetect(_out, _begin, _end, _bin_width, peaks, _peak_count); + Utils::peakDetect(_out, _begin, _end, _bin_width, peaks, _peak_count); // sort peaks by freq - Math::peakSort(peaks, _peak_count); + Utils::peakSort(peaks, _peak_count); _phase = PHASE_COLLECT; return 1; @@ -108,7 +108,7 @@ int FFTAnalyzer::update(float v) template void FFTAnalyzer::clearPeaks() { - for(size_t i = 0; i < PEAKS_MAX; i++) peaks[i] = Math::Peak(); + for(size_t i = 0; i < PEAKS_MAX; i++) peaks[i] = Utils::Peak(); //std::fill(peaks, peaks + PEAKS_MAX, Peak()); } diff --git a/lib/Espfc/src/Utils/Filter.cpp b/lib/Espfc/src/Utils/Filter.cpp index 7a10f11d..56daf695 100644 --- a/lib/Espfc/src/Utils/Filter.cpp +++ b/lib/Espfc/src/Utils/Filter.cpp @@ -1,11 +1,11 @@ #include #include "Utils/Filter.h" -#include "Math/Utils.h" +#include "Utils/Math.hpp" #include "Utils/MemoryHelper.h" static inline float pt1Gain(float rate, float freq) { - float rc = 1.f / (2.f * Espfc::Math::pi() * freq); + float rc = 1.f / (2.f * Espfc::Utils::pi() * freq); float dt = 1.f / rate; return dt / (dt + rc); } @@ -19,8 +19,8 @@ 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 + int16_t f = Utils::clamp((int)freq, 0, halfRate); // adj cut freq below nyquist rule + int16_t c = Utils::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 @@ -78,7 +78,7 @@ void FilterStateBiquad::reset() void FilterStateBiquad::init(BiquadFilterType filterType, float rate, float freq, float q) { - const float omega = (2.0f * Math::pi() * freq) / rate; + const float omega = (2.0f * Utils::pi() * freq) / rate; const float sn = sinf(omega); const float cs = cosf(omega); const float alpha = sn / (2.0f * q); @@ -158,9 +158,9 @@ void FilterStateFirstOrder::reset() void FilterStateFirstOrder::init(float rate, float freq) { - freq = Math::clamp(freq, 0.0f, rate * 0.48f); + freq = Utils::clamp(freq, 0.0f, rate * 0.48f); - const float W = std::tan(Math::pi() * freq / rate); + const float W = std::tan(Utils::pi() * freq / rate); a1 = (W - 1) / (W + 1); b1 = b0 = W / (W + 1); diff --git a/lib/Espfc/src/Utils/FreqAnalyzer.cpp b/lib/Espfc/src/Utils/FreqAnalyzer.cpp index f7f5d141..d04f4563 100644 --- a/lib/Espfc/src/Utils/FreqAnalyzer.cpp +++ b/lib/Espfc/src/Utils/FreqAnalyzer.cpp @@ -1,5 +1,5 @@ #include "Utils/FreqAnalyzer.hpp" -#include "Math/Utils.h" +#include "Utils/Math.hpp" namespace Espfc { @@ -28,14 +28,14 @@ void FreqAnalyzer::update(float v) // detect rising zero crossing if(sign && !_sign_prev) { - float f = Math::clamp(_rate / std::max(_pitch_count_raise, 1), _freq_min, _freq_max); + float f = Utils::clamp(_rate / std::max(_pitch_count_raise, 1), _freq_min, _freq_max); _pitch_freq_raise += k * (f - _pitch_freq_raise); _pitch_count_raise = 0; } // detect falling zero crossing if(!sign && _sign_prev) { - float f = Math::clamp(_rate / std::max(_pitch_count_fall, 1), _freq_min, _freq_max); + float f = Utils::clamp(_rate / std::max(_pitch_count_fall, 1), _freq_min, _freq_max); _pitch_freq_fall += k * (f - _pitch_freq_fall); _pitch_count_fall = 0; } diff --git a/lib/Espfc/src/Math/Utils.h b/lib/Espfc/src/Utils/Math.hpp similarity index 99% rename from lib/Espfc/src/Math/Utils.h rename to lib/Espfc/src/Utils/Math.hpp index 2c2f4640..bd6fb1f6 100644 --- a/lib/Espfc/src/Math/Utils.h +++ b/lib/Espfc/src/Utils/Math.hpp @@ -7,7 +7,7 @@ namespace Espfc { -namespace Math { +namespace Utils { class Peak { @@ -166,6 +166,7 @@ class Peak return a.freq < b.freq; }); } + } } diff --git a/test/test_math/test_math.cpp b/test/test_math/test_math.cpp index 72116c24..bef47759 100644 --- a/test/test_math/test_math.cpp +++ b/test/test_math/test_math.cpp @@ -3,7 +3,7 @@ #include #include #include "msp/msp_protocol.h" -#include "Math/Utils.h" +#include "Utils/Math.hpp" #include "Utils/Bits.hpp" #include "Utils/Filter.h" #include "Control/Pid.h" @@ -25,38 +25,38 @@ using namespace Espfc::Utils; 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(1.f, 0.f, Utils::map( 0.0f, -100.0f, 100.0f, -1000.0f, 1000.0f)); + TEST_ASSERT_FLOAT_WITHIN(1.f, 1000.f, Utils::map( 100.0f, -100.0f, 100.0f, -1000.0f, 1000.0f)); + TEST_ASSERT_FLOAT_WITHIN(1.f, -1000.f, Utils::map(-100.0f, -100.0f, 100.0f, -1000.0f, 1000.0f)); + TEST_ASSERT_FLOAT_WITHIN(1.f, 200.f, Utils::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(.001f, 0.f, Utils::map( 0.0f, -100.0f, 100.0f, -1.0f, 1.0f)); + TEST_ASSERT_FLOAT_WITHIN(.001f, 1.f, Utils::map( 100.0f, -100.0f, 100.0f, -1.0f, 1.0f)); + TEST_ASSERT_FLOAT_WITHIN(.001f, -1.f, Utils::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, Utils::map3( 0.0f, -100.0f, 0.0f, 100.0f, -1000.0f, 0.0f, 1000.0f)); + TEST_ASSERT_FLOAT_WITHIN(1.f, 500.f, Utils::map3( 50.0f, -100.0f, 0.0f, 100.0f, -1000.0f, 0.0f, 1000.0f)); + TEST_ASSERT_FLOAT_WITHIN(1.f, -500.f, Utils::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, Utils::toAltitude(101325.f)); // sea level + TEST_ASSERT_FLOAT_WITHIN(0.1f, 27.0f, Utils::toAltitude(101000.f)); + TEST_ASSERT_FLOAT_WITHIN(0.1f, 110.9f, Utils::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, Utils::deadband( 0, 10)); + TEST_ASSERT_EQUAL_INT32( 0, Utils::deadband( 10, 10)); + TEST_ASSERT_EQUAL_INT32( 1, Utils::deadband( 11, 10)); + TEST_ASSERT_EQUAL_INT32(-1, Utils::deadband(-11, 10)); + TEST_ASSERT_EQUAL_INT32( 0, Utils::deadband( -5, 10)); + TEST_ASSERT_EQUAL_INT32(10, Utils::deadband( 20, 10)); } void test_math_bit() @@ -129,25 +129,25 @@ void test_math_bits_msb() 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)); + TEST_ASSERT_EQUAL_INT( 250, Utils::alignToClock(1000, 332)); + TEST_ASSERT_EQUAL_INT( 333, Utils::alignToClock(1000, 333)); + TEST_ASSERT_EQUAL_INT( 333, Utils::alignToClock(1000, 334)); + TEST_ASSERT_EQUAL_INT( 333, Utils::alignToClock(1000, 400)); + TEST_ASSERT_EQUAL_INT( 500, Utils::alignToClock(1000, 500)); + TEST_ASSERT_EQUAL_INT( 500, Utils::alignToClock(1000, 800)); + TEST_ASSERT_EQUAL_INT(1000, Utils::alignToClock(1000, 2000)); + TEST_ASSERT_EQUAL_INT( 500, Utils::alignToClock(8000, 500)); + TEST_ASSERT_EQUAL_INT( 476, Utils::alignToClock(6667, 500)); } void test_math_peak_detect_full() { - using Math::Peak; + using Utils::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); + Utils::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); @@ -164,12 +164,12 @@ void test_math_peak_detect_full() void test_math_peak_detect_partial() { - using Math::Peak; + using Utils::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); + Utils::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); @@ -183,11 +183,11 @@ void test_math_peak_detect_partial() void test_math_peak_sort() { - using Math::Peak; + using Utils::Peak; Peak peaks[8] = { Peak(20, 10), Peak(10, 10), Peak(0, 10), Peak(5, 5) }; - Math::peakSort(peaks, 4); + Utils::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); @@ -1227,12 +1227,12 @@ void test_ring_buf2() 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)); + TEST_ASSERT_EQUAL_UINT32( 0, Utils::alignAddressToWrite( 0, 8, 16)); + TEST_ASSERT_EQUAL_UINT32( 16, Utils::alignAddressToWrite( 0, 16, 16)); + TEST_ASSERT_EQUAL_UINT32( 16, Utils::alignAddressToWrite( 0, 24, 16)); + TEST_ASSERT_EQUAL_UINT32(144, Utils::alignAddressToWrite(128, 16, 16)); + TEST_ASSERT_EQUAL_UINT32( 32, Utils::alignAddressToWrite( 0, 32, 16)); + TEST_ASSERT_EQUAL_UINT32(128, Utils::alignAddressToWrite(100, 32, 16)); } void test_rotation_matrix_no_rotation() @@ -1252,9 +1252,9 @@ void test_rotation_matrix_90_roll() VectorFloat v{0.f, 0.f, 1.f}; RotationMatrixFloat rm; rm.init(VectorFloat{ - Math::toRad(90), - Math::toRad(0), - Math::toRad(0), + Utils::toRad(90), + Utils::toRad(0), + Utils::toRad(0), }); VectorFloat r = rm.apply(v); @@ -1269,9 +1269,9 @@ void test_rotation_matrix_90_pitch() VectorFloat v{0.f, 0.f, 1.f}; RotationMatrixFloat rm; rm.init(VectorFloat{ - Math::toRad(0), - Math::toRad(90), - Math::toRad(0), + Utils::toRad(0), + Utils::toRad(90), + Utils::toRad(0), }); VectorFloat r = rm.apply(v); @@ -1286,9 +1286,9 @@ void test_rotation_matrix_90_yaw() VectorFloat v{1.f, 2.f, 3.f}; RotationMatrixFloat rm; rm.init(VectorFloat{ - Math::toRad(0), - Math::toRad(0), - Math::toRad(90), + Utils::toRad(0), + Utils::toRad(0), + Utils::toRad(90), }); VectorFloat r = rm.apply(v); From 59f6c859df22e1d3a2eeda5dda837b25e86c5f36 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Wed, 13 Nov 2024 22:27:32 +0100 Subject: [PATCH 58/68] move buzzer to connect ns --- lib/Espfc/src/{ => Connect}/Buzzer.cpp | 6 +++++- lib/Espfc/src/{Buzzer.h => Connect/Buzzer.hpp} | 4 ++++ lib/Espfc/src/Espfc.h | 4 ++-- 3 files changed, 11 insertions(+), 3 deletions(-) rename lib/Espfc/src/{ => Connect}/Buzzer.cpp (99%) rename lib/Espfc/src/{Buzzer.h => Connect/Buzzer.hpp} (95%) diff --git a/lib/Espfc/src/Buzzer.cpp b/lib/Espfc/src/Connect/Buzzer.cpp similarity index 99% rename from lib/Espfc/src/Buzzer.cpp rename to lib/Espfc/src/Connect/Buzzer.cpp index 0035fe8d..2ce8c669 100644 --- a/lib/Espfc/src/Buzzer.cpp +++ b/lib/Espfc/src/Connect/Buzzer.cpp @@ -1,8 +1,10 @@ -#include "Buzzer.h" +#include "Buzzer.hpp" #include "Hal/Gpio.h" namespace Espfc { +namespace Connect { + Buzzer::Buzzer(Model& model): _model(model), _status(BUZZER_STATUS_IDLE), _wait(0), _scheme(NULL), _e(BUZZER_SILENCE) {} int Buzzer::begin() @@ -145,3 +147,5 @@ const uint8_t** Buzzer::schemes() } } + +} diff --git a/lib/Espfc/src/Buzzer.h b/lib/Espfc/src/Connect/Buzzer.hpp similarity index 95% rename from lib/Espfc/src/Buzzer.h rename to lib/Espfc/src/Connect/Buzzer.hpp index 30331458..3b84d7c4 100644 --- a/lib/Espfc/src/Buzzer.h +++ b/lib/Espfc/src/Connect/Buzzer.hpp @@ -4,6 +4,8 @@ namespace Espfc { +namespace Connect { + enum BuzzerPlayStatus { BUZZER_STATUS_IDLE, @@ -35,3 +37,5 @@ class Buzzer }; } + +} diff --git a/lib/Espfc/src/Espfc.h b/lib/Espfc/src/Espfc.h index 59700fca..5da2181e 100644 --- a/lib/Espfc/src/Espfc.h +++ b/lib/Espfc/src/Espfc.h @@ -10,7 +10,7 @@ #include "SerialManager.h" #include "Output/Mixer.h" #include "Blackbox/Blackbox.h" -#include "Buzzer.h" +#include "Connect/Buzzer.hpp" namespace Espfc { @@ -39,7 +39,7 @@ class Espfc SensorManager _sensor; Output::Mixer _mixer; Blackbox::Blackbox _blackbox; - Buzzer _buzzer; + Connect::Buzzer _buzzer; SerialManager _serial; uint32_t _loop_next; }; From da810793a50548230055a7a76ada95222593375d Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 14 Nov 2024 00:09:29 +0100 Subject: [PATCH 59/68] split msp classes --- lib/Espfc/src/Connect/Cli.h | 7 +- lib/Espfc/src/Connect/Msp.cpp | 174 +++ lib/Espfc/src/Connect/Msp.h | 251 ---- lib/Espfc/src/Connect/Msp.hpp | 105 ++ lib/Espfc/src/Connect/MspParser.cpp | 137 ++ lib/Espfc/src/Connect/MspParser.h | 146 --- lib/Espfc/src/Connect/MspParser.hpp | 21 + lib/Espfc/src/Connect/MspProcessor.cpp | 1598 +++++++++++++++++++++++ lib/Espfc/src/Connect/MspProcessor.h | 1611 ------------------------ lib/Espfc/src/Connect/MspProcessor.hpp | 37 + lib/Espfc/src/Hal/Pgm.h | 9 + lib/Espfc/src/ModelState.h | 2 +- lib/Espfc/src/Rc/Crsf.h | 2 +- lib/Espfc/src/SerialManager.h | 2 +- lib/Espfc/src/TelemetryManager.h | 4 +- test/test_msp/test_msp.cpp | 5 +- 16 files changed, 2093 insertions(+), 2018 deletions(-) create mode 100644 lib/Espfc/src/Connect/Msp.cpp delete mode 100644 lib/Espfc/src/Connect/Msp.h create mode 100644 lib/Espfc/src/Connect/Msp.hpp create mode 100644 lib/Espfc/src/Connect/MspParser.cpp delete mode 100644 lib/Espfc/src/Connect/MspParser.h create mode 100644 lib/Espfc/src/Connect/MspParser.hpp create mode 100644 lib/Espfc/src/Connect/MspProcessor.cpp delete mode 100644 lib/Espfc/src/Connect/MspProcessor.h create mode 100644 lib/Espfc/src/Connect/MspProcessor.hpp diff --git a/lib/Espfc/src/Connect/Cli.h b/lib/Espfc/src/Connect/Cli.h index fb2ddc1d..ebe612c9 100644 --- a/lib/Espfc/src/Connect/Cli.h +++ b/lib/Espfc/src/Connect/Cli.h @@ -8,6 +8,7 @@ #include "Hardware.h" #include "Device/GyroDevice.h" #include "Hal/Pgm.h" +#include "msp/msp_protocol.h" #ifdef USE_FLASHFS #include "Device/FlashDevice.h" @@ -1485,7 +1486,7 @@ class Cli } private: - void print(const Param& param, Stream& s) + void print(const Param& param, Stream& s) const { s.print(F("set ")); s.print(FPSTR(param.name)); @@ -1494,7 +1495,7 @@ class Cli s.println(); } - void printVersion(Stream& s) + void printVersion(Stream& s) const { s.print(boardIdentifier); s.print(' '); @@ -1517,7 +1518,7 @@ class Cli s.print(__cplusplus); } - void printStats(Stream& s) + void printStats(Stream& s) const { s.print(F(" cpu freq: ")); s.print(targetCpuFreq()); diff --git a/lib/Espfc/src/Connect/Msp.cpp b/lib/Espfc/src/Connect/Msp.cpp new file mode 100644 index 00000000..ea903751 --- /dev/null +++ b/lib/Espfc/src/Connect/Msp.cpp @@ -0,0 +1,174 @@ +#include "Connect/Msp.hpp" +#include "Hal/Pgm.h" +#include "Utils/Crc.hpp" + +namespace Espfc { + +namespace Connect { + +MspMessage::MspMessage(): state(MSP_STATE_IDLE), expected(0), received(0), read(0) {} + +bool MspMessage::isReady() const +{ + return state == MSP_STATE_RECEIVED; +} + +bool MspMessage::isCmd() const +{ + return dir == MSP_TYPE_CMD; +} + +bool MspMessage::isIdle() const +{ + return state == MSP_STATE_IDLE; +} + +int MspMessage::remain() const +{ + return received - read; +} + +void MspMessage::advance(size_t size) +{ + read += size; +} + +uint8_t MspMessage::readU8() +{ + return buffer[read++]; +} + +uint16_t MspMessage::readU16() +{ + uint16_t ret; + ret = readU8(); + ret |= readU8() << 8; + return ret; +} + +uint32_t MspMessage::readU32() +{ + uint32_t ret; + ret = readU8(); + ret |= readU8() << 8; + ret |= readU8() << 16; + ret |= readU8() << 24; + return ret; +} + +MspResponse::MspResponse(): len(0) {} + +int MspResponse::remain() const +{ + return MSP_BUF_OUT_SIZE - len; +} + +void MspResponse::advance(size_t size) +{ + len += size; +} + +void MspResponse::writeData(const char * v, int size) +{ + while(size-- > 0) writeU8(*v++); +} + +void MspResponse::writeString(const char * v) +{ + while(*v) writeU8(*v++); +} + +void MspResponse::writeString(const __FlashStringHelper *ifsh) +{ + PGM_P p = reinterpret_cast(ifsh); + while(true) + { + uint8_t c = pgm_read_byte(p++); + if (c == 0) break; + writeU8(c); + } +} + +void MspResponse::writeU8(uint8_t v) +{ + data[len++] = v; +} + +void MspResponse::writeU16(uint16_t v) +{ + writeU8(v >> 0); + writeU8(v >> 8); +} + +void MspResponse::writeU32(uint32_t v) +{ + writeU8(v >> 0); + writeU8(v >> 8); + writeU8(v >> 16); + writeU8(v >> 24); +} + +size_t MspResponse::serialize(uint8_t * buff, size_t len_max) const +{ + switch(version) + { + case MSP_V1: + return serializeV1(buff, len_max); + case MSP_V2: + return serializeV2(buff, len_max); + } + return 0; +} + +size_t MspResponse::serializeV1(uint8_t * buff, size_t len_max) const +{ + // not enough space in target buffer + if(len + 6ul > len_max) return 0; + + buff[0] = '$'; + buff[1] = 'M'; + buff[2] = result == -1 ? '!' : '>'; + buff[3] = len; + buff[4] = cmd; + + uint8_t checksum = Utils::crc8_xor(0, &buff[3], 2); + size_t i = 5; + for(size_t j = 0; j < len; j++) + { + checksum = Utils::crc8_xor(checksum, data[j]); + buff[i++] = data[j]; + } + buff[i] = checksum; + + return i + 1; +} + +size_t MspResponse::serializeV2(uint8_t * buff, size_t len_max) const +{ + // not enough space in target buffer + if(len + 9ul > len_max) return 0; + + buff[0] = '$'; + buff[1] = 'X'; + buff[2] = result == -1 ? '!' : '>'; + buff[3] = 0; + buff[4] = cmd & 0xff; + buff[5] = (cmd >> 8) & 0xff; + buff[6] = len & 0xff; + buff[7] = (len >> 8) & 0xff; + + uint8_t checksum = Utils::crc8_dvb_s2(0, &buff[3], 5); + size_t i = 8; + for(size_t j = 0; j < len; j++) + { + checksum = Utils::crc8_dvb_s2(checksum, data[j]); + buff[i++] = data[j]; + } + buff[i] = checksum; + + return i + 1; +} + +} + +} diff --git a/lib/Espfc/src/Connect/Msp.h b/lib/Espfc/src/Connect/Msp.h deleted file mode 100644 index 855a9063..00000000 --- a/lib/Espfc/src/Connect/Msp.h +++ /dev/null @@ -1,251 +0,0 @@ -#ifndef _ESPFC_MSP_MSP_H_ -#define _ESPFC_MSP_MSP_H_ - -#include -#include "Hal/Pgm.h" -#include "Utils/Crc.hpp" - -extern "C" { -#include "msp/msp_protocol.h" -#include "msp/msp_protocol_v2_common.h" -#include "msp/msp_protocol_v2_betaflight.h" -} - -namespace Espfc { - -namespace Connect { - -static const size_t MSP_BUF_SIZE = 192; -static const size_t MSP_BUF_OUT_SIZE = 240; - -enum MspState { - MSP_STATE_IDLE, - MSP_STATE_HEADER_START, - - MSP_STATE_HEADER_M, - MSP_STATE_HEADER_V1, - MSP_STATE_PAYLOAD_V1, - MSP_STATE_CHECKSUM_V1, - - MSP_STATE_HEADER_X, - MSP_STATE_HEADER_V2, - MSP_STATE_PAYLOAD_V2, - MSP_STATE_CHECKSUM_V2, - - MSP_STATE_RECEIVED, -}; - -enum MspType { - MSP_TYPE_CMD, - MSP_TYPE_REPLY -}; - -enum MspVersion { - MSP_V1, - MSP_V2 -}; - -struct MspHeaderV1 { - uint8_t size; - uint8_t cmd; -} __attribute__((packed)); - -struct MspHeaderV2 { - uint8_t flags; - uint16_t cmd; - uint16_t size; -} __attribute__((packed)); - -class MspMessage -{ - public: - MspMessage(): state(MSP_STATE_IDLE), expected(0), received(0), read(0) {} - MspState state; - MspType dir; - MspVersion version; - uint8_t flags; - uint16_t cmd; - uint16_t expected; - uint16_t received; - uint16_t read; - uint8_t checksum; - uint8_t checksum2; - uint8_t buffer[MSP_BUF_SIZE]; - - bool isReady() const - { - return state == MSP_STATE_RECEIVED; - } - - bool isCmd() const - { - return dir == MSP_TYPE_CMD; - } - - bool isIdle() const - { - return state == MSP_STATE_IDLE; - } - - int remain() const - { - return received - read; - } - - void advance(size_t size) - { - read += size; - } - - uint8_t readU8() - { - return buffer[read++]; - } - - uint16_t readU16() - { - uint16_t ret; - ret = readU8(); - ret |= readU8() << 8; - return ret; - } - - uint32_t readU32() - { - uint32_t ret; - ret = readU8(); - ret |= readU8() << 8; - ret |= readU8() << 16; - ret |= readU8() << 24; - return ret; - } -}; - -class MspResponse -{ - public: - MspResponse(): len(0) {} - MspVersion version; - uint16_t cmd; - int8_t result; - uint8_t direction; - uint16_t len; - 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) - { - while(size-- > 0) writeU8(*v++); - } - - void writeString(const char * v) - { - while(*v) writeU8(*v++); - } - - void writeString(const __FlashStringHelper *ifsh) - { - PGM_P p = reinterpret_cast(ifsh); - while(true) - { - uint8_t c = pgm_read_byte(p++); - if (c == 0) break; - writeU8(c); - } - } - - void writeU8(uint8_t v) - { - data[len++] = v; - } - - void writeU16(uint16_t v) - { - writeU8(v >> 0); - writeU8(v >> 8); - } - - void writeU32(uint32_t v) - { - writeU8(v >> 0); - writeU8(v >> 8); - writeU8(v >> 16); - writeU8(v >> 24); - } - - size_t serialize(uint8_t * buff, size_t len_max) const - { - switch(version) - { - case MSP_V1: - return serializeV1(buff, len_max); - case MSP_V2: - return serializeV2(buff, len_max); - } - return 0; - } - - size_t serializeV1(uint8_t * buff, size_t len_max) const - { - // not enough space in target buffer - if(len + 6ul > len_max) return 0; - - buff[0] = '$'; - buff[1] = 'M'; - buff[2] = result == -1 ? '!' : '>'; - buff[3] = len; - buff[4] = cmd; - - uint8_t checksum = Utils::crc8_xor(0, &buff[3], 2); - size_t i = 5; - for(size_t j = 0; j < len; j++) - { - checksum = Utils::crc8_xor(checksum, data[j]); - buff[i++] = data[j]; - } - buff[i] = checksum; - - return i + 1; - } - - size_t serializeV2(uint8_t * buff, size_t len_max) const - { - // not enough space in target buffer - if(len + 9ul > len_max) return 0; - - buff[0] = '$'; - buff[1] = 'X'; - buff[2] = result == -1 ? '!' : '>'; - buff[3] = 0; - buff[4] = cmd & 0xff; - buff[5] = (cmd >> 8) & 0xff; - buff[6] = len & 0xff; - buff[7] = (len >> 8) & 0xff; - - uint8_t checksum = Utils::crc8_dvb_s2(0, &buff[3], 5); - size_t i = 8; - for(size_t j = 0; j < len; j++) - { - checksum = Utils::crc8_dvb_s2(checksum, data[j]); - buff[i++] = data[j]; - } - buff[i] = checksum; - - return i + 1; - } -}; - -} - -} - -#endif diff --git a/lib/Espfc/src/Connect/Msp.hpp b/lib/Espfc/src/Connect/Msp.hpp new file mode 100644 index 00000000..39ac2633 --- /dev/null +++ b/lib/Espfc/src/Connect/Msp.hpp @@ -0,0 +1,105 @@ +#pragma once + +#include +#include +#include "Hal/Pgm.h" + +namespace Espfc { + +namespace Connect { + +constexpr size_t MSP_BUF_SIZE = 192; +constexpr size_t MSP_BUF_OUT_SIZE = 240; + +enum MspState { + MSP_STATE_IDLE, + MSP_STATE_HEADER_START, + + MSP_STATE_HEADER_M, + MSP_STATE_HEADER_V1, + MSP_STATE_PAYLOAD_V1, + MSP_STATE_CHECKSUM_V1, + + MSP_STATE_HEADER_X, + MSP_STATE_HEADER_V2, + MSP_STATE_PAYLOAD_V2, + MSP_STATE_CHECKSUM_V2, + + MSP_STATE_RECEIVED, +}; + +enum MspType { + MSP_TYPE_CMD, + MSP_TYPE_REPLY +}; + +enum MspVersion { + MSP_V1, + MSP_V2 +}; + +struct MspHeaderV1 { + uint8_t size; + uint8_t cmd; +} __attribute__((packed)); + +struct MspHeaderV2 { + uint8_t flags; + uint16_t cmd; + uint16_t size; +} __attribute__((packed)); + +class MspMessage +{ +public: + MspMessage(); + bool isReady() const; + bool isCmd() const; + bool isIdle() const; + int remain() const; + void advance(size_t size); + uint8_t readU8(); + uint16_t readU16(); + uint32_t readU32(); + + MspState state; + MspType dir; + MspVersion version; + uint8_t flags; + uint16_t cmd; + uint16_t expected; + uint16_t received; + uint16_t read; + uint8_t checksum; + uint8_t checksum2; + uint8_t buffer[MSP_BUF_SIZE]; +}; + +class MspResponse +{ +public: + MspResponse(); + + MspVersion version; + uint16_t cmd; + int8_t result; + uint8_t direction; + uint16_t len; + uint8_t data[MSP_BUF_OUT_SIZE]; + + int remain() const; + void advance(size_t size); + void writeData(const char * v, int size); + void writeString(const char * v); + void writeString(const __FlashStringHelper *ifsh); + void writeU8(uint8_t v); + void writeU16(uint16_t v); + void writeU32(uint32_t v); + size_t serialize(uint8_t * buff, size_t len_max) const; + size_t serializeV1(uint8_t * buff, size_t len_max) const; + size_t serializeV2(uint8_t * buff, size_t len_max) const; +}; + +} + +} diff --git a/lib/Espfc/src/Connect/MspParser.cpp b/lib/Espfc/src/Connect/MspParser.cpp new file mode 100644 index 00000000..03ccdb8b --- /dev/null +++ b/lib/Espfc/src/Connect/MspParser.cpp @@ -0,0 +1,137 @@ +#include "Connect/MspParser.hpp" +#include "Utils/Crc.hpp" + +namespace Espfc { + +namespace Connect { + +MspParser::MspParser() {} + +void MspParser::parse(char c, MspMessage& msg) +{ + switch(msg.state) + { + case MSP_STATE_IDLE: // sync char 1 '$' + if(c == '$') msg.state = MSP_STATE_HEADER_START; + break; + + case MSP_STATE_HEADER_START: // sync char 2 'M' + msg.read = 0; + msg.received = 0; + msg.checksum = 0; + msg.checksum2 = 0; + if(c == 'M') + { + msg.version = MSP_V1; + msg.state = MSP_STATE_HEADER_M; + } + else if(c == 'X') + { + msg.version = MSP_V2; + msg.state = MSP_STATE_HEADER_X; + } + else msg.state = MSP_STATE_IDLE; + break; + + case MSP_STATE_HEADER_M: // type '<','>','!' + switch(c) + { + case '>': + msg.dir = MSP_TYPE_REPLY; + msg.state = MSP_STATE_HEADER_V1; + break; + case '<': + msg.dir = MSP_TYPE_CMD; + msg.state = MSP_STATE_HEADER_V1; + break; + default: + msg.state = MSP_STATE_IDLE; + } + break; + + case MSP_STATE_HEADER_X: // type '<','>','!' + switch(c) + { + case '>': + msg.dir = MSP_TYPE_REPLY; + msg.state = MSP_STATE_HEADER_V2; + break; + case '<': + msg.dir = MSP_TYPE_CMD; + msg.state = MSP_STATE_HEADER_V2; + break; + default: + msg.state = MSP_STATE_IDLE; + } + break; + + case MSP_STATE_HEADER_V1: + msg.buffer[msg.received++] = c; + msg.checksum = Utils::crc8_xor(msg.checksum, c); + if(msg.received == sizeof(MspHeaderV1)) + { + const MspHeaderV1 * hdr = reinterpret_cast(msg.buffer); + if(hdr->size > MSP_BUF_SIZE) msg.state = MSP_STATE_IDLE; + else + { + msg.expected = hdr->size; + msg.cmd = hdr->cmd; + msg.received = 0; + msg.state = msg.expected > 0 ? MSP_STATE_PAYLOAD_V1 : MSP_STATE_CHECKSUM_V1; + } + } + break; + + case MSP_STATE_PAYLOAD_V1: + msg.buffer[msg.received++] = c; + msg.checksum = Utils::crc8_xor(msg.checksum, c); + if(msg.received == msg.expected) + { + msg.state = MSP_STATE_CHECKSUM_V1; + } + break; + + case MSP_STATE_CHECKSUM_V1: + msg.state = msg.checksum == c ? MSP_STATE_RECEIVED : MSP_STATE_IDLE; + break; + + case MSP_STATE_HEADER_V2: + msg.buffer[msg.received++] = c; + msg.checksum2 = Utils::crc8_dvb_s2(msg.checksum2, c); + if(msg.received == sizeof(MspHeaderV2)) + { + const MspHeaderV2 * hdr = reinterpret_cast(msg.buffer); + if(hdr->size > MSP_BUF_SIZE) msg.state = MSP_STATE_IDLE; + else + { + msg.flags = hdr->flags; + msg.cmd = hdr->cmd; + msg.expected = hdr->size; + msg.received = 0; + msg.state = msg.expected > 0 ? MSP_STATE_PAYLOAD_V2 : MSP_STATE_CHECKSUM_V2; + } + } + break; + + case MSP_STATE_PAYLOAD_V2: + msg.buffer[msg.received++] = c; + msg.checksum2 = Utils::crc8_dvb_s2(msg.checksum2, c); + if(msg.received == msg.expected) + { + msg.state = MSP_STATE_CHECKSUM_V2; + } + break; + + case MSP_STATE_CHECKSUM_V2: + msg.state = msg.checksum2 == c ? MSP_STATE_RECEIVED : MSP_STATE_IDLE; + break; + + default: + //msg.state = MSP_STATE_IDLE; + break; + } +} + +} + +} diff --git a/lib/Espfc/src/Connect/MspParser.h b/lib/Espfc/src/Connect/MspParser.h deleted file mode 100644 index f156fef3..00000000 --- a/lib/Espfc/src/Connect/MspParser.h +++ /dev/null @@ -1,146 +0,0 @@ -#ifndef _ESPFC_MSP_PARSER_H_ -#define _ESPFC_MSP_PARSER_H_ - -#include "Msp.h" -#include "Utils/Crc.hpp" - -namespace Espfc { - -namespace Connect { - -class MspParser -{ - public: - MspParser() {} - - void parse(char c, MspMessage& msg) - { - switch(msg.state) - { - case MSP_STATE_IDLE: // sync char 1 '$' - if(c == '$') msg.state = MSP_STATE_HEADER_START; - break; - - case MSP_STATE_HEADER_START: // sync char 2 'M' - msg.read = 0; - msg.received = 0; - msg.checksum = 0; - msg.checksum2 = 0; - if(c == 'M') - { - msg.version = MSP_V1; - msg.state = MSP_STATE_HEADER_M; - } - else if(c == 'X') - { - msg.version = MSP_V2; - msg.state = MSP_STATE_HEADER_X; - } - else msg.state = MSP_STATE_IDLE; - break; - - case MSP_STATE_HEADER_M: // type '<','>','!' - switch(c) - { - case '>': - msg.dir = MSP_TYPE_REPLY; - msg.state = MSP_STATE_HEADER_V1; - break; - case '<': - msg.dir = MSP_TYPE_CMD; - msg.state = MSP_STATE_HEADER_V1; - break; - default: - msg.state = MSP_STATE_IDLE; - } - break; - - case MSP_STATE_HEADER_X: // type '<','>','!' - switch(c) - { - case '>': - msg.dir = MSP_TYPE_REPLY; - msg.state = MSP_STATE_HEADER_V2; - break; - case '<': - msg.dir = MSP_TYPE_CMD; - msg.state = MSP_STATE_HEADER_V2; - break; - default: - msg.state = MSP_STATE_IDLE; - } - break; - - case MSP_STATE_HEADER_V1: - msg.buffer[msg.received++] = c; - msg.checksum = Utils::crc8_xor(msg.checksum, c); - if(msg.received == sizeof(MspHeaderV1)) - { - const MspHeaderV1 * hdr = reinterpret_cast(msg.buffer); - if(hdr->size > MSP_BUF_SIZE) msg.state = MSP_STATE_IDLE; - else - { - msg.expected = hdr->size; - msg.cmd = hdr->cmd; - msg.received = 0; - msg.state = msg.expected > 0 ? MSP_STATE_PAYLOAD_V1 : MSP_STATE_CHECKSUM_V1; - } - } - break; - - case MSP_STATE_PAYLOAD_V1: - msg.buffer[msg.received++] = c; - msg.checksum = Utils::crc8_xor(msg.checksum, c); - if(msg.received == msg.expected) - { - msg.state = MSP_STATE_CHECKSUM_V1; - } - break; - - case MSP_STATE_CHECKSUM_V1: - msg.state = msg.checksum == c ? MSP_STATE_RECEIVED : MSP_STATE_IDLE; - break; - - case MSP_STATE_HEADER_V2: - msg.buffer[msg.received++] = c; - msg.checksum2 = Utils::crc8_dvb_s2(msg.checksum2, c); - if(msg.received == sizeof(MspHeaderV2)) - { - const MspHeaderV2 * hdr = reinterpret_cast(msg.buffer); - if(hdr->size > MSP_BUF_SIZE) msg.state = MSP_STATE_IDLE; - else - { - msg.flags = hdr->flags; - msg.cmd = hdr->cmd; - msg.expected = hdr->size; - msg.received = 0; - msg.state = msg.expected > 0 ? MSP_STATE_PAYLOAD_V2 : MSP_STATE_CHECKSUM_V2; - } - } - break; - - case MSP_STATE_PAYLOAD_V2: - msg.buffer[msg.received++] = c; - msg.checksum2 = Utils::crc8_dvb_s2(msg.checksum2, c); - if(msg.received == msg.expected) - { - msg.state = MSP_STATE_CHECKSUM_V2; - } - break; - - case MSP_STATE_CHECKSUM_V2: - msg.state = msg.checksum2 == c ? MSP_STATE_RECEIVED : MSP_STATE_IDLE; - break; - - default: - //msg.state = MSP_STATE_IDLE; - break; - } - } -}; - -} - -} - -#endif diff --git a/lib/Espfc/src/Connect/MspParser.hpp b/lib/Espfc/src/Connect/MspParser.hpp new file mode 100644 index 00000000..751853b6 --- /dev/null +++ b/lib/Espfc/src/Connect/MspParser.hpp @@ -0,0 +1,21 @@ +#ifndef _ESPFC_MSP_PARSER_H_ +#define _ESPFC_MSP_PARSER_H_ + +#include "Connect/Msp.hpp" + +namespace Espfc { + +namespace Connect { + +class MspParser +{ + public: + MspParser(); + void parse(char c, MspMessage& msg); +}; + +} + +} + +#endif diff --git a/lib/Espfc/src/Connect/MspProcessor.cpp b/lib/Espfc/src/Connect/MspProcessor.cpp new file mode 100644 index 00000000..1f87b3c0 --- /dev/null +++ b/lib/Espfc/src/Connect/MspProcessor.cpp @@ -0,0 +1,1598 @@ +#include "Connect/MspProcessor.hpp" +#include "Hardware.h" +#include +#if defined(ESPFC_MULTI_CORE) && defined(ESPFC_FREE_RTOS) +#include +#endif + +extern "C" { + #include "msp/msp_protocol.h" + #include "msp/msp_protocol_v2_common.h" + #include "msp/msp_protocol_v2_betaflight.h" + #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); + uint16_t blackboxGetPRatio(void); +} + +namespace { + +enum SerialSpeedIndex { + SERIAL_SPEED_INDEX_AUTO = 0, + SERIAL_SPEED_INDEX_9600, + SERIAL_SPEED_INDEX_19200, + SERIAL_SPEED_INDEX_38400, + SERIAL_SPEED_INDEX_57600, + SERIAL_SPEED_INDEX_115200, + SERIAL_SPEED_INDEX_230400, + SERIAL_SPEED_INDEX_250000, + SERIAL_SPEED_INDEX_400000, + SERIAL_SPEED_INDEX_460800, + SERIAL_SPEED_INDEX_500000, + SERIAL_SPEED_INDEX_921600, + SERIAL_SPEED_INDEX_1000000, + SERIAL_SPEED_INDEX_1500000, + SERIAL_SPEED_INDEX_2000000, + SERIAL_SPEED_INDEX_2470000, +}; + +static SerialSpeedIndex toBaudIndex(int32_t speed) +{ + using namespace Espfc; + if(speed >= SERIAL_SPEED_2470000) return SERIAL_SPEED_INDEX_2470000; + if(speed >= SERIAL_SPEED_2000000) return SERIAL_SPEED_INDEX_2000000; + if(speed >= SERIAL_SPEED_1500000) return SERIAL_SPEED_INDEX_1500000; + if(speed >= SERIAL_SPEED_1000000) return SERIAL_SPEED_INDEX_1000000; + if(speed >= SERIAL_SPEED_921600) return SERIAL_SPEED_INDEX_921600; + if(speed >= SERIAL_SPEED_500000) return SERIAL_SPEED_INDEX_500000; + if(speed >= SERIAL_SPEED_460800) return SERIAL_SPEED_INDEX_460800; + if(speed >= SERIAL_SPEED_400000) return SERIAL_SPEED_INDEX_400000; + if(speed >= SERIAL_SPEED_250000) return SERIAL_SPEED_INDEX_250000; + if(speed >= SERIAL_SPEED_230400) return SERIAL_SPEED_INDEX_230400; + if(speed >= SERIAL_SPEED_115200) return SERIAL_SPEED_INDEX_115200; + if(speed >= SERIAL_SPEED_57600) return SERIAL_SPEED_INDEX_57600; + if(speed >= SERIAL_SPEED_38400) return SERIAL_SPEED_INDEX_38400; + if(speed >= SERIAL_SPEED_19200) return SERIAL_SPEED_INDEX_19200; + if(speed >= SERIAL_SPEED_9600) return SERIAL_SPEED_INDEX_9600; + return SERIAL_SPEED_INDEX_AUTO; +} + +static Espfc::SerialSpeed fromBaudIndex(SerialSpeedIndex index) +{ + using namespace Espfc; + switch(index) + { + case SERIAL_SPEED_INDEX_9600: return SERIAL_SPEED_9600; + case SERIAL_SPEED_INDEX_19200: return SERIAL_SPEED_19200; + case SERIAL_SPEED_INDEX_38400: return SERIAL_SPEED_38400; + case SERIAL_SPEED_INDEX_57600: return SERIAL_SPEED_57600; + case SERIAL_SPEED_INDEX_115200: return SERIAL_SPEED_115200; + case SERIAL_SPEED_INDEX_230400: return SERIAL_SPEED_230400; + case SERIAL_SPEED_INDEX_250000: return SERIAL_SPEED_250000; + case SERIAL_SPEED_INDEX_400000: return SERIAL_SPEED_400000; + case SERIAL_SPEED_INDEX_460800: return SERIAL_SPEED_460800; + case SERIAL_SPEED_INDEX_500000: return SERIAL_SPEED_500000; + case SERIAL_SPEED_INDEX_921600: return SERIAL_SPEED_921600; + case SERIAL_SPEED_INDEX_1000000: return SERIAL_SPEED_1000000; + case SERIAL_SPEED_INDEX_1500000: return SERIAL_SPEED_1500000; + case SERIAL_SPEED_INDEX_2000000: return SERIAL_SPEED_2000000; + case SERIAL_SPEED_INDEX_2470000: return SERIAL_SPEED_2470000; + case SERIAL_SPEED_INDEX_AUTO: + default: + return SERIAL_SPEED_NONE; + } +} + +static uint8_t toFilterTypeDerivative(uint8_t t) +{ + switch(t) { + case 0: return Espfc::FILTER_NONE; + case 1: return Espfc::FILTER_PT3; + case 2: return Espfc::FILTER_BIQUAD; + default: return Espfc::FILTER_PT3; + } +} + +static uint8_t fromFilterTypeDerivative(uint8_t t) +{ + switch(t) { + case Espfc::FILTER_NONE: return 0; + case Espfc::FILTER_PT3: return 1; + case Espfc::FILTER_BIQUAD: return 2; + default: return 1; + } +} + +static uint8_t fromGyroDlpf(uint8_t t) +{ + switch(t) { + case Espfc::GYRO_DLPF_256: return 0; + case Espfc::GYRO_DLPF_EX: return 1; + default: return 2; + } +} + +static int8_t toVbatSource(uint8_t t) +{ + switch(t) { + case 0: return 0; // none + case 1: return 1; // internal adc + default: return 0; + } +} + +static int8_t toIbatSource(uint8_t t) +{ + switch(t) { + case 0: return 0; // none + case 1: return 1; // internal adc + default: return 0; + } +} + +static uint8_t toVbatVoltageLegacy(float voltage) +{ + return constrain(lrintf(voltage * 10.0f), 0, 255); +} + +static uint16_t toVbatVoltage(float voltage) +{ + return constrain(lrintf(voltage * 100.0f), 0, 32000); +} + +static uint16_t toIbatCurrent(float current) +{ + return constrain(lrintf(current * 100.0f), -32000, 32000); +} + +constexpr uint8_t MSP_PASSTHROUGH_ESC_4WAY = 0xff; + +} + +namespace Espfc { + +namespace Connect { + +MspProcessor::MspProcessor(Model& model): _model(model) {} + +bool MspProcessor::parse(char c, MspMessage& msg) +{ + _parser.parse(c, msg); + + if(msg.isReady()) debugMessage(msg); + + return !msg.isIdle(); +} + +void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialDevice& s) +{ + r.cmd = m.cmd; + r.version = m.version; + r.result = 1; + switch(m.cmd) + { + case MSP_API_VERSION: + r.writeU8(MSP_PROTOCOL_VERSION); + r.writeU8(API_VERSION_MAJOR); + r.writeU8(API_VERSION_MINOR); + break; + + case MSP_FC_VARIANT: + r.writeData(flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH); + break; + + case MSP_FC_VERSION: + r.writeU8(FC_VERSION_MAJOR); + r.writeU8(FC_VERSION_MINOR); + r.writeU8(FC_VERSION_PATCH_LEVEL); + break; + + case MSP_BOARD_INFO: + r.writeData(boardIdentifier, BOARD_IDENTIFIER_LENGTH); + r.writeU16(0); // No other build targets currently have hardware revision detection. + r.writeU8(0); // 0 == FC + r.writeU8(0); // target capabilities + r.writeU8(strlen(targetName)); // target name + r.writeData(targetName, strlen(targetName)); + r.writeU8(0); // board name + r.writeU8(0); // manufacturer name + for(size_t i = 0; i < 32; i++) r.writeU8(0); // signature + r.writeU8(255); // mcu id: unknown + // 1.42 + r.writeU8(2); // configuration state: configured + // 1.43 + r.writeU16(_model.state.gyro.present ? _model.state.gyro.timer.rate : 0); // sample rate + { + uint32_t problems = 0; + if(_model.state.accel.bias.x == 0 && _model.state.accel.bias.y == 0 && _model.state.accel.bias.z == 0) { + problems |= 1 << 0; // acc calibration required + } + if(_model.config.output.protocol == ESC_PROTOCOL_DISABLED) { + problems |= 1 << 1; // no motor protocol + } + r.writeU32(problems); // configuration problems + } + // 1.44 + r.writeU8(0); // spi dev count + r.writeU8(0); // i2c dev count + break; + + case MSP_BUILD_INFO: + r.writeData(buildDate, BUILD_DATE_LENGTH); + r.writeData(buildTime, BUILD_TIME_LENGTH); + r.writeData(shortGitRevision, GIT_SHORT_REVISION_LENGTH); + break; + + case MSP_UID: + r.writeU32(getBoardId0()); + r.writeU32(getBoardId1()); + r.writeU32(getBoardId2()); + break; + + case MSP_STATUS_EX: + case MSP_STATUS: + //r.writeU16(_model.state.loopTimer.delta); + r.writeU16(_model.state.stats.loopTime()); + r.writeU16(_model.state.i2cErrorCount); // i2c error count + // acc, baro, mag, gps, sonar, gyro + r.writeU16(_model.accelActive() | _model.baroActive() << 1 | _model.magActive() << 2 | 0 << 3 | 0 << 4 | _model.gyroActive() << 5); + r.writeU32(_model.state.mode.mask); // flight mode flags + r.writeU8(0); // pid profile + r.writeU16(lrintf(_model.state.stats.getCpuLoad())); + if (m.cmd == MSP_STATUS_EX) { + r.writeU8(1); // max profile count + r.writeU8(0); // current rate profile index + } else { // MSP_STATUS + //r.writeU16(_model.state.gyro.timer.interval); // gyro cycle time + r.writeU16(0); + } + + // flight mode flags (above 32 bits) + r.writeU8(0); // count + + // Write arming disable flags + r.writeU8(ARMING_DISABLED_FLAGS_COUNT); // 1 byte, flag count + r.writeU32(_model.state.mode.armingDisabledFlags); // 4 bytes, flags + r.writeU8(0); // reboot required + break; + + case MSP_NAME: + r.writeString(_model.config.modelName); + break; + + case MSP_SET_NAME: + memset(&_model.config.modelName, 0, MODEL_NAME_LEN + 1); + for(size_t i = 0; i < std::min((size_t)m.received, MODEL_NAME_LEN); i++) + { + _model.config.modelName[i] = m.readU8(); + } + break; + + case MSP_BOXNAMES: + r.writeString(F("ARM;ANGLE;AIRMODE;BEEPER;FAILSAFE;BLACKBOX;BLACKBOXERASE;")); + break; + + case MSP_BOXIDS: + r.writeU8(MODE_ARMED); + r.writeU8(MODE_ANGLE); + 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: + for(size_t i = 0; i < ACTUATOR_CONDITIONS; i++) + { + r.writeU8(_model.config.conditions[i].id); + r.writeU8(_model.config.conditions[i].ch - AXIS_AUX_1); + r.writeU8((_model.config.conditions[i].min - 900) / 25); + r.writeU8((_model.config.conditions[i].max - 900) / 25); + } + break; + + case MSP_MODE_RANGES_EXTRA: + r.writeU8(ACTUATOR_CONDITIONS); + for(size_t i = 0; i < ACTUATOR_CONDITIONS; i++) + { + r.writeU8(_model.config.conditions[i].id); + r.writeU8(_model.config.conditions[i].logicMode); + r.writeU8(_model.config.conditions[i].linkId); + } + + break; + + case MSP_SET_MODE_RANGE: + { + size_t i = m.readU8(); + if(i < ACTUATOR_CONDITIONS) + { + _model.config.conditions[i].id = m.readU8(); + _model.config.conditions[i].ch = m.readU8() + AXIS_AUX_1; + _model.config.conditions[i].min = m.readU8() * 25 + 900; + _model.config.conditions[i].max = m.readU8() * 25 + 900; + if(m.remain() >= 2) { + _model.config.conditions[i].logicMode = m.readU8(); // mode logic + _model.config.conditions[i].linkId = m.readU8(); // link to + } + } + else + { + r.result = -1; + } + } + break; + + case MSP_ANALOG: + r.writeU8(toVbatVoltageLegacy(_model.state.battery.voltage)); // voltage in 0.1V + r.writeU16(0); // mah drawn + r.writeU16(_model.getRssi()); // rssi + r.writeU16(toIbatCurrent(_model.state.battery.current)); // amperage in 0.01A + r.writeU16(toVbatVoltage(_model.state.battery.voltage)); // voltage in 0.01V + break; + + case MSP_FEATURE_CONFIG: + r.writeU32(_model.config.featureMask); + break; + + case MSP_SET_FEATURE_CONFIG: + _model.config.featureMask = m.readU32(); + _model.reload(); + break; + + case MSP_BATTERY_CONFIG: + r.writeU8(34); // vbatmincellvoltage + r.writeU8(42); // vbatmaxcellvoltage + r.writeU8((_model.config.vbat.cellWarning + 5) / 10); // vbatwarningcellvoltage + r.writeU16(0); // batteryCapacity + r.writeU8(_model.config.vbat.source); // voltageMeterSource + r.writeU8(_model.config.ibat.source); // currentMeterSource + r.writeU16(340); // vbatmincellvoltage + r.writeU16(420); // vbatmaxcellvoltage + r.writeU16(_model.config.vbat.cellWarning); // vbatwarningcellvoltage + break; + + case MSP_SET_BATTERY_CONFIG: + m.readU8(); // vbatmincellvoltage + m.readU8(); // vbatmaxcellvoltage + _model.config.vbat.cellWarning = m.readU8() * 10; // vbatwarningcellvoltage + m.readU16(); // batteryCapacity + _model.config.vbat.source = toVbatSource(m.readU8()); // voltageMeterSource + _model.config.ibat.source = toIbatSource(m.readU8()); // currentMeterSource + if(m.remain() >= 6) + { + m.readU16(); // vbatmincellvoltage + m.readU16(); // vbatmaxcellvoltage + _model.config.vbat.cellWarning = m.readU16(); + } + break; + + case MSP_BATTERY_STATE: + // battery characteristics + r.writeU8(_model.state.battery.cells); // cell count, 0 indicates battery not detected. + r.writeU16(0); // capacity in mAh + + // battery state + r.writeU8(toVbatVoltageLegacy(_model.state.battery.voltage)); // in 0.1V steps + r.writeU16(0); // milliamp hours drawn from battery + r.writeU16(toIbatCurrent(_model.state.battery.current)); // send current in 0.01 A steps, range is -320A to 320A + + // battery alerts + r.writeU8(0); + r.writeU16(toVbatVoltage(_model.state.battery.voltage)); // in 0.01 steps + break; + + case MSP_VOLTAGE_METERS: + for(int i = 0; i < 1; i++) + { + r.writeU8(i + 10); // meter id (10-19 vbat adc) + r.writeU8(toVbatVoltageLegacy(_model.state.battery.voltage)); // meter value + } + break; + + case MSP_CURRENT_METERS: + for(int i = 0; i < 1; i++) + { + r.writeU8(i + 10); // meter id (10-19 ibat adc) + r.writeU16(0); // mah drawn + r.writeU16(constrain(toIbatCurrent(_model.state.battery.current) * 10, 0, 0xffff)); // meter value + } + break; + + case MSP_VOLTAGE_METER_CONFIG: + r.writeU8(1); // num voltage sensors + for(int i = 0; i < 1; i++) + { + r.writeU8(5); // frame size (5) + r.writeU8(i + 10); // id (10-19 vbat adc) + r.writeU8(0); // type resistor divider + r.writeU8(_model.config.vbat.scale); // scale + r.writeU8(_model.config.vbat.resDiv); // resdivval + r.writeU8(_model.config.vbat.resMult); // resdivmultiplier + } + break; + + case MSP_SET_VOLTAGE_METER_CONFIG: + { + int id = m.readU8(); + if(id == 10 + 0) // id (10-19 vbat adc, allow only 10) + { + _model.config.vbat.scale = m.readU8(); + _model.config.vbat.resDiv = m.readU8(); + _model.config.vbat.resMult = m.readU8(); + } + } + break; + + case MSP_CURRENT_METER_CONFIG: + r.writeU8(1); // num voltage sensors + for(int i = 0; i < 1; i++) + { + r.writeU8(6); // frame size (6) + r.writeU8(i + 10); // id (10-19 ibat adc) + r.writeU8(1); // type adc + r.writeU16(_model.config.ibat.scale); // scale + r.writeU16(_model.config.ibat.offset); // offset + } + break; + + case MSP_SET_CURRENT_METER_CONFIG: + { + int id = m.readU8(); + if(id == 10 + 0) // id (10-19 ibat adc, allow only 10) + { + _model.config.ibat.scale = m.readU16(); + _model.config.ibat.offset = m.readU16(); + } + } + break; + + case MSP_DATAFLASH_SUMMARY: +#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: + r.writeU16(0); // pitch + r.writeU16(0); // roll + break; + + case MSP_MIXER_CONFIG: + r.writeU8(_model.config.mixer.type); // mixerMode, QUAD_X + r.writeU8(_model.config.mixer.yawReverse); // yaw_motors_reversed + break; + + case MSP_SET_MIXER_CONFIG: + _model.config.mixer.type = m.readU8(); // mixerMode, QUAD_X + _model.config.mixer.yawReverse = m.readU8(); // yaw_motors_reversed + break; + + case MSP_SENSOR_CONFIG: + r.writeU8(_model.config.accel.dev); // 3 acc mpu6050 + r.writeU8(_model.config.baro.dev); // 2 baro bmp085 + r.writeU8(_model.config.mag.dev); // 3 mag hmc5883l + break; + + case MSP_SET_SENSOR_CONFIG: + _model.config.accel.dev = m.readU8(); // 3 acc mpu6050 + _model.config.baro.dev = m.readU8(); // 2 baro bmp085 + _model.config.mag.dev = m.readU8(); // 3 mag hmc5883l + _model.reload(); + break; + + case MSP_SENSOR_ALIGNMENT: + r.writeU8(_model.config.gyro.align); // gyro align + r.writeU8(_model.config.gyro.align); // acc align, Starting with 4.0 gyro and acc alignment are the same + r.writeU8(_model.config.mag.align); // mag align + //1.41+ + r.writeU8(_model.state.gyro.present ? 1 : 0); // gyro detection mask GYRO_1_MASK + r.writeU8(0); // gyro_to_use + r.writeU8(_model.config.gyro.align); // gyro 1 + r.writeU8(0); // gyro 2 + break; + + case MSP_SET_SENSOR_ALIGNMENT: + { + uint8_t gyroAlign = m.readU8(); // gyro align + m.readU8(); // discard deprecated acc align + _model.config.mag.align = m.readU8(); // mag align + // API >= 1.41 - support the gyro_to_use and alignment for gyros 1 & 2 + if(m.remain() >= 3) + { + m.readU8(); // gyro_to_use + gyroAlign = m.readU8(); // gyro 1 align + m.readU8(); // gyro 2 align + } + _model.config.gyro.align = gyroAlign; + } + break; + + 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.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 + r.writeU8(0); // gps_baudrateIndex + r.writeU8(0); // telemetry_baudrateIndex + r.writeU8(toBaudIndex(_model.config.serial[i].blackboxBaud)); // blackbox_baudrateIndex + } + break; + + case MSP2_COMMON_SERIAL_CONFIG: + { + uint8_t count = 0; + for (int i = 0; i < SERIAL_UART_COUNT; i++) + { + 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.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 + r.writeU8(0); // gps_baudrateIndex + r.writeU8(0); // telemetry_baudrateIndex + r.writeU8(toBaudIndex(_model.config.serial[i].blackboxBaud)); // blackbox_baudrateIndex + } + } + break; + + case MSP_SET_CF_SERIAL_CONFIG: + { + const int packetSize = 1 + 2 + 4; + while(m.remain() >= packetSize) + { + int id = m.readU8(); + int k = _model.getSerialIndex((SerialPortId)id); + { + m.advance(packetSize - 1); + continue; + } + _model.config.serial[k].id = id; + _model.config.serial[k].functionMask = m.readU16(); + _model.config.serial[k].baud = fromBaudIndex((SerialSpeedIndex)m.readU8()); + m.readU8(); + m.readU8(); + _model.config.serial[k].blackboxBaud = fromBaudIndex((SerialSpeedIndex)m.readU8()); + } + } + _model.reload(); + break; + + case MSP2_COMMON_SET_SERIAL_CONFIG: + { + size_t count = m.readU8(); + (void)count; // ignore + const int packetSize = 1 + 4 + 4; + while(m.remain() >= packetSize) + { + int id = m.readU8(); + int k = _model.getSerialIndex((SerialPortId)id); + if(k == -1) + { + m.advance(packetSize - 1); + continue; + } + _model.config.serial[k].id = id; + _model.config.serial[k].functionMask = m.readU32(); + _model.config.serial[k].baud = fromBaudIndex((SerialSpeedIndex)m.readU8()); + m.readU8(); + m.readU8(); + _model.config.serial[k].blackboxBaud = fromBaudIndex((SerialSpeedIndex)m.readU8()); + } + } + _model.reload(); + break; + + case MSP_BLACKBOX_CONFIG: + r.writeU8(1); // Blackbox supported + r.writeU8(_model.config.blackbox.dev); // device serial or none + r.writeU8(1); // blackboxGetRateNum()); // unused + r.writeU8(1); // blackboxGetRateDenom()); + r.writeU16(_model.config.blackbox.pDenom);//blackboxGetPRatio()); // p_denom + //r.writeU8(_model.config.blackbox.pDenom); // sample_rate + //r.writeU32(~_model.config.blackbox.fieldsMask); + break; + + case MSP_SET_BLACKBOX_CONFIG: + // Don't allow config to be updated while Blackbox is logging + if (true) { + _model.config.blackbox.dev = m.readU8(); + const int rateNum = m.readU8(); // was rate_num + const int rateDenom = m.readU8(); // was rate_denom + uint16_t pRatio = 0; + if (m.remain() >= 2) { + pRatio = m.readU16(); // p_denom specified, so use it directly + } else { + // p_denom not specified in MSP, so calculate it from old rateNum and rateDenom + //pRatio = blackboxCalculatePDenom(rateNum, rateDenom); + (void)(rateNum + rateDenom); + } + _model.config.blackbox.pDenom = pRatio; + + /*if (m.remain() >= 1) { + _model.config.blackbox.pDenom = m.readU8(); + } else if(pRatio > 0) { + _model.config.blackbox.pDenom = blackboxCalculateSampleRate(pRatio); + //_model.config.blackbox.pDenom = pRatio; + } + if (m.remain() >= 4) { + _model.config.blackbox.fieldsMask = ~m.readU32(); + }*/ + } + break; + + case MSP_ATTITUDE: + r.writeU16(lrintf(Utils::toDeg(_model.state.attitude.euler.x) * 10.f)); // roll [decidegrees] + r.writeU16(lrintf(Utils::toDeg(_model.state.attitude.euler.y) * 10.f)); // pitch [decidegrees] + r.writeU16(lrintf(Utils::toDeg(-_model.state.attitude.euler.z))); // yaw [degrees] + break; + + case MSP_ALTITUDE: + r.writeU32(lrintf(_model.state.baro.altitude * 100.f)); // alt [cm] + r.writeU16(0); // vario + break; + + case MSP_BEEPER_CONFIG: + r.writeU32(~_model.config.buzzer.beeperMask); // beeper mask + r.writeU8(0); // dshot beacon tone + r.writeU32(0); // dshot beacon off flags + break; + + case MSP_SET_BEEPER_CONFIG: + _model.config.buzzer.beeperMask = ~m.readU32(); // beeper mask + break; + + case MSP_BOARD_ALIGNMENT_CONFIG: + r.writeU16(_model.config.boardAlignment[0]); // roll + r.writeU16(_model.config.boardAlignment[1]); // pitch + r.writeU16(_model.config.boardAlignment[2]); // yaw + break; + + case MSP_SET_BOARD_ALIGNMENT_CONFIG: + _model.config.boardAlignment[0] = m.readU16(); + _model.config.boardAlignment[1] = m.readU16(); + _model.config.boardAlignment[2] = m.readU16(); + break; + + case MSP_RX_MAP: + for(size_t i = 0; i < INPUT_CHANNELS; i++) + { + r.writeU8(_model.config.input.channel[i].map); + } + break; + + case MSP_SET_RX_MAP: + for(size_t i = 0; i < 8; i++) + { + _model.config.input.channel[i].map = m.readU8(); + } + break; + + case MSP_RSSI_CONFIG: + r.writeU8(_model.config.input.rssiChannel); + break; + + case MSP_SET_RSSI_CONFIG: + _model.config.input.rssiChannel = m.readU8(); + break; + + case MSP_MOTOR_CONFIG: + r.writeU16(_model.config.output.minThrottle); // minthrottle + r.writeU16(_model.config.output.maxThrottle); // maxthrottle + r.writeU16(_model.config.output.minCommand); // mincommand + r.writeU8(_model.state.currentMixer.count); // motor count + // 1.42+ + r.writeU8(_model.config.output.motorPoles); // motor pole count + r.writeU8(_model.config.output.dshotTelemetry); // dshot telemtery + r.writeU8(0); // esc sensor + break; + + case MSP_SET_MOTOR_CONFIG: + _model.config.output.minThrottle = m.readU16(); // minthrottle + _model.config.output.maxThrottle = m.readU16(); // maxthrottle + _model.config.output.minCommand = m.readU16(); // mincommand + if(m.remain() >= 2) + { +#ifdef ESPFC_DSHOT_TELEMETRY + _model.config.output.motorPoles = m.readU8(); + _model.config.output.dshotTelemetry = m.readU8(); +#else + m.readU8(); + m.readU8(); +#endif + } + _model.reload(); + break; + + case MSP_MOTOR_3D_CONFIG: + r.writeU16(1406); // deadband3d_low; + r.writeU16(1514); // deadband3d_high; + r.writeU16(1460); // neutral3d; + break; + + case MSP_ARMING_CONFIG: + r.writeU8(5); // auto_disarm delay + r.writeU8(0); // disarm kill switch + r.writeU8(180); // small angle + break; + + case MSP_RC_DEADBAND: + r.writeU8(_model.config.input.deadband); + r.writeU8(0); // yaw deadband + r.writeU8(0); // alt hold deadband + r.writeU16(0); // deadband 3d throttle + break; + + case MSP_SET_RC_DEADBAND: + _model.config.input.deadband = m.readU8(); + m.readU8(); // yaw deadband + m.readU8(); // alt hod deadband + m.readU16(); // deadband 3d throttle + break; + + case MSP_RX_CONFIG: + r.writeU8(_model.config.input.serialRxProvider); // serialrx_provider + r.writeU16(_model.config.input.maxCheck); //maxcheck + r.writeU16(_model.config.input.midRc); //midrc + r.writeU16(_model.config.input.minCheck); //mincheck + r.writeU8(0); // spectrum bind + r.writeU16(_model.config.input.minRc); //min_us + r.writeU16(_model.config.input.maxRc); //max_us + r.writeU8(_model.config.input.interpolationMode); // rc interpolation + r.writeU8(_model.config.input.interpolationInterval); // rc interpolation interval + r.writeU16(1500); // airmode activate threshold + r.writeU8(0); // rx spi prot + r.writeU32(0); // rx spi id + r.writeU8(0); // rx spi chan count + r.writeU8(0); // fpv camera angle + r.writeU8(2); // rc iterpolation channels: RPYT + r.writeU8(_model.config.input.filterType); // rc_smoothing_type + r.writeU8(_model.config.input.filter.freq); // rc_smoothing_input_cutoff + r.writeU8(_model.config.input.filterDerivative.freq); // rc_smoothing_derivative_cutoff + r.writeU8(0);//_model.config.input.filter.type); // rc_smoothing_input_type + r.writeU8(fromFilterTypeDerivative(_model.config.input.filterDerivative.type)); // rc_smoothing_derivative_type + r.writeU8(0); // usb type + // 1.42+ + r.writeU8(_model.config.input.filterAutoFactor); // rc_smoothing_auto_factor + break; + + case MSP_SET_RX_CONFIG: + _model.config.input.serialRxProvider = m.readU8(); // serialrx_provider + _model.config.input.maxCheck = m.readU16(); //maxcheck + _model.config.input.midRc = m.readU16(); //midrc + _model.config.input.minCheck = m.readU16(); //mincheck + m.readU8(); // spectrum bind + _model.config.input.minRc = m.readU16(); //min_us + _model.config.input.maxRc = m.readU16(); //max_us + if (m.remain() >= 4) { + _model.config.input.interpolationMode = m.readU8(); // rc interpolation + _model.config.input.interpolationInterval = m.readU8(); // rc interpolation interval + m.readU16(); // airmode activate threshold + } + if (m.remain() >= 6) { + m.readU8(); // rx spi prot + m.readU32(); // rx spi id + m.readU8(); // rx spi chan count + } + if (m.remain() >= 1) { + m.readU8(); // fpv camera angle + } + // 1.40+ + if (m.remain() >= 6) { + m.readU8(); // rc iterpolation channels + _model.config.input.filterType = m.readU8(); // rc_smoothing_type + _model.config.input.filter.freq = m.readU8(); // rc_smoothing_input_cutoff + _model.config.input.filterDerivative.freq = m.readU8(); // rc_smoothing_derivative_cutoff + //_model.config.input.filter.type = m.readU8() == 1 ? FILTER_BIQUAD : FILTER_PT1; // rc_smoothing_input_type + m.readU8(); + _model.config.input.filterDerivative.type = toFilterTypeDerivative(m.readU8()); // rc_smoothing_derivative_type + } + if (m.remain() >= 1) { + m.readU8(); // usb type + } + // 1.42+ + if (m.remain() >= 1) { + _model.config.input.filterAutoFactor = m.readU8(); // rc_smoothing_auto_factor + } + + _model.reload(); + break; + + case MSP_FAILSAFE_CONFIG: + r.writeU8(_model.config.failsafe.delay); // failsafe_delay + r.writeU8(0); // failsafe_off_delay + r.writeU16(1000); //failsafe_throttle + r.writeU8(_model.config.failsafe.killSwitch); // failsafe_kill_switch + r.writeU16(0); // failsafe_throttle_low_delay + r.writeU8(1); //failsafe_procedure; default drop + break; + + case MSP_SET_FAILSAFE_CONFIG: + _model.config.failsafe.delay = m.readU8(); //failsafe_delay + m.readU8(); //failsafe_off_delay + m.readU16(); //failsafe_throttle + _model.config.failsafe.killSwitch = m.readU8(); //failsafe_kill_switch + m.readU16(); //failsafe_throttle_low_delay + m.readU8(); //failsafe_procedure + break; + + case MSP_RXFAIL_CONFIG: + for (size_t i = 0; i < _model.state.input.channelCount; i++) + { + r.writeU8(_model.config.input.channel[i].fsMode); + r.writeU16(_model.config.input.channel[i].fsValue); + } + break; + + case MSP_SET_RXFAIL_CONFIG: + { + size_t i = m.readU8(); + if(i < INPUT_CHANNELS) + { + _model.config.input.channel[i].fsMode = m.readU8(); // mode + _model.config.input.channel[i].fsValue = m.readU16(); // pulse + } + else + { + r.result = -1; + } + } + break; + + case MSP_RC: + for(size_t i = 0; i < _model.state.input.channelCount; i++) + { + r.writeU16(lrintf(_model.state.input.us[i])); + } + break; + + case MSP_RC_TUNING: + r.writeU8(_model.config.input.rate[AXIS_ROLL]); + r.writeU8(_model.config.input.expo[AXIS_ROLL]); + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) + { + r.writeU8(_model.config.input.superRate[i]); + } + r.writeU8(_model.config.controller.tpaScale); // dyn thr pid + r.writeU8(50); // thrMid8 + r.writeU8(0); // thr expo + r.writeU16(_model.config.controller.tpaBreakpoint); // tpa breakpoint + r.writeU8(_model.config.input.expo[AXIS_YAW]); // yaw expo + r.writeU8(_model.config.input.rate[AXIS_YAW]); // yaw rate + r.writeU8(_model.config.input.rate[AXIS_PITCH]); // pitch rate + r.writeU8(_model.config.input.expo[AXIS_PITCH]); // pitch expo + // 1.41+ + r.writeU8(_model.config.output.throttleLimitType); // throttle_limit_type (off) + r.writeU8(_model.config.output.throttleLimitPercent); // throtle_limit_percent (100%) + //1.42+ + r.writeU16(_model.config.input.rateLimit[0]); // rate limit roll + r.writeU16(_model.config.input.rateLimit[1]); // rate limit pitch + r.writeU16(_model.config.input.rateLimit[2]); // rate limit yaw + // 1.43+ + r.writeU8(_model.config.input.rateType); // rates type + + break; + + case MSP_SET_RC_TUNING: + if(m.remain() >= 10) + { + const uint8_t rate = m.readU8(); + if(_model.config.input.rate[AXIS_PITCH] == _model.config.input.rate[AXIS_ROLL]) + { + _model.config.input.rate[AXIS_PITCH] = rate; + } + _model.config.input.rate[AXIS_ROLL] = rate; + + const uint8_t expo = m.readU8(); + if(_model.config.input.expo[AXIS_PITCH] == _model.config.input.expo[AXIS_ROLL]) + { + _model.config.input.expo[AXIS_PITCH] = expo; + } + _model.config.input.expo[AXIS_ROLL] = expo; + + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) + { + _model.config.input.superRate[i] = m.readU8(); + } + _model.config.controller.tpaScale = Utils::clamp(m.readU8(), (uint8_t)0, (uint8_t)90); // dyn thr pid + m.readU8(); // thrMid8 + m.readU8(); // thr expo + _model.config.controller.tpaBreakpoint = Utils::clamp(m.readU16(), (uint16_t)1000, (uint16_t)2000); // tpa breakpoint + if(m.remain() >= 1) + { + _model.config.input.expo[AXIS_YAW] = m.readU8(); // yaw expo + } + if(m.remain() >= 1) + { + _model.config.input.rate[AXIS_YAW] = m.readU8(); // yaw rate + } + if(m.remain() >= 1) + { + _model.config.input.rate[AXIS_PITCH] = m.readU8(); // pitch rate + } + if(m.remain() >= 1) + { + _model.config.input.expo[AXIS_PITCH] = m.readU8(); // pitch expo + } + // 1.41 + if(m.remain() >= 2) + { + _model.config.output.throttleLimitType = m.readU8(); // throttle_limit_type + _model.config.output.throttleLimitPercent = m.readU8(); // throttle_limit_percent + } + // 1.42 + if(m.remain() >= 6) + { + _model.config.input.rateLimit[0] = m.readU16(); // roll + _model.config.input.rateLimit[1] = m.readU16(); // pitch + _model.config.input.rateLimit[2] = m.readU16(); // yaw + } + // 1.43 + if (m.remain() >= 1) + { + _model.config.input.rateType = m.readU8(); + } + } + else + { + r.result = -1; + // error + } + break; + + case MSP_ADVANCED_CONFIG: + r.writeU8(1); // gyroSync unused + r.writeU8(_model.config.loopSync); + r.writeU8(_model.config.output.async); + r.writeU8(_model.config.output.protocol); + r.writeU16(_model.config.output.rate); + r.writeU16(_model.config.output.dshotIdle); + r.writeU8(0); // 32k gyro + r.writeU8(0); // PWM inversion + r.writeU8(0); // gyro_to_use: {1:0, 2:1. 2:both} + r.writeU8(0); // gyro high fsr (flase) + r.writeU8(48); // gyro cal threshold + r.writeU16(125); // gyro cal duration (1.25s) + r.writeU16(0); // gyro offset yaw + r.writeU8(0); // check overflow + r.writeU8(_model.config.debug.mode); + r.writeU8(DEBUG_COUNT); + break; + + case MSP_SET_ADVANCED_CONFIG: + m.readU8(); // ignore gyroSync, removed in 1.43 + _model.config.loopSync = m.readU8(); + _model.config.output.async = m.readU8(); + _model.config.output.protocol = m.readU8(); + _model.config.output.rate = m.readU16(); + if(m.remain() >= 2) { + _model.config.output.dshotIdle = m.readU16(); // dshot idle + } + if(m.remain()) { + m.readU8(); // 32k gyro + } + if(m.remain()) { + m.readU8(); // PWM inversion + } + if(m.remain() >= 8) { + m.readU8(); // gyro_to_use + m.readU8(); // gyro high fsr + m.readU8(); // gyro cal threshold + m.readU16(); // gyro cal duration + m.readU16(); // gyro offset yaw + m.readU8(); // check overflow + } + if(m.remain()) { + _model.config.debug.mode = m.readU8(); + } + _model.reload(); + break; + + case MSP_GPS_CONFIG: + r.writeU8(0); // provider + r.writeU8(0); // sbasMode + r.writeU8(0); // autoConfig + r.writeU8(0); // autoBaud + // 1.43+ + r.writeU8(0); // gps_set_home_point_once + r.writeU8(0); // gps_ublox_use_galileo + break; + + //case MSP_COMPASS_CONFIG: + // r.writeU16(0); // mag_declination * 10 + // break; + + case MSP_FILTER_CONFIG: + r.writeU8(_model.config.gyro.filter.freq); // gyro lpf + r.writeU16(_model.config.dterm.filter.freq); // dterm lpf + r.writeU16(_model.config.yaw.filter.freq); // yaw lpf + r.writeU16(_model.config.gyro.notch1Filter.freq); // gyro notch 1 hz + r.writeU16(_model.config.gyro.notch1Filter.cutoff); // gyro notch 1 cutoff + r.writeU16(_model.config.dterm.notchFilter.freq); // dterm notch hz + r.writeU16(_model.config.dterm.notchFilter.cutoff); // dterm notch cutoff + r.writeU16(_model.config.gyro.notch2Filter.freq); // gyro notch 2 hz + r.writeU16(_model.config.gyro.notch2Filter.cutoff); // gyro notch 2 cutoff + r.writeU8(_model.config.dterm.filter.type); // dterm type + r.writeU8(fromGyroDlpf(_model.config.gyro.dlpf)); + r.writeU8(0); // dlfp 32khz type + r.writeU16(_model.config.gyro.filter.freq); // lowpass1 freq + r.writeU16(_model.config.gyro.filter2.freq); // lowpass2 freq + r.writeU8(_model.config.gyro.filter.type); // lowpass1 type + r.writeU8(_model.config.gyro.filter2.type); // lowpass2 type + r.writeU16(_model.config.dterm.filter2.freq); // dterm lopwass2 freq + // 1.41+ + r.writeU8(_model.config.dterm.filter2.type); // dterm lopwass2 type + r.writeU16(_model.config.gyro.dynLpfFilter.cutoff); // dyn lpf gyro min + r.writeU16(_model.config.gyro.dynLpfFilter.freq); // dyn lpf gyro max + r.writeU16(_model.config.dterm.dynLpfFilter.cutoff); // dyn lpf dterm min + r.writeU16(_model.config.dterm.dynLpfFilter.freq); // dyn lpf dterm max + // gyro analyse + r.writeU8(3); // deprecated dyn notch range + r.writeU8(_model.config.gyro.dynamicFilter.count); // dyn_notch_width_percent + r.writeU16(_model.config.gyro.dynamicFilter.q); // dyn_notch_q + r.writeU16(_model.config.gyro.dynamicFilter.min_freq); // dyn_notch_min_hz + // rpm filter + r.writeU8(_model.config.gyro.rpmFilter.harmonics); // gyro_rpm_notch_harmonics + r.writeU8(_model.config.gyro.rpmFilter.minFreq); // gyro_rpm_notch_min + // 1.43+ + r.writeU16(_model.config.gyro.dynamicFilter.max_freq); // dyn_notch_max_hz + break; + + case MSP_SET_FILTER_CONFIG: + _model.config.gyro.filter.freq = m.readU8(); + _model.config.dterm.filter.freq = m.readU16(); + _model.config.yaw.filter.freq = m.readU16(); + if (m.remain() >= 8) { + _model.config.gyro.notch1Filter.freq = m.readU16(); + _model.config.gyro.notch1Filter.cutoff = m.readU16(); + _model.config.dterm.notchFilter.freq = m.readU16(); + _model.config.dterm.notchFilter.cutoff = m.readU16(); + } + if (m.remain() >= 4) { + _model.config.gyro.notch2Filter.freq = m.readU16(); + _model.config.gyro.notch2Filter.cutoff = m.readU16(); + } + if (m.remain() >= 1) { + _model.config.dterm.filter.type = (FilterType)m.readU8(); + } + if (m.remain() >= 10) { + m.readU8(); // dlfp type + m.readU8(); // 32k dlfp type + _model.config.gyro.filter.freq = m.readU16(); + _model.config.gyro.filter2.freq = m.readU16(); + _model.config.gyro.filter.type = m.readU8(); + _model.config.gyro.filter2.type = m.readU8(); + _model.config.dterm.filter2.freq = m.readU16(); + } + // 1.41+ + if (m.remain() >= 9) { + _model.config.dterm.filter2.type = m.readU8(); + _model.config.gyro.dynLpfFilter.cutoff = m.readU16(); // dyn gyro lpf min + _model.config.gyro.dynLpfFilter.freq = m.readU16(); // dyn gyro lpf max + _model.config.dterm.dynLpfFilter.cutoff = m.readU16(); // dyn dterm lpf min + _model.config.dterm.dynLpfFilter.freq = m.readU16(); // dyn dterm lpf min + } + if (m.remain() >= 8) { + m.readU8(); // deprecated dyn_notch_range + _model.config.gyro.dynamicFilter.count = m.readU8(); // dyn_notch_width_percent + _model.config.gyro.dynamicFilter.q = m.readU16(); // dyn_notch_q + _model.config.gyro.dynamicFilter.min_freq = m.readU16(); // dyn_notch_min_hz + _model.config.gyro.rpmFilter.harmonics = m.readU8(); // gyro_rpm_notch_harmonics + _model.config.gyro.rpmFilter.minFreq = m.readU8(); // gyro_rpm_notch_min + } + // 1.43+ + if (m.remain() >= 1) { + _model.config.gyro.dynamicFilter.max_freq = m.readU16(); // dyn_notch_max_hz + } + _model.reload(); + break; + + case MSP_PID_CONTROLLER: + r.writeU8(1); // betaflight controller id + break; + + case MSP_PIDNAMES: + r.writeString(F("ROLL;PITCH;YAW;ALT;Pos;PosR;NavR;LEVEL;MAG;VEL;")); + break; + + case MSP_PID: + for(size_t i = 0; i < PID_ITEM_COUNT; i++) + { + r.writeU8(_model.config.pid[i].P); + r.writeU8(_model.config.pid[i].I); + r.writeU8(_model.config.pid[i].D); + } + break; + + case MSP_SET_PID: + for (int i = 0; i < PID_ITEM_COUNT; i++) + { + _model.config.pid[i].P = m.readU8(); + _model.config.pid[i].I = m.readU8(); + _model.config.pid[i].D = m.readU8(); + } + _model.reload(); + break; + + case MSP_PID_ADVANCED: /// !!!FINISHED HERE!!! + r.writeU16(0); + r.writeU16(0); + r.writeU16(0); // was pidProfile.yaw_p_limit + r.writeU8(0); // reserved + r.writeU8(0); // vbatPidCompensation; + r.writeU8(0); // feedForwardTransition; + r.writeU8((uint8_t)std::min(_model.config.dterm.setpointWeight, (int16_t)255)); // was low byte of dtermSetpointWeight + r.writeU8(0); // reserved + r.writeU8(0); // reserved + r.writeU8(0); // reserved + r.writeU16(0); // rateAccelLimit; + r.writeU16(0); // yawRateAccelLimit; + r.writeU8(_model.config.level.angleLimit); // levelAngleLimit; + r.writeU8(0); // was pidProfile.levelSensitivity + r.writeU16(0); // itermThrottleThreshold; + r.writeU16(1000); // itermAcceleratorGain; anti_gravity_gain, 0 in 1.45+ + r.writeU16(_model.config.dterm.setpointWeight); + r.writeU8(0); // iterm rotation + r.writeU8(0); // smart feed forward + r.writeU8(_model.config.iterm.relax); // iterm relax + r.writeU8(1); // iterm relax type (setpoint only) + r.writeU8(0); // abs control gain + r.writeU8(0); // throttle boost + r.writeU8(0); // acro trainer max angle + r.writeU16(_model.config.pid[FC_PID_ROLL].F); //pid roll f + r.writeU16(_model.config.pid[FC_PID_PITCH].F); //pid pitch f + r.writeU16(_model.config.pid[FC_PID_YAW].F); //pid yaw f + r.writeU8(0); // antigravity mode + // 1.41+ + r.writeU8(0); // d min roll + r.writeU8(0); // d min pitch + r.writeU8(0); // d min yaw + r.writeU8(0); // d min gain + r.writeU8(0); // d min advance + r.writeU8(0); // use_integrated_yaw + r.writeU8(0); // integrated_yaw_relax + // 1.42+ + r.writeU8(_model.config.iterm.relaxCutoff); // iterm_relax_cutoff + // 1.43+ + r.writeU8(_model.config.output.motorLimit); // motor_output_limit + r.writeU8(0); // auto_profile_cell_count + r.writeU8(0); // idle_min_rpm + break; + + case MSP_SET_PID_ADVANCED: + m.readU16(); + m.readU16(); + m.readU16(); // was pidProfile.yaw_p_limit + m.readU8(); // reserved + m.readU8(); + m.readU8(); + _model.config.dterm.setpointWeight = m.readU8(); + m.readU8(); // reserved + m.readU8(); // reserved + m.readU8(); // reserved + m.readU16(); + m.readU16(); + if (m.remain() >= 2) { + _model.config.level.angleLimit = m.readU8(); + m.readU8(); // was pidProfile.levelSensitivity + } + if (m.remain() >= 4) { + m.readU16(); // itermThrottleThreshold; + m.readU16(); // itermAcceleratorGain; anti_gravity_gain + } + if (m.remain() >= 2) { + _model.config.dterm.setpointWeight = m.readU16(); + } + if (m.remain() >= 14) { + m.readU8(); //iterm rotation + m.readU8(); //smart feed forward + _model.config.iterm.relax = m.readU8(); //iterm relax + m.readU8(); //iterm relax type + m.readU8(); //abs control gain + m.readU8(); //throttle boost + m.readU8(); //acro trainer max angle + _model.config.pid[FC_PID_ROLL].F = m.readU16(); // pid roll f + _model.config.pid[FC_PID_PITCH].F = m.readU16(); // pid pitch f + _model.config.pid[FC_PID_YAW].F = m.readU16(); // pid yaw f + m.readU8(); //antigravity mode + } + // 1.41+ + if (m.remain() >= 7) { + m.readU8(); // d min roll + m.readU8(); // d min pitch + m.readU8(); // d min yaw + m.readU8(); // d min gain + m.readU8(); // d min advance + m.readU8(); // use_integrated_yaw + m.readU8(); // integrated_yaw_relax + } + // 1.42+ + if (m.remain() >= 1) { + _model.config.iterm.relaxCutoff = m.readU8(); // iterm_relax_cutoff + } + // 1.43+ + if (m.remain() >= 3) { + _model.config.output.motorLimit = m.readU8(); // motor_output_limit + m.readU8(); // auto_profile_cell_count + m.readU8(); // idle_min_rpm + } + _model.reload(); + break; + + case MSP_RAW_IMU: + for (int i = 0; i < AXIS_COUNT_RPY; i++) + { + r.writeU16(lrintf(_model.state.accel.adc[i] * ACCEL_G_INV * 2048.f)); + } + for (int i = 0; i < AXIS_COUNT_RPY; i++) + { + r.writeU16(lrintf(Utils::toDeg(_model.state.gyro.adc[i]))); + } + for (int i = 0; i < AXIS_COUNT_RPY; i++) + { + r.writeU16(lrintf(_model.state.mag.adc[i] * 1090)); + } + break; + + case MSP_MOTOR: + for (size_t i = 0; i < 8; i++) + { + if (i >= OUTPUT_CHANNELS || _model.config.pin[i + PIN_OUTPUT_0] == -1) + { + r.writeU16(0); + continue; + } + r.writeU16(_model.state.output.us[i]); + } + break; + + case MSP_MOTOR_TELEMETRY: + r.writeU8(OUTPUT_CHANNELS); + for (size_t i = 0; i < OUTPUT_CHANNELS; i++) + { + int rpm = 0; + uint16_t invalidPct = 0; + uint8_t escTemperature = 0; // degrees celcius + uint16_t escVoltage = 0; // 0.01V per unit + uint16_t escCurrent = 0; // 0.01A per unit + uint16_t escConsumption = 0; // mAh + + if (_model.config.pin[i + PIN_OUTPUT_0] != -1) + { + rpm = lrintf(_model.state.output.telemetry.rpm[i]); + invalidPct = _model.state.output.telemetry.errors[i]; + escTemperature = _model.state.output.telemetry.temperature[i]; + escVoltage = _model.state.output.telemetry.voltage[i]; + escCurrent = _model.state.output.telemetry.current[i]; + } + + r.writeU32(rpm); + r.writeU16(invalidPct); + r.writeU8(escTemperature); + r.writeU16(escVoltage); + r.writeU16(escCurrent); + r.writeU16(escConsumption); + } + break; + + case MSP_SET_MOTOR: + for(size_t i = 0; i < OUTPUT_CHANNELS; i++) + { + _model.state.output.disarmed[i] = m.readU16(); + } + break; + + case MSP_SERVO: + for(size_t i = 0; i < OUTPUT_CHANNELS; i++) + { + if (i >= OUTPUT_CHANNELS || _model.config.pin[i + PIN_OUTPUT_0] == -1) + { + r.writeU16(1500); + continue; + } + r.writeU16(_model.state.output.us[i]); + } + break; + + case MSP_SERVO_CONFIGURATIONS: + for(size_t i = 0; i < 8; i++) + { + if(i < OUTPUT_CHANNELS) + { + r.writeU16(_model.config.output.channel[i].min); + r.writeU16(_model.config.output.channel[i].max); + r.writeU16(_model.config.output.channel[i].neutral); + } + else + { + r.writeU16(1000); + r.writeU16(2000); + r.writeU16(1500); + } + r.writeU8(100); + r.writeU8(-1); + r.writeU32(0); + } + break; + + case MSP_SET_SERVO_CONFIGURATION: + { + uint8_t i = m.readU8(); + if(i < OUTPUT_CHANNELS) + { + _model.config.output.channel[i].min = m.readU16(); + _model.config.output.channel[i].max = m.readU16(); + _model.config.output.channel[i].neutral = m.readU16(); + m.readU8(); + m.readU8(); + m.readU32(); + } + else + { + r.result = -1; + } + } + break; + + case MSP_ACC_CALIBRATION: + if(!_model.isModeActive(MODE_ARMED)) _model.calibrateGyro(); + break; + + case MSP_MAG_CALIBRATION: + if(!_model.isModeActive(MODE_ARMED)) _model.calibrateMag(); + break; + + case MSP_VTX_CONFIG: + r.writeU8(0xff); // vtx type unknown + r.writeU8(0); // band + r.writeU8(0); // channel + r.writeU8(0); // power + r.writeU8(0); // status + r.writeU16(0); // freq + r.writeU8(0); // ready + r.writeU8(0); // low power disarm + // 1.42 + r.writeU16(0); // pit mode freq + r.writeU8(0); // vtx table available (no) + r.writeU8(0); // vtx table bands + r.writeU8(0); // vtx table channels + r.writeU8(0); // vtx power levels + break; + + case MSP_SET_ARMING_DISABLED: + { + const uint8_t cmd = m.readU8(); + uint8_t disableRunawayTakeoff = 0; + if(m.remain()) { + disableRunawayTakeoff = m.readU8(); + } + (void)disableRunawayTakeoff; + _model.setArmingDisabled(ARMING_DISABLED_MSP, cmd); + if (_model.isModeActive(MODE_ARMED)) _model.disarm(DISARM_REASON_ARMING_DISABLED); + } + break; + + case MSP_SET_PASSTHROUGH: + { + uint8_t ptMode = MSP_PASSTHROUGH_ESC_4WAY; + uint8_t ptArg = 0; + if(m.remain() >= 2) { + ptMode = m.readU8(); + ptArg = m.readU8(); + } + switch (ptMode) + { + case MSP_PASSTHROUGH_ESC_4WAY: + r.writeU8(esc4wayInit()); + serialDeviceInit(&s, 0); + _postCommand = std::bind(&MspProcessor::processEsc4way, this); + break; + default: + r.writeU8(0); + break; + } + (void)ptArg; + } + break; + + case MSP_DEBUG: + for (int i = 0; i < 4; i++) { + r.writeU16(_model.state.debug[i]); + } + break; + + case MSP_EEPROM_WRITE: + _model.save(); + break; + + case MSP_RESET_CONF: + if(!_model.isModeActive(MODE_ARMED)) + { + _model.reset(); + } + break; + + case MSP_REBOOT: + r.writeU8(0); // reboot to firmware + _postCommand = std::bind(&MspProcessor::processRestart, this); + break; + + default: + r.result = 0; + break; + } +} + +void MspProcessor::processEsc4way() +{ +#if defined(ESPFC_MULTI_CORE) && defined(ESPFC_FREE_RTOS) + timer_pause(TIMER_GROUP_0, TIMER_0); +#endif + esc4wayProcess(getSerialPort()); + processRestart(); +} + +void MspProcessor::processRestart() +{ + Hardware::restart(_model); +} + +void MspProcessor::serializeFlashData(MspResponse& r, uint32_t address, const uint16_t size, bool useLegacyFormat, bool allowCompression) +{ +#ifdef USE_FLASHFS + (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 MspProcessor::sendResponse(MspResponse& r, Device::SerialDevice& s) +{ + debugResponse(r); + uint8_t buff[256]; + size_t len = r.serialize(buff, 256); + s.write(buff, len); +} + +void MspProcessor::postCommand() +{ + if(!_postCommand) return; + std::function cb = _postCommand; + _postCommand = {}; + cb(); +} + +bool MspProcessor::debugSkip(uint8_t cmd) +{ + //return true; + //return false; + if(cmd == MSP_STATUS) return true; + if(cmd == MSP_STATUS_EX) return true; + if(cmd == MSP_BOXNAMES) return true; + if(cmd == MSP_ANALOG) return true; + if(cmd == MSP_ATTITUDE) return true; + if(cmd == MSP_ALTITUDE) return true; + if(cmd == MSP_RC) return true; + if(cmd == MSP_RAW_IMU) return true; + if(cmd == MSP_MOTOR) return true; + if(cmd == MSP_SERVO) return true; + if(cmd == MSP_BATTERY_STATE) return true; + if(cmd == MSP_VOLTAGE_METERS) return true; + if(cmd == MSP_CURRENT_METERS) return true; + return false; +} + +void MspProcessor::debugMessage(const MspMessage& m) +{ + if(debugSkip(m.cmd)) return; + Device::SerialDevice * s = _model.getSerialStream(SERIAL_FUNCTION_TELEMETRY_HOTT); + if(!s) return; + + 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], HEX); s->print(' '); + } + s->println(); +} + +void MspProcessor::debugResponse(const MspResponse& r) +{ + if(debugSkip(r.cmd)) return; + Device::SerialDevice * s = _model.getSerialStream(SERIAL_FUNCTION_TELEMETRY_HOTT); + if(!s) return; + + 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], HEX); s->print(' '); + } + s->println(); +} + +} + +} diff --git a/lib/Espfc/src/Connect/MspProcessor.h b/lib/Espfc/src/Connect/MspProcessor.h deleted file mode 100644 index 9e70258f..00000000 --- a/lib/Espfc/src/Connect/MspProcessor.h +++ /dev/null @@ -1,1611 +0,0 @@ -#ifndef _ESPFC_MSP_PROCESSOR_H_ -#define _ESPFC_MSP_PROCESSOR_H_ - -#include "Connect/Msp.h" -#include "Model.h" -#include "Hardware.h" -#include "Connect/MspParser.h" -#include "platform.h" -#if defined(ESPFC_MULTI_CORE) && defined(ESPFC_FREE_RTOS) -#include -#endif - -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); - uint16_t blackboxGetPRatio(void); -} - -namespace { - -enum SerialSpeedIndex { - SERIAL_SPEED_INDEX_AUTO = 0, - SERIAL_SPEED_INDEX_9600, - SERIAL_SPEED_INDEX_19200, - SERIAL_SPEED_INDEX_38400, - SERIAL_SPEED_INDEX_57600, - SERIAL_SPEED_INDEX_115200, - SERIAL_SPEED_INDEX_230400, - SERIAL_SPEED_INDEX_250000, - SERIAL_SPEED_INDEX_400000, - SERIAL_SPEED_INDEX_460800, - SERIAL_SPEED_INDEX_500000, - SERIAL_SPEED_INDEX_921600, - SERIAL_SPEED_INDEX_1000000, - SERIAL_SPEED_INDEX_1500000, - SERIAL_SPEED_INDEX_2000000, - SERIAL_SPEED_INDEX_2470000, -}; - -static SerialSpeedIndex toBaudIndex(int32_t speed) -{ - using namespace Espfc; - if(speed >= SERIAL_SPEED_2470000) return SERIAL_SPEED_INDEX_2470000; - if(speed >= SERIAL_SPEED_2000000) return SERIAL_SPEED_INDEX_2000000; - if(speed >= SERIAL_SPEED_1500000) return SERIAL_SPEED_INDEX_1500000; - if(speed >= SERIAL_SPEED_1000000) return SERIAL_SPEED_INDEX_1000000; - if(speed >= SERIAL_SPEED_921600) return SERIAL_SPEED_INDEX_921600; - if(speed >= SERIAL_SPEED_500000) return SERIAL_SPEED_INDEX_500000; - if(speed >= SERIAL_SPEED_460800) return SERIAL_SPEED_INDEX_460800; - if(speed >= SERIAL_SPEED_400000) return SERIAL_SPEED_INDEX_400000; - if(speed >= SERIAL_SPEED_250000) return SERIAL_SPEED_INDEX_250000; - if(speed >= SERIAL_SPEED_230400) return SERIAL_SPEED_INDEX_230400; - if(speed >= SERIAL_SPEED_115200) return SERIAL_SPEED_INDEX_115200; - if(speed >= SERIAL_SPEED_57600) return SERIAL_SPEED_INDEX_57600; - if(speed >= SERIAL_SPEED_38400) return SERIAL_SPEED_INDEX_38400; - if(speed >= SERIAL_SPEED_19200) return SERIAL_SPEED_INDEX_19200; - if(speed >= SERIAL_SPEED_9600) return SERIAL_SPEED_INDEX_9600; - return SERIAL_SPEED_INDEX_AUTO; -} - -static Espfc::SerialSpeed fromBaudIndex(SerialSpeedIndex index) -{ - using namespace Espfc; - switch(index) - { - case SERIAL_SPEED_INDEX_9600: return SERIAL_SPEED_9600; - case SERIAL_SPEED_INDEX_19200: return SERIAL_SPEED_19200; - case SERIAL_SPEED_INDEX_38400: return SERIAL_SPEED_38400; - case SERIAL_SPEED_INDEX_57600: return SERIAL_SPEED_57600; - case SERIAL_SPEED_INDEX_115200: return SERIAL_SPEED_115200; - case SERIAL_SPEED_INDEX_230400: return SERIAL_SPEED_230400; - case SERIAL_SPEED_INDEX_250000: return SERIAL_SPEED_250000; - case SERIAL_SPEED_INDEX_400000: return SERIAL_SPEED_400000; - case SERIAL_SPEED_INDEX_460800: return SERIAL_SPEED_460800; - case SERIAL_SPEED_INDEX_500000: return SERIAL_SPEED_500000; - case SERIAL_SPEED_INDEX_921600: return SERIAL_SPEED_921600; - case SERIAL_SPEED_INDEX_1000000: return SERIAL_SPEED_1000000; - case SERIAL_SPEED_INDEX_1500000: return SERIAL_SPEED_1500000; - case SERIAL_SPEED_INDEX_2000000: return SERIAL_SPEED_2000000; - case SERIAL_SPEED_INDEX_2470000: return SERIAL_SPEED_2470000; - case SERIAL_SPEED_INDEX_AUTO: - default: - return SERIAL_SPEED_NONE; - } -} - -static uint8_t toFilterTypeDerivative(uint8_t t) -{ - switch(t) { - case 0: return Espfc::FILTER_NONE; - case 1: return Espfc::FILTER_PT3; - case 2: return Espfc::FILTER_BIQUAD; - default: return Espfc::FILTER_PT3; - } -} - -static uint8_t fromFilterTypeDerivative(uint8_t t) -{ - switch(t) { - case Espfc::FILTER_NONE: return 0; - case Espfc::FILTER_PT3: return 1; - case Espfc::FILTER_BIQUAD: return 2; - default: return 1; - } -} - -static uint8_t fromGyroDlpf(uint8_t t) -{ - switch(t) { - case Espfc::GYRO_DLPF_256: return 0; - case Espfc::GYRO_DLPF_EX: return 1; - default: return 2; - } -} - -static int8_t toVbatSource(uint8_t t) -{ - switch(t) { - case 0: return 0; // none - case 1: return 1; // internal adc - default: return 0; - } -} - -static int8_t toIbatSource(uint8_t t) -{ - switch(t) { - case 0: return 0; // none - case 1: return 1; // internal adc - default: return 0; - } -} - -static uint8_t toVbatVoltageLegacy(float voltage) -{ - return constrain(lrintf(voltage * 10.0f), 0, 255); -} - -static uint16_t toVbatVoltage(float voltage) -{ - return constrain(lrintf(voltage * 100.0f), 0, 32000); -} - -static uint16_t toIbatCurrent(float current) -{ - return constrain(lrintf(current * 100.0f), -32000, 32000); -} - -} - -#define MSP_PASSTHROUGH_ESC_4WAY 0xff - -namespace Espfc { - -namespace Connect { - -class MspProcessor -{ - public: - MspProcessor(Model& model): _model(model) {} - - bool parse(char c, MspMessage& msg) - { - _parser.parse(c, msg); - - if(msg.isReady()) debugMessage(msg); - - return !msg.isIdle(); - } - - void processCommand(MspMessage& m, MspResponse& r, Device::SerialDevice& s) - { - r.cmd = m.cmd; - r.version = m.version; - r.result = 1; - switch(m.cmd) - { - case MSP_API_VERSION: - r.writeU8(MSP_PROTOCOL_VERSION); - r.writeU8(API_VERSION_MAJOR); - r.writeU8(API_VERSION_MINOR); - break; - - case MSP_FC_VARIANT: - r.writeData(flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH); - break; - - case MSP_FC_VERSION: - r.writeU8(FC_VERSION_MAJOR); - r.writeU8(FC_VERSION_MINOR); - r.writeU8(FC_VERSION_PATCH_LEVEL); - break; - - case MSP_BOARD_INFO: - r.writeData(boardIdentifier, BOARD_IDENTIFIER_LENGTH); - r.writeU16(0); // No other build targets currently have hardware revision detection. - r.writeU8(0); // 0 == FC - r.writeU8(0); // target capabilities - r.writeU8(strlen(targetName)); // target name - r.writeData(targetName, strlen(targetName)); - r.writeU8(0); // board name - r.writeU8(0); // manufacturer name - for(size_t i = 0; i < 32; i++) r.writeU8(0); // signature - r.writeU8(255); // mcu id: unknown - // 1.42 - r.writeU8(2); // configuration state: configured - // 1.43 - r.writeU16(_model.state.gyro.present ? _model.state.gyro.timer.rate : 0); // sample rate - { - uint32_t problems = 0; - if(_model.state.accel.bias.x == 0 && _model.state.accel.bias.y == 0 && _model.state.accel.bias.z == 0) { - problems |= 1 << 0; // acc calibration required - } - if(_model.config.output.protocol == ESC_PROTOCOL_DISABLED) { - problems |= 1 << 1; // no motor protocol - } - r.writeU32(problems); // configuration problems - } - // 1.44 - r.writeU8(0); // spi dev count - r.writeU8(0); // i2c dev count - break; - - case MSP_BUILD_INFO: - r.writeData(buildDate, BUILD_DATE_LENGTH); - r.writeData(buildTime, BUILD_TIME_LENGTH); - r.writeData(shortGitRevision, GIT_SHORT_REVISION_LENGTH); - break; - - case MSP_UID: - r.writeU32(getBoardId0()); - r.writeU32(getBoardId1()); - r.writeU32(getBoardId2()); - break; - - case MSP_STATUS_EX: - case MSP_STATUS: - //r.writeU16(_model.state.loopTimer.delta); - r.writeU16(_model.state.stats.loopTime()); - r.writeU16(_model.state.i2cErrorCount); // i2c error count - // acc, baro, mag, gps, sonar, gyro - r.writeU16(_model.accelActive() | _model.baroActive() << 1 | _model.magActive() << 2 | 0 << 3 | 0 << 4 | _model.gyroActive() << 5); - r.writeU32(_model.state.mode.mask); // flight mode flags - r.writeU8(0); // pid profile - r.writeU16(lrintf(_model.state.stats.getCpuLoad())); - if (m.cmd == MSP_STATUS_EX) { - r.writeU8(1); // max profile count - r.writeU8(0); // current rate profile index - } else { // MSP_STATUS - //r.writeU16(_model.state.gyro.timer.interval); // gyro cycle time - r.writeU16(0); - } - - // flight mode flags (above 32 bits) - r.writeU8(0); // count - - // Write arming disable flags - r.writeU8(ARMING_DISABLED_FLAGS_COUNT); // 1 byte, flag count - r.writeU32(_model.state.mode.armingDisabledFlags); // 4 bytes, flags - r.writeU8(0); // reboot required - break; - - case MSP_NAME: - r.writeString(_model.config.modelName); - break; - - case MSP_SET_NAME: - memset(&_model.config.modelName, 0, MODEL_NAME_LEN + 1); - for(size_t i = 0; i < std::min((size_t)m.received, MODEL_NAME_LEN); i++) - { - _model.config.modelName[i] = m.readU8(); - } - break; - - case MSP_BOXNAMES: - r.writeString(F("ARM;ANGLE;AIRMODE;BEEPER;FAILSAFE;BLACKBOX;BLACKBOXERASE;")); - break; - - case MSP_BOXIDS: - r.writeU8(MODE_ARMED); - r.writeU8(MODE_ANGLE); - 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: - for(size_t i = 0; i < ACTUATOR_CONDITIONS; i++) - { - r.writeU8(_model.config.conditions[i].id); - r.writeU8(_model.config.conditions[i].ch - AXIS_AUX_1); - r.writeU8((_model.config.conditions[i].min - 900) / 25); - r.writeU8((_model.config.conditions[i].max - 900) / 25); - } - break; - - case MSP_MODE_RANGES_EXTRA: - r.writeU8(ACTUATOR_CONDITIONS); - for(size_t i = 0; i < ACTUATOR_CONDITIONS; i++) - { - r.writeU8(_model.config.conditions[i].id); - r.writeU8(_model.config.conditions[i].logicMode); - r.writeU8(_model.config.conditions[i].linkId); - } - - break; - - case MSP_SET_MODE_RANGE: - { - size_t i = m.readU8(); - if(i < ACTUATOR_CONDITIONS) - { - _model.config.conditions[i].id = m.readU8(); - _model.config.conditions[i].ch = m.readU8() + AXIS_AUX_1; - _model.config.conditions[i].min = m.readU8() * 25 + 900; - _model.config.conditions[i].max = m.readU8() * 25 + 900; - if(m.remain() >= 2) { - _model.config.conditions[i].logicMode = m.readU8(); // mode logic - _model.config.conditions[i].linkId = m.readU8(); // link to - } - } - else - { - r.result = -1; - } - } - break; - - case MSP_ANALOG: - r.writeU8(toVbatVoltageLegacy(_model.state.battery.voltage)); // voltage in 0.1V - r.writeU16(0); // mah drawn - r.writeU16(_model.getRssi()); // rssi - r.writeU16(toIbatCurrent(_model.state.battery.current)); // amperage in 0.01A - r.writeU16(toVbatVoltage(_model.state.battery.voltage)); // voltage in 0.01V - break; - - case MSP_FEATURE_CONFIG: - r.writeU32(_model.config.featureMask); - break; - - case MSP_SET_FEATURE_CONFIG: - _model.config.featureMask = m.readU32(); - _model.reload(); - break; - - case MSP_BATTERY_CONFIG: - r.writeU8(34); // vbatmincellvoltage - r.writeU8(42); // vbatmaxcellvoltage - r.writeU8((_model.config.vbat.cellWarning + 5) / 10); // vbatwarningcellvoltage - r.writeU16(0); // batteryCapacity - r.writeU8(_model.config.vbat.source); // voltageMeterSource - r.writeU8(_model.config.ibat.source); // currentMeterSource - r.writeU16(340); // vbatmincellvoltage - r.writeU16(420); // vbatmaxcellvoltage - r.writeU16(_model.config.vbat.cellWarning); // vbatwarningcellvoltage - break; - - case MSP_SET_BATTERY_CONFIG: - m.readU8(); // vbatmincellvoltage - m.readU8(); // vbatmaxcellvoltage - _model.config.vbat.cellWarning = m.readU8() * 10; // vbatwarningcellvoltage - m.readU16(); // batteryCapacity - _model.config.vbat.source = toVbatSource(m.readU8()); // voltageMeterSource - _model.config.ibat.source = toIbatSource(m.readU8()); // currentMeterSource - if(m.remain() >= 6) - { - m.readU16(); // vbatmincellvoltage - m.readU16(); // vbatmaxcellvoltage - _model.config.vbat.cellWarning = m.readU16(); - } - break; - - case MSP_BATTERY_STATE: - // battery characteristics - r.writeU8(_model.state.battery.cells); // cell count, 0 indicates battery not detected. - r.writeU16(0); // capacity in mAh - - // battery state - r.writeU8(toVbatVoltageLegacy(_model.state.battery.voltage)); // in 0.1V steps - r.writeU16(0); // milliamp hours drawn from battery - r.writeU16(toIbatCurrent(_model.state.battery.current)); // send current in 0.01 A steps, range is -320A to 320A - - // battery alerts - r.writeU8(0); - r.writeU16(toVbatVoltage(_model.state.battery.voltage)); // in 0.01 steps - break; - - case MSP_VOLTAGE_METERS: - for(int i = 0; i < 1; i++) - { - r.writeU8(i + 10); // meter id (10-19 vbat adc) - r.writeU8(toVbatVoltageLegacy(_model.state.battery.voltage)); // meter value - } - break; - - case MSP_CURRENT_METERS: - for(int i = 0; i < 1; i++) - { - r.writeU8(i + 10); // meter id (10-19 ibat adc) - r.writeU16(0); // mah drawn - r.writeU16(constrain(toIbatCurrent(_model.state.battery.current) * 10, 0, 0xffff)); // meter value - } - break; - - case MSP_VOLTAGE_METER_CONFIG: - r.writeU8(1); // num voltage sensors - for(int i = 0; i < 1; i++) - { - r.writeU8(5); // frame size (5) - r.writeU8(i + 10); // id (10-19 vbat adc) - r.writeU8(0); // type resistor divider - r.writeU8(_model.config.vbat.scale); // scale - r.writeU8(_model.config.vbat.resDiv); // resdivval - r.writeU8(_model.config.vbat.resMult); // resdivmultiplier - } - break; - - case MSP_SET_VOLTAGE_METER_CONFIG: - { - int id = m.readU8(); - if(id == 10 + 0) // id (10-19 vbat adc, allow only 10) - { - _model.config.vbat.scale = m.readU8(); - _model.config.vbat.resDiv = m.readU8(); - _model.config.vbat.resMult = m.readU8(); - } - } - break; - - case MSP_CURRENT_METER_CONFIG: - r.writeU8(1); // num voltage sensors - for(int i = 0; i < 1; i++) - { - r.writeU8(6); // frame size (6) - r.writeU8(i + 10); // id (10-19 ibat adc) - r.writeU8(1); // type adc - r.writeU16(_model.config.ibat.scale); // scale - r.writeU16(_model.config.ibat.offset); // offset - } - break; - - case MSP_SET_CURRENT_METER_CONFIG: - { - int id = m.readU8(); - if(id == 10 + 0) // id (10-19 ibat adc, allow only 10) - { - _model.config.ibat.scale = m.readU16(); - _model.config.ibat.offset = m.readU16(); - } - } - break; - - case MSP_DATAFLASH_SUMMARY: -#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: - r.writeU16(0); // pitch - r.writeU16(0); // roll - break; - - case MSP_MIXER_CONFIG: - r.writeU8(_model.config.mixer.type); // mixerMode, QUAD_X - r.writeU8(_model.config.mixer.yawReverse); // yaw_motors_reversed - break; - - case MSP_SET_MIXER_CONFIG: - _model.config.mixer.type = m.readU8(); // mixerMode, QUAD_X - _model.config.mixer.yawReverse = m.readU8(); // yaw_motors_reversed - break; - - case MSP_SENSOR_CONFIG: - r.writeU8(_model.config.accel.dev); // 3 acc mpu6050 - r.writeU8(_model.config.baro.dev); // 2 baro bmp085 - r.writeU8(_model.config.mag.dev); // 3 mag hmc5883l - break; - - case MSP_SET_SENSOR_CONFIG: - _model.config.accel.dev = m.readU8(); // 3 acc mpu6050 - _model.config.baro.dev = m.readU8(); // 2 baro bmp085 - _model.config.mag.dev = m.readU8(); // 3 mag hmc5883l - _model.reload(); - break; - - case MSP_SENSOR_ALIGNMENT: - r.writeU8(_model.config.gyro.align); // gyro align - r.writeU8(_model.config.gyro.align); // acc align, Starting with 4.0 gyro and acc alignment are the same - r.writeU8(_model.config.mag.align); // mag align - //1.41+ - r.writeU8(_model.state.gyro.present ? 1 : 0); // gyro detection mask GYRO_1_MASK - r.writeU8(0); // gyro_to_use - r.writeU8(_model.config.gyro.align); // gyro 1 - r.writeU8(0); // gyro 2 - break; - - case MSP_SET_SENSOR_ALIGNMENT: - { - uint8_t gyroAlign = m.readU8(); // gyro align - m.readU8(); // discard deprecated acc align - _model.config.mag.align = m.readU8(); // mag align - // API >= 1.41 - support the gyro_to_use and alignment for gyros 1 & 2 - if(m.remain() >= 3) - { - m.readU8(); // gyro_to_use - gyroAlign = m.readU8(); // gyro 1 align - m.readU8(); // gyro 2 align - } - _model.config.gyro.align = gyroAlign; - } - break; - - 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.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 - r.writeU8(0); // gps_baudrateIndex - r.writeU8(0); // telemetry_baudrateIndex - r.writeU8(toBaudIndex(_model.config.serial[i].blackboxBaud)); // blackbox_baudrateIndex - } - break; - - case MSP2_COMMON_SERIAL_CONFIG: - { - uint8_t count = 0; - for (int i = 0; i < SERIAL_UART_COUNT; i++) - { - 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.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 - r.writeU8(0); // gps_baudrateIndex - r.writeU8(0); // telemetry_baudrateIndex - r.writeU8(toBaudIndex(_model.config.serial[i].blackboxBaud)); // blackbox_baudrateIndex - } - } - break; - - case MSP_SET_CF_SERIAL_CONFIG: - { - const int packetSize = 1 + 2 + 4; - while(m.remain() >= packetSize) - { - int id = m.readU8(); - int k = _model.getSerialIndex((SerialPortId)id); - { - m.advance(packetSize - 1); - continue; - } - _model.config.serial[k].id = id; - _model.config.serial[k].functionMask = m.readU16(); - _model.config.serial[k].baud = fromBaudIndex((SerialSpeedIndex)m.readU8()); - m.readU8(); - m.readU8(); - _model.config.serial[k].blackboxBaud = fromBaudIndex((SerialSpeedIndex)m.readU8()); - } - } - _model.reload(); - break; - - case MSP2_COMMON_SET_SERIAL_CONFIG: - { - size_t count = m.readU8(); - (void)count; // ignore - const int packetSize = 1 + 4 + 4; - while(m.remain() >= packetSize) - { - int id = m.readU8(); - int k = _model.getSerialIndex((SerialPortId)id); - if(k == -1) - { - m.advance(packetSize - 1); - continue; - } - _model.config.serial[k].id = id; - _model.config.serial[k].functionMask = m.readU32(); - _model.config.serial[k].baud = fromBaudIndex((SerialSpeedIndex)m.readU8()); - m.readU8(); - m.readU8(); - _model.config.serial[k].blackboxBaud = fromBaudIndex((SerialSpeedIndex)m.readU8()); - } - } - _model.reload(); - break; - - case MSP_BLACKBOX_CONFIG: - r.writeU8(1); // Blackbox supported - r.writeU8(_model.config.blackbox.dev); // device serial or none - r.writeU8(1); // blackboxGetRateNum()); // unused - r.writeU8(1); // blackboxGetRateDenom()); - r.writeU16(_model.config.blackbox.pDenom);//blackboxGetPRatio()); // p_denom - //r.writeU8(_model.config.blackbox.pDenom); // sample_rate - //r.writeU32(~_model.config.blackbox.fieldsMask); - break; - - case MSP_SET_BLACKBOX_CONFIG: - // Don't allow config to be updated while Blackbox is logging - if (true) { - _model.config.blackbox.dev = m.readU8(); - const int rateNum = m.readU8(); // was rate_num - const int rateDenom = m.readU8(); // was rate_denom - uint16_t pRatio = 0; - if (m.remain() >= 2) { - pRatio = m.readU16(); // p_denom specified, so use it directly - } else { - // p_denom not specified in MSP, so calculate it from old rateNum and rateDenom - //pRatio = blackboxCalculatePDenom(rateNum, rateDenom); - (void)(rateNum + rateDenom); - } - _model.config.blackbox.pDenom = pRatio; - - /*if (m.remain() >= 1) { - _model.config.blackbox.pDenom = m.readU8(); - } else if(pRatio > 0) { - _model.config.blackbox.pDenom = blackboxCalculateSampleRate(pRatio); - //_model.config.blackbox.pDenom = pRatio; - } - if (m.remain() >= 4) { - _model.config.blackbox.fieldsMask = ~m.readU32(); - }*/ - } - break; - - case MSP_ATTITUDE: - r.writeU16(lrintf(Utils::toDeg(_model.state.attitude.euler.x) * 10.f)); // roll [decidegrees] - r.writeU16(lrintf(Utils::toDeg(_model.state.attitude.euler.y) * 10.f)); // pitch [decidegrees] - r.writeU16(lrintf(Utils::toDeg(-_model.state.attitude.euler.z))); // yaw [degrees] - break; - - case MSP_ALTITUDE: - r.writeU32(lrintf(_model.state.baro.altitude * 100.f)); // alt [cm] - r.writeU16(0); // vario - break; - - case MSP_BEEPER_CONFIG: - r.writeU32(~_model.config.buzzer.beeperMask); // beeper mask - r.writeU8(0); // dshot beacon tone - r.writeU32(0); // dshot beacon off flags - break; - - case MSP_SET_BEEPER_CONFIG: - _model.config.buzzer.beeperMask = ~m.readU32(); // beeper mask - break; - - case MSP_BOARD_ALIGNMENT_CONFIG: - r.writeU16(_model.config.boardAlignment[0]); // roll - r.writeU16(_model.config.boardAlignment[1]); // pitch - r.writeU16(_model.config.boardAlignment[2]); // yaw - break; - - case MSP_SET_BOARD_ALIGNMENT_CONFIG: - _model.config.boardAlignment[0] = m.readU16(); - _model.config.boardAlignment[1] = m.readU16(); - _model.config.boardAlignment[2] = m.readU16(); - break; - - case MSP_RX_MAP: - for(size_t i = 0; i < INPUT_CHANNELS; i++) - { - r.writeU8(_model.config.input.channel[i].map); - } - break; - - case MSP_SET_RX_MAP: - for(size_t i = 0; i < 8; i++) - { - _model.config.input.channel[i].map = m.readU8(); - } - break; - - case MSP_RSSI_CONFIG: - r.writeU8(_model.config.input.rssiChannel); - break; - - case MSP_SET_RSSI_CONFIG: - _model.config.input.rssiChannel = m.readU8(); - break; - - case MSP_MOTOR_CONFIG: - r.writeU16(_model.config.output.minThrottle); // minthrottle - r.writeU16(_model.config.output.maxThrottle); // maxthrottle - r.writeU16(_model.config.output.minCommand); // mincommand - r.writeU8(_model.state.currentMixer.count); // motor count - // 1.42+ - r.writeU8(_model.config.output.motorPoles); // motor pole count - r.writeU8(_model.config.output.dshotTelemetry); // dshot telemtery - r.writeU8(0); // esc sensor - break; - - case MSP_SET_MOTOR_CONFIG: - _model.config.output.minThrottle = m.readU16(); // minthrottle - _model.config.output.maxThrottle = m.readU16(); // maxthrottle - _model.config.output.minCommand = m.readU16(); // mincommand - if(m.remain() >= 2) - { -#ifdef ESPFC_DSHOT_TELEMETRY - _model.config.output.motorPoles = m.readU8(); - _model.config.output.dshotTelemetry = m.readU8(); -#else - m.readU8(); - m.readU8(); -#endif - } - _model.reload(); - break; - - case MSP_MOTOR_3D_CONFIG: - r.writeU16(1406); // deadband3d_low; - r.writeU16(1514); // deadband3d_high; - r.writeU16(1460); // neutral3d; - break; - - case MSP_ARMING_CONFIG: - r.writeU8(5); // auto_disarm delay - r.writeU8(0); // disarm kill switch - r.writeU8(180); // small angle - break; - - case MSP_RC_DEADBAND: - r.writeU8(_model.config.input.deadband); - r.writeU8(0); // yaw deadband - r.writeU8(0); // alt hold deadband - r.writeU16(0); // deadband 3d throttle - break; - - case MSP_SET_RC_DEADBAND: - _model.config.input.deadband = m.readU8(); - m.readU8(); // yaw deadband - m.readU8(); // alt hod deadband - m.readU16(); // deadband 3d throttle - break; - - case MSP_RX_CONFIG: - r.writeU8(_model.config.input.serialRxProvider); // serialrx_provider - r.writeU16(_model.config.input.maxCheck); //maxcheck - r.writeU16(_model.config.input.midRc); //midrc - r.writeU16(_model.config.input.minCheck); //mincheck - r.writeU8(0); // spectrum bind - r.writeU16(_model.config.input.minRc); //min_us - r.writeU16(_model.config.input.maxRc); //max_us - r.writeU8(_model.config.input.interpolationMode); // rc interpolation - r.writeU8(_model.config.input.interpolationInterval); // rc interpolation interval - r.writeU16(1500); // airmode activate threshold - r.writeU8(0); // rx spi prot - r.writeU32(0); // rx spi id - r.writeU8(0); // rx spi chan count - r.writeU8(0); // fpv camera angle - r.writeU8(2); // rc iterpolation channels: RPYT - r.writeU8(_model.config.input.filterType); // rc_smoothing_type - r.writeU8(_model.config.input.filter.freq); // rc_smoothing_input_cutoff - r.writeU8(_model.config.input.filterDerivative.freq); // rc_smoothing_derivative_cutoff - r.writeU8(0);//_model.config.input.filter.type); // rc_smoothing_input_type - r.writeU8(fromFilterTypeDerivative(_model.config.input.filterDerivative.type)); // rc_smoothing_derivative_type - r.writeU8(0); // usb type - // 1.42+ - r.writeU8(_model.config.input.filterAutoFactor); // rc_smoothing_auto_factor - break; - - case MSP_SET_RX_CONFIG: - _model.config.input.serialRxProvider = m.readU8(); // serialrx_provider - _model.config.input.maxCheck = m.readU16(); //maxcheck - _model.config.input.midRc = m.readU16(); //midrc - _model.config.input.minCheck = m.readU16(); //mincheck - m.readU8(); // spectrum bind - _model.config.input.minRc = m.readU16(); //min_us - _model.config.input.maxRc = m.readU16(); //max_us - if (m.remain() >= 4) { - _model.config.input.interpolationMode = m.readU8(); // rc interpolation - _model.config.input.interpolationInterval = m.readU8(); // rc interpolation interval - m.readU16(); // airmode activate threshold - } - if (m.remain() >= 6) { - m.readU8(); // rx spi prot - m.readU32(); // rx spi id - m.readU8(); // rx spi chan count - } - if (m.remain() >= 1) { - m.readU8(); // fpv camera angle - } - // 1.40+ - if (m.remain() >= 6) { - m.readU8(); // rc iterpolation channels - _model.config.input.filterType = m.readU8(); // rc_smoothing_type - _model.config.input.filter.freq = m.readU8(); // rc_smoothing_input_cutoff - _model.config.input.filterDerivative.freq = m.readU8(); // rc_smoothing_derivative_cutoff - //_model.config.input.filter.type = m.readU8() == 1 ? FILTER_BIQUAD : FILTER_PT1; // rc_smoothing_input_type - m.readU8(); - _model.config.input.filterDerivative.type = toFilterTypeDerivative(m.readU8()); // rc_smoothing_derivative_type - } - if (m.remain() >= 1) { - m.readU8(); // usb type - } - // 1.42+ - if (m.remain() >= 1) { - _model.config.input.filterAutoFactor = m.readU8(); // rc_smoothing_auto_factor - } - - _model.reload(); - break; - - case MSP_FAILSAFE_CONFIG: - r.writeU8(_model.config.failsafe.delay); // failsafe_delay - r.writeU8(0); // failsafe_off_delay - r.writeU16(1000); //failsafe_throttle - r.writeU8(_model.config.failsafe.killSwitch); // failsafe_kill_switch - r.writeU16(0); // failsafe_throttle_low_delay - r.writeU8(1); //failsafe_procedure; default drop - break; - - case MSP_SET_FAILSAFE_CONFIG: - _model.config.failsafe.delay = m.readU8(); //failsafe_delay - m.readU8(); //failsafe_off_delay - m.readU16(); //failsafe_throttle - _model.config.failsafe.killSwitch = m.readU8(); //failsafe_kill_switch - m.readU16(); //failsafe_throttle_low_delay - m.readU8(); //failsafe_procedure - break; - - case MSP_RXFAIL_CONFIG: - for (size_t i = 0; i < _model.state.input.channelCount; i++) - { - r.writeU8(_model.config.input.channel[i].fsMode); - r.writeU16(_model.config.input.channel[i].fsValue); - } - break; - - case MSP_SET_RXFAIL_CONFIG: - { - size_t i = m.readU8(); - if(i < INPUT_CHANNELS) - { - _model.config.input.channel[i].fsMode = m.readU8(); // mode - _model.config.input.channel[i].fsValue = m.readU16(); // pulse - } - else - { - r.result = -1; - } - } - break; - - case MSP_RC: - for(size_t i = 0; i < _model.state.input.channelCount; i++) - { - r.writeU16(lrintf(_model.state.input.us[i])); - } - break; - - case MSP_RC_TUNING: - r.writeU8(_model.config.input.rate[AXIS_ROLL]); - r.writeU8(_model.config.input.expo[AXIS_ROLL]); - for(size_t i = 0; i < AXIS_COUNT_RPY; i++) - { - r.writeU8(_model.config.input.superRate[i]); - } - r.writeU8(_model.config.controller.tpaScale); // dyn thr pid - r.writeU8(50); // thrMid8 - r.writeU8(0); // thr expo - r.writeU16(_model.config.controller.tpaBreakpoint); // tpa breakpoint - r.writeU8(_model.config.input.expo[AXIS_YAW]); // yaw expo - r.writeU8(_model.config.input.rate[AXIS_YAW]); // yaw rate - r.writeU8(_model.config.input.rate[AXIS_PITCH]); // pitch rate - r.writeU8(_model.config.input.expo[AXIS_PITCH]); // pitch expo - // 1.41+ - r.writeU8(_model.config.output.throttleLimitType); // throttle_limit_type (off) - r.writeU8(_model.config.output.throttleLimitPercent); // throtle_limit_percent (100%) - //1.42+ - r.writeU16(_model.config.input.rateLimit[0]); // rate limit roll - r.writeU16(_model.config.input.rateLimit[1]); // rate limit pitch - r.writeU16(_model.config.input.rateLimit[2]); // rate limit yaw - // 1.43+ - r.writeU8(_model.config.input.rateType); // rates type - - break; - - case MSP_SET_RC_TUNING: - if(m.remain() >= 10) - { - const uint8_t rate = m.readU8(); - if(_model.config.input.rate[AXIS_PITCH] == _model.config.input.rate[AXIS_ROLL]) - { - _model.config.input.rate[AXIS_PITCH] = rate; - } - _model.config.input.rate[AXIS_ROLL] = rate; - - const uint8_t expo = m.readU8(); - if(_model.config.input.expo[AXIS_PITCH] == _model.config.input.expo[AXIS_ROLL]) - { - _model.config.input.expo[AXIS_PITCH] = expo; - } - _model.config.input.expo[AXIS_ROLL] = expo; - - for(size_t i = 0; i < AXIS_COUNT_RPY; i++) - { - _model.config.input.superRate[i] = m.readU8(); - } - _model.config.controller.tpaScale = Utils::clamp(m.readU8(), (uint8_t)0, (uint8_t)90); // dyn thr pid - m.readU8(); // thrMid8 - m.readU8(); // thr expo - _model.config.controller.tpaBreakpoint = Utils::clamp(m.readU16(), (uint16_t)1000, (uint16_t)2000); // tpa breakpoint - if(m.remain() >= 1) - { - _model.config.input.expo[AXIS_YAW] = m.readU8(); // yaw expo - } - if(m.remain() >= 1) - { - _model.config.input.rate[AXIS_YAW] = m.readU8(); // yaw rate - } - if(m.remain() >= 1) - { - _model.config.input.rate[AXIS_PITCH] = m.readU8(); // pitch rate - } - if(m.remain() >= 1) - { - _model.config.input.expo[AXIS_PITCH] = m.readU8(); // pitch expo - } - // 1.41 - if(m.remain() >= 2) - { - _model.config.output.throttleLimitType = m.readU8(); // throttle_limit_type - _model.config.output.throttleLimitPercent = m.readU8(); // throttle_limit_percent - } - // 1.42 - if(m.remain() >= 6) - { - _model.config.input.rateLimit[0] = m.readU16(); // roll - _model.config.input.rateLimit[1] = m.readU16(); // pitch - _model.config.input.rateLimit[2] = m.readU16(); // yaw - } - // 1.43 - if (m.remain() >= 1) - { - _model.config.input.rateType = m.readU8(); - } - } - else - { - r.result = -1; - // error - } - break; - - case MSP_ADVANCED_CONFIG: - r.writeU8(1); // gyroSync unused - r.writeU8(_model.config.loopSync); - r.writeU8(_model.config.output.async); - r.writeU8(_model.config.output.protocol); - r.writeU16(_model.config.output.rate); - r.writeU16(_model.config.output.dshotIdle); - r.writeU8(0); // 32k gyro - r.writeU8(0); // PWM inversion - r.writeU8(0); // gyro_to_use: {1:0, 2:1. 2:both} - r.writeU8(0); // gyro high fsr (flase) - r.writeU8(48); // gyro cal threshold - r.writeU16(125); // gyro cal duration (1.25s) - r.writeU16(0); // gyro offset yaw - r.writeU8(0); // check overflow - r.writeU8(_model.config.debug.mode); - r.writeU8(DEBUG_COUNT); - break; - - case MSP_SET_ADVANCED_CONFIG: - m.readU8(); // ignore gyroSync, removed in 1.43 - _model.config.loopSync = m.readU8(); - _model.config.output.async = m.readU8(); - _model.config.output.protocol = m.readU8(); - _model.config.output.rate = m.readU16(); - if(m.remain() >= 2) { - _model.config.output.dshotIdle = m.readU16(); // dshot idle - } - if(m.remain()) { - m.readU8(); // 32k gyro - } - if(m.remain()) { - m.readU8(); // PWM inversion - } - if(m.remain() >= 8) { - m.readU8(); // gyro_to_use - m.readU8(); // gyro high fsr - m.readU8(); // gyro cal threshold - m.readU16(); // gyro cal duration - m.readU16(); // gyro offset yaw - m.readU8(); // check overflow - } - if(m.remain()) { - _model.config.debug.mode = m.readU8(); - } - _model.reload(); - break; - - case MSP_GPS_CONFIG: - r.writeU8(0); // provider - r.writeU8(0); // sbasMode - r.writeU8(0); // autoConfig - r.writeU8(0); // autoBaud - // 1.43+ - r.writeU8(0); // gps_set_home_point_once - r.writeU8(0); // gps_ublox_use_galileo - break; - - //case MSP_COMPASS_CONFIG: - // r.writeU16(0); // mag_declination * 10 - // break; - - case MSP_FILTER_CONFIG: - r.writeU8(_model.config.gyro.filter.freq); // gyro lpf - r.writeU16(_model.config.dterm.filter.freq); // dterm lpf - r.writeU16(_model.config.yaw.filter.freq); // yaw lpf - r.writeU16(_model.config.gyro.notch1Filter.freq); // gyro notch 1 hz - r.writeU16(_model.config.gyro.notch1Filter.cutoff); // gyro notch 1 cutoff - r.writeU16(_model.config.dterm.notchFilter.freq); // dterm notch hz - r.writeU16(_model.config.dterm.notchFilter.cutoff); // dterm notch cutoff - r.writeU16(_model.config.gyro.notch2Filter.freq); // gyro notch 2 hz - r.writeU16(_model.config.gyro.notch2Filter.cutoff); // gyro notch 2 cutoff - r.writeU8(_model.config.dterm.filter.type); // dterm type - r.writeU8(fromGyroDlpf(_model.config.gyro.dlpf)); - r.writeU8(0); // dlfp 32khz type - r.writeU16(_model.config.gyro.filter.freq); // lowpass1 freq - r.writeU16(_model.config.gyro.filter2.freq); // lowpass2 freq - r.writeU8(_model.config.gyro.filter.type); // lowpass1 type - r.writeU8(_model.config.gyro.filter2.type); // lowpass2 type - r.writeU16(_model.config.dterm.filter2.freq); // dterm lopwass2 freq - // 1.41+ - r.writeU8(_model.config.dterm.filter2.type); // dterm lopwass2 type - r.writeU16(_model.config.gyro.dynLpfFilter.cutoff); // dyn lpf gyro min - r.writeU16(_model.config.gyro.dynLpfFilter.freq); // dyn lpf gyro max - r.writeU16(_model.config.dterm.dynLpfFilter.cutoff); // dyn lpf dterm min - r.writeU16(_model.config.dterm.dynLpfFilter.freq); // dyn lpf dterm max - // gyro analyse - r.writeU8(3); // deprecated dyn notch range - r.writeU8(_model.config.gyro.dynamicFilter.count); // dyn_notch_width_percent - r.writeU16(_model.config.gyro.dynamicFilter.q); // dyn_notch_q - r.writeU16(_model.config.gyro.dynamicFilter.min_freq); // dyn_notch_min_hz - // rpm filter - r.writeU8(_model.config.gyro.rpmFilter.harmonics); // gyro_rpm_notch_harmonics - r.writeU8(_model.config.gyro.rpmFilter.minFreq); // gyro_rpm_notch_min - // 1.43+ - r.writeU16(_model.config.gyro.dynamicFilter.max_freq); // dyn_notch_max_hz - break; - - case MSP_SET_FILTER_CONFIG: - _model.config.gyro.filter.freq = m.readU8(); - _model.config.dterm.filter.freq = m.readU16(); - _model.config.yaw.filter.freq = m.readU16(); - if (m.remain() >= 8) { - _model.config.gyro.notch1Filter.freq = m.readU16(); - _model.config.gyro.notch1Filter.cutoff = m.readU16(); - _model.config.dterm.notchFilter.freq = m.readU16(); - _model.config.dterm.notchFilter.cutoff = m.readU16(); - } - if (m.remain() >= 4) { - _model.config.gyro.notch2Filter.freq = m.readU16(); - _model.config.gyro.notch2Filter.cutoff = m.readU16(); - } - if (m.remain() >= 1) { - _model.config.dterm.filter.type = (FilterType)m.readU8(); - } - if (m.remain() >= 10) { - m.readU8(); // dlfp type - m.readU8(); // 32k dlfp type - _model.config.gyro.filter.freq = m.readU16(); - _model.config.gyro.filter2.freq = m.readU16(); - _model.config.gyro.filter.type = m.readU8(); - _model.config.gyro.filter2.type = m.readU8(); - _model.config.dterm.filter2.freq = m.readU16(); - } - // 1.41+ - if (m.remain() >= 9) { - _model.config.dterm.filter2.type = m.readU8(); - _model.config.gyro.dynLpfFilter.cutoff = m.readU16(); // dyn gyro lpf min - _model.config.gyro.dynLpfFilter.freq = m.readU16(); // dyn gyro lpf max - _model.config.dterm.dynLpfFilter.cutoff = m.readU16(); // dyn dterm lpf min - _model.config.dterm.dynLpfFilter.freq = m.readU16(); // dyn dterm lpf min - } - if (m.remain() >= 8) { - m.readU8(); // deprecated dyn_notch_range - _model.config.gyro.dynamicFilter.count = m.readU8(); // dyn_notch_width_percent - _model.config.gyro.dynamicFilter.q = m.readU16(); // dyn_notch_q - _model.config.gyro.dynamicFilter.min_freq = m.readU16(); // dyn_notch_min_hz - _model.config.gyro.rpmFilter.harmonics = m.readU8(); // gyro_rpm_notch_harmonics - _model.config.gyro.rpmFilter.minFreq = m.readU8(); // gyro_rpm_notch_min - } - // 1.43+ - if (m.remain() >= 1) { - _model.config.gyro.dynamicFilter.max_freq = m.readU16(); // dyn_notch_max_hz - } - _model.reload(); - break; - - case MSP_PID_CONTROLLER: - r.writeU8(1); // betaflight controller id - break; - - case MSP_PIDNAMES: - r.writeString(F("ROLL;PITCH;YAW;ALT;Pos;PosR;NavR;LEVEL;MAG;VEL;")); - break; - - case MSP_PID: - for(size_t i = 0; i < PID_ITEM_COUNT; i++) - { - r.writeU8(_model.config.pid[i].P); - r.writeU8(_model.config.pid[i].I); - r.writeU8(_model.config.pid[i].D); - } - break; - - case MSP_SET_PID: - for (int i = 0; i < PID_ITEM_COUNT; i++) - { - _model.config.pid[i].P = m.readU8(); - _model.config.pid[i].I = m.readU8(); - _model.config.pid[i].D = m.readU8(); - } - _model.reload(); - break; - - case MSP_PID_ADVANCED: /// !!!FINISHED HERE!!! - r.writeU16(0); - r.writeU16(0); - r.writeU16(0); // was pidProfile.yaw_p_limit - r.writeU8(0); // reserved - r.writeU8(0); // vbatPidCompensation; - r.writeU8(0); // feedForwardTransition; - r.writeU8((uint8_t)std::min(_model.config.dterm.setpointWeight, (int16_t)255)); // was low byte of dtermSetpointWeight - r.writeU8(0); // reserved - r.writeU8(0); // reserved - r.writeU8(0); // reserved - r.writeU16(0); // rateAccelLimit; - r.writeU16(0); // yawRateAccelLimit; - r.writeU8(_model.config.level.angleLimit); // levelAngleLimit; - r.writeU8(0); // was pidProfile.levelSensitivity - r.writeU16(0); // itermThrottleThreshold; - r.writeU16(1000); // itermAcceleratorGain; anti_gravity_gain, 0 in 1.45+ - r.writeU16(_model.config.dterm.setpointWeight); - r.writeU8(0); // iterm rotation - r.writeU8(0); // smart feed forward - r.writeU8(_model.config.iterm.relax); // iterm relax - r.writeU8(1); // iterm relax type (setpoint only) - r.writeU8(0); // abs control gain - r.writeU8(0); // throttle boost - r.writeU8(0); // acro trainer max angle - r.writeU16(_model.config.pid[FC_PID_ROLL].F); //pid roll f - r.writeU16(_model.config.pid[FC_PID_PITCH].F); //pid pitch f - r.writeU16(_model.config.pid[FC_PID_YAW].F); //pid yaw f - r.writeU8(0); // antigravity mode - // 1.41+ - r.writeU8(0); // d min roll - r.writeU8(0); // d min pitch - r.writeU8(0); // d min yaw - r.writeU8(0); // d min gain - r.writeU8(0); // d min advance - r.writeU8(0); // use_integrated_yaw - r.writeU8(0); // integrated_yaw_relax - // 1.42+ - r.writeU8(_model.config.iterm.relaxCutoff); // iterm_relax_cutoff - // 1.43+ - r.writeU8(_model.config.output.motorLimit); // motor_output_limit - r.writeU8(0); // auto_profile_cell_count - r.writeU8(0); // idle_min_rpm - break; - - case MSP_SET_PID_ADVANCED: - m.readU16(); - m.readU16(); - m.readU16(); // was pidProfile.yaw_p_limit - m.readU8(); // reserved - m.readU8(); - m.readU8(); - _model.config.dterm.setpointWeight = m.readU8(); - m.readU8(); // reserved - m.readU8(); // reserved - m.readU8(); // reserved - m.readU16(); - m.readU16(); - if (m.remain() >= 2) { - _model.config.level.angleLimit = m.readU8(); - m.readU8(); // was pidProfile.levelSensitivity - } - if (m.remain() >= 4) { - m.readU16(); // itermThrottleThreshold; - m.readU16(); // itermAcceleratorGain; anti_gravity_gain - } - if (m.remain() >= 2) { - _model.config.dterm.setpointWeight = m.readU16(); - } - if (m.remain() >= 14) { - m.readU8(); //iterm rotation - m.readU8(); //smart feed forward - _model.config.iterm.relax = m.readU8(); //iterm relax - m.readU8(); //iterm relax type - m.readU8(); //abs control gain - m.readU8(); //throttle boost - m.readU8(); //acro trainer max angle - _model.config.pid[FC_PID_ROLL].F = m.readU16(); // pid roll f - _model.config.pid[FC_PID_PITCH].F = m.readU16(); // pid pitch f - _model.config.pid[FC_PID_YAW].F = m.readU16(); // pid yaw f - m.readU8(); //antigravity mode - } - // 1.41+ - if (m.remain() >= 7) { - m.readU8(); // d min roll - m.readU8(); // d min pitch - m.readU8(); // d min yaw - m.readU8(); // d min gain - m.readU8(); // d min advance - m.readU8(); // use_integrated_yaw - m.readU8(); // integrated_yaw_relax - } - // 1.42+ - if (m.remain() >= 1) { - _model.config.iterm.relaxCutoff = m.readU8(); // iterm_relax_cutoff - } - // 1.43+ - if (m.remain() >= 3) { - _model.config.output.motorLimit = m.readU8(); // motor_output_limit - m.readU8(); // auto_profile_cell_count - m.readU8(); // idle_min_rpm - } - _model.reload(); - break; - - case MSP_RAW_IMU: - for (int i = 0; i < AXIS_COUNT_RPY; i++) - { - r.writeU16(lrintf(_model.state.accel.adc[i] * ACCEL_G_INV * 2048.f)); - } - for (int i = 0; i < AXIS_COUNT_RPY; i++) - { - r.writeU16(lrintf(Utils::toDeg(_model.state.gyro.adc[i]))); - } - for (int i = 0; i < AXIS_COUNT_RPY; i++) - { - r.writeU16(lrintf(_model.state.mag.adc[i] * 1090)); - } - break; - - case MSP_MOTOR: - for (size_t i = 0; i < 8; i++) - { - if (i >= OUTPUT_CHANNELS || _model.config.pin[i + PIN_OUTPUT_0] == -1) - { - r.writeU16(0); - continue; - } - r.writeU16(_model.state.output.us[i]); - } - break; - - case MSP_MOTOR_TELEMETRY: - r.writeU8(OUTPUT_CHANNELS); - for (size_t i = 0; i < OUTPUT_CHANNELS; i++) - { - int rpm = 0; - uint16_t invalidPct = 0; - uint8_t escTemperature = 0; // degrees celcius - uint16_t escVoltage = 0; // 0.01V per unit - uint16_t escCurrent = 0; // 0.01A per unit - uint16_t escConsumption = 0; // mAh - - if (_model.config.pin[i + PIN_OUTPUT_0] != -1) - { - rpm = lrintf(_model.state.output.telemetry.rpm[i]); - invalidPct = _model.state.output.telemetry.errors[i]; - escTemperature = _model.state.output.telemetry.temperature[i]; - escVoltage = _model.state.output.telemetry.voltage[i]; - escCurrent = _model.state.output.telemetry.current[i]; - } - - r.writeU32(rpm); - r.writeU16(invalidPct); - r.writeU8(escTemperature); - r.writeU16(escVoltage); - r.writeU16(escCurrent); - r.writeU16(escConsumption); - } - break; - - case MSP_SET_MOTOR: - for(size_t i = 0; i < OUTPUT_CHANNELS; i++) - { - _model.state.output.disarmed[i] = m.readU16(); - } - break; - - case MSP_SERVO: - for(size_t i = 0; i < OUTPUT_CHANNELS; i++) - { - if (i >= OUTPUT_CHANNELS || _model.config.pin[i + PIN_OUTPUT_0] == -1) - { - r.writeU16(1500); - continue; - } - r.writeU16(_model.state.output.us[i]); - } - break; - - case MSP_SERVO_CONFIGURATIONS: - for(size_t i = 0; i < 8; i++) - { - if(i < OUTPUT_CHANNELS) - { - r.writeU16(_model.config.output.channel[i].min); - r.writeU16(_model.config.output.channel[i].max); - r.writeU16(_model.config.output.channel[i].neutral); - } - else - { - r.writeU16(1000); - r.writeU16(2000); - r.writeU16(1500); - } - r.writeU8(100); - r.writeU8(-1); - r.writeU32(0); - } - break; - - case MSP_SET_SERVO_CONFIGURATION: - { - uint8_t i = m.readU8(); - if(i < OUTPUT_CHANNELS) - { - _model.config.output.channel[i].min = m.readU16(); - _model.config.output.channel[i].max = m.readU16(); - _model.config.output.channel[i].neutral = m.readU16(); - m.readU8(); - m.readU8(); - m.readU32(); - } - else - { - r.result = -1; - } - } - break; - - case MSP_ACC_CALIBRATION: - if(!_model.isModeActive(MODE_ARMED)) _model.calibrateGyro(); - break; - - case MSP_MAG_CALIBRATION: - if(!_model.isModeActive(MODE_ARMED)) _model.calibrateMag(); - break; - - case MSP_VTX_CONFIG: - r.writeU8(0xff); // vtx type unknown - r.writeU8(0); // band - r.writeU8(0); // channel - r.writeU8(0); // power - r.writeU8(0); // status - r.writeU16(0); // freq - r.writeU8(0); // ready - r.writeU8(0); // low power disarm - // 1.42 - r.writeU16(0); // pit mode freq - r.writeU8(0); // vtx table available (no) - r.writeU8(0); // vtx table bands - r.writeU8(0); // vtx table channels - r.writeU8(0); // vtx power levels - break; - - case MSP_SET_ARMING_DISABLED: - { - const uint8_t cmd = m.readU8(); - uint8_t disableRunawayTakeoff = 0; - if(m.remain()) { - disableRunawayTakeoff = m.readU8(); - } - (void)disableRunawayTakeoff; - _model.setArmingDisabled(ARMING_DISABLED_MSP, cmd); - if (_model.isModeActive(MODE_ARMED)) _model.disarm(DISARM_REASON_ARMING_DISABLED); - } - break; - - case MSP_SET_PASSTHROUGH: - { - uint8_t ptMode = MSP_PASSTHROUGH_ESC_4WAY; - uint8_t ptArg = 0; - if(m.remain() >= 2) { - ptMode = m.readU8(); - ptArg = m.readU8(); - } - switch (ptMode) - { - case MSP_PASSTHROUGH_ESC_4WAY: - r.writeU8(esc4wayInit()); - serialDeviceInit(&s, 0); - _postCommand = std::bind(&MspProcessor::processEsc4way, this); - break; - default: - r.writeU8(0); - break; - } - (void)ptArg; - } - break; - - case MSP_DEBUG: - for (int i = 0; i < 4; i++) { - r.writeU16(_model.state.debug[i]); - } - break; - - case MSP_EEPROM_WRITE: - _model.save(); - break; - - case MSP_RESET_CONF: - if(!_model.isModeActive(MODE_ARMED)) - { - _model.reset(); - } - break; - - case MSP_REBOOT: - r.writeU8(0); // reboot to firmware - _postCommand = std::bind(&MspProcessor::processRestart, this); - break; - - default: - r.result = 0; - break; - } - } - - void processEsc4way() - { -#if defined(ESPFC_MULTI_CORE) && defined(ESPFC_FREE_RTOS) - timer_pause(TIMER_GROUP_0, TIMER_0); -#endif - esc4wayProcess(getSerialPort()); - processRestart(); - } - - void processRestart() - { - 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); - size_t len = r.serialize(_buff, 256); - s.write(_buff, len); - } - - void postCommand() - { - if(!_postCommand) return; - std::function cb = _postCommand; - _postCommand = {}; - cb(); - } - - bool debugSkip(uint8_t cmd) - { - //return true; - //return false; - if(cmd == MSP_STATUS) return true; - if(cmd == MSP_STATUS_EX) return true; - if(cmd == MSP_BOXNAMES) return true; - if(cmd == MSP_ANALOG) return true; - if(cmd == MSP_ATTITUDE) return true; - if(cmd == MSP_ALTITUDE) return true; - if(cmd == MSP_RC) return true; - if(cmd == MSP_RAW_IMU) return true; - if(cmd == MSP_MOTOR) return true; - if(cmd == MSP_SERVO) return true; - if(cmd == MSP_BATTERY_STATE) return true; - if(cmd == MSP_VOLTAGE_METERS) return true; - if(cmd == MSP_CURRENT_METERS) return true; - return false; - } - - void debugMessage(const MspMessage& m) - { - if(debugSkip(m.cmd)) return; - Device::SerialDevice * s = _model.getSerialStream(SERIAL_FUNCTION_TELEMETRY_HOTT); - if(!s) return; - - 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], HEX); s->print(' '); - } - s->println(); - } - - void debugResponse(const MspResponse& r) - { - if(debugSkip(r.cmd)) return; - Device::SerialDevice * s = _model.getSerialStream(SERIAL_FUNCTION_TELEMETRY_HOTT); - if(!s) return; - - 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], HEX); s->print(' '); - } - s->println(); - } - - private: - Model& _model; - MspParser _parser; - std::function _postCommand; - uint8_t _buff[256]; -}; - -} - -} - -#endif diff --git a/lib/Espfc/src/Connect/MspProcessor.hpp b/lib/Espfc/src/Connect/MspProcessor.hpp new file mode 100644 index 00000000..d63eb118 --- /dev/null +++ b/lib/Espfc/src/Connect/MspProcessor.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include "Model.h" +#include "Connect/Msp.hpp" +#include "Connect/MspParser.hpp" +#include "Device/SerialDevice.h" +#include + +namespace Espfc { + +namespace Connect { + +class MspProcessor +{ +public: + MspProcessor(Model& model); + bool parse(char c, MspMessage& msg); + void processCommand(MspMessage& m, MspResponse& r, Device::SerialDevice& s); + void processEsc4way(); + void processRestart(); + void serializeFlashData(MspResponse& r, uint32_t address, const uint16_t size, bool useLegacyFormat, bool allowCompression); + + void sendResponse(MspResponse& r, Device::SerialDevice& s); + void postCommand(); + bool debugSkip(uint8_t cmd); + void debugMessage(const MspMessage& m); + void debugResponse(const MspResponse& r); + +private: + Model& _model; + MspParser _parser; + std::function _postCommand; +}; + +} + +} diff --git a/lib/Espfc/src/Hal/Pgm.h b/lib/Espfc/src/Hal/Pgm.h index f613b19f..1f1590e8 100644 --- a/lib/Espfc/src/Hal/Pgm.h +++ b/lib/Espfc/src/Hal/Pgm.h @@ -22,3 +22,12 @@ #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) #endif // UNIT_TEST + +#ifdef ARCH_RP2040 +namespace arduino { +class __FlashStringHelper; +} +using __FlashStringHelper = arduino::__FlashStringHelper; +#else +class __FlashStringHelper; +#endif diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 4990a04c..0f38d7cb 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -13,7 +13,7 @@ #include "Utils/Timer.h" #include "Utils/Stats.h" #include "Device/SerialDevice.h" -#include "Connect/Msp.h" +#include "Connect/Msp.hpp" namespace Espfc { diff --git a/lib/Espfc/src/Rc/Crsf.h b/lib/Espfc/src/Rc/Crsf.h index e8bc2d20..a3768ba1 100644 --- a/lib/Espfc/src/Rc/Crsf.h +++ b/lib/Espfc/src/Rc/Crsf.h @@ -3,7 +3,7 @@ #include #include #include "Utils/Crc.hpp" -#include "Connect/Msp.h" +#include "Connect/Msp.hpp" namespace Espfc { diff --git a/lib/Espfc/src/SerialManager.h b/lib/Espfc/src/SerialManager.h index 7e7f6c6d..fff321bc 100644 --- a/lib/Espfc/src/SerialManager.h +++ b/lib/Espfc/src/SerialManager.h @@ -2,7 +2,7 @@ #include "Model.h" #include "Device/SerialDevice.h" -#include "Connect/MspProcessor.h" +#include "Connect/MspProcessor.hpp" #include "Connect/Cli.h" #include "TelemetryManager.h" #ifdef ESPFC_SERIAL_SOFT_0_WIFI diff --git a/lib/Espfc/src/TelemetryManager.h b/lib/Espfc/src/TelemetryManager.h index 881ed9c3..f06db65b 100644 --- a/lib/Espfc/src/TelemetryManager.h +++ b/lib/Espfc/src/TelemetryManager.h @@ -4,8 +4,8 @@ #include "Device/SerialDevice.h" #include "Telemetry/TelemetryText.h" #include "Telemetry/TelemetryCRSF.h" -#include "Connect/Msp.h" -#include "Connect/MspProcessor.h" +#include "Connect/Msp.hpp" +#include "Connect/MspProcessor.hpp" namespace Espfc { diff --git a/test/test_msp/test_msp.cpp b/test/test_msp/test_msp.cpp index 1d705dc2..68688bc1 100644 --- a/test/test_msp/test_msp.cpp +++ b/test/test_msp/test_msp.cpp @@ -5,8 +5,9 @@ #include #include #include -#include "Connect/Msp.h" -#include "Connect/MspParser.h" +#include "Connect/Msp.hpp" +#include "Connect/MspParser.hpp" +#include "msp/msp_protocol.h" using namespace fakeit; using namespace Espfc; From c33c2e5957768a15259bc0a102e7703fda129591 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 14 Nov 2024 00:10:49 +0100 Subject: [PATCH 60/68] msp pragma once --- lib/Espfc/src/Connect/MspParser.hpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/lib/Espfc/src/Connect/MspParser.hpp b/lib/Espfc/src/Connect/MspParser.hpp index 751853b6..f48e3ad8 100644 --- a/lib/Espfc/src/Connect/MspParser.hpp +++ b/lib/Espfc/src/Connect/MspParser.hpp @@ -1,5 +1,4 @@ -#ifndef _ESPFC_MSP_PARSER_H_ -#define _ESPFC_MSP_PARSER_H_ +#pragma once #include "Connect/Msp.hpp" @@ -17,5 +16,3 @@ class MspParser } } - -#endif From e8c84df0b3a8fadeca3194079dee5d7be5c1100e Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 14 Nov 2024 21:21:25 +0100 Subject: [PATCH 61/68] split cli module --- lib/Espfc/src/Connect/Cli.cpp | 1492 +++++++++++++++++++++++++++++++ lib/Espfc/src/Connect/Cli.h | 1566 --------------------------------- lib/Espfc/src/Connect/Cli.hpp | 107 +++ lib/Espfc/src/SerialManager.h | 2 +- 4 files changed, 1600 insertions(+), 1567 deletions(-) create mode 100644 lib/Espfc/src/Connect/Cli.cpp delete mode 100644 lib/Espfc/src/Connect/Cli.h create mode 100644 lib/Espfc/src/Connect/Cli.hpp diff --git a/lib/Espfc/src/Connect/Cli.cpp b/lib/Espfc/src/Connect/Cli.cpp new file mode 100644 index 00000000..f852ecae --- /dev/null +++ b/lib/Espfc/src/Connect/Cli.cpp @@ -0,0 +1,1492 @@ +#include "Connect/Cli.hpp" +#include +#include +#include "Hardware.h" +#include "Device/GyroDevice.h" +#include "Hal/Pgm.h" +#include "msp/msp_protocol.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 +#endif + +namespace Espfc { + +namespace Connect { + +void Cli::Param::print(Stream& stream) const +{ + if(!addr) + { + stream.print(F("UNSET")); + return; + } + switch(type) + { + case PARAM_NONE: + stream.print(F("NONE")); + break; + case PARAM_BOOL: + stream.print(*addr != 0); + break; + case PARAM_BYTE: + print(stream, *reinterpret_cast(addr)); + break; + case PARAM_BYTE_U: + print(stream, *reinterpret_cast(addr)); + break; + case PARAM_SHORT: + print(stream, *reinterpret_cast(addr)); + break; + case PARAM_INT: + print(stream, *reinterpret_cast(addr)); + break; + case PARAM_FLOAT: + stream.print(*reinterpret_cast(addr), 4); + break; + case PARAM_STRING: + stream.print(addr); + break; + case PARAM_BITMASK: + stream.print((*reinterpret_cast(addr) & (1ul << maxLen)) ? 1 : 0); + break; + case PARAM_INPUT_CHANNEL: + print(stream, *reinterpret_cast(addr)); + break; + case PARAM_OUTPUT_CHANNEL: + print(stream, *reinterpret_cast(addr)); + break; + case PARAM_SCALER: + print(stream, *reinterpret_cast(addr)); + break; + case PARAM_MODE: + print(stream, *reinterpret_cast(addr)); + break; + case PARAM_MIXER: + print(stream, *reinterpret_cast(addr)); + break; + case PARAM_SERIAL: + print(stream, *reinterpret_cast(addr)); + break; + } +} + +void Cli::Param::print(Stream& stream, const OutputChannelConfig& och) const +{ + stream.print(och.servo ? 'S' : 'M'); + stream.print(' '); + stream.print(och.reverse ? 'R' : 'N'); + stream.print(' '); + stream.print(och.min); + stream.print(' '); + stream.print(och.neutral); + stream.print(' '); + stream.print(och.max); +} + +void Cli::Param::print(Stream& stream, const InputChannelConfig& ich) const +{ + stream.print(ich.map); + stream.print(' '); + stream.print(ich.min); + stream.print(' '); + stream.print(ich.neutral); + stream.print(' '); + stream.print(ich.max); + stream.print(' '); + stream.print(ich.fsMode == 0 ? 'A' : (ich.fsMode == 1 ? 'H' : (ich.fsMode == 2 ? 'S' : '?'))); + stream.print(' '); + stream.print(ich.fsValue); +} + +void Cli::Param::print(Stream& stream, const ScalerConfig& sc) const +{ + stream.print(sc.dimension); + stream.print(' '); + stream.print(sc.channel); + stream.print(' '); + stream.print(sc.minScale); + stream.print(' '); + stream.print(sc.maxScale); +} + +void Cli::Param::print(Stream& stream, const ActuatorCondition& ac) const +{ + stream.print(ac.id); + stream.print(' '); + stream.print(ac.ch); + stream.print(' '); + stream.print(ac.min); + stream.print(' '); + stream.print(ac.max); + stream.print(' '); + stream.print(ac.logicMode); + stream.print(' '); + stream.print(ac.linkId); +} + +void Cli::Param::print(Stream& stream, const MixerEntry& me) const +{ + stream.print(me.src); + stream.print(' '); + stream.print(me.dst); + stream.print(' '); + stream.print(me.rate); +} + +void Cli::Param::print(Stream& stream, const SerialPortConfig& sc) const +{ + stream.print(sc.functionMask); + stream.print(' '); + stream.print(sc.baud); + stream.print(' '); + stream.print(sc.blackboxBaud); +} + +void Cli::Param::print(Stream& stream, int32_t v) const +{ + if(choices) + { + for(int32_t i = 0; choices[i]; i++) + { + if(i == v) + { + stream.print(FPSTR(choices[i])); + return; + } + } + } + stream.print(v); +} + +void Cli::Param::update(const char ** args) const +{ + const char * v = args[2]; + 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 ? v : "")); + break; + case PARAM_BITMASK: + if(!v) return; + if(*v == '0') + { + *reinterpret_cast(addr) &= ~(1ul << maxLen); + } + if(*v == '1') + { + *reinterpret_cast(addr) |= (1ul << maxLen); + } + 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: + break; + } +} + +void Cli::Param::write(OutputChannelConfig& och, const char ** args) const +{ + if(args[2]) och.servo = *args[2] == 'S'; + if(args[3]) och.reverse = *args[3] == 'R'; + if(args[4]) och.min = String(args[4]).toInt(); + if(args[5]) och.neutral = String(args[5]).toInt(); + if(args[6]) och.max = String(args[6]).toInt(); +} + +void Cli::Param::write(InputChannelConfig& ich, const char ** args) const +{ + if(args[2]) ich.map = String(args[2]).toInt(); + if(args[3]) ich.min = String(args[3]).toInt(); + if(args[4]) ich.neutral = String(args[4]).toInt(); + if(args[5]) ich.max = String(args[5]).toInt(); + if(args[6]) ich.fsMode = *args[6] == 'A' ? 0 : (*args[6] == 'H' ? 1 : (*args[6] == 'S' ? 2 : 0)); + if(args[7]) ich.fsValue = String(args[7]).toInt(); +} + +void Cli::Param::write(ScalerConfig& sc, const char ** args) const +{ + if(args[2]) sc.dimension = (ScalerDimension)String(args[2]).toInt(); + if(args[3]) sc.channel = String(args[3]).toInt(); + if(args[4]) sc.minScale = String(args[4]).toInt(); + if(args[5]) sc.maxScale = String(args[5]).toInt(); +} + +void Cli::Param::write(ActuatorCondition& ac, const char ** args) const +{ + if(args[2]) ac.id = String(args[2]).toInt(); + if(args[3]) ac.ch = String(args[3]).toInt(); + if(args[4]) ac.min = String(args[4]).toInt(); + if(args[5]) ac.max = String(args[5]).toInt(); + if(args[6]) ac.max = String(args[6]).toInt(); + if(args[7]) ac.max = String(args[7]).toInt(); +} + +void Cli::Param::write(MixerEntry& ac, const char ** args) const +{ + if(args[2]) ac.src = constrain(String(args[2]).toInt(), 0, MIXER_SOURCE_MAX - 1); + if(args[3]) ac.dst = constrain(String(args[3]).toInt(), 0, (int)(OUTPUT_CHANNELS - 1)); + if(args[4]) ac.rate = constrain(String(args[4]).toInt(), -1000, 1000); +} + +void Cli::Param::write(SerialPortConfig& sc, const char ** args) const +{ + if(args[2]) sc.functionMask = String(args[2]).toInt(); + if(args[3]) sc.baud = String(args[3]).toInt(); + if(args[4]) sc.blackboxBaud = String(args[4]).toInt(); +} + +void Cli::Param::write(const String& v) const +{ + *addr = 0; + strncat(addr, v.c_str(), maxLen); +} + +int32_t Cli::Param::parse(const char * v) const +{ + if(choices) + { + for(int32_t i = 0; choices[i]; i++) + { + if(strcasecmp_P(v, choices[i]) == 0) return i; + } + } + String tmp = v; + return tmp.toInt(); +} + +Cli::Cli(Model& model): _model(model), _ignore(false), _active(false) +{ + _params = initialize(_model.config); +} + +const Cli::Param * Cli::initialize(ModelConfig& c) +{ + const char ** busDevChoices = Device::BusDevice::getNames(); + const char ** gyroDevChoices = Device::GyroDevice::getNames(); + const char ** baroDevChoices = Device::BaroDevice::getNames(); + const char ** magDevChoices = Device::MagDevice::getNames(); + + const char ** fusionModeChoices = FusionConfig::getModeNames(); + static const char* gyroDlpfChoices[] = { PSTR("256Hz"), PSTR("188Hz"), PSTR("98Hz"), PSTR("42Hz"), PSTR("20Hz"), PSTR("10Hz"), PSTR("5Hz"), PSTR("EXPERIMENTAL"), NULL }; + static const char* debugModeChoices[] = { PSTR("NONE"), PSTR("CYCLETIME"), PSTR("BATTERY"), PSTR("GYRO_FILTERED"), PSTR("ACCELEROMETER"), PSTR("PIDLOOP"), PSTR("GYRO_SCALED"), PSTR("RC_INTERPOLATION"), + PSTR("ANGLERATE"), PSTR("ESC_SENSOR"), PSTR("SCHEDULER"), PSTR("STACK"), PSTR("ESC_SENSOR_RPM"), PSTR("ESC_SENSOR_TMP"), PSTR("ALTITUDE"), PSTR("FFT"), + PSTR("FFT_TIME"), PSTR("FFT_FREQ"), PSTR("RX_FRSKY_SPI"), PSTR("RX_SFHSS_SPI"), PSTR("GYRO_RAW"), PSTR("DUAL_GYRO_RAW"), PSTR("DUAL_GYRO_DIFF"), + PSTR("MAX7456_SIGNAL"), PSTR("MAX7456_SPICLOCK"), PSTR("SBUS"), PSTR("FPORT"), PSTR("RANGEFINDER"), PSTR("RANGEFINDER_QUALITY"), PSTR("LIDAR_TF"), + PSTR("ADC_INTERNAL"), PSTR("RUNAWAY_TAKEOFF"), PSTR("SDIO"), PSTR("CURRENT_SENSOR"), PSTR("USB"), PSTR("SMARTAUDIO"), PSTR("RTH"), PSTR("ITERM_RELAX"), + PSTR("ACRO_TRAINER"), PSTR("RC_SMOOTHING"), PSTR("RX_SIGNAL_LOSS"), PSTR("RC_SMOOTHING_RATE"), PSTR("ANTI_GRAVITY"), PSTR("DYN_LPF"), PSTR("RX_SPEKTRUM_SPI"), + PSTR("DSHOT_RPM_TELEMETRY"), PSTR("RPM_FILTER"), PSTR("D_MIN"), PSTR("AC_CORRECTION"), PSTR("AC_ERROR"), PSTR("DUAL_GYRO_SCALED"), PSTR("DSHOT_RPM_ERRORS"), + PSTR("CRSF_LINK_STATISTICS_UPLINK"), PSTR("CRSF_LINK_STATISTICS_PWR"), PSTR("CRSF_LINK_STATISTICS_DOWN"), PSTR("BARO"), PSTR("GPS_RESCUE_THROTTLE_PID"), + PSTR("DYN_IDLE"), PSTR("FF_LIMIT"), PSTR("FF_INTERPOLATED"), PSTR("BLACKBOX_OUTPUT"), PSTR("GYRO_SAMPLE"), PSTR("RX_TIMING"), NULL }; + static const char* filterTypeChoices[] = { PSTR("PT1"), PSTR("BIQUAD"), PSTR("PT2"), PSTR("PT3"), PSTR("NOTCH"), PSTR("NOTCH_DF1"), PSTR("BPF"), PSTR("FO"), PSTR("FIR2"), PSTR("MEDIAN3"), PSTR("NONE"), NULL }; + static const char* alignChoices[] = { PSTR("DEFAULT"), PSTR("CW0"), PSTR("CW90"), PSTR("CW180"), PSTR("CW270"), PSTR("CW0_FLIP"), PSTR("CW90_FLIP"), PSTR("CW180_FLIP"), PSTR("CW270_FLIP"), PSTR("CUSTOM"), NULL }; + static const char* mixerTypeChoices[] = { PSTR("NONE"), PSTR("TRI"), PSTR("QUADP"), PSTR("QUADX"), PSTR("BI"), + PSTR("GIMBAL"), PSTR("Y6"), PSTR("HEX6"), PSTR("FWING"), PSTR("Y4"), + PSTR("HEX6X"), PSTR("OCTOX8"), PSTR("OCTOFLATP"), PSTR("OCTOFLATX"), PSTR("AIRPLANE"), + PSTR("HELI120"), PSTR("HELI90"), PSTR("VTAIL4"), PSTR("HEX6H"), PSTR("PPMSERVO"), + PSTR("DUALCOPTER"), PSTR("SINGLECOPTER"), PSTR("ATAIL4"), PSTR("CUSTOM"), PSTR("CUSTOMAIRPLANE"), + PSTR("CUSTOMTRI"), PSTR("QUADX1234"), NULL }; + static const char* interpolChoices[] = { PSTR("NONE"), PSTR("DEFAULT"), PSTR("AUTO"), PSTR("MANUAL"), NULL }; + static const char* protocolChoices[] = { PSTR("PWM"), PSTR("ONESHOT125"), PSTR("ONESHOT42"), PSTR("MULTISHOT"), PSTR("BRUSHED"), + PSTR("DSHOT150"), PSTR("DSHOT300"), PSTR("DSHOT600"), PSTR("PROSHOT1000"), PSTR("DISABLED"), NULL }; + static const char* inputRateTypeChoices[] = { PSTR("BETAFLIGHT"), PSTR("RACEFLIGHT"), PSTR("KISS"), PSTR("ACTUAL"), PSTR("QUICK"), NULL }; + static const char* throtleLimitTypeChoices[] = { PSTR("NONE"), PSTR("SCALE"), PSTR("CLIP"), NULL }; + static const char* inputFilterChoices[] = { PSTR("INTERPOLATION"), PSTR("FILTER"), NULL }; + static const char* inputItermRelaxChoices[] = { PSTR("OFF"), PSTR("RP"), PSTR("RPY"), PSTR("RP_INC"), PSTR("RPY_INC"), NULL }; + + static const char* voltageSourceChoices[] = { PSTR("NONE"), PSTR("ADC"), NULL }; + static const char* currentSourceChoices[] = { PSTR("NONE"), PSTR("ADC"), NULL }; + static const char* blackboxDevChoices[] = { PSTR("NONE"), PSTR("FLASH"), PSTR("SD_CARD"), PSTR("SERIAL"), NULL }; + static const char* blackboxModeChoices[] = { PSTR("NORMAL"), PSTR("TEST"), PSTR("ALWAYS"), NULL }; + + size_t i = 0; + static const Param params[] = { + + Param(PSTR("feature_dyn_notch"), &c.featureMask, 29), + Param(PSTR("feature_motor_stop"), &c.featureMask, 4), + Param(PSTR("feature_rx_ppm"), &c.featureMask, 0), + Param(PSTR("feature_rx_serial"), &c.featureMask, 3), + Param(PSTR("feature_rx_spi"), &c.featureMask, 25), + Param(PSTR("feature_soft_serial"), &c.featureMask, 6), + Param(PSTR("feature_telemetry"), &c.featureMask, 10), + + Param(PSTR("debug_mode"), &c.debug.mode, debugModeChoices), + Param(PSTR("debug_axis"), &c.debug.axis), + + Param(PSTR("gyro_bus"), &c.gyro.bus, busDevChoices), + Param(PSTR("gyro_dev"), &c.gyro.dev, gyroDevChoices), + Param(PSTR("gyro_dlpf"), &c.gyro.dlpf, gyroDlpfChoices), + Param(PSTR("gyro_align"), &c.gyro.align, alignChoices), + Param(PSTR("gyro_lpf_type"), &c.gyro.filter.type, filterTypeChoices), + Param(PSTR("gyro_lpf_freq"), &c.gyro.filter.freq), + Param(PSTR("gyro_lpf2_type"), &c.gyro.filter2.type, filterTypeChoices), + Param(PSTR("gyro_lpf2_freq"), &c.gyro.filter2.freq), + Param(PSTR("gyro_lpf3_type"), &c.gyro.filter3.type, filterTypeChoices), + Param(PSTR("gyro_lpf3_freq"), &c.gyro.filter3.freq), + Param(PSTR("gyro_notch1_freq"), &c.gyro.notch1Filter.freq), + Param(PSTR("gyro_notch1_cutoff"), &c.gyro.notch1Filter.cutoff), + Param(PSTR("gyro_notch2_freq"), &c.gyro.notch2Filter.freq), + Param(PSTR("gyro_notch2_cutoff"), &c.gyro.notch2Filter.cutoff), + Param(PSTR("gyro_dyn_lpf_min"), &c.gyro.dynLpfFilter.cutoff), + Param(PSTR("gyro_dyn_lpf_max"), &c.gyro.dynLpfFilter.freq), + Param(PSTR("gyro_dyn_notch_q"), &c.gyro.dynamicFilter.q), + Param(PSTR("gyro_dyn_notch_count"), &c.gyro.dynamicFilter.count), + Param(PSTR("gyro_dyn_notch_min"), &c.gyro.dynamicFilter.min_freq), + Param(PSTR("gyro_dyn_notch_max"), &c.gyro.dynamicFilter.max_freq), + Param(PSTR("gyro_rpm_harmonics"), &c.gyro.rpmFilter.harmonics), + Param(PSTR("gyro_rpm_q"), &c.gyro.rpmFilter.q), + Param(PSTR("gyro_rpm_min_freq"), &c.gyro.rpmFilter.minFreq), + Param(PSTR("gyro_rpm_fade"), &c.gyro.rpmFilter.fade), + Param(PSTR("gyro_rpm_weight_1"), &c.gyro.rpmFilter.weights[0]), + Param(PSTR("gyro_rpm_weight_2"), &c.gyro.rpmFilter.weights[1]), + Param(PSTR("gyro_rpm_weight_3"), &c.gyro.rpmFilter.weights[2]), + Param(PSTR("gyro_rpm_tlm_lpf_freq"), &c.gyro.rpmFilter.freqLpf), + Param(PSTR("gyro_offset_x"), &c.gyro.bias[0]), + Param(PSTR("gyro_offset_y"), &c.gyro.bias[1]), + Param(PSTR("gyro_offset_z"), &c.gyro.bias[2]), + + Param(PSTR("accel_bus"), &c.accel.bus, busDevChoices), + Param(PSTR("accel_dev"), &c.accel.dev, gyroDevChoices), + Param(PSTR("accel_lpf_type"), &c.accel.filter.type, filterTypeChoices), + Param(PSTR("accel_lpf_freq"), &c.accel.filter.freq), + Param(PSTR("accel_offset_x"), &c.accel.bias[0]), + Param(PSTR("accel_offset_y"), &c.accel.bias[1]), + Param(PSTR("accel_offset_z"), &c.accel.bias[2]), + + Param(PSTR("mag_bus"), &c.mag.bus, busDevChoices), + Param(PSTR("mag_dev"), &c.mag.dev, magDevChoices), + Param(PSTR("mag_align"), &c.mag.align, alignChoices), + Param(PSTR("mag_filter_type"), &c.mag.filter.type, filterTypeChoices), + Param(PSTR("mag_filter_lpf"), &c.mag.filter.freq), + Param(PSTR("mag_offset_x"), &c.mag.offset[0]), + Param(PSTR("mag_offset_y"), &c.mag.offset[1]), + Param(PSTR("mag_offset_z"), &c.mag.offset[2]), + Param(PSTR("mag_scale_x"), &c.mag.scale[0]), + Param(PSTR("mag_scale_y"), &c.mag.scale[1]), + Param(PSTR("mag_scale_z"), &c.mag.scale[2]), + + Param(PSTR("baro_bus"), &c.baro.bus, busDevChoices), + Param(PSTR("baro_dev"), &c.baro.dev, baroDevChoices), + Param(PSTR("baro_lpf_type"), &c.baro.filter.type, filterTypeChoices), + Param(PSTR("baro_lpf_freq"), &c.baro.filter.freq), + + Param(PSTR("board_align_roll"), &c.boardAlignment[0]), + Param(PSTR("board_align_pitch"), &c.boardAlignment[1]), + Param(PSTR("board_align_yaw"), &c.boardAlignment[2]), + + Param(PSTR("vbat_source"), &c.vbat.source, voltageSourceChoices), + Param(PSTR("vbat_scale"), &c.vbat.scale), + Param(PSTR("vbat_mul"), &c.vbat.resMult), + Param(PSTR("vbat_div"), &c.vbat.resDiv), + Param(PSTR("vbat_cell_warn"), &c.vbat.cellWarning), + + Param(PSTR("ibat_source"), &c.ibat.source, currentSourceChoices), + Param(PSTR("ibat_scale"), &c.ibat.scale), + Param(PSTR("ibat_offset"), &c.ibat.offset), + + Param(PSTR("fusion_mode"), &c.fusion.mode, fusionModeChoices), + Param(PSTR("fusion_gain_p"), &c.fusion.gain), + Param(PSTR("fusion_gain_i"), &c.fusion.gainI), + + Param(PSTR("input_rate_type"), &c.input.rateType, inputRateTypeChoices), + + Param(PSTR("input_roll_rate"), &c.input.rate[0]), + Param(PSTR("input_roll_srate"), &c.input.superRate[0]), + Param(PSTR("input_roll_expo"), &c.input.expo[0]), + Param(PSTR("input_roll_limit"), &c.input.rateLimit[0]), + + Param(PSTR("input_pitch_rate"), &c.input.rate[1]), + Param(PSTR("input_pitch_srate"), &c.input.superRate[1]), + Param(PSTR("input_pitch_expo"), &c.input.expo[1]), + Param(PSTR("input_pitch_limit"), &c.input.rateLimit[1]), + + Param(PSTR("input_yaw_rate"), &c.input.rate[2]), + Param(PSTR("input_yaw_srate"), &c.input.superRate[2]), + Param(PSTR("input_yaw_expo"), &c.input.expo[2]), + Param(PSTR("input_yaw_limit"), &c.input.rateLimit[2]), + + Param(PSTR("input_deadband"), &c.input.deadband), + + Param(PSTR("input_min"), &c.input.minRc), + Param(PSTR("input_mid"), &c.input.midRc), + Param(PSTR("input_max"), &c.input.maxRc), + + Param(PSTR("input_interpolation"), &c.input.interpolationMode, interpolChoices), + Param(PSTR("input_interpolation_interval"), &c.input.interpolationInterval), + + 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_rssi_channel"), &c.input.rssiChannel), + + Param(PSTR("input_0"), &c.input.channel[0]), + Param(PSTR("input_1"), &c.input.channel[1]), + Param(PSTR("input_2"), &c.input.channel[2]), + Param(PSTR("input_3"), &c.input.channel[3]), + Param(PSTR("input_4"), &c.input.channel[4]), + Param(PSTR("input_5"), &c.input.channel[5]), + Param(PSTR("input_6"), &c.input.channel[6]), + Param(PSTR("input_7"), &c.input.channel[7]), + Param(PSTR("input_8"), &c.input.channel[8]), + Param(PSTR("input_9"), &c.input.channel[9]), + Param(PSTR("input_10"), &c.input.channel[10]), + Param(PSTR("input_11"), &c.input.channel[11]), + Param(PSTR("input_12"), &c.input.channel[12]), + Param(PSTR("input_13"), &c.input.channel[13]), + Param(PSTR("input_14"), &c.input.channel[14]), + Param(PSTR("input_15"), &c.input.channel[15]), + + Param(PSTR("failsafe_delay"), &c.failsafe.delay), + Param(PSTR("failsafe_kill_switch"), &c.failsafe.killSwitch), + +#ifdef ESPFC_SERIAL_0 + Param(PSTR("serial_0"), &c.serial[SERIAL_UART_0]), +#endif +#ifdef ESPFC_SERIAL_1 + Param(PSTR("serial_1"), &c.serial[SERIAL_UART_1]), +#endif +#ifdef ESPFC_SERIAL_2 + Param(PSTR("serial_2"), &c.serial[SERIAL_UART_2]), +#endif +#ifdef ESPFC_SERIAL_SOFT_0 + Param(PSTR("serial_soft_0"), &c.serial[SERIAL_SOFT_0]), +#endif +#ifdef ESPFC_SERIAL_USB + Param(PSTR("serial_usb"), &c.serial[SERIAL_USB]), +#endif + + Param(PSTR("scaler_0"), &c.scaler[0]), + Param(PSTR("scaler_1"), &c.scaler[1]), + Param(PSTR("scaler_2"), &c.scaler[2]), + + Param(PSTR("mode_0"), &c.conditions[0]), + Param(PSTR("mode_1"), &c.conditions[1]), + Param(PSTR("mode_2"), &c.conditions[2]), + Param(PSTR("mode_3"), &c.conditions[3]), + Param(PSTR("mode_4"), &c.conditions[4]), + Param(PSTR("mode_5"), &c.conditions[5]), + Param(PSTR("mode_6"), &c.conditions[6]), + Param(PSTR("mode_7"), &c.conditions[7]), + + Param(PSTR("pid_sync"), &c.loopSync), + + Param(PSTR("pid_roll_p"), &c.pid[FC_PID_ROLL].P), + Param(PSTR("pid_roll_i"), &c.pid[FC_PID_ROLL].I), + Param(PSTR("pid_roll_d"), &c.pid[FC_PID_ROLL].D), + Param(PSTR("pid_roll_f"), &c.pid[FC_PID_ROLL].F), + + Param(PSTR("pid_pitch_p"), &c.pid[FC_PID_PITCH].P), + Param(PSTR("pid_pitch_i"), &c.pid[FC_PID_PITCH].I), + Param(PSTR("pid_pitch_d"), &c.pid[FC_PID_PITCH].D), + Param(PSTR("pid_pitch_f"), &c.pid[FC_PID_PITCH].F), + + Param(PSTR("pid_yaw_p"), &c.pid[FC_PID_YAW].P), + Param(PSTR("pid_yaw_i"), &c.pid[FC_PID_YAW].I), + Param(PSTR("pid_yaw_d"), &c.pid[FC_PID_YAW].D), + Param(PSTR("pid_yaw_f"), &c.pid[FC_PID_YAW].F), + + Param(PSTR("pid_level_p"), &c.pid[FC_PID_LEVEL].P), + Param(PSTR("pid_level_i"), &c.pid[FC_PID_LEVEL].I), + Param(PSTR("pid_level_d"), &c.pid[FC_PID_LEVEL].D), + + Param(PSTR("pid_level_angle_limit"), &c.level.angleLimit), + Param(PSTR("pid_level_rate_limit"), &c.level.rateLimit), + Param(PSTR("pid_level_lpf_type"), &c.level.ptermFilter.type, filterTypeChoices), + Param(PSTR("pid_level_lpf_freq"), &c.level.ptermFilter.freq), + + Param(PSTR("pid_yaw_lpf_type"), &c.yaw.filter.type, filterTypeChoices), + Param(PSTR("pid_yaw_lpf_freq"), &c.yaw.filter.freq), + + Param(PSTR("pid_dterm_lpf_type"), &c.dterm.filter.type, filterTypeChoices), + Param(PSTR("pid_dterm_lpf_freq"), &c.dterm.filter.freq), + Param(PSTR("pid_dterm_lpf2_type"), &c.dterm.filter2.type, filterTypeChoices), + Param(PSTR("pid_dterm_lpf2_freq"), &c.dterm.filter2.freq), + Param(PSTR("pid_dterm_notch_freq"), &c.dterm.notchFilter.freq), + Param(PSTR("pid_dterm_notch_cutoff"), &c.dterm.notchFilter.cutoff), + Param(PSTR("pid_dterm_dyn_lpf_min"), &c.dterm.dynLpfFilter.cutoff), + Param(PSTR("pid_dterm_dyn_lpf_max"), &c.dterm.dynLpfFilter.freq), + + Param(PSTR("pid_dterm_weight"), &c.dterm.setpointWeight), + Param(PSTR("pid_iterm_limit"), &c.iterm.limit), + Param(PSTR("pid_iterm_zero"), &c.iterm.lowThrottleZeroIterm), + Param(PSTR("pid_iterm_relax"), &c.iterm.relax, inputItermRelaxChoices), + Param(PSTR("pid_iterm_relax_cutoff"), &c.iterm.relaxCutoff), + Param(PSTR("pid_tpa_scale"), &c.controller.tpaScale), + Param(PSTR("pid_tpa_breakpoint"), &c.controller.tpaBreakpoint), + + Param(PSTR("mixer_sync"), &c.mixerSync), + Param(PSTR("mixer_type"), &c.mixer.type, mixerTypeChoices), + Param(PSTR("mixer_yaw_reverse"), &c.mixer.yawReverse), + Param(PSTR("mixer_throttle_limit_type"), &c.output.throttleLimitType, throtleLimitTypeChoices), + Param(PSTR("mixer_throttle_limit_percent"), &c.output.throttleLimitPercent), + Param(PSTR("mixer_output_limit"), &c.output.motorLimit), + + Param(PSTR("output_motor_protocol"), &c.output.protocol, protocolChoices), + Param(PSTR("output_motor_async"), &c.output.async), + Param(PSTR("output_motor_rate"), &c.output.rate), +#ifdef ESPFC_DSHOT_TELEMETRY + Param(PSTR("output_motor_poles"), &c.output.motorPoles), +#endif + Param(PSTR("output_servo_rate"), &c.output.servoRate), + + Param(PSTR("output_min_command"), &c.output.minCommand), + Param(PSTR("output_min_throttle"), &c.output.minThrottle), + Param(PSTR("output_max_throttle"), &c.output.maxThrottle), + Param(PSTR("output_dshot_idle"), &c.output.dshotIdle), +#ifdef ESPFC_DSHOT_TELEMETRY + Param(PSTR("output_dshot_telemetry"), &c.output.dshotTelemetry), +#endif + Param(PSTR("output_0"), &c.output.channel[0]), + Param(PSTR("output_1"), &c.output.channel[1]), + Param(PSTR("output_2"), &c.output.channel[2]), + Param(PSTR("output_3"), &c.output.channel[3]), +#if ESPFC_OUTPUT_COUNT > 4 + Param(PSTR("output_4"), &c.output.channel[4]), +#endif +#if ESPFC_OUTPUT_COUNT > 5 + Param(PSTR("output_5"), &c.output.channel[5]), +#endif +#if ESPFC_OUTPUT_COUNT > 6 + Param(PSTR("output_6"), &c.output.channel[6]), +#endif +#if ESPFC_OUTPUT_COUNT > 7 + Param(PSTR("output_7"), &c.output.channel[7]), +#endif +#ifdef ESPFC_INPUT + Param(PSTR("pin_input_rx"), &c.pin[PIN_INPUT_RX]), +#endif + Param(PSTR("pin_output_0"), &c.pin[PIN_OUTPUT_0]), + Param(PSTR("pin_output_1"), &c.pin[PIN_OUTPUT_1]), + Param(PSTR("pin_output_2"), &c.pin[PIN_OUTPUT_2]), + Param(PSTR("pin_output_3"), &c.pin[PIN_OUTPUT_3]), +#if ESPFC_OUTPUT_COUNT > 4 + Param(PSTR("pin_output_4"), &c.pin[PIN_OUTPUT_4]), +#endif +#if ESPFC_OUTPUT_COUNT > 5 + Param(PSTR("pin_output_5"), &c.pin[PIN_OUTPUT_5]), +#endif +#if ESPFC_OUTPUT_COUNT > 6 + Param(PSTR("pin_output_6"), &c.pin[PIN_OUTPUT_6]), +#endif +#if ESPFC_OUTPUT_COUNT > 7 + Param(PSTR("pin_output_7"), &c.pin[PIN_OUTPUT_7]), +#endif + Param(PSTR("pin_buzzer"), &c.pin[PIN_BUZZER]), +#if defined(ESPFC_SERIAL_0) && defined(ESPFC_SERIAL_REMAP_PINS) + Param(PSTR("pin_serial_0_tx"), &c.pin[PIN_SERIAL_0_TX]), + Param(PSTR("pin_serial_0_rx"), &c.pin[PIN_SERIAL_0_RX]), +#endif +#if defined(ESPFC_SERIAL_1) && defined(ESPFC_SERIAL_REMAP_PINS) + Param(PSTR("pin_serial_1_tx"), &c.pin[PIN_SERIAL_1_TX]), + Param(PSTR("pin_serial_1_rx"), &c.pin[PIN_SERIAL_1_RX]), +#endif +#if defined(ESPFC_SERIAL_2) && defined(ESPFC_SERIAL_REMAP_PINS) + Param(PSTR("pin_serial_2_tx"), &c.pin[PIN_SERIAL_2_TX]), + Param(PSTR("pin_serial_2_rx"), &c.pin[PIN_SERIAL_2_RX]), +#endif +#ifdef ESPFC_I2C_0 + Param(PSTR("pin_i2c_scl"), &c.pin[PIN_I2C_0_SCL]), + Param(PSTR("pin_i2c_sda"), &c.pin[PIN_I2C_0_SDA]), +#endif +#ifdef ESPFC_ADC_0 + Param(PSTR("pin_input_adc_0"), &c.pin[PIN_INPUT_ADC_0]), +#endif +#ifdef ESPFC_ADC_1 + Param(PSTR("pin_input_adc_1"), &c.pin[PIN_INPUT_ADC_1]), +#endif +#ifdef ESPFC_SPI_0 + Param(PSTR("pin_spi_0_sck"), &c.pin[PIN_SPI_0_SCK]), + Param(PSTR("pin_spi_0_mosi"), &c.pin[PIN_SPI_0_MOSI]), + Param(PSTR("pin_spi_0_miso"), &c.pin[PIN_SPI_0_MISO]), + Param(PSTR("pin_spi_cs_0"), &c.pin[PIN_SPI_CS0]), + Param(PSTR("pin_spi_cs_1"), &c.pin[PIN_SPI_CS1]), + Param(PSTR("pin_spi_cs_2"), &c.pin[PIN_SPI_CS2]), +#endif + Param(PSTR("pin_buzzer_invert"), &c.buzzer.inverted), + +#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("blackbox_dev"), &c.blackbox.dev, blackboxDevChoices), + Param(PSTR("blackbox_mode"), &c.blackbox.mode, blackboxModeChoices), + Param(PSTR("blackbox_rate"), &c.blackbox.pDenom), + Param(PSTR("blackbox_log_acc"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_ACC), + Param(PSTR("blackbox_log_alt"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_ALTITUDE), + Param(PSTR("blackbox_log_bat"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_BATTERY), + Param(PSTR("blackbox_log_debug"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_DEBUG_LOG), + Param(PSTR("blackbox_log_gps"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_GPS), + Param(PSTR("blackbox_log_gyro"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_GYRO), + Param(PSTR("blackbox_log_gyro_raw"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_GYROUNFILT), + Param(PSTR("blackbox_log_mag"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_MAG), + Param(PSTR("blackbox_log_motor"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_MOTOR), + Param(PSTR("blackbox_log_pid"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_PID), + Param(PSTR("blackbox_log_rc"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_RC_COMMANDS), + Param(PSTR("blackbox_log_rpm"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_RPM), + Param(PSTR("blackbox_log_rssi"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_RSSI), + Param(PSTR("blackbox_log_sp"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_SETPOINT), + + Param(PSTR("model_name"), PARAM_STRING, &c.modelName[0], NULL, MODEL_NAME_LEN), + +#ifdef ESPFC_SERIAL_SOFT_0_WIFI + Param(PSTR("wifi_ssid"), PARAM_STRING, &c.wireless.ssid[0], NULL, WirelessConfig::MAX_LEN), + Param(PSTR("wifi_pass"), PARAM_STRING, &c.wireless.pass[0], NULL, WirelessConfig::MAX_LEN), + Param(PSTR("wifi_tcp_port"), &c.wireless.port), +#endif + + Param(PSTR("mix_outputs"), &c.customMixerCount), + Param(PSTR("mix_0"), &c.customMixes[i++]), + Param(PSTR("mix_1"), &c.customMixes[i++]), + Param(PSTR("mix_2"), &c.customMixes[i++]), + Param(PSTR("mix_3"), &c.customMixes[i++]), + Param(PSTR("mix_4"), &c.customMixes[i++]), + Param(PSTR("mix_5"), &c.customMixes[i++]), + Param(PSTR("mix_6"), &c.customMixes[i++]), + Param(PSTR("mix_7"), &c.customMixes[i++]), + Param(PSTR("mix_8"), &c.customMixes[i++]), + Param(PSTR("mix_9"), &c.customMixes[i++]), + Param(PSTR("mix_10"), &c.customMixes[i++]), + Param(PSTR("mix_11"), &c.customMixes[i++]), + Param(PSTR("mix_12"), &c.customMixes[i++]), + Param(PSTR("mix_13"), &c.customMixes[i++]), + Param(PSTR("mix_14"), &c.customMixes[i++]), + Param(PSTR("mix_15"), &c.customMixes[i++]), + Param(PSTR("mix_16"), &c.customMixes[i++]), + Param(PSTR("mix_17"), &c.customMixes[i++]), + Param(PSTR("mix_18"), &c.customMixes[i++]), + Param(PSTR("mix_19"), &c.customMixes[i++]), + Param(PSTR("mix_20"), &c.customMixes[i++]), + Param(PSTR("mix_21"), &c.customMixes[i++]), + Param(PSTR("mix_22"), &c.customMixes[i++]), + Param(PSTR("mix_23"), &c.customMixes[i++]), + Param(PSTR("mix_24"), &c.customMixes[i++]), + Param(PSTR("mix_25"), &c.customMixes[i++]), + Param(PSTR("mix_26"), &c.customMixes[i++]), + Param(PSTR("mix_27"), &c.customMixes[i++]), + Param(PSTR("mix_28"), &c.customMixes[i++]), + Param(PSTR("mix_29"), &c.customMixes[i++]), + Param(PSTR("mix_30"), &c.customMixes[i++]), + Param(PSTR("mix_31"), &c.customMixes[i++]), + Param(PSTR("mix_32"), &c.customMixes[i++]), + Param(PSTR("mix_33"), &c.customMixes[i++]), + Param(PSTR("mix_34"), &c.customMixes[i++]), + Param(PSTR("mix_35"), &c.customMixes[i++]), + Param(PSTR("mix_36"), &c.customMixes[i++]), + Param(PSTR("mix_37"), &c.customMixes[i++]), + Param(PSTR("mix_38"), &c.customMixes[i++]), + Param(PSTR("mix_39"), &c.customMixes[i++]), + Param(PSTR("mix_40"), &c.customMixes[i++]), + Param(PSTR("mix_41"), &c.customMixes[i++]), + Param(PSTR("mix_42"), &c.customMixes[i++]), + Param(PSTR("mix_43"), &c.customMixes[i++]), + Param(PSTR("mix_44"), &c.customMixes[i++]), + Param(PSTR("mix_45"), &c.customMixes[i++]), + Param(PSTR("mix_46"), &c.customMixes[i++]), + Param(PSTR("mix_47"), &c.customMixes[i++]), + Param(PSTR("mix_48"), &c.customMixes[i++]), + Param(PSTR("mix_49"), &c.customMixes[i++]), + Param(PSTR("mix_50"), &c.customMixes[i++]), + Param(PSTR("mix_51"), &c.customMixes[i++]), + Param(PSTR("mix_52"), &c.customMixes[i++]), + Param(PSTR("mix_53"), &c.customMixes[i++]), + Param(PSTR("mix_54"), &c.customMixes[i++]), + Param(PSTR("mix_55"), &c.customMixes[i++]), + Param(PSTR("mix_56"), &c.customMixes[i++]), + Param(PSTR("mix_57"), &c.customMixes[i++]), + Param(PSTR("mix_58"), &c.customMixes[i++]), + Param(PSTR("mix_59"), &c.customMixes[i++]), + Param(PSTR("mix_60"), &c.customMixes[i++]), + Param(PSTR("mix_61"), &c.customMixes[i++]), + Param(PSTR("mix_62"), &c.customMixes[i++]), + Param(PSTR("mix_63"), &c.customMixes[i++]), + + Param() // terminate + }; + return params; +} + +bool Cli::process(const char c, CliCmd& cmd, Stream& stream) +{ + // configurator handshake + if(!_active && c == '#') + { + //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(); + _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; + } + + bool endl = c == '\n' || c == '\r'; + if(cmd.index && endl) + { + parse(cmd); + execute(cmd, stream); + cmd = CliCmd(); + return true; + } + + if(c == '#') _ignore = true; + else if(endl) _ignore = false; + + // don't put characters into buffer in specific conditions + if(_ignore || endl || cmd.index >= CLI_BUFF_SIZE - 1) return false; + + if(c == '\b') // handle backspace + { + cmd.buff[--cmd.index] = '\0'; + } + else + { + cmd.buff[cmd.index] = c; + cmd.buff[++cmd.index] = '\0'; + } + return false; +} + +void Cli::parse(CliCmd& cmd) +{ + const char * DELIM = " \t"; + char * pch = strtok(cmd.buff, DELIM); + size_t count = 0; + while(pch) + { + cmd.args[count++] = pch; + pch = strtok(NULL, DELIM); + } +} + +void Cli::execute(CliCmd& cmd, Stream& s) +{ + if(cmd.args[0]) s.print(F("# ")); + for(size_t i = 0; i < CLI_ARGS_SIZE; ++i) + { + if(!cmd.args[i]) break; + s.print(cmd.args[i]); + s.print(' '); + } + s.println(); + + if(!cmd.args[0]) return; + + if(strcmp_P(cmd.args[0], PSTR("help")) == 0) + { + static const char * helps[] = { + PSTR("available commands:"), + PSTR(" help"), PSTR(" dump"), PSTR(" get param"), PSTR(" set param value ..."), PSTR(" cal [gyro]"), + PSTR(" defaults"), PSTR(" save"), PSTR(" reboot"), PSTR(" scaler"), PSTR(" mixer"), + PSTR(" stats"), PSTR(" status"), PSTR(" devinfo"), PSTR(" version"), PSTR(" logs"), + //PSTR(" load"), PSTR(" eeprom"), + //PSTR(" fsinfo"), PSTR(" fsformat"), PSTR(" log"), + NULL + }; + for(const char ** ptr = helps; *ptr; ptr++) + { + s.println(FPSTR(*ptr)); + } + } + else if(strcmp_P(cmd.args[0], PSTR("version")) == 0) + { + 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("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); + } +#endif +#if defined(ESPFC_FREE_RTOS) + else if(strcmp_P(cmd.args[0], PSTR("tasks")) == 0) + { + printVersion(s); + s.println(); + + size_t numTasks = uxTaskGetNumberOfTasks(); + + s.print(F("num tasks: ")); + s.print(numTasks); + s.println(); + } +#endif + else if(strcmp_P(cmd.args[0], PSTR("devinfo")) == 0) + { + printVersion(s); + s.println(); + + s.print(F("config size: ")); + s.println(sizeof(ModelConfig)); + + s.print(F(" free heap: ")); + s.println(targetFreeHeap()); + + s.print(F(" cpu freq: ")); + s.print(targetCpuFreq()); + s.println(F(" MHz")); + } + else if(strcmp_P(cmd.args[0], PSTR("get")) == 0) + { + bool found = false; + for(size_t i = 0; _params[i].name; ++i) + { + String ts = FPSTR(_params[i].name); + if(!cmd.args[1] || ts.indexOf(cmd.args[1]) >= 0) + { + print(_params[i], s); + found = true; + } + } + if(!found) + { + s.print(F("param not found: ")); + s.print(cmd.args[1]); + } + s.println(); + } + else if(strcmp_P(cmd.args[0], PSTR("set")) == 0) + { + if(!cmd.args[1]) + { + s.println(F("param required")); + s.println(); + return; + } + bool found = false; + for(size_t i = 0; _params[i].name; ++i) + { + if(strcmp_P(cmd.args[1], _params[i].name) == 0) + { + _params[i].update(cmd.args); + print(_params[i], s); + found = true; + break; + } + } + if(!found) + { + s.print(F("param not found: ")); + s.println(cmd.args[1]); + } + } + else if(strcmp_P(cmd.args[0], PSTR("dump")) == 0) + { + s.println(F("defaults")); + for(size_t i = 0; _params[i].name; ++i) + { + print(_params[i], s); + } + s.println(F("save")); + } + else if(strcmp_P(cmd.args[0], PSTR("cal")) == 0) + { + if(!cmd.args[1]) + { + s.print(F(" gyro offset: ")); + s.print(_model.config.gyro.bias[0]); s.print(' '); + s.print(_model.config.gyro.bias[1]); s.print(' '); + s.print(_model.config.gyro.bias[2]); s.print(F(" [")); + s.print(Utils::toDeg(_model.state.gyro.bias[0])); s.print(' '); + s.print(Utils::toDeg(_model.state.gyro.bias[1])); s.print(' '); + s.print(Utils::toDeg(_model.state.gyro.bias[2])); s.println(F("]")); + + s.print(F("accel offset: ")); + s.print(_model.config.accel.bias[0]); s.print(' '); + s.print(_model.config.accel.bias[1]); s.print(' '); + s.print(_model.config.accel.bias[2]); s.print(F(" [")); + s.print(_model.state.accel.bias[0]); s.print(' '); + s.print(_model.state.accel.bias[1]); s.print(' '); + s.print(_model.state.accel.bias[2]); s.println(F("]")); + + s.print(F(" mag offset: ")); + s.print(_model.config.mag.offset[0]); s.print(' '); + s.print(_model.config.mag.offset[1]); s.print(' '); + s.print(_model.config.mag.offset[2]); s.print(F(" [")); + s.print(_model.state.mag.calibrationOffset[0]); s.print(' '); + s.print(_model.state.mag.calibrationOffset[1]); s.print(' '); + s.print(_model.state.mag.calibrationOffset[2]); s.println(F("]")); + + s.print(F(" mag scale: ")); + s.print(_model.config.mag.scale[0]); s.print(' '); + s.print(_model.config.mag.scale[1]); s.print(' '); + s.print(_model.config.mag.scale[2]); s.print(F(" [")); + s.print(_model.state.mag.calibrationScale[0]); s.print(' '); + s.print(_model.state.mag.calibrationScale[1]); s.print(' '); + s.print(_model.state.mag.calibrationScale[2]); s.println(F("]")); + } + else if(strcmp_P(cmd.args[1], PSTR("gyro")) == 0) + { + if(!_model.isModeActive(MODE_ARMED)) _model.calibrateGyro(); + s.println(F("OK")); + } + else if(strcmp_P(cmd.args[1], PSTR("mag")) == 0) + { + if(!_model.isModeActive(MODE_ARMED)) _model.calibrateMag(); + s.println(F("OK")); + } + else if(strcmp_P(cmd.args[1], PSTR("reset_accel")) == 0 || strcmp_P(cmd.args[1], PSTR("reset_all")) == 0) + { + _model.state.accel.bias = VectorFloat(); + s.println(F("OK")); + } + else if(strcmp_P(cmd.args[1], PSTR("reset_gyro")) == 0 || strcmp_P(cmd.args[1], PSTR("reset_all")) == 0) + { + _model.state.gyro.bias = VectorFloat(); + s.println(F("OK")); + } + else if(strcmp_P(cmd.args[1], PSTR("reset_mag")) == 0 || strcmp_P(cmd.args[1], PSTR("reset_all")) == 0) + { + _model.state.mag.calibrationOffset = VectorFloat(); + _model.state.mag.calibrationScale = VectorFloat(1.f, 1.f, 1.f); + s.println(F("OK")); + } + } + else if(strcmp_P(cmd.args[0], PSTR("preset")) == 0) + { + if(!cmd.args[1]) + { + s.println(F("Available presets: scaler, modes, micrus, brobot")); + } + else if(strcmp_P(cmd.args[1], PSTR("scaler")) == 0) + { + _model.config.scaler[0].dimension = (ScalerDimension)(ACT_INNER_P | ACT_AXIS_PITCH | ACT_AXIS_ROLL); + _model.config.scaler[0].channel = 5; + _model.config.scaler[0].minScale = 25; //% + _model.config.scaler[0].maxScale = 400; + + _model.config.scaler[1].dimension = (ScalerDimension)(ACT_INNER_I | ACT_AXIS_PITCH | ACT_AXIS_ROLL); + _model.config.scaler[1].channel = 6; + _model.config.scaler[1].minScale = 25; //% + _model.config.scaler[1].maxScale = 400; + + _model.config.scaler[2].dimension = (ScalerDimension)(ACT_INNER_D | ACT_AXIS_PITCH | ACT_AXIS_ROLL); + _model.config.scaler[2].channel = 7; + _model.config.scaler[2].minScale = 25; //% + _model.config.scaler[2].maxScale = 400; + + s.println(F("OK")); + } + else if(strcmp_P(cmd.args[1], PSTR("modes")) == 0) + { + _model.config.conditions[0].id = MODE_ARMED; + _model.config.conditions[0].ch = AXIS_AUX_1 + 0; + _model.config.conditions[0].min = 1700; + _model.config.conditions[0].max = 2100; + + _model.config.conditions[1].id = MODE_ANGLE; + _model.config.conditions[1].ch = AXIS_AUX_1 + 0; // aux1 + _model.config.conditions[1].min = 1900; + _model.config.conditions[1].max = 2100; + + _model.config.conditions[2].id = MODE_AIRMODE; + _model.config.conditions[2].ch = 0; // aux1 + _model.config.conditions[2].min = (1700 - 900) / 25; + _model.config.conditions[2].max = (2100 - 900) / 25; + + s.println(F("OK")); + } + else if(strcmp_P(cmd.args[1], PSTR("micrus")) == 0) + { + s.println(F("OK")); + } + else if(strcmp_P(cmd.args[1], PSTR("brobot")) == 0) + { + s.println(F("OK")); + } + else + { + s.println(F("NOT OK")); + } + } + else if(strcmp_P(cmd.args[0], PSTR("load")) == 0) + { + _model.load(); + s.println(F("OK")); + } + else if(strcmp_P(cmd.args[0], PSTR("save")) == 0) + { + _model.save(); + s.println(F("# Saved, type reboot to apply changes")); + s.println(); + } + else if(strcmp_P(cmd.args[0], PSTR("eeprom")) == 0) + { + /* + int start = 0; + if(cmd.args[1]) + { + start = std::max(String(cmd.args[1]).toInt(), 0L); + } + + for(int i = start; i < start + 32; ++i) + { + uint8_t v = EEPROM.read(i); + if(v <= 0xf) s.print('0'); + s.print(v, HEX); + s.print(' '); + } + s.println(); + + for(int i = start; i < start + 32; ++i) + { + s.print((int8_t)EEPROM.read(i)); + s.print(' '); + } + s.println(); + */ + } + else if(strcmp_P(cmd.args[0], PSTR("scaler")) == 0) + { + for(size_t i = 0; i < SCALER_COUNT; i++) + { + uint32_t mode = _model.config.scaler[i].dimension; + if(!mode) continue; + short c = _model.config.scaler[i].channel; + float v = _model.state.input.ch[c]; + float min = _model.config.scaler[i].minScale * 0.01f; + float max = _model.config.scaler[i].maxScale * 0.01f; + float scale = Utils::map3(v, -1.f, 0.f, 1.f, min, min < 0 ? 0.f : 1.f, max); + s.print(F("scaler: ")); + s.print(i); + s.print(' '); + s.print(mode); + s.print(' '); + s.print(min); + s.print(' '); + s.print(max); + s.print(' '); + s.print(v); + s.print(' '); + s.println(scale); + } + } + else if(strcmp_P(cmd.args[0], PSTR("mixer")) == 0) + { + const MixerConfig& mixer = _model.state.currentMixer; + s.print(F("set mix_outputs ")); + s.println(mixer.count); + Param p; + for(size_t i = 0; i < MIXER_RULE_MAX; i++) + { + s.print(F("set mix_")); + s.print(i); + s.print(' '); + p.print(s, mixer.mixes[i]); + s.println(); + if(mixer.mixes[i].src == MIXER_SOURCE_NULL) break; + } + } + else if(strcmp_P(cmd.args[0], PSTR("status")) == 0) + { + printVersion(s); + s.println(); + s.println(F("STATUS: ")); + printStats(s); + s.println(); + + Device::GyroDevice * gyro = _model.state.gyro.dev; + Device::BaroDevice * baro = _model.state.baro.dev; + Device::MagDevice * mag = _model.state.mag.dev; + s.print(F(" devices: ")); + if(gyro) + { + s.print(FPSTR(Device::GyroDevice::getName(gyro->getType()))); + s.print('/'); + s.print(FPSTR(Device::BusDevice::getName(gyro->getBus()->getType()))); + } + else + { + s.print(F("NO_GYRO")); + } + + if(baro) + { + s.print(F(", ")); + s.print(FPSTR(Device::BaroDevice::getName(baro->getType()))); + s.print('/'); + s.print(FPSTR(Device::BusDevice::getName(baro->getBus()->getType()))); + } + else + { + s.print(F(", NO_BARO")); + } + + if(mag) + { + s.print(F(", ")); + s.print(FPSTR(Device::MagDevice::getName(mag->getType()))); + s.print('/'); + s.print(FPSTR(Device::BusDevice::getName(mag->getBus()->getType()))); + } + else + { + s.print(F(", NO_MAG")); + } + s.println(); + + s.print(F(" input: ")); + s.print(_model.state.input.frameRate); + s.print(F(" Hz, ")); + s.print(_model.state.input.autoFreq); + s.print(F(" Hz, ")); + s.println(_model.state.input.autoFactor); + + static const char* armingDisableNames[] = { + PSTR("NO_GYRO"), PSTR("FAILSAFE"), PSTR("RX_FAILSAFE"), PSTR("BAD_RX_RECOVERY"), + PSTR("BOXFAILSAFE"), PSTR("RUNAWAY_TAKEOFF"), PSTR("CRASH_DETECTED"), PSTR("THROTTLE"), + PSTR("ANGLE"), PSTR("BOOT_GRACE_TIME"), PSTR("NOPREARM"), PSTR("LOAD"), + PSTR("CALIBRATING"), PSTR("CLI"), PSTR("CMS_MENU"), PSTR("BST"), + PSTR("MSP"), PSTR("PARALYZE"), PSTR("GPS"), PSTR("RESC"), + PSTR("RPMFILTER"), PSTR("REBOOT_REQUIRED"), PSTR("DSHOT_BITBANG"), PSTR("ACC_CALIBRATION"), + PSTR("MOTOR_PROTOCOL"), PSTR("ARM_SWITCH") + }; + const size_t armingDisableNamesLength = sizeof(armingDisableNames) / sizeof(armingDisableNames[0]); + + s.print(F("arming flags:")); + for(size_t i = 0; i < armingDisableNamesLength; i++) + { + if(_model.state.mode.armingDisabledFlags & (1 << i)) { + s.print(' '); + s.print(armingDisableNames[i]); + } + } + s.println(); + s.print(F(" rescue mode: ")); + s.print(_model.state.mode.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) + { + printVersion(s); + s.println(); + printStats(s); + s.println(); + for(int i = 0; i < COUNTER_COUNT; ++i) + { + StatCounter c = (StatCounter)i; + int time = lrintf(_model.state.stats.getTime(c)); + float load = _model.state.stats.getLoad(c); + int freq = lrintf(_model.state.stats.getFreq(c)); + int real = lrintf(_model.state.stats.getReal(c)); + if(freq == 0) continue; + + s.print(FPSTR(_model.state.stats.getName(c))); + s.print(": "); + if(time < 100) s.print(' '); + if(time < 10) s.print(' '); + s.print(time); + s.print("us, "); + + if(real < 100) s.print(' '); + if(real < 10) s.print(' '); + s.print(real); + s.print("us/i, "); + + if(load < 10) s.print(' '); + s.print(load, 1); + s.print("%, "); + + if(freq < 1000) s.print(' '); + if(freq < 100) s.print(' '); + if(freq < 10) s.print(' '); + s.print(freq); + s.print(" Hz"); + s.println(); + } + s.print(F(" TOTAL: ")); + s.print((int)(_model.state.stats.getCpuTime())); + s.print(F("us, ")); + s.print(_model.state.stats.getCpuLoad(), 1); + s.print(F("%")); + s.println(); + } + 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("motors")) == 0) + { + s.print(PSTR("count: ")); + s.println(getMotorCount()); + for (size_t i = 0; i < 8; i++) + { + s.print(i); + s.print(PSTR(": ")); + if (i >= OUTPUT_CHANNELS || _model.config.pin[i + PIN_OUTPUT_0] == -1) + { + s.print(-1); + s.print(' '); + s.println(0); + } else { + s.print(_model.config.pin[i + PIN_OUTPUT_0]); + s.print(' '); + s.println(_model.state.output.us[i]); + } + } + } + else if(strcmp_P(cmd.args[0], PSTR("logs")) == 0) + { + s.print(_model.logger.c_str()); + 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(); + } + size_t size = 0; + if(cmd.args[3]) + { + size = String(cmd.args[3]).toInt(); + } + size = Utils::clamp(size, 8u, 128 * 1024u); + size_t chunk_size = 256; + + uint8_t* data = new uint8_t[chunk_size]; + while(size) + { + size_t len = std::min(size, chunk_size); + flashfsReadAbs(addr, data, len); + s.write(data, len); + + if(size > chunk_size) + { + size -= chunk_size; + addr += chunk_size; + } + else break; + } + s.println(); + delete[] data; + } + else + { + s.println(F("wrong param!")); + } + } +#endif + else + { + s.print(F("unknown command: ")); + s.println(cmd.args[0]); + } + s.println(); +} + +void Cli::print(const Param& param, Stream& s) const +{ + s.print(F("set ")); + s.print(FPSTR(param.name)); + s.print(' '); + param.print(s); + s.println(); +} + +void Cli::printVersion(Stream& s) const +{ + s.print(boardIdentifier); + s.print(' '); + s.print(targetName); + s.print(' '); + s.print(targetVersion); + s.print(' '); + s.print(shortGitRevision); + s.print(' '); + s.print(buildDate); + 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); +} + +void Cli::printStats(Stream& s) const +{ + s.print(F(" cpu freq: ")); + s.print(targetCpuFreq()); + s.println(F(" MHz")); + + s.print(F(" gyro clock: ")); + s.print(_model.state.gyro.clock); + s.println(F(" Hz")); + + s.print(F(" gyro rate: ")); + s.print(_model.state.gyro.timer.rate); + s.println(F(" Hz")); + + s.print(F(" loop rate: ")); + s.print(_model.state.loopTimer.rate); + s.println(F(" Hz")); + + s.print(F(" mixer rate: ")); + s.print(_model.state.mixer.timer.rate); + s.println(F(" Hz")); + + s.print(F(" accel rate: ")); + s.print(_model.state.accel.timer.rate); + s.println(F(" Hz")); + + s.print(F(" baro rate: ")); + s.print(_model.state.baro.rate); + s.println(F(" Hz")); + + s.print(F(" mag rate: ")); + s.print(_model.state.mag.timer.rate); + s.println(F(" Hz")); +} + +} + +} diff --git a/lib/Espfc/src/Connect/Cli.h b/lib/Espfc/src/Connect/Cli.h deleted file mode 100644 index ebe612c9..00000000 --- a/lib/Espfc/src/Connect/Cli.h +++ /dev/null @@ -1,1566 +0,0 @@ -#ifndef _ESPFC_CLI_H_ -#define _ESPFC_CLI_H_ - -#include -#include - -#include "Model.h" -#include "Hardware.h" -#include "Device/GyroDevice.h" -#include "Hal/Pgm.h" -#include "msp/msp_protocol.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 -#endif - -namespace Espfc { - -namespace Connect { - -class Cli -{ - public: - enum ParamType { - PARAM_NONE, // unused - PARAM_BOOL, // boolean - PARAM_BYTE, // 8 bit int - PARAM_BYTE_U, // 8 bit uint - PARAM_SHORT, // 16 bit int - PARAM_INT, // 32 bit int - PARAM_FLOAT, // 32 bit float - PARAM_INPUT_CHANNEL, // input channel config - PARAM_OUTPUT_CHANNEL, // output channel config - PARAM_SCALER, // scaler config - PARAM_MODE, // scaler config - PARAM_MIXER, // mixer config - PARAM_SERIAL, // mixer config - PARAM_STRING, // string - PARAM_BITMASK, // set or clear bit - }; - - class Param - { - public: - Param(): Param(NULL, PARAM_NONE, NULL, NULL) {} - Param(const Param& p): Param(p.name, p.type, p.addr, p.choices) {} - - Param(const char * n, ParamType t, char * a, const char ** c, size_t l = 16): name(n), type(t), addr(a), choices(c), maxLen(l) {} - - Param(const char * n, 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) {} - Param(const char * n, uint8_t * a): Param(n, PARAM_BYTE_U, reinterpret_cast(a), NULL) {} - Param(const char * n, int16_t * a): Param(n, PARAM_SHORT, reinterpret_cast(a), NULL) {} - Param(const char * n, int32_t * a): Param(n, PARAM_INT, reinterpret_cast(a), NULL) {} - - Param(const char * n, int8_t * a, const char ** c): Param(n, PARAM_BYTE, reinterpret_cast(a), c) {} - Param(const char * n, int32_t * a, uint8_t b): Param(n, PARAM_BITMASK, reinterpret_cast(a), NULL, b) {} - - Param(const char * n, InputChannelConfig * a): Param(n, PARAM_INPUT_CHANNEL, reinterpret_cast(a), NULL) {} - Param(const char * n, OutputChannelConfig * a): Param(n, PARAM_OUTPUT_CHANNEL, reinterpret_cast(a), NULL) {} - Param(const char * n, ScalerConfig * a): Param(n, PARAM_SCALER, reinterpret_cast(a), NULL) {} - Param(const char * n, ActuatorCondition * a): Param(n, PARAM_MODE, reinterpret_cast(a), NULL) {} - Param(const char * n, MixerEntry * a): Param(n, PARAM_MIXER, reinterpret_cast(a), NULL) {} - Param(const char * n, SerialPortConfig * a): Param(n, PARAM_SERIAL, reinterpret_cast(a), NULL) {} - - void print(Stream& stream) const - { - if(!addr) - { - stream.print(F("UNSET")); - return; - } - switch(type) - { - case PARAM_NONE: - stream.print(F("NONE")); - break; - case PARAM_BOOL: - stream.print(*addr != 0); - break; - case PARAM_BYTE: - print(stream, *reinterpret_cast(addr)); - break; - case PARAM_BYTE_U: - print(stream, *reinterpret_cast(addr)); - break; - case PARAM_SHORT: - print(stream, *reinterpret_cast(addr)); - break; - case PARAM_INT: - print(stream, *reinterpret_cast(addr)); - break; - case PARAM_FLOAT: - stream.print(*reinterpret_cast(addr), 4); - break; - case PARAM_STRING: - stream.print(addr); - break; - case PARAM_BITMASK: - stream.print((*reinterpret_cast(addr) & (1ul << maxLen)) ? 1 : 0); - break; - case PARAM_INPUT_CHANNEL: - print(stream, *reinterpret_cast(addr)); - break; - case PARAM_OUTPUT_CHANNEL: - print(stream, *reinterpret_cast(addr)); - break; - case PARAM_SCALER: - print(stream, *reinterpret_cast(addr)); - break; - case PARAM_MODE: - print(stream, *reinterpret_cast(addr)); - break; - case PARAM_MIXER: - print(stream, *reinterpret_cast(addr)); - break; - case PARAM_SERIAL: - print(stream, *reinterpret_cast(addr)); - break; - } - } - - void print(Stream& stream, const OutputChannelConfig& och) const - { - stream.print(och.servo ? 'S' : 'M'); - stream.print(' '); - stream.print(och.reverse ? 'R' : 'N'); - stream.print(' '); - stream.print(och.min); - stream.print(' '); - stream.print(och.neutral); - stream.print(' '); - stream.print(och.max); - } - - void print(Stream& stream, const InputChannelConfig& ich) const - { - stream.print(ich.map); - stream.print(' '); - stream.print(ich.min); - stream.print(' '); - stream.print(ich.neutral); - stream.print(' '); - stream.print(ich.max); - stream.print(' '); - stream.print(ich.fsMode == 0 ? 'A' : (ich.fsMode == 1 ? 'H' : (ich.fsMode == 2 ? 'S' : '?'))); - stream.print(' '); - stream.print(ich.fsValue); - } - - void print(Stream& stream, const ScalerConfig& sc) const - { - stream.print(sc.dimension); - stream.print(' '); - stream.print(sc.channel); - stream.print(' '); - stream.print(sc.minScale); - stream.print(' '); - stream.print(sc.maxScale); - } - - void print(Stream& stream, const ActuatorCondition& ac) const - { - stream.print(ac.id); - stream.print(' '); - stream.print(ac.ch); - stream.print(' '); - stream.print(ac.min); - stream.print(' '); - stream.print(ac.max); - stream.print(' '); - stream.print(ac.logicMode); - stream.print(' '); - stream.print(ac.linkId); - } - - void print(Stream& stream, const MixerEntry& me) const - { - stream.print(me.src); - stream.print(' '); - stream.print(me.dst); - stream.print(' '); - stream.print(me.rate); - } - - void print(Stream& stream, const SerialPortConfig& sc) const - { - stream.print(sc.functionMask); - stream.print(' '); - stream.print(sc.baud); - stream.print(' '); - stream.print(sc.blackboxBaud); - } - - void print(Stream& stream, int32_t v) const - { - if(choices) - { - for(int32_t i = 0; choices[i]; i++) - { - if(i == v) - { - stream.print(FPSTR(choices[i])); - return; - } - } - } - stream.print(v); - } - - void update(const char ** args) const - { - const char * v = args[2]; - 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 ? v : "")); - break; - case PARAM_BITMASK: - if(!v) return; - if(*v == '0') - { - *reinterpret_cast(addr) &= ~(1ul << maxLen); - } - if(*v == '1') - { - *reinterpret_cast(addr) |= (1ul << maxLen); - } - 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: - break; - } - } - - void write(OutputChannelConfig& och, const char ** args) const - { - if(args[2]) och.servo = *args[2] == 'S'; - if(args[3]) och.reverse = *args[3] == 'R'; - if(args[4]) och.min = String(args[4]).toInt(); - if(args[5]) och.neutral = String(args[5]).toInt(); - if(args[6]) och.max = String(args[6]).toInt(); - } - - void write(InputChannelConfig& ich, const char ** args) const - { - if(args[2]) ich.map = String(args[2]).toInt(); - if(args[3]) ich.min = String(args[3]).toInt(); - if(args[4]) ich.neutral = String(args[4]).toInt(); - if(args[5]) ich.max = String(args[5]).toInt(); - if(args[6]) ich.fsMode = *args[6] == 'A' ? 0 : (*args[6] == 'H' ? 1 : (*args[6] == 'S' ? 2 : 0)); - if(args[7]) ich.fsValue = String(args[7]).toInt(); - } - - void write(ScalerConfig& sc, const char ** args) const - { - if(args[2]) sc.dimension = (ScalerDimension)String(args[2]).toInt(); - if(args[3]) sc.channel = String(args[3]).toInt(); - if(args[4]) sc.minScale = String(args[4]).toInt(); - if(args[5]) sc.maxScale = String(args[5]).toInt(); - } - - void write(ActuatorCondition& ac, const char ** args) const - { - if(args[2]) ac.id = String(args[2]).toInt(); - if(args[3]) ac.ch = String(args[3]).toInt(); - if(args[4]) ac.min = String(args[4]).toInt(); - if(args[5]) ac.max = String(args[5]).toInt(); - if(args[6]) ac.logicMode = String(args[6]).toInt(); - if(args[7]) ac.linkId = String(args[7]).toInt(); - } - - void write(MixerEntry& ac, const char ** args) const - { - if(args[2]) ac.src = constrain(String(args[2]).toInt(), 0, MIXER_SOURCE_MAX - 1); - if(args[3]) ac.dst = constrain(String(args[3]).toInt(), 0, (int)(OUTPUT_CHANNELS - 1)); - if(args[4]) ac.rate = constrain(String(args[4]).toInt(), -1000, 1000); - } - - void write(SerialPortConfig& sc, const char ** args) const - { - if(args[2]) sc.functionMask = String(args[2]).toInt(); - if(args[3]) sc.baud = String(args[3]).toInt(); - if(args[4]) sc.blackboxBaud = String(args[4]).toInt(); - } - - template - void write(const T v) const - { - *reinterpret_cast(addr) = v; - } - - void write(const String& v) const - { - *addr = 0; - strncat(addr, v.c_str(), maxLen); - } - - int32_t parse(const char * v) const - { - if(choices) - { - for(int32_t i = 0; choices[i]; i++) - { - if(strcasecmp_P(v, choices[i]) == 0) return i; - } - } - String tmp = v; - return tmp.toInt(); - } - - const char * name; - ParamType type; - char * addr; - const char ** choices; - size_t maxLen; - }; - - Cli(Model& model): _model(model), _ignore(false), _active(false) - { - _params = initialize(_model.config); - } - - static const Param * initialize(ModelConfig& c) - { - const char ** busDevChoices = Device::BusDevice::getNames(); - const char ** gyroDevChoices = Device::GyroDevice::getNames(); - const char ** baroDevChoices = Device::BaroDevice::getNames(); - const char ** magDevChoices = Device::MagDevice::getNames(); - - const char ** fusionModeChoices = FusionConfig::getModeNames(); - static const char* gyroDlpfChoices[] = { PSTR("256Hz"), PSTR("188Hz"), PSTR("98Hz"), PSTR("42Hz"), PSTR("20Hz"), PSTR("10Hz"), PSTR("5Hz"), PSTR("EXPERIMENTAL"), NULL }; - static const char* debugModeChoices[] = { PSTR("NONE"), PSTR("CYCLETIME"), PSTR("BATTERY"), PSTR("GYRO_FILTERED"), PSTR("ACCELEROMETER"), PSTR("PIDLOOP"), PSTR("GYRO_SCALED"), PSTR("RC_INTERPOLATION"), - PSTR("ANGLERATE"), PSTR("ESC_SENSOR"), PSTR("SCHEDULER"), PSTR("STACK"), PSTR("ESC_SENSOR_RPM"), PSTR("ESC_SENSOR_TMP"), PSTR("ALTITUDE"), PSTR("FFT"), - PSTR("FFT_TIME"), PSTR("FFT_FREQ"), PSTR("RX_FRSKY_SPI"), PSTR("RX_SFHSS_SPI"), PSTR("GYRO_RAW"), PSTR("DUAL_GYRO_RAW"), PSTR("DUAL_GYRO_DIFF"), - PSTR("MAX7456_SIGNAL"), PSTR("MAX7456_SPICLOCK"), PSTR("SBUS"), PSTR("FPORT"), PSTR("RANGEFINDER"), PSTR("RANGEFINDER_QUALITY"), PSTR("LIDAR_TF"), - PSTR("ADC_INTERNAL"), PSTR("RUNAWAY_TAKEOFF"), PSTR("SDIO"), PSTR("CURRENT_SENSOR"), PSTR("USB"), PSTR("SMARTAUDIO"), PSTR("RTH"), PSTR("ITERM_RELAX"), - PSTR("ACRO_TRAINER"), PSTR("RC_SMOOTHING"), PSTR("RX_SIGNAL_LOSS"), PSTR("RC_SMOOTHING_RATE"), PSTR("ANTI_GRAVITY"), PSTR("DYN_LPF"), PSTR("RX_SPEKTRUM_SPI"), - PSTR("DSHOT_RPM_TELEMETRY"), PSTR("RPM_FILTER"), PSTR("D_MIN"), PSTR("AC_CORRECTION"), PSTR("AC_ERROR"), PSTR("DUAL_GYRO_SCALED"), PSTR("DSHOT_RPM_ERRORS"), - PSTR("CRSF_LINK_STATISTICS_UPLINK"), PSTR("CRSF_LINK_STATISTICS_PWR"), PSTR("CRSF_LINK_STATISTICS_DOWN"), PSTR("BARO"), PSTR("GPS_RESCUE_THROTTLE_PID"), - PSTR("DYN_IDLE"), PSTR("FF_LIMIT"), PSTR("FF_INTERPOLATED"), PSTR("BLACKBOX_OUTPUT"), PSTR("GYRO_SAMPLE"), PSTR("RX_TIMING"), NULL }; - static const char* filterTypeChoices[] = { PSTR("PT1"), PSTR("BIQUAD"), PSTR("PT2"), PSTR("PT3"), PSTR("NOTCH"), PSTR("NOTCH_DF1"), PSTR("BPF"), PSTR("FO"), PSTR("FIR2"), PSTR("MEDIAN3"), PSTR("NONE"), NULL }; - static const char* alignChoices[] = { PSTR("DEFAULT"), PSTR("CW0"), PSTR("CW90"), PSTR("CW180"), PSTR("CW270"), PSTR("CW0_FLIP"), PSTR("CW90_FLIP"), PSTR("CW180_FLIP"), PSTR("CW270_FLIP"), PSTR("CUSTOM"), NULL }; - static const char* mixerTypeChoices[] = { PSTR("NONE"), PSTR("TRI"), PSTR("QUADP"), PSTR("QUADX"), PSTR("BI"), - PSTR("GIMBAL"), PSTR("Y6"), PSTR("HEX6"), PSTR("FWING"), PSTR("Y4"), - PSTR("HEX6X"), PSTR("OCTOX8"), PSTR("OCTOFLATP"), PSTR("OCTOFLATX"), PSTR("AIRPLANE"), - PSTR("HELI120"), PSTR("HELI90"), PSTR("VTAIL4"), PSTR("HEX6H"), PSTR("PPMSERVO"), - PSTR("DUALCOPTER"), PSTR("SINGLECOPTER"), PSTR("ATAIL4"), PSTR("CUSTOM"), PSTR("CUSTOMAIRPLANE"), - PSTR("CUSTOMTRI"), PSTR("QUADX1234"), NULL }; - static const char* interpolChoices[] = { PSTR("NONE"), PSTR("DEFAULT"), PSTR("AUTO"), PSTR("MANUAL"), NULL }; - static const char* protocolChoices[] = { PSTR("PWM"), PSTR("ONESHOT125"), PSTR("ONESHOT42"), PSTR("MULTISHOT"), PSTR("BRUSHED"), - PSTR("DSHOT150"), PSTR("DSHOT300"), PSTR("DSHOT600"), PSTR("PROSHOT1000"), PSTR("DISABLED"), NULL }; - static const char* inputRateTypeChoices[] = { PSTR("BETAFLIGHT"), PSTR("RACEFLIGHT"), PSTR("KISS"), PSTR("ACTUAL"), PSTR("QUICK"), NULL }; - static const char* throtleLimitTypeChoices[] = { PSTR("NONE"), PSTR("SCALE"), PSTR("CLIP"), NULL }; - static const char* inputFilterChoices[] = { PSTR("INTERPOLATION"), PSTR("FILTER"), NULL }; - static const char* inputItermRelaxChoices[] = { PSTR("OFF"), PSTR("RP"), PSTR("RPY"), PSTR("RP_INC"), PSTR("RPY_INC"), NULL }; - - static const char* voltageSourceChoices[] = { PSTR("NONE"), PSTR("ADC"), NULL }; - static const char* currentSourceChoices[] = { PSTR("NONE"), PSTR("ADC"), NULL }; - static const char* blackboxDevChoices[] = { PSTR("NONE"), PSTR("FLASH"), PSTR("SD_CARD"), PSTR("SERIAL"), NULL }; - static const char* blackboxModeChoices[] = { PSTR("NORMAL"), PSTR("TEST"), PSTR("ALWAYS"), NULL }; - - size_t i = 0; - static const Param params[] = { - - Param(PSTR("feature_dyn_notch"), &c.featureMask, 29), - Param(PSTR("feature_motor_stop"), &c.featureMask, 4), - Param(PSTR("feature_rx_ppm"), &c.featureMask, 0), - Param(PSTR("feature_rx_serial"), &c.featureMask, 3), - Param(PSTR("feature_rx_spi"), &c.featureMask, 25), - Param(PSTR("feature_soft_serial"), &c.featureMask, 6), - Param(PSTR("feature_telemetry"), &c.featureMask, 10), - - Param(PSTR("debug_mode"), &c.debug.mode, debugModeChoices), - Param(PSTR("debug_axis"), &c.debug.axis), - - Param(PSTR("gyro_bus"), &c.gyro.bus, busDevChoices), - Param(PSTR("gyro_dev"), &c.gyro.dev, gyroDevChoices), - Param(PSTR("gyro_dlpf"), &c.gyro.dlpf, gyroDlpfChoices), - Param(PSTR("gyro_align"), &c.gyro.align, alignChoices), - Param(PSTR("gyro_lpf_type"), &c.gyro.filter.type, filterTypeChoices), - Param(PSTR("gyro_lpf_freq"), &c.gyro.filter.freq), - Param(PSTR("gyro_lpf2_type"), &c.gyro.filter2.type, filterTypeChoices), - Param(PSTR("gyro_lpf2_freq"), &c.gyro.filter2.freq), - Param(PSTR("gyro_lpf3_type"), &c.gyro.filter3.type, filterTypeChoices), - Param(PSTR("gyro_lpf3_freq"), &c.gyro.filter3.freq), - Param(PSTR("gyro_notch1_freq"), &c.gyro.notch1Filter.freq), - Param(PSTR("gyro_notch1_cutoff"), &c.gyro.notch1Filter.cutoff), - Param(PSTR("gyro_notch2_freq"), &c.gyro.notch2Filter.freq), - Param(PSTR("gyro_notch2_cutoff"), &c.gyro.notch2Filter.cutoff), - Param(PSTR("gyro_dyn_lpf_min"), &c.gyro.dynLpfFilter.cutoff), - Param(PSTR("gyro_dyn_lpf_max"), &c.gyro.dynLpfFilter.freq), - Param(PSTR("gyro_dyn_notch_q"), &c.gyro.dynamicFilter.q), - Param(PSTR("gyro_dyn_notch_count"), &c.gyro.dynamicFilter.count), - Param(PSTR("gyro_dyn_notch_min"), &c.gyro.dynamicFilter.min_freq), - Param(PSTR("gyro_dyn_notch_max"), &c.gyro.dynamicFilter.max_freq), - Param(PSTR("gyro_rpm_harmonics"), &c.gyro.rpmFilter.harmonics), - Param(PSTR("gyro_rpm_q"), &c.gyro.rpmFilter.q), - Param(PSTR("gyro_rpm_min_freq"), &c.gyro.rpmFilter.minFreq), - Param(PSTR("gyro_rpm_fade"), &c.gyro.rpmFilter.fade), - Param(PSTR("gyro_rpm_weight_1"), &c.gyro.rpmFilter.weights[0]), - Param(PSTR("gyro_rpm_weight_2"), &c.gyro.rpmFilter.weights[1]), - Param(PSTR("gyro_rpm_weight_3"), &c.gyro.rpmFilter.weights[2]), - Param(PSTR("gyro_rpm_tlm_lpf_freq"), &c.gyro.rpmFilter.freqLpf), - Param(PSTR("gyro_offset_x"), &c.gyro.bias[0]), - Param(PSTR("gyro_offset_y"), &c.gyro.bias[1]), - Param(PSTR("gyro_offset_z"), &c.gyro.bias[2]), - - Param(PSTR("accel_bus"), &c.accel.bus, busDevChoices), - Param(PSTR("accel_dev"), &c.accel.dev, gyroDevChoices), - Param(PSTR("accel_lpf_type"), &c.accel.filter.type, filterTypeChoices), - Param(PSTR("accel_lpf_freq"), &c.accel.filter.freq), - Param(PSTR("accel_offset_x"), &c.accel.bias[0]), - Param(PSTR("accel_offset_y"), &c.accel.bias[1]), - Param(PSTR("accel_offset_z"), &c.accel.bias[2]), - - Param(PSTR("mag_bus"), &c.mag.bus, busDevChoices), - Param(PSTR("mag_dev"), &c.mag.dev, magDevChoices), - Param(PSTR("mag_align"), &c.mag.align, alignChoices), - Param(PSTR("mag_filter_type"), &c.mag.filter.type, filterTypeChoices), - Param(PSTR("mag_filter_lpf"), &c.mag.filter.freq), - Param(PSTR("mag_offset_x"), &c.mag.offset[0]), - Param(PSTR("mag_offset_y"), &c.mag.offset[1]), - Param(PSTR("mag_offset_z"), &c.mag.offset[2]), - Param(PSTR("mag_scale_x"), &c.mag.scale[0]), - Param(PSTR("mag_scale_y"), &c.mag.scale[1]), - Param(PSTR("mag_scale_z"), &c.mag.scale[2]), - - Param(PSTR("baro_bus"), &c.baro.bus, busDevChoices), - Param(PSTR("baro_dev"), &c.baro.dev, baroDevChoices), - Param(PSTR("baro_lpf_type"), &c.baro.filter.type, filterTypeChoices), - Param(PSTR("baro_lpf_freq"), &c.baro.filter.freq), - - Param(PSTR("board_align_roll"), &c.boardAlignment[0]), - Param(PSTR("board_align_pitch"), &c.boardAlignment[1]), - Param(PSTR("board_align_yaw"), &c.boardAlignment[2]), - - Param(PSTR("vbat_source"), &c.vbat.source, voltageSourceChoices), - Param(PSTR("vbat_scale"), &c.vbat.scale), - Param(PSTR("vbat_mul"), &c.vbat.resMult), - Param(PSTR("vbat_div"), &c.vbat.resDiv), - Param(PSTR("vbat_cell_warn"), &c.vbat.cellWarning), - - Param(PSTR("ibat_source"), &c.ibat.source, currentSourceChoices), - Param(PSTR("ibat_scale"), &c.ibat.scale), - Param(PSTR("ibat_offset"), &c.ibat.offset), - - Param(PSTR("fusion_mode"), &c.fusion.mode, fusionModeChoices), - Param(PSTR("fusion_gain_p"), &c.fusion.gain), - Param(PSTR("fusion_gain_i"), &c.fusion.gainI), - - Param(PSTR("input_rate_type"), &c.input.rateType, inputRateTypeChoices), - - Param(PSTR("input_roll_rate"), &c.input.rate[0]), - Param(PSTR("input_roll_srate"), &c.input.superRate[0]), - Param(PSTR("input_roll_expo"), &c.input.expo[0]), - Param(PSTR("input_roll_limit"), &c.input.rateLimit[0]), - - Param(PSTR("input_pitch_rate"), &c.input.rate[1]), - Param(PSTR("input_pitch_srate"), &c.input.superRate[1]), - Param(PSTR("input_pitch_expo"), &c.input.expo[1]), - Param(PSTR("input_pitch_limit"), &c.input.rateLimit[1]), - - Param(PSTR("input_yaw_rate"), &c.input.rate[2]), - Param(PSTR("input_yaw_srate"), &c.input.superRate[2]), - Param(PSTR("input_yaw_expo"), &c.input.expo[2]), - Param(PSTR("input_yaw_limit"), &c.input.rateLimit[2]), - - Param(PSTR("input_deadband"), &c.input.deadband), - - Param(PSTR("input_min"), &c.input.minRc), - Param(PSTR("input_mid"), &c.input.midRc), - Param(PSTR("input_max"), &c.input.maxRc), - - Param(PSTR("input_interpolation"), &c.input.interpolationMode, interpolChoices), - Param(PSTR("input_interpolation_interval"), &c.input.interpolationInterval), - - 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_rssi_channel"), &c.input.rssiChannel), - - Param(PSTR("input_0"), &c.input.channel[0]), - Param(PSTR("input_1"), &c.input.channel[1]), - Param(PSTR("input_2"), &c.input.channel[2]), - Param(PSTR("input_3"), &c.input.channel[3]), - Param(PSTR("input_4"), &c.input.channel[4]), - Param(PSTR("input_5"), &c.input.channel[5]), - Param(PSTR("input_6"), &c.input.channel[6]), - Param(PSTR("input_7"), &c.input.channel[7]), - Param(PSTR("input_8"), &c.input.channel[8]), - Param(PSTR("input_9"), &c.input.channel[9]), - Param(PSTR("input_10"), &c.input.channel[10]), - Param(PSTR("input_11"), &c.input.channel[11]), - Param(PSTR("input_12"), &c.input.channel[12]), - Param(PSTR("input_13"), &c.input.channel[13]), - Param(PSTR("input_14"), &c.input.channel[14]), - Param(PSTR("input_15"), &c.input.channel[15]), - - Param(PSTR("failsafe_delay"), &c.failsafe.delay), - Param(PSTR("failsafe_kill_switch"), &c.failsafe.killSwitch), - -#ifdef ESPFC_SERIAL_0 - Param(PSTR("serial_0"), &c.serial[SERIAL_UART_0]), -#endif -#ifdef ESPFC_SERIAL_1 - Param(PSTR("serial_1"), &c.serial[SERIAL_UART_1]), -#endif -#ifdef ESPFC_SERIAL_2 - Param(PSTR("serial_2"), &c.serial[SERIAL_UART_2]), -#endif -#ifdef ESPFC_SERIAL_SOFT_0 - Param(PSTR("serial_soft_0"), &c.serial[SERIAL_SOFT_0]), -#endif -#ifdef ESPFC_SERIAL_USB - Param(PSTR("serial_usb"), &c.serial[SERIAL_USB]), -#endif - - Param(PSTR("scaler_0"), &c.scaler[0]), - Param(PSTR("scaler_1"), &c.scaler[1]), - Param(PSTR("scaler_2"), &c.scaler[2]), - - Param(PSTR("mode_0"), &c.conditions[0]), - Param(PSTR("mode_1"), &c.conditions[1]), - Param(PSTR("mode_2"), &c.conditions[2]), - Param(PSTR("mode_3"), &c.conditions[3]), - Param(PSTR("mode_4"), &c.conditions[4]), - Param(PSTR("mode_5"), &c.conditions[5]), - Param(PSTR("mode_6"), &c.conditions[6]), - Param(PSTR("mode_7"), &c.conditions[7]), - - Param(PSTR("pid_sync"), &c.loopSync), - - Param(PSTR("pid_roll_p"), &c.pid[FC_PID_ROLL].P), - Param(PSTR("pid_roll_i"), &c.pid[FC_PID_ROLL].I), - Param(PSTR("pid_roll_d"), &c.pid[FC_PID_ROLL].D), - Param(PSTR("pid_roll_f"), &c.pid[FC_PID_ROLL].F), - - Param(PSTR("pid_pitch_p"), &c.pid[FC_PID_PITCH].P), - Param(PSTR("pid_pitch_i"), &c.pid[FC_PID_PITCH].I), - Param(PSTR("pid_pitch_d"), &c.pid[FC_PID_PITCH].D), - Param(PSTR("pid_pitch_f"), &c.pid[FC_PID_PITCH].F), - - Param(PSTR("pid_yaw_p"), &c.pid[FC_PID_YAW].P), - Param(PSTR("pid_yaw_i"), &c.pid[FC_PID_YAW].I), - Param(PSTR("pid_yaw_d"), &c.pid[FC_PID_YAW].D), - Param(PSTR("pid_yaw_f"), &c.pid[FC_PID_YAW].F), - - Param(PSTR("pid_level_p"), &c.pid[FC_PID_LEVEL].P), - Param(PSTR("pid_level_i"), &c.pid[FC_PID_LEVEL].I), - Param(PSTR("pid_level_d"), &c.pid[FC_PID_LEVEL].D), - - Param(PSTR("pid_level_angle_limit"), &c.level.angleLimit), - Param(PSTR("pid_level_rate_limit"), &c.level.rateLimit), - Param(PSTR("pid_level_lpf_type"), &c.level.ptermFilter.type, filterTypeChoices), - Param(PSTR("pid_level_lpf_freq"), &c.level.ptermFilter.freq), - - Param(PSTR("pid_yaw_lpf_type"), &c.yaw.filter.type, filterTypeChoices), - Param(PSTR("pid_yaw_lpf_freq"), &c.yaw.filter.freq), - - Param(PSTR("pid_dterm_lpf_type"), &c.dterm.filter.type, filterTypeChoices), - Param(PSTR("pid_dterm_lpf_freq"), &c.dterm.filter.freq), - Param(PSTR("pid_dterm_lpf2_type"), &c.dterm.filter2.type, filterTypeChoices), - Param(PSTR("pid_dterm_lpf2_freq"), &c.dterm.filter2.freq), - Param(PSTR("pid_dterm_notch_freq"), &c.dterm.notchFilter.freq), - Param(PSTR("pid_dterm_notch_cutoff"), &c.dterm.notchFilter.cutoff), - Param(PSTR("pid_dterm_dyn_lpf_min"), &c.dterm.dynLpfFilter.cutoff), - Param(PSTR("pid_dterm_dyn_lpf_max"), &c.dterm.dynLpfFilter.freq), - - Param(PSTR("pid_dterm_weight"), &c.dterm.setpointWeight), - Param(PSTR("pid_iterm_limit"), &c.iterm.limit), - Param(PSTR("pid_iterm_zero"), &c.iterm.lowThrottleZeroIterm), - Param(PSTR("pid_iterm_relax"), &c.iterm.relax, inputItermRelaxChoices), - Param(PSTR("pid_iterm_relax_cutoff"), &c.iterm.relaxCutoff), - Param(PSTR("pid_tpa_scale"), &c.controller.tpaScale), - Param(PSTR("pid_tpa_breakpoint"), &c.controller.tpaBreakpoint), - - Param(PSTR("mixer_sync"), &c.mixerSync), - Param(PSTR("mixer_type"), &c.mixer.type, mixerTypeChoices), - Param(PSTR("mixer_yaw_reverse"), &c.mixer.yawReverse), - Param(PSTR("mixer_throttle_limit_type"), &c.output.throttleLimitType, throtleLimitTypeChoices), - Param(PSTR("mixer_throttle_limit_percent"), &c.output.throttleLimitPercent), - Param(PSTR("mixer_output_limit"), &c.output.motorLimit), - - Param(PSTR("output_motor_protocol"), &c.output.protocol, protocolChoices), - Param(PSTR("output_motor_async"), &c.output.async), - Param(PSTR("output_motor_rate"), &c.output.rate), -#ifdef ESPFC_DSHOT_TELEMETRY - Param(PSTR("output_motor_poles"), &c.output.motorPoles), -#endif - Param(PSTR("output_servo_rate"), &c.output.servoRate), - - Param(PSTR("output_min_command"), &c.output.minCommand), - Param(PSTR("output_min_throttle"), &c.output.minThrottle), - Param(PSTR("output_max_throttle"), &c.output.maxThrottle), - Param(PSTR("output_dshot_idle"), &c.output.dshotIdle), -#ifdef ESPFC_DSHOT_TELEMETRY - Param(PSTR("output_dshot_telemetry"), &c.output.dshotTelemetry), -#endif - Param(PSTR("output_0"), &c.output.channel[0]), - Param(PSTR("output_1"), &c.output.channel[1]), - Param(PSTR("output_2"), &c.output.channel[2]), - Param(PSTR("output_3"), &c.output.channel[3]), -#if ESPFC_OUTPUT_COUNT > 4 - Param(PSTR("output_4"), &c.output.channel[4]), -#endif -#if ESPFC_OUTPUT_COUNT > 5 - Param(PSTR("output_5"), &c.output.channel[5]), -#endif -#if ESPFC_OUTPUT_COUNT > 6 - Param(PSTR("output_6"), &c.output.channel[6]), -#endif -#if ESPFC_OUTPUT_COUNT > 7 - Param(PSTR("output_7"), &c.output.channel[7]), -#endif -#ifdef ESPFC_INPUT - Param(PSTR("pin_input_rx"), &c.pin[PIN_INPUT_RX]), -#endif - Param(PSTR("pin_output_0"), &c.pin[PIN_OUTPUT_0]), - Param(PSTR("pin_output_1"), &c.pin[PIN_OUTPUT_1]), - Param(PSTR("pin_output_2"), &c.pin[PIN_OUTPUT_2]), - Param(PSTR("pin_output_3"), &c.pin[PIN_OUTPUT_3]), -#if ESPFC_OUTPUT_COUNT > 4 - Param(PSTR("pin_output_4"), &c.pin[PIN_OUTPUT_4]), -#endif -#if ESPFC_OUTPUT_COUNT > 5 - Param(PSTR("pin_output_5"), &c.pin[PIN_OUTPUT_5]), -#endif -#if ESPFC_OUTPUT_COUNT > 6 - Param(PSTR("pin_output_6"), &c.pin[PIN_OUTPUT_6]), -#endif -#if ESPFC_OUTPUT_COUNT > 7 - Param(PSTR("pin_output_7"), &c.pin[PIN_OUTPUT_7]), -#endif - Param(PSTR("pin_buzzer"), &c.pin[PIN_BUZZER]), -#if defined(ESPFC_SERIAL_0) && defined(ESPFC_SERIAL_REMAP_PINS) - Param(PSTR("pin_serial_0_tx"), &c.pin[PIN_SERIAL_0_TX]), - Param(PSTR("pin_serial_0_rx"), &c.pin[PIN_SERIAL_0_RX]), -#endif -#if defined(ESPFC_SERIAL_1) && defined(ESPFC_SERIAL_REMAP_PINS) - Param(PSTR("pin_serial_1_tx"), &c.pin[PIN_SERIAL_1_TX]), - Param(PSTR("pin_serial_1_rx"), &c.pin[PIN_SERIAL_1_RX]), -#endif -#if defined(ESPFC_SERIAL_2) && defined(ESPFC_SERIAL_REMAP_PINS) - Param(PSTR("pin_serial_2_tx"), &c.pin[PIN_SERIAL_2_TX]), - Param(PSTR("pin_serial_2_rx"), &c.pin[PIN_SERIAL_2_RX]), -#endif -#ifdef ESPFC_I2C_0 - Param(PSTR("pin_i2c_scl"), &c.pin[PIN_I2C_0_SCL]), - Param(PSTR("pin_i2c_sda"), &c.pin[PIN_I2C_0_SDA]), -#endif -#ifdef ESPFC_ADC_0 - Param(PSTR("pin_input_adc_0"), &c.pin[PIN_INPUT_ADC_0]), -#endif -#ifdef ESPFC_ADC_1 - Param(PSTR("pin_input_adc_1"), &c.pin[PIN_INPUT_ADC_1]), -#endif -#ifdef ESPFC_SPI_0 - Param(PSTR("pin_spi_0_sck"), &c.pin[PIN_SPI_0_SCK]), - Param(PSTR("pin_spi_0_mosi"), &c.pin[PIN_SPI_0_MOSI]), - Param(PSTR("pin_spi_0_miso"), &c.pin[PIN_SPI_0_MISO]), - Param(PSTR("pin_spi_cs_0"), &c.pin[PIN_SPI_CS0]), - Param(PSTR("pin_spi_cs_1"), &c.pin[PIN_SPI_CS1]), - Param(PSTR("pin_spi_cs_2"), &c.pin[PIN_SPI_CS2]), -#endif - Param(PSTR("pin_buzzer_invert"), &c.buzzer.inverted), - -#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("blackbox_dev"), &c.blackbox.dev, blackboxDevChoices), - Param(PSTR("blackbox_mode"), &c.blackbox.mode, blackboxModeChoices), - Param(PSTR("blackbox_rate"), &c.blackbox.pDenom), - Param(PSTR("blackbox_log_acc"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_ACC), - Param(PSTR("blackbox_log_alt"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_ALTITUDE), - Param(PSTR("blackbox_log_bat"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_BATTERY), - Param(PSTR("blackbox_log_debug"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_DEBUG_LOG), - Param(PSTR("blackbox_log_gps"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_GPS), - Param(PSTR("blackbox_log_gyro"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_GYRO), - Param(PSTR("blackbox_log_gyro_raw"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_GYROUNFILT), - Param(PSTR("blackbox_log_mag"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_MAG), - Param(PSTR("blackbox_log_motor"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_MOTOR), - Param(PSTR("blackbox_log_pid"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_PID), - Param(PSTR("blackbox_log_rc"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_RC_COMMANDS), - Param(PSTR("blackbox_log_rpm"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_RPM), - Param(PSTR("blackbox_log_rssi"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_RSSI), - Param(PSTR("blackbox_log_sp"), &c.blackbox.fieldsMask, BLACKBOX_FIELD_SETPOINT), - - Param(PSTR("model_name"), PARAM_STRING, &c.modelName[0], NULL, MODEL_NAME_LEN), - -#ifdef ESPFC_SERIAL_SOFT_0_WIFI - Param(PSTR("wifi_ssid"), PARAM_STRING, &c.wireless.ssid[0], NULL, WirelessConfig::MAX_LEN), - Param(PSTR("wifi_pass"), PARAM_STRING, &c.wireless.pass[0], NULL, WirelessConfig::MAX_LEN), - Param(PSTR("wifi_tcp_port"), &c.wireless.port), -#endif - - Param(PSTR("mix_outputs"), &c.customMixerCount), - Param(PSTR("mix_0"), &c.customMixes[i++]), - Param(PSTR("mix_1"), &c.customMixes[i++]), - Param(PSTR("mix_2"), &c.customMixes[i++]), - Param(PSTR("mix_3"), &c.customMixes[i++]), - Param(PSTR("mix_4"), &c.customMixes[i++]), - Param(PSTR("mix_5"), &c.customMixes[i++]), - Param(PSTR("mix_6"), &c.customMixes[i++]), - Param(PSTR("mix_7"), &c.customMixes[i++]), - Param(PSTR("mix_8"), &c.customMixes[i++]), - Param(PSTR("mix_9"), &c.customMixes[i++]), - Param(PSTR("mix_10"), &c.customMixes[i++]), - Param(PSTR("mix_11"), &c.customMixes[i++]), - Param(PSTR("mix_12"), &c.customMixes[i++]), - Param(PSTR("mix_13"), &c.customMixes[i++]), - Param(PSTR("mix_14"), &c.customMixes[i++]), - Param(PSTR("mix_15"), &c.customMixes[i++]), - Param(PSTR("mix_16"), &c.customMixes[i++]), - Param(PSTR("mix_17"), &c.customMixes[i++]), - Param(PSTR("mix_18"), &c.customMixes[i++]), - Param(PSTR("mix_19"), &c.customMixes[i++]), - Param(PSTR("mix_20"), &c.customMixes[i++]), - Param(PSTR("mix_21"), &c.customMixes[i++]), - Param(PSTR("mix_22"), &c.customMixes[i++]), - Param(PSTR("mix_23"), &c.customMixes[i++]), - Param(PSTR("mix_24"), &c.customMixes[i++]), - Param(PSTR("mix_25"), &c.customMixes[i++]), - Param(PSTR("mix_26"), &c.customMixes[i++]), - Param(PSTR("mix_27"), &c.customMixes[i++]), - Param(PSTR("mix_28"), &c.customMixes[i++]), - Param(PSTR("mix_29"), &c.customMixes[i++]), - Param(PSTR("mix_30"), &c.customMixes[i++]), - Param(PSTR("mix_31"), &c.customMixes[i++]), - Param(PSTR("mix_32"), &c.customMixes[i++]), - Param(PSTR("mix_33"), &c.customMixes[i++]), - Param(PSTR("mix_34"), &c.customMixes[i++]), - Param(PSTR("mix_35"), &c.customMixes[i++]), - Param(PSTR("mix_36"), &c.customMixes[i++]), - Param(PSTR("mix_37"), &c.customMixes[i++]), - Param(PSTR("mix_38"), &c.customMixes[i++]), - Param(PSTR("mix_39"), &c.customMixes[i++]), - Param(PSTR("mix_40"), &c.customMixes[i++]), - Param(PSTR("mix_41"), &c.customMixes[i++]), - Param(PSTR("mix_42"), &c.customMixes[i++]), - Param(PSTR("mix_43"), &c.customMixes[i++]), - Param(PSTR("mix_44"), &c.customMixes[i++]), - Param(PSTR("mix_45"), &c.customMixes[i++]), - Param(PSTR("mix_46"), &c.customMixes[i++]), - Param(PSTR("mix_47"), &c.customMixes[i++]), - Param(PSTR("mix_48"), &c.customMixes[i++]), - Param(PSTR("mix_49"), &c.customMixes[i++]), - Param(PSTR("mix_50"), &c.customMixes[i++]), - Param(PSTR("mix_51"), &c.customMixes[i++]), - Param(PSTR("mix_52"), &c.customMixes[i++]), - Param(PSTR("mix_53"), &c.customMixes[i++]), - Param(PSTR("mix_54"), &c.customMixes[i++]), - Param(PSTR("mix_55"), &c.customMixes[i++]), - Param(PSTR("mix_56"), &c.customMixes[i++]), - Param(PSTR("mix_57"), &c.customMixes[i++]), - Param(PSTR("mix_58"), &c.customMixes[i++]), - Param(PSTR("mix_59"), &c.customMixes[i++]), - Param(PSTR("mix_60"), &c.customMixes[i++]), - Param(PSTR("mix_61"), &c.customMixes[i++]), - Param(PSTR("mix_62"), &c.customMixes[i++]), - Param(PSTR("mix_63"), &c.customMixes[i++]), - - Param() // terminate - }; - return params; - } - - bool process(const char c, CliCmd& cmd, Stream& stream) - { - // configurator handshake - if(!_active && c == '#') - { - //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(); - _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; - } - - bool endl = c == '\n' || c == '\r'; - if(cmd.index && endl) - { - parse(cmd); - execute(cmd, stream); - cmd = CliCmd(); - return true; - } - - if(c == '#') _ignore = true; - else if(endl) _ignore = false; - - // don't put characters into buffer in specific conditions - if(_ignore || endl || cmd.index >= CLI_BUFF_SIZE - 1) return false; - - if(c == '\b') // handle backspace - { - cmd.buff[--cmd.index] = '\0'; - } - else - { - cmd.buff[cmd.index] = c; - cmd.buff[++cmd.index] = '\0'; - } - return false; - } - - void parse(CliCmd& cmd) - { - const char * DELIM = " \t"; - char * pch = strtok(cmd.buff, DELIM); - size_t count = 0; - while(pch) - { - cmd.args[count++] = pch; - pch = strtok(NULL, DELIM); - } - } - - void execute(CliCmd& cmd, Stream& s) - { - if(cmd.args[0]) s.print(F("# ")); - for(size_t i = 0; i < CLI_ARGS_SIZE; ++i) - { - if(!cmd.args[i]) break; - s.print(cmd.args[i]); - s.print(' '); - } - s.println(); - - if(!cmd.args[0]) return; - - if(strcmp_P(cmd.args[0], PSTR("help")) == 0) - { - static const char * helps[] = { - PSTR("available commands:"), - PSTR(" help"), PSTR(" dump"), PSTR(" get param"), PSTR(" set param value ..."), PSTR(" cal [gyro]"), - PSTR(" defaults"), PSTR(" save"), PSTR(" reboot"), PSTR(" scaler"), PSTR(" mixer"), - PSTR(" stats"), PSTR(" status"), PSTR(" devinfo"), PSTR(" version"), PSTR(" logs"), - //PSTR(" load"), PSTR(" eeprom"), - //PSTR(" fsinfo"), PSTR(" fsformat"), PSTR(" log"), - NULL - }; - for(const char ** ptr = helps; *ptr; ptr++) - { - s.println(FPSTR(*ptr)); - } - } - else if(strcmp_P(cmd.args[0], PSTR("version")) == 0) - { - 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("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); - } -#endif -#if defined(ESPFC_FREE_RTOS) - else if(strcmp_P(cmd.args[0], PSTR("tasks")) == 0) - { - printVersion(s); - s.println(); - - size_t numTasks = uxTaskGetNumberOfTasks(); - - s.print(F("num tasks: ")); - s.print(numTasks); - s.println(); - } -#endif - else if(strcmp_P(cmd.args[0], PSTR("devinfo")) == 0) - { - printVersion(s); - s.println(); - - s.print(F("config size: ")); - s.println(sizeof(ModelConfig)); - - s.print(F(" free heap: ")); - s.println(targetFreeHeap()); - - s.print(F(" cpu freq: ")); - s.print(targetCpuFreq()); - s.println(F(" MHz")); - } - else if(strcmp_P(cmd.args[0], PSTR("get")) == 0) - { - bool found = false; - for(size_t i = 0; _params[i].name; ++i) - { - String ts = FPSTR(_params[i].name); - if(!cmd.args[1] || ts.indexOf(cmd.args[1]) >= 0) - { - print(_params[i], s); - found = true; - } - } - if(!found) - { - s.print(F("param not found: ")); - s.print(cmd.args[1]); - } - s.println(); - } - else if(strcmp_P(cmd.args[0], PSTR("set")) == 0) - { - if(!cmd.args[1]) - { - s.println(F("param required")); - s.println(); - return; - } - bool found = false; - for(size_t i = 0; _params[i].name; ++i) - { - if(strcmp_P(cmd.args[1], _params[i].name) == 0) - { - _params[i].update(cmd.args); - print(_params[i], s); - found = true; - break; - } - } - if(!found) - { - s.print(F("param not found: ")); - s.println(cmd.args[1]); - } - } - else if(strcmp_P(cmd.args[0], PSTR("dump")) == 0) - { - //s.print(F("# ")); - //printVersion(s); - //s.println(); - s.println(F("defaults")); - for(size_t i = 0; _params[i].name; ++i) - { - print(_params[i], s); - } - s.println(F("save")); - } - else if(strcmp_P(cmd.args[0], PSTR("cal")) == 0) - { - if(!cmd.args[1]) - { - s.print(F(" gyro offset: ")); - s.print(_model.config.gyro.bias[0]); s.print(' '); - s.print(_model.config.gyro.bias[1]); s.print(' '); - s.print(_model.config.gyro.bias[2]); s.print(F(" [")); - s.print(Utils::toDeg(_model.state.gyro.bias[0])); s.print(' '); - s.print(Utils::toDeg(_model.state.gyro.bias[1])); s.print(' '); - s.print(Utils::toDeg(_model.state.gyro.bias[2])); s.println(F("]")); - - s.print(F("accel offset: ")); - s.print(_model.config.accel.bias[0]); s.print(' '); - s.print(_model.config.accel.bias[1]); s.print(' '); - s.print(_model.config.accel.bias[2]); s.print(F(" [")); - s.print(_model.state.accel.bias[0]); s.print(' '); - s.print(_model.state.accel.bias[1]); s.print(' '); - s.print(_model.state.accel.bias[2]); s.println(F("]")); - - s.print(F(" mag offset: ")); - s.print(_model.config.mag.offset[0]); s.print(' '); - s.print(_model.config.mag.offset[1]); s.print(' '); - s.print(_model.config.mag.offset[2]); s.print(F(" [")); - s.print(_model.state.mag.calibrationOffset[0]); s.print(' '); - s.print(_model.state.mag.calibrationOffset[1]); s.print(' '); - s.print(_model.state.mag.calibrationOffset[2]); s.println(F("]")); - - s.print(F(" mag scale: ")); - s.print(_model.config.mag.scale[0]); s.print(' '); - s.print(_model.config.mag.scale[1]); s.print(' '); - s.print(_model.config.mag.scale[2]); s.print(F(" [")); - s.print(_model.state.mag.calibrationScale[0]); s.print(' '); - s.print(_model.state.mag.calibrationScale[1]); s.print(' '); - s.print(_model.state.mag.calibrationScale[2]); s.println(F("]")); - } - else if(strcmp_P(cmd.args[1], PSTR("gyro")) == 0) - { - if(!_model.isModeActive(MODE_ARMED)) _model.calibrateGyro(); - s.println(F("OK")); - } - else if(strcmp_P(cmd.args[1], PSTR("mag")) == 0) - { - if(!_model.isModeActive(MODE_ARMED)) _model.calibrateMag(); - s.println(F("OK")); - } - else if(strcmp_P(cmd.args[1], PSTR("reset_accel")) == 0 || strcmp_P(cmd.args[1], PSTR("reset_all")) == 0) - { - _model.state.accel.bias = VectorFloat(); - s.println(F("OK")); - } - else if(strcmp_P(cmd.args[1], PSTR("reset_gyro")) == 0 || strcmp_P(cmd.args[1], PSTR("reset_all")) == 0) - { - _model.state.gyro.bias = VectorFloat(); - s.println(F("OK")); - } - else if(strcmp_P(cmd.args[1], PSTR("reset_mag")) == 0 || strcmp_P(cmd.args[1], PSTR("reset_all")) == 0) - { - _model.state.mag.calibrationOffset = VectorFloat(); - _model.state.mag.calibrationScale = VectorFloat(1.f, 1.f, 1.f); - s.println(F("OK")); - } - } - else if(strcmp_P(cmd.args[0], PSTR("preset")) == 0) - { - if(!cmd.args[1]) - { - s.println(F("Available presets: scaler, modes, micrus, brobot")); - } - else if(strcmp_P(cmd.args[1], PSTR("scaler")) == 0) - { - _model.config.scaler[0].dimension = (ScalerDimension)(ACT_INNER_P | ACT_AXIS_PITCH | ACT_AXIS_ROLL); - _model.config.scaler[0].channel = 5; - _model.config.scaler[0].minScale = 25; //% - _model.config.scaler[0].maxScale = 400; - - _model.config.scaler[1].dimension = (ScalerDimension)(ACT_INNER_I | ACT_AXIS_PITCH | ACT_AXIS_ROLL); - _model.config.scaler[1].channel = 6; - _model.config.scaler[1].minScale = 25; //% - _model.config.scaler[1].maxScale = 400; - - _model.config.scaler[2].dimension = (ScalerDimension)(ACT_INNER_D | ACT_AXIS_PITCH | ACT_AXIS_ROLL); - _model.config.scaler[2].channel = 7; - _model.config.scaler[2].minScale = 25; //% - _model.config.scaler[2].maxScale = 400; - - s.println(F("OK")); - } - else if(strcmp_P(cmd.args[1], PSTR("modes")) == 0) - { - _model.config.conditions[0].id = MODE_ARMED; - _model.config.conditions[0].ch = AXIS_AUX_1 + 0; - _model.config.conditions[0].min = 1700; - _model.config.conditions[0].max = 2100; - - _model.config.conditions[1].id = MODE_ANGLE; - _model.config.conditions[1].ch = AXIS_AUX_1 + 0; // aux1 - _model.config.conditions[1].min = 1900; - _model.config.conditions[1].max = 2100; - - _model.config.conditions[2].id = MODE_AIRMODE; - _model.config.conditions[2].ch = 0; // aux1 - _model.config.conditions[2].min = (1700 - 900) / 25; - _model.config.conditions[2].max = (2100 - 900) / 25; - - s.println(F("OK")); - } - else if(strcmp_P(cmd.args[1], PSTR("micrus")) == 0) - { - s.println(F("OK")); - } - else if(strcmp_P(cmd.args[1], PSTR("brobot")) == 0) - { - s.println(F("OK")); - } - else - { - s.println(F("NOT OK")); - } - } - else if(strcmp_P(cmd.args[0], PSTR("load")) == 0) - { - _model.load(); - s.println(F("OK")); - } - else if(strcmp_P(cmd.args[0], PSTR("save")) == 0) - { - _model.save(); - s.println(F("# Saved, type reboot to apply changes")); - s.println(); - } - else if(strcmp_P(cmd.args[0], PSTR("eeprom")) == 0) - { - /* - int start = 0; - if(cmd.args[1]) - { - start = std::max(String(cmd.args[1]).toInt(), 0L); - } - - for(int i = start; i < start + 32; ++i) - { - uint8_t v = EEPROM.read(i); - if(v <= 0xf) s.print('0'); - s.print(v, HEX); - s.print(' '); - } - s.println(); - - for(int i = start; i < start + 32; ++i) - { - s.print((int8_t)EEPROM.read(i)); - s.print(' '); - } - s.println(); - */ - } - else if(strcmp_P(cmd.args[0], PSTR("scaler")) == 0) - { - for(size_t i = 0; i < SCALER_COUNT; i++) - { - uint32_t mode = _model.config.scaler[i].dimension; - if(!mode) continue; - short c = _model.config.scaler[i].channel; - float v = _model.state.input.ch[c]; - float min = _model.config.scaler[i].minScale * 0.01f; - float max = _model.config.scaler[i].maxScale * 0.01f; - float scale = Utils::map3(v, -1.f, 0.f, 1.f, min, min < 0 ? 0.f : 1.f, max); - s.print(F("scaler: ")); - s.print(i); - s.print(' '); - s.print(mode); - s.print(' '); - s.print(min); - s.print(' '); - s.print(max); - s.print(' '); - s.print(v); - s.print(' '); - s.println(scale); - } - } - else if(strcmp_P(cmd.args[0], PSTR("mixer")) == 0) - { - const MixerConfig& mixer = _model.state.currentMixer; - s.print(F("set mix_outputs ")); - s.println(mixer.count); - Param p; - for(size_t i = 0; i < MIXER_RULE_MAX; i++) - { - s.print(F("set mix_")); - s.print(i); - s.print(' '); - p.print(s, mixer.mixes[i]); - s.println(); - if(mixer.mixes[i].src == MIXER_SOURCE_NULL) break; - } - } - else if(strcmp_P(cmd.args[0], PSTR("status")) == 0) - { - printVersion(s); - s.println(); - s.println(F("STATUS: ")); - printStats(s); - s.println(); - - Device::GyroDevice * gyro = _model.state.gyro.dev; - Device::BaroDevice * baro = _model.state.baro.dev; - Device::MagDevice * mag = _model.state.mag.dev; - s.print(F(" devices: ")); - if(gyro) - { - s.print(FPSTR(Device::GyroDevice::getName(gyro->getType()))); - s.print('/'); - s.print(FPSTR(Device::BusDevice::getName(gyro->getBus()->getType()))); - } - else - { - s.print(F("NO_GYRO")); - } - - if(baro) - { - s.print(F(", ")); - s.print(FPSTR(Device::BaroDevice::getName(baro->getType()))); - s.print('/'); - s.print(FPSTR(Device::BusDevice::getName(baro->getBus()->getType()))); - } - else - { - s.print(F(", NO_BARO")); - } - - if(mag) - { - s.print(F(", ")); - s.print(FPSTR(Device::MagDevice::getName(mag->getType()))); - s.print('/'); - s.print(FPSTR(Device::BusDevice::getName(mag->getBus()->getType()))); - } - else - { - s.print(F(", NO_MAG")); - } - s.println(); - - s.print(F(" input: ")); - s.print(_model.state.input.frameRate); - s.print(F(" Hz, ")); - s.print(_model.state.input.autoFreq); - s.print(F(" Hz, ")); - s.println(_model.state.input.autoFactor); - - static const char* armingDisableNames[] = { - PSTR("NO_GYRO"), PSTR("FAILSAFE"), PSTR("RX_FAILSAFE"), PSTR("BAD_RX_RECOVERY"), - PSTR("BOXFAILSAFE"), PSTR("RUNAWAY_TAKEOFF"), PSTR("CRASH_DETECTED"), PSTR("THROTTLE"), - PSTR("ANGLE"), PSTR("BOOT_GRACE_TIME"), PSTR("NOPREARM"), PSTR("LOAD"), - PSTR("CALIBRATING"), PSTR("CLI"), PSTR("CMS_MENU"), PSTR("BST"), - PSTR("MSP"), PSTR("PARALYZE"), PSTR("GPS"), PSTR("RESC"), - PSTR("RPMFILTER"), PSTR("REBOOT_REQUIRED"), PSTR("DSHOT_BITBANG"), PSTR("ACC_CALIBRATION"), - PSTR("MOTOR_PROTOCOL"), PSTR("ARM_SWITCH") - }; - const size_t armingDisableNamesLength = sizeof(armingDisableNames) / sizeof(armingDisableNames[0]); - - s.print(F("arming flags:")); - for(size_t i = 0; i < armingDisableNamesLength; i++) - { - if(_model.state.mode.armingDisabledFlags & (1 << i)) { - s.print(' '); - s.print(armingDisableNames[i]); - } - } - s.println(); - s.print(F(" rescue mode: ")); - s.print(_model.state.mode.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) - { - printVersion(s); - s.println(); - printStats(s); - s.println(); - for(int i = 0; i < COUNTER_COUNT; ++i) - { - StatCounter c = (StatCounter)i; - int time = lrintf(_model.state.stats.getTime(c)); - float load = _model.state.stats.getLoad(c); - int freq = lrintf(_model.state.stats.getFreq(c)); - int real = lrintf(_model.state.stats.getReal(c)); - if(freq == 0) continue; - - s.print(FPSTR(_model.state.stats.getName(c))); - s.print(": "); - if(time < 100) s.print(' '); - if(time < 10) s.print(' '); - s.print(time); - s.print("us, "); - - if(real < 100) s.print(' '); - if(real < 10) s.print(' '); - s.print(real); - s.print("us/i, "); - - if(load < 10) s.print(' '); - s.print(load, 1); - s.print("%, "); - - if(freq < 1000) s.print(' '); - if(freq < 100) s.print(' '); - if(freq < 10) s.print(' '); - s.print(freq); - s.print(" Hz"); - s.println(); - } - s.print(F(" TOTAL: ")); - s.print((int)(_model.state.stats.getCpuTime())); - s.print(F("us, ")); - s.print(_model.state.stats.getCpuLoad(), 1); - s.print(F("%")); - s.println(); - } - 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("motors")) == 0) - { - s.print(PSTR("count: ")); - s.println(getMotorCount()); - for (size_t i = 0; i < 8; i++) - { - s.print(i); - s.print(PSTR(": ")); - if (i >= OUTPUT_CHANNELS || _model.config.pin[i + PIN_OUTPUT_0] == -1) - { - s.print(-1); - s.print(' '); - s.println(0); - } else { - s.print(_model.config.pin[i + PIN_OUTPUT_0]); - s.print(' '); - s.println(_model.state.output.us[i]); - } - } - } - else if(strcmp_P(cmd.args[0], PSTR("logs")) == 0) - { - s.print(_model.logger.c_str()); - 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(); - } - size_t size = 0; - if(cmd.args[3]) - { - size = String(cmd.args[3]).toInt(); - } - size = Utils::clamp(size, 8u, 128 * 1024u); - size_t chunk_size = 256; - - uint8_t* data = new uint8_t[chunk_size]; - while(size) - { - size_t len = std::min(size, chunk_size); - flashfsReadAbs(addr, data, len); - s.write(data, len); - - if(size > chunk_size) - { - size -= chunk_size; - addr += chunk_size; - } - else break; - } - s.println(); - delete[] data; - } - else - { - s.println(F("wrong param!")); - } - } -#endif - else - { - s.print(F("unknown command: ")); - s.println(cmd.args[0]); - } - s.println(); - } - - private: - void print(const Param& param, Stream& s) const - { - s.print(F("set ")); - s.print(FPSTR(param.name)); - s.print(' '); - param.print(s); - s.println(); - } - - void printVersion(Stream& s) const - { - s.print(boardIdentifier); - s.print(' '); - s.print(targetName); - s.print(' '); - s.print(targetVersion); - s.print(' '); - s.print(shortGitRevision); - s.print(' '); - s.print(buildDate); - 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); - } - - void printStats(Stream& s) const - { - s.print(F(" cpu freq: ")); - s.print(targetCpuFreq()); - s.println(F(" MHz")); - - s.print(F(" gyro clock: ")); - s.print(_model.state.gyro.clock); - s.println(F(" Hz")); - - s.print(F(" gyro rate: ")); - s.print(_model.state.gyro.timer.rate); - s.println(F(" Hz")); - - s.print(F(" loop rate: ")); - s.print(_model.state.loopTimer.rate); - s.println(F(" Hz")); - - s.print(F(" mixer rate: ")); - s.print(_model.state.mixer.timer.rate); - s.println(F(" Hz")); - - s.print(F(" accel rate: ")); - s.print(_model.state.accel.timer.rate); - s.println(F(" Hz")); - - s.print(F(" baro rate: ")); - s.print(_model.state.baro.rate); - s.println(F(" Hz")); - - s.print(F(" mag rate: ")); - s.print(_model.state.mag.timer.rate); - s.println(F(" Hz")); - } - - Model& _model; - const Param * _params; - bool _ignore; - bool _active; -}; - -} - -} - -#endif diff --git a/lib/Espfc/src/Connect/Cli.hpp b/lib/Espfc/src/Connect/Cli.hpp new file mode 100644 index 00000000..384cd70e --- /dev/null +++ b/lib/Espfc/src/Connect/Cli.hpp @@ -0,0 +1,107 @@ +#pragma once + +#include "Model.h" + +namespace Espfc { + +namespace Connect { + +class Cli +{ +public: + enum ParamType { + PARAM_NONE, // unused + PARAM_BOOL, // boolean + PARAM_BYTE, // 8 bit int + PARAM_BYTE_U, // 8 bit uint + PARAM_SHORT, // 16 bit int + PARAM_INT, // 32 bit int + PARAM_FLOAT, // 32 bit float + PARAM_INPUT_CHANNEL, // input channel config + PARAM_OUTPUT_CHANNEL, // output channel config + PARAM_SCALER, // scaler config + PARAM_MODE, // scaler config + PARAM_MIXER, // mixer config + PARAM_SERIAL, // mixer config + PARAM_STRING, // string + PARAM_BITMASK, // set or clear bit + }; + + class Param + { + public: + Param(): Param(NULL, PARAM_NONE, NULL, NULL) {} + Param(const Param& p): Param(p.name, p.type, p.addr, p.choices) {} + + Param(const char * n, ParamType t, char * a, const char ** c, size_t l = 16): name(n), type(t), addr(a), choices(c), maxLen(l) {} + + Param(const char * n, 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) {} + Param(const char * n, uint8_t * a): Param(n, PARAM_BYTE_U, reinterpret_cast(a), NULL) {} + Param(const char * n, int16_t * a): Param(n, PARAM_SHORT, reinterpret_cast(a), NULL) {} + Param(const char * n, int32_t * a): Param(n, PARAM_INT, reinterpret_cast(a), NULL) {} + + Param(const char * n, int8_t * a, const char ** c): Param(n, PARAM_BYTE, reinterpret_cast(a), c) {} + Param(const char * n, int32_t * a, uint8_t b): Param(n, PARAM_BITMASK, reinterpret_cast(a), NULL, b) {} + + Param(const char * n, InputChannelConfig * a): Param(n, PARAM_INPUT_CHANNEL, reinterpret_cast(a), NULL) {} + Param(const char * n, OutputChannelConfig * a): Param(n, PARAM_OUTPUT_CHANNEL, reinterpret_cast(a), NULL) {} + Param(const char * n, ScalerConfig * a): Param(n, PARAM_SCALER, reinterpret_cast(a), NULL) {} + Param(const char * n, ActuatorCondition * a): Param(n, PARAM_MODE, reinterpret_cast(a), NULL) {} + Param(const char * n, MixerEntry * a): Param(n, PARAM_MIXER, reinterpret_cast(a), NULL) {} + Param(const char * n, SerialPortConfig * a): Param(n, PARAM_SERIAL, reinterpret_cast(a), NULL) {} + + void print(Stream& stream) const; + void print(Stream& stream, const OutputChannelConfig& och) const; + void print(Stream& stream, const InputChannelConfig& ich) const; + void print(Stream& stream, const ScalerConfig& sc) const; + void print(Stream& stream, const ActuatorCondition& ac) const; + void print(Stream& stream, const MixerEntry& me) const; + void print(Stream& stream, const SerialPortConfig& sc) const; + void print(Stream& stream, int32_t v) const; + + void update(const char ** args) const; + + void write(OutputChannelConfig& och, const char ** args) const; + void write(InputChannelConfig& ich, const char ** args) const; + void write(ScalerConfig& sc, const char ** args) const; + void write(ActuatorCondition& ac, const char ** args) const; + void write(MixerEntry& ac, const char ** args) const; + void write(SerialPortConfig& sc, const char ** args) const; + + template + void write(const T v) const + { + *reinterpret_cast(addr) = v; + } + + void write(const String& v) const; + int32_t parse(const char * v) const; + + const char * name; + ParamType type; + char * addr; + const char ** choices; + size_t maxLen; + }; + + Cli(Model& model); + static const Param * initialize(ModelConfig& c); + bool process(const char c, CliCmd& cmd, Stream& stream); + void parse(CliCmd& cmd); + void execute(CliCmd& cmd, Stream& s); + +private: + void print(const Param& param, Stream& s) const; + void printVersion(Stream& s) const; + void printStats(Stream& s) const; + + Model& _model; + const Param * _params; + bool _ignore; + bool _active; +}; + +} + +} diff --git a/lib/Espfc/src/SerialManager.h b/lib/Espfc/src/SerialManager.h index fff321bc..9ab77774 100644 --- a/lib/Espfc/src/SerialManager.h +++ b/lib/Espfc/src/SerialManager.h @@ -3,7 +3,7 @@ #include "Model.h" #include "Device/SerialDevice.h" #include "Connect/MspProcessor.hpp" -#include "Connect/Cli.h" +#include "Connect/Cli.hpp" #include "TelemetryManager.h" #ifdef ESPFC_SERIAL_SOFT_0_WIFI #include "Wireless.h" From cc76de1aa561cdc15114381bb5fd86ab0c5f1a35 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 14 Nov 2024 21:34:16 +0100 Subject: [PATCH 62/68] loop index type --- lib/Espfc/src/Connect/Cli.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/Espfc/src/Connect/Cli.cpp b/lib/Espfc/src/Connect/Cli.cpp index f852ecae..16b45f02 100644 --- a/lib/Espfc/src/Connect/Cli.cpp +++ b/lib/Espfc/src/Connect/Cli.cpp @@ -304,7 +304,7 @@ int32_t Cli::Param::parse(const char * v) const { if(choices) { - for(int32_t i = 0; choices[i]; i++) + for(size_t i = 0; choices[i]; i++) { if(strcasecmp_P(v, choices[i]) == 0) return i; } From 251d711bdafc4066a52ed33def540558d5102c97 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 14 Nov 2024 21:46:14 +0100 Subject: [PATCH 63/68] move Kalman to AHRS lib --- lib/{Kalman => AHRS}/src/Kalman.cpp | 0 lib/{Kalman => AHRS}/src/Kalman.h | 0 lib/Kalman/library.json | 4 ---- 3 files changed, 4 deletions(-) rename lib/{Kalman => AHRS}/src/Kalman.cpp (100%) rename lib/{Kalman => AHRS}/src/Kalman.h (100%) delete mode 100644 lib/Kalman/library.json diff --git a/lib/Kalman/src/Kalman.cpp b/lib/AHRS/src/Kalman.cpp similarity index 100% rename from lib/Kalman/src/Kalman.cpp rename to lib/AHRS/src/Kalman.cpp diff --git a/lib/Kalman/src/Kalman.h b/lib/AHRS/src/Kalman.h similarity index 100% rename from lib/Kalman/src/Kalman.h rename to lib/AHRS/src/Kalman.h diff --git a/lib/Kalman/library.json b/lib/Kalman/library.json deleted file mode 100644 index 9c3100dc..00000000 --- a/lib/Kalman/library.json +++ /dev/null @@ -1,4 +0,0 @@ -{ - "name": "Kalman", - "version": "2.0.0" -} From 05b675ee146faa0e77cdcf8b62093057303dc2af Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 23 Nov 2024 00:15:44 +0100 Subject: [PATCH 64/68] split EscDriverBase --- lib/EscDriver/src/EscDriver.h | 196 +--------------------------- lib/EscDriver/src/EscDriverBase.cpp | 146 +++++++++++++++++++++ lib/EscDriver/src/EscDriverBase.hpp | 67 ++++++++++ 3 files changed, 215 insertions(+), 194 deletions(-) create mode 100644 lib/EscDriver/src/EscDriverBase.cpp create mode 100644 lib/EscDriver/src/EscDriverBase.hpp diff --git a/lib/EscDriver/src/EscDriver.h b/lib/EscDriver/src/EscDriver.h index 21f7881c..50c256c3 100644 --- a/lib/EscDriver/src/EscDriver.h +++ b/lib/EscDriver/src/EscDriver.h @@ -1,196 +1,6 @@ -#ifndef _ESC_DRIVER_H_ -#define _ESC_DRIVER_H_ - -#include - -enum EscProtocol { - ESC_PROTOCOL_PWM, - ESC_PROTOCOL_ONESHOT125, - ESC_PROTOCOL_ONESHOT42, - ESC_PROTOCOL_MULTISHOT, - ESC_PROTOCOL_BRUSHED, - ESC_PROTOCOL_DSHOT150, - ESC_PROTOCOL_DSHOT300, - ESC_PROTOCOL_DSHOT600, - ESC_PROTOCOL_PROSHOT, - ESC_PROTOCOL_DISABLED, - ESC_PROTOCOL_COUNT -}; - -struct EscConfig -{ - int timer; - EscProtocol protocol; - int rate; - bool async; - bool dshotTelemetry; -}; - -#define PWM_TO_DSHOT(v) (((v - 1000) * 2) + 47) -#define ESC_PROTOCOL_SANITIZE(p) (p > ESC_PROTOCOL_DSHOT600 && p != ESC_PROTOCOL_DISABLED ? ESC_PROTOCOL_DSHOT600 : p) - -class EscDriverBase -{ - public: -#if defined(UNIT_TEST) - int begin(const EscConfig& conf) { return 1; } - void end() {} - int attach(size_t channel, int pin, int pulse) { return 1; } - int write(size_t channel, int pulse) { return 1; } - void apply() {} - int pin(size_t channel) const { return -1; } - uint32_t telemetry(size_t channel) const { return 0; } -#endif +#pragma once - static uint16_t IRAM_ATTR dshotConvert(uint16_t pulse) - { - return pulse > 1000 ? PWM_TO_DSHOT(pulse) : 0; - } - - static uint16_t IRAM_ATTR dshotEncode(uint16_t value, bool inverted = false) - { - value <<= 1; - - // compute checksum - int csum = 0; - int csum_data = value; - for (int i = 0; i < 3; i++) - { - csum ^= csum_data; // xor - csum_data >>= 4; - } - if(inverted) - { - csum = ~csum; - } - csum &= 0xf; - - return (value << 4) | csum; - } - - static uint32_t IRAM_ATTR durationToBitLen(uint32_t duration, uint32_t len) - { - return (duration + (len >> 1)) / len; - } - - static uint32_t IRAM_ATTR pushBits(uint32_t value, uint32_t bitVal, size_t bitLen) - { - while(bitLen--) - { - value <<= 1; - value |= bitVal; - } - return value; - } - - /** - * @param data expected data layout (bits): duration0(15), level0(1), duration(15), level1(1) - * @param len number of data items - * @param bitLen duration of single bit - * @return uint32_t raw gcr value - */ - static uint32_t IRAM_ATTR extractTelemetryGcr(uint32_t* data, size_t len, uint32_t bitLen) - { - int bitCount = 0; - uint32_t value = 0; - for(size_t i = 0; i < len; i++) - { - uint32_t duration0 = data[i] & 0x7fff; - if(!duration0) break; - uint32_t level0 = (data[i] >> 15) & 0x01; - uint32_t len0 = durationToBitLen(duration0, bitLen); - if(len0) - { - value = pushBits(value, level0, len0); - bitCount += len0; - } - - uint32_t duration1 = (data[i] >> 16) & 0x7fff; - if(!duration1) break; - uint32_t level1 = (data[i] >> 31) & 0x01; - uint32_t len1 = durationToBitLen(duration1, bitLen); - if(len1) - { - value = pushBits(value, level1, len1); - bitCount += len1; - } - } - - // fill missing bits with 1 - if(bitCount < 21) - { - value = pushBits(value, 0x1, 21 - bitCount); - } - - return value; - } - - static constexpr uint32_t INVALID_TELEMETRY_VALUE = 0xffff; - static constexpr int SECONDS_PER_MINUTE = 60; - static constexpr float ERPM_PER_LSB = 100.0f; - - static float IRAM_ATTR getErpmToHzRatio(int poles) - { - return ERPM_PER_LSB / SECONDS_PER_MINUTE / (poles / 2.0f); - } - - static uint32_t IRAM_ATTR convertToErpm(uint32_t value) - { - if(!value) return 0; - - if(!value || value == INVALID_TELEMETRY_VALUE) - { - return INVALID_TELEMETRY_VALUE; - } - - // Convert period to erpm * 100 - return (1000000 * 60 / 100 + value / 2) / value; - } - - static uint32_t IRAM_ATTR convertToValue(uint32_t value) - { - // eRPM range - if(value == 0x0fff) - { - return 0; - } - - // Convert value to 16 bit from the GCR telemetry format (eeem mmmm mmmm) - return (value & 0x01ff) << ((value & 0xfe00) >> 9); - } - - static uint32_t IRAM_ATTR gcrToRawValue(uint32_t value) - { - value = value ^ (value >> 1); // extract gcr - - constexpr uint32_t iv = 0xffffffff; // invalid code - // First bit is start bit so discard it. - value &= 0xfffff; - static const uint32_t decode[32] = { - iv, iv, iv, iv, iv, iv, iv, iv, iv, 9, 10, 11, iv, 13, 14, 15, - iv, iv, 2, 3, iv, 5, 6, 7, iv, 0, 8, 1, iv, 4, 12, iv, - }; - - uint32_t decodedValue = decode[value & 0x1f]; - decodedValue |= decode[(value >> 5) & 0x1f] << 4; - decodedValue |= decode[(value >> 10) & 0x1f] << 8; - decodedValue |= decode[(value >> 15) & 0x1f] << 12; - - uint32_t csum = decodedValue; - csum = csum ^ (csum >> 8); // xor bytes - csum = csum ^ (csum >> 4); // xor nibbles - - if((csum & 0xf) != 0xf || decodedValue > 0xffff) - { - return INVALID_TELEMETRY_VALUE; - } - value = decodedValue >> 4; - - return value; - } - - static const size_t DSHOT_BIT_COUNT = 16; -}; +#include "EscDriverBase.hpp" #if defined(ESP8266) @@ -250,5 +60,3 @@ class EscDriverBase #error "Unsupported platform" #endif - -#endif diff --git a/lib/EscDriver/src/EscDriverBase.cpp b/lib/EscDriver/src/EscDriverBase.cpp new file mode 100644 index 00000000..c3cf4aea --- /dev/null +++ b/lib/EscDriver/src/EscDriverBase.cpp @@ -0,0 +1,146 @@ +#include "EscDriverBase.hpp" +#include + +uint16_t IRAM_ATTR EscDriverBase::dshotConvert(uint16_t pulse) +{ + return pulse > 1000 ? PWM_TO_DSHOT(pulse) : 0; +} + +uint16_t IRAM_ATTR EscDriverBase::dshotEncode(uint16_t value, bool inverted) +{ + value <<= 1; + + // compute checksum + int csum = 0; + int csum_data = value; + for (int i = 0; i < 3; i++) + { + csum ^= csum_data; // xor + csum_data >>= 4; + } + if(inverted) + { + csum = ~csum; + } + csum &= 0xf; + + return (value << 4) | csum; +} + +uint32_t IRAM_ATTR EscDriverBase::durationToBitLen(uint32_t duration, uint32_t len) +{ + return (duration + (len >> 1)) / len; +} + +uint32_t IRAM_ATTR EscDriverBase::pushBits(uint32_t value, uint32_t bitVal, size_t bitLen) +{ + while(bitLen--) + { + value <<= 1; + value |= bitVal; + } + return value; +} + +/** + * @param data expected data layout (bits): duration0(15), level0(1), duration(15), level1(1) + * @param len number of data items + * @param bitLen duration of single bit + * @return uint32_t raw gcr value + */ +uint32_t IRAM_ATTR EscDriverBase::extractTelemetryGcr(uint32_t* data, size_t len, uint32_t bitLen) +{ + int bitCount = 0; + uint32_t value = 0; + for(size_t i = 0; i < len; i++) + { + uint32_t duration0 = data[i] & 0x7fff; + if(!duration0) break; + uint32_t level0 = (data[i] >> 15) & 0x01; + uint32_t len0 = durationToBitLen(duration0, bitLen); + if(len0) + { + value = pushBits(value, level0, len0); + bitCount += len0; + } + + uint32_t duration1 = (data[i] >> 16) & 0x7fff; + if(!duration1) break; + uint32_t level1 = (data[i] >> 31) & 0x01; + uint32_t len1 = durationToBitLen(duration1, bitLen); + if(len1) + { + value = pushBits(value, level1, len1); + bitCount += len1; + } + } + + // fill missing bits with 1 + if(bitCount < 21) + { + value = pushBits(value, 0x1, 21 - bitCount); + } + + return value; +} + +float IRAM_ATTR EscDriverBase::getErpmToHzRatio(int poles) +{ + return ERPM_PER_LSB / SECONDS_PER_MINUTE / (poles / 2.0f); +} + +uint32_t IRAM_ATTR EscDriverBase::convertToErpm(uint32_t value) +{ + if(!value) return 0; + + if(!value || value == INVALID_TELEMETRY_VALUE) + { + return INVALID_TELEMETRY_VALUE; + } + + // Convert period to erpm * 100 + return (1000000 * 60 / 100 + value / 2) / value; +} + +uint32_t IRAM_ATTR EscDriverBase::convertToValue(uint32_t value) +{ + // eRPM range + if(value == 0x0fff) + { + return 0; + } + + // Convert value to 16 bit from the GCR telemetry format (eeem mmmm mmmm) + return (value & 0x01ff) << ((value & 0xfe00) >> 9); +} + +uint32_t IRAM_ATTR EscDriverBase::gcrToRawValue(uint32_t value) +{ + value = value ^ (value >> 1); // extract gcr + + constexpr uint32_t iv = 0xffffffff; // invalid code + // First bit is start bit so discard it. + value &= 0xfffff; + static const uint32_t decode[32] = { + iv, iv, iv, iv, iv, iv, iv, iv, iv, 9, 10, 11, iv, 13, 14, 15, + iv, iv, 2, 3, iv, 5, 6, 7, iv, 0, 8, 1, iv, 4, 12, iv, + }; + + uint32_t decodedValue = decode[value & 0x1f]; + decodedValue |= decode[(value >> 5) & 0x1f] << 4; + decodedValue |= decode[(value >> 10) & 0x1f] << 8; + decodedValue |= decode[(value >> 15) & 0x1f] << 12; + + uint32_t csum = decodedValue; + csum = csum ^ (csum >> 8); // xor bytes + csum = csum ^ (csum >> 4); // xor nibbles + + if((csum & 0xf) != 0xf || decodedValue > 0xffff) + { + return INVALID_TELEMETRY_VALUE; + } + value = decodedValue >> 4; + + return value; +} + diff --git a/lib/EscDriver/src/EscDriverBase.hpp b/lib/EscDriver/src/EscDriverBase.hpp new file mode 100644 index 00000000..c8c590a6 --- /dev/null +++ b/lib/EscDriver/src/EscDriverBase.hpp @@ -0,0 +1,67 @@ +#pragma once + +#include +#include + +enum EscProtocol { + ESC_PROTOCOL_PWM, + ESC_PROTOCOL_ONESHOT125, + ESC_PROTOCOL_ONESHOT42, + ESC_PROTOCOL_MULTISHOT, + ESC_PROTOCOL_BRUSHED, + ESC_PROTOCOL_DSHOT150, + ESC_PROTOCOL_DSHOT300, + ESC_PROTOCOL_DSHOT600, + ESC_PROTOCOL_PROSHOT, + ESC_PROTOCOL_DISABLED, + ESC_PROTOCOL_COUNT +}; + +struct EscConfig +{ + int timer; + EscProtocol protocol; + int rate; + bool async; + bool dshotTelemetry; +}; + +#define PWM_TO_DSHOT(v) (((v - 1000) * 2) + 47) +#define ESC_PROTOCOL_SANITIZE(p) (p > ESC_PROTOCOL_DSHOT600 && p != ESC_PROTOCOL_DISABLED ? ESC_PROTOCOL_DSHOT600 : p) + +class EscDriverBase +{ + public: + static constexpr size_t DSHOT_BIT_COUNT = 16; + static constexpr uint32_t INVALID_TELEMETRY_VALUE = 0xffff; + static constexpr int SECONDS_PER_MINUTE = 60; + static constexpr float ERPM_PER_LSB = 100.0f; + + static uint16_t dshotConvert(uint16_t pulse); + static uint16_t dshotEncode(uint16_t value, bool inverted = false); + + /** + * @param data expected data layout (bits): duration0(15), level0(1), duration(15), level1(1) + * @param len number of data items + * @param bitLen duration of single bit + * @return uint32_t raw gcr value + */ + static uint32_t extractTelemetryGcr(uint32_t* data, size_t len, uint32_t bitLen); + static uint32_t durationToBitLen(uint32_t duration, uint32_t len); + static uint32_t pushBits(uint32_t value, uint32_t bitVal, size_t bitLen); + static uint32_t gcrToRawValue(uint32_t value); + + static float getErpmToHzRatio(int poles); + static uint32_t convertToErpm(uint32_t value); + static uint32_t convertToValue(uint32_t value); + +#if defined(UNIT_TEST) + int begin(const EscConfig& conf) { return 1; } + void end() {} + int attach(size_t channel, int pin, int pulse) { return 1; } + int write(size_t channel, int pulse) { return 1; } + void apply() {} + int pin(size_t channel) const { return -1; } + uint32_t telemetry(size_t channel) const { return 0; } +#endif +}; From 13910390699501049a0db150a99833f21034215e Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 23 Nov 2024 01:52:39 +0100 Subject: [PATCH 65/68] fix esc drivers and pgm imports --- lib/EscDriver/src/EscDriverEsp32c3.cpp | 35 +++++++++++++------------- lib/EscDriver/src/EscDriverEsp32c3.h | 22 ++++++++-------- lib/EscDriver/src/EscDriverEsp8266.cpp | 27 ++++++++++---------- lib/EscDriver/src/EscDriverEsp8266.h | 31 +++++++++++------------ lib/EscDriver/src/EscDriverRP2040.cpp | 11 ++++---- lib/EscDriver/src/EscDriverRP2040.h | 10 ++++---- lib/Espfc/src/Device/BaroDevice.cpp | 19 ++++++++++++++ lib/Espfc/src/Device/BaroDevice.h | 18 +++---------- lib/Espfc/src/Device/BusDevice.cpp | 19 ++++++++++++++ lib/Espfc/src/Device/BusDevice.h | 19 +++----------- lib/Espfc/src/Device/GyroDevice.cpp | 19 ++++++++++++++ lib/Espfc/src/Device/GyroDevice.h | 18 +++---------- lib/Espfc/src/Device/MagDevice.cpp | 19 ++++++++++++++ lib/Espfc/src/Device/MagDevice.h | 18 +++---------- 14 files changed, 156 insertions(+), 129 deletions(-) create mode 100644 lib/Espfc/src/Device/BaroDevice.cpp create mode 100644 lib/Espfc/src/Device/BusDevice.cpp create mode 100644 lib/Espfc/src/Device/GyroDevice.cpp create mode 100644 lib/Espfc/src/Device/MagDevice.cpp diff --git a/lib/EscDriver/src/EscDriverEsp32c3.cpp b/lib/EscDriver/src/EscDriverEsp32c3.cpp index 1e2d4e26..439b1041 100644 --- a/lib/EscDriver/src/EscDriverEsp32c3.cpp +++ b/lib/EscDriver/src/EscDriverEsp32c3.cpp @@ -1,6 +1,7 @@ #if defined(ESP32C3) #include "EscDriverEsp32c3.h" +#include #include #include @@ -18,12 +19,12 @@ void EscDriverEsp32c3::_isr_init(EscDriverTimer timer, void * driver) { timer_group_t group = (timer_group_t)timer; 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_DIS, - .divider = ESC_TIMER_DIVIDER, + .alarm_en = TIMER_ALARM_EN, + .counter_en = TIMER_PAUSE, + .intr_type = TIMER_INTR_LEVEL, + .counter_dir = TIMER_COUNT_UP, + .auto_reload = TIMER_AUTORELOAD_DIS, + .divider = ESC_TIMER_DIVIDER, }; timer_init(group, ESC_TIMER_IDX, &config); timer_set_counter_value(group, ESC_TIMER_IDX, 0); @@ -32,7 +33,7 @@ void EscDriverEsp32c3::_isr_init(EscDriverTimer timer, void * driver) timer_start(group, ESC_TIMER_IDX); } -bool EscDriverEsp32c3::_isr_wait(EscDriverTimer timer, const uint32_t ticks) +bool IRAM_ATTR EscDriverEsp32c3::_isr_wait(EscDriverTimer timer, const uint32_t ticks) { if(ticks >= TIMER_WAIT_EDGE) { // yield uint64_t value = timer_group_get_counter_value_in_isr((timer_group_t)timer, ESC_TIMER_IDX); @@ -53,7 +54,7 @@ bool EscDriverEsp32c3::_isr_wait(EscDriverTimer timer, const uint32_t ticks) } // run as soon as possible -void EscDriverEsp32c3::_isr_start(EscDriverTimer timer) +void IRAM_ATTR EscDriverEsp32c3::_isr_start(EscDriverTimer timer) { _isr_wait(timer, TIMER_WAIT_EDGE + 10); //timer_start((timer_group_t)timer, ESC_TIMER_IDX); @@ -65,7 +66,7 @@ void EscDriverEsp32c3::_isr_end(EscDriverTimer timer, void* p) timer_disable_intr((timer_group_t)timer, ESC_TIMER_IDX); } -int EscDriverEsp32c3::attach(size_t channel, int pin, int pulse) +int IRAM_ATTR EscDriverEsp32c3::attach(size_t channel, int pin, int pulse) { if(channel < 0 || channel >= ESC_CHANNEL_COUNT) return 0; _slots[channel].pin = pin; @@ -75,7 +76,7 @@ int EscDriverEsp32c3::attach(size_t channel, int pin, int pulse) return 1; } -int EscDriverEsp32c3::write(size_t channel, int pulse) +int IRAM_ATTR EscDriverEsp32c3::write(size_t channel, int pulse) { if(channel < 0 || channel >= ESC_CHANNEL_COUNT) return 0; _slots[channel].pulse = usToTicks(pulse); @@ -93,7 +94,7 @@ uint32_t EscDriverEsp32c3::telemetry(size_t channel) const return 0; } -void EscDriverEsp32c3::apply() +void IRAM_ATTR EscDriverEsp32c3::apply() { if(_protocol == ESC_PROTOCOL_DISABLED) return; if(_protocol >= ESC_PROTOCOL_DSHOT150) @@ -105,7 +106,7 @@ void EscDriverEsp32c3::apply() _isr_start(_timer); } -bool EscDriverEsp32c3::handle(void * p) +bool IRAM_ATTR EscDriverEsp32c3::handle(void * p) { // Time critical section EscDriver * instance = (EscDriver *)p; @@ -145,7 +146,7 @@ bool EscDriverEsp32c3::handle(void * p) return false; } -void EscDriverEsp32c3::commit() +void IRAM_ATTR EscDriverEsp32c3::commit() { Slot sorted[ESC_CHANNEL_COUNT]; std::copy(_slots, _slots + ESC_CHANNEL_COUNT, sorted); @@ -208,17 +209,17 @@ void EscDriverEsp32c3::commit() } } -uint32_t EscDriverEsp32c3::usToTicksReal(EscDriverTimer timer, uint32_t us) +uint32_t IRAM_ATTR EscDriverEsp32c3::usToTicksReal(EscDriverTimer timer, uint32_t us) { return (APB_CLK_FREQ / ESC_TIMER_DIVIDER / 1000000L) * us; } -int32_t EscDriverEsp32c3::minTicks(EscDriverTimer timer) +int32_t IRAM_ATTR EscDriverEsp32c3::minTicks(EscDriverTimer timer) { return 150L; } -uint32_t EscDriverEsp32c3::usToTicks(uint32_t us) +uint32_t IRAM_ATTR EscDriverEsp32c3::usToTicks(uint32_t us) { uint32_t ticks = 0; switch(_protocol) @@ -312,7 +313,7 @@ static inline void dshotDelay(int delay) while(delay--) __asm__ __volatile__ ("nop"); } -void EscDriverEsp32c3::dshotWrite() +void IRAM_ATTR EscDriverEsp32c3::dshotWrite() { // zero mask arrays mask_t * sm = dshotSetMask; diff --git a/lib/EscDriver/src/EscDriverEsp32c3.h b/lib/EscDriver/src/EscDriverEsp32c3.h index 93134dd2..325ad9b3 100644 --- a/lib/EscDriver/src/EscDriverEsp32c3.h +++ b/lib/EscDriver/src/EscDriverEsp32c3.h @@ -45,23 +45,23 @@ class EscDriverEsp32c3: public EscDriverBase int begin(const EscConfig& conf); void end(); - int attach(size_t channel, int pin, int pulse) IRAM_ATTR; - int write(size_t channel, int pulse) IRAM_ATTR; - void apply() IRAM_ATTR; + int attach(size_t channel, int pin, int pulse); + int write(size_t channel, int pulse); + void apply(); int pin(size_t channel) const; uint32_t telemetry(size_t channel) const; - static bool handle(void * p) IRAM_ATTR; + static bool handle(void * p); private: - void commit() IRAM_ATTR; - uint32_t usToTicks(uint32_t us) IRAM_ATTR; - uint32_t usToTicksReal(EscDriverTimer timer, uint32_t us) IRAM_ATTR; - int32_t minTicks(EscDriverTimer timer) IRAM_ATTR; - void dshotWrite() IRAM_ATTR; + void commit(); + uint32_t usToTicks(uint32_t us); + uint32_t usToTicksReal(EscDriverTimer timer, uint32_t us); + int32_t minTicks(EscDriverTimer timer); + void dshotWrite(); static void _isr_init(EscDriverTimer timer, void * driver); - static bool _isr_wait(EscDriverTimer timer, const uint32_t ticks) IRAM_ATTR; - static void _isr_start(EscDriverTimer timer) IRAM_ATTR; + static bool _isr_wait(EscDriverTimer timer, const uint32_t ticks); + static void _isr_start(EscDriverTimer timer); static void _isr_end(EscDriverTimer timer, void* p); volatile bool _busy; diff --git a/lib/EscDriver/src/EscDriverEsp8266.cpp b/lib/EscDriver/src/EscDriverEsp8266.cpp index 06ec9448..47684197 100644 --- a/lib/EscDriver/src/EscDriverEsp8266.cpp +++ b/lib/EscDriver/src/EscDriverEsp8266.cpp @@ -1,6 +1,7 @@ #if defined(ESP8266) #include "EscDriverEsp8266.h" +#include #include #include @@ -40,7 +41,7 @@ void EscDriverEsp8266::_isr_init(EscDriverTimer timer, void * driver) } } -void EscDriverEsp8266::_isr_begin(EscDriverTimer timer) +void IRAM_ATTR EscDriverEsp8266::_isr_begin(EscDriverTimer timer) { switch(timer) { @@ -62,7 +63,7 @@ void EscDriverEsp8266::_isr_begin(EscDriverTimer timer) #define TIMER1_WAIT_EDGE 140UL #define TIMER1_WAIT_COMP 115UL -bool EscDriverEsp8266::_isr_wait(EscDriverTimer timer, const uint32_t ticks) +bool IRAM_ATTR EscDriverEsp8266::_isr_wait(EscDriverTimer timer, const uint32_t ticks) { switch(timer) { @@ -103,7 +104,7 @@ bool EscDriverEsp8266::_isr_wait(EscDriverTimer timer, const uint32_t ticks) } // run as soon as possible -void EscDriverEsp8266::_isr_start(EscDriverTimer timer) +void IRAM_ATTR EscDriverEsp8266::_isr_start(EscDriverTimer timer) { switch(timer) { @@ -122,7 +123,7 @@ void EscDriverEsp8266::_isr_start(EscDriverTimer timer) } } -void EscDriverEsp8266::_isr_reboot(void* p) +void IRAM_ATTR EscDriverEsp8266::_isr_reboot(void* p) { EscDriver* d = (EscDriver*)p; _isr_begin(d->_timer); @@ -152,7 +153,7 @@ void EscDriverEsp8266::_isr_end(EscDriverTimer timer, void* p) } } -int EscDriverEsp8266::attach(size_t channel, int pin, int pulse) +int IRAM_ATTR EscDriverEsp8266::attach(size_t channel, int pin, int pulse) { if(channel < 0 || channel >= ESC_CHANNEL_COUNT) return 0; _slots[channel].pin = pin; @@ -173,14 +174,14 @@ uint32_t EscDriverEsp8266::telemetry(size_t channel) const return 0; } -int EscDriverEsp8266::write(size_t channel, int pulse) +int IRAM_ATTR EscDriverEsp8266::write(size_t channel, int pulse) { if(channel < 0 || channel >= ESC_CHANNEL_COUNT) return 0; _slots[channel].pulse = usToTicks(pulse); return 1; } -void EscDriverEsp8266::apply() +void IRAM_ATTR EscDriverEsp8266::apply() { if(_protocol == ESC_PROTOCOL_DISABLED) return; if(_protocol >= ESC_PROTOCOL_DSHOT150) @@ -192,7 +193,7 @@ void EscDriverEsp8266::apply() _isr_start(_timer); } -void EscDriverEsp8266::handle(void * p, void * x) +void IRAM_ATTR EscDriverEsp8266::handle(void * p, void * x) { // Time critical section EscDriver * instance = (EscDriver *)p; @@ -234,7 +235,7 @@ void EscDriverEsp8266::handle(void * p, void * x) } } -void EscDriverEsp8266::commit() +void IRAM_ATTR EscDriverEsp8266::commit() { Slot sorted[ESC_CHANNEL_COUNT]; std::copy(_slots, _slots + ESC_CHANNEL_COUNT, sorted); @@ -296,7 +297,7 @@ void EscDriverEsp8266::commit() } } -uint32_t EscDriverEsp8266::usToTicksReal(EscDriverTimer timer, uint32_t us) +uint32_t IRAM_ATTR EscDriverEsp8266::usToTicksReal(EscDriverTimer timer, uint32_t us) { switch(timer) { @@ -307,7 +308,7 @@ uint32_t EscDriverEsp8266::usToTicksReal(EscDriverTimer timer, uint32_t us) } } -int32_t EscDriverEsp8266::minTicks(EscDriverTimer timer) +int32_t IRAM_ATTR EscDriverEsp8266::minTicks(EscDriverTimer timer) { switch(timer) { @@ -318,7 +319,7 @@ int32_t EscDriverEsp8266::minTicks(EscDriverTimer timer) } } -uint32_t EscDriverEsp8266::usToTicks(uint32_t us) +uint32_t IRAM_ATTR EscDriverEsp8266::usToTicks(uint32_t us) { uint32_t ticks = 0; switch(_protocol) @@ -415,7 +416,7 @@ static IRAM_ATTR void dshotDelay(int delay) while(delay--) __asm__ __volatile__ ("nop"); } -void EscDriverEsp8266::dshotWrite() +void IRAM_ATTR EscDriverEsp8266::dshotWrite() { // zero mask arrays mask_t smask[DSHOT_BIT_COUNT]; diff --git a/lib/EscDriver/src/EscDriverEsp8266.h b/lib/EscDriver/src/EscDriverEsp8266.h index b0bc9c6c..81c449d0 100644 --- a/lib/EscDriver/src/EscDriverEsp8266.h +++ b/lib/EscDriver/src/EscDriverEsp8266.h @@ -1,5 +1,4 @@ -#ifndef _ESC_DRIVER_ESP8266_H_ -#define _ESC_DRIVER_ESP8266_H_ +#pragma once #if defined(ESP8266) @@ -52,25 +51,25 @@ class EscDriverEsp8266: public EscDriverBase int begin(const EscConfig& conf); void end(); - int attach(size_t channel, int pin, int pulse) IRAM_ATTR; - int write(size_t channel, int pulse) IRAM_ATTR; - void apply() IRAM_ATTR; + int attach(size_t channel, int pin, int pulse); + int write(size_t channel, int pulse); + void apply(); int pin(size_t channel) const; uint32_t telemetry(size_t channel) const; - static void handle(void * p, void * x) IRAM_ATTR; + static void handle(void * p, void * x); private: - void commit() IRAM_ATTR; - uint32_t usToTicks(uint32_t us) IRAM_ATTR; - uint32_t usToTicksReal(EscDriverTimer timer, uint32_t us) IRAM_ATTR; - int32_t minTicks(EscDriverTimer timer) IRAM_ATTR; - void dshotWrite() IRAM_ATTR; + void commit(); + uint32_t usToTicks(uint32_t us); + uint32_t usToTicksReal(EscDriverTimer timer, uint32_t us); + int32_t minTicks(EscDriverTimer timer); + void dshotWrite(); static void _isr_init(EscDriverTimer timer, void * driver); - static void _isr_begin(EscDriverTimer timer) IRAM_ATTR; - static bool _isr_wait(EscDriverTimer timer, const uint32_t ticks) IRAM_ATTR; - static void _isr_start(EscDriverTimer timer) IRAM_ATTR; - static void _isr_reboot(void* p) IRAM_ATTR; + static void _isr_begin(EscDriverTimer timer); + static bool _isr_wait(EscDriverTimer timer, const uint32_t ticks); + static void _isr_start(EscDriverTimer timer); + static void _isr_reboot(void* p); static void _isr_end(EscDriverTimer timer, void* p); volatile bool _busy; @@ -94,5 +93,3 @@ class EscDriverEsp8266: public EscDriverBase }; #endif // ESP8266 - -#endif diff --git a/lib/EscDriver/src/EscDriverRP2040.cpp b/lib/EscDriver/src/EscDriverRP2040.cpp index fbc13fb3..6b9ae673 100644 --- a/lib/EscDriver/src/EscDriverRP2040.cpp +++ b/lib/EscDriver/src/EscDriverRP2040.cpp @@ -1,6 +1,7 @@ #if defined(ARCH_RP2040) #include "EscDriverRP2040.h" +#include #include #include #include @@ -15,7 +16,7 @@ #define DSHOT150_T1H 4666u #define DSHOT150_T 6666u -int EscDriverRP2040::attach(size_t channel, int pin, int pulse) +int IRAM_ATTR EscDriverRP2040::attach(size_t channel, int pin, int pulse) { if(channel >= ESC_CHANNEL_COUNT) return 0; if(pin > 28) return 0; @@ -96,14 +97,14 @@ bool EscDriverRP2040::isSliceDriven(int slice) return false; } -int EscDriverRP2040::write(size_t channel, int pulse) +int IRAM_ATTR EscDriverRP2040::write(size_t channel, int pulse) { if(channel >= ESC_CHANNEL_COUNT) return 0; _slots[channel].pulse = usToTicks(pulse); return 1; } -void EscDriverRP2040::apply() +void IRAM_ATTR EscDriverRP2040::apply() { if(_protocol >= ESC_PROTOCOL_DSHOT150 && _protocol <= ESC_PROTOCOL_DSHOT600) { @@ -117,7 +118,7 @@ void EscDriverRP2040::apply() } } -uint32_t EscDriverRP2040::usToTicks(uint32_t us) +uint32_t IRAM_ATTR EscDriverRP2040::usToTicks(uint32_t us) { uint32_t ticks = 0; switch(_protocol) @@ -146,7 +147,7 @@ uint32_t EscDriverRP2040::usToTicks(uint32_t us) return ticks; } -uint32_t EscDriverRP2040::usToTicksReal(uint32_t us) +uint32_t IRAM_ATTR EscDriverRP2040::usToTicksReal(uint32_t us) { uint64_t t = (uint64_t)us * (F_CPU / _divider); return t / 1000000ul; diff --git a/lib/EscDriver/src/EscDriverRP2040.h b/lib/EscDriver/src/EscDriverRP2040.h index 75abfef3..bdbe9cc9 100644 --- a/lib/EscDriver/src/EscDriverRP2040.h +++ b/lib/EscDriver/src/EscDriverRP2040.h @@ -40,15 +40,15 @@ class EscDriverRP2040: public EscDriverBase int begin(const EscConfig& conf); void end(); - int attach(size_t channel, int pin, int pulse) IRAM_ATTR; - int write(size_t channel, int pulse) IRAM_ATTR; + int attach(size_t channel, int pin, int pulse); + int write(size_t channel, int pulse); int pin(size_t channel) const; uint32_t telemetry(size_t channel) const; - void apply() IRAM_ATTR; + void apply(); private: - uint32_t usToTicks(uint32_t us) IRAM_ATTR; - uint32_t usToTicksReal(uint32_t us) IRAM_ATTR; + uint32_t usToTicks(uint32_t us); + uint32_t usToTicksReal(uint32_t us); uint32_t nsToDshotTicks(uint32_t ns); void dshotWriteDMA(); bool isSliceDriven(int slice); diff --git a/lib/Espfc/src/Device/BaroDevice.cpp b/lib/Espfc/src/Device/BaroDevice.cpp new file mode 100644 index 00000000..bcfcf2ad --- /dev/null +++ b/lib/Espfc/src/Device/BaroDevice.cpp @@ -0,0 +1,19 @@ +#include "Device/BaroDevice.h" +#include "Hal/Pgm.h" +#include + +namespace Espfc::Device { + +const char ** BaroDevice::getNames() +{ + static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("BMP085"), PSTR("MS5611"), PSTR("BMP280"), PSTR("SPL06-001"), NULL }; + return devChoices; +} + +const char * BaroDevice::getName(DeviceType type) +{ + if(type >= BARO_MAX) return PSTR("?"); + return getNames()[type]; +} + +} diff --git a/lib/Espfc/src/Device/BaroDevice.h b/lib/Espfc/src/Device/BaroDevice.h index e94f327a..f3227796 100644 --- a/lib/Espfc/src/Device/BaroDevice.h +++ b/lib/Espfc/src/Device/BaroDevice.h @@ -1,5 +1,4 @@ -#ifndef _ESPFC_DEVICE_BARO_DEVICE_H_ -#define _ESPFC_DEVICE_BARO_DEVICE_H_ +#pragma once #include "BusDevice.h" #include "BusAwareDevice.h" @@ -40,21 +39,10 @@ class BaroDevice: public BusAwareDevice virtual bool testConnection() = 0; - static const char ** getNames() - { - static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("BMP085"), PSTR("MS5611"), PSTR("BMP280"), PSTR("SPL06-001"), NULL }; - return devChoices; - } - - static const char * getName(DeviceType type) - { - if(type >= BARO_MAX) return PSTR("?"); - return getNames()[type]; - } + static const char ** getNames(); + static const char * getName(DeviceType type); }; } } - -#endif diff --git a/lib/Espfc/src/Device/BusDevice.cpp b/lib/Espfc/src/Device/BusDevice.cpp new file mode 100644 index 00000000..a9ca598d --- /dev/null +++ b/lib/Espfc/src/Device/BusDevice.cpp @@ -0,0 +1,19 @@ +#include "Device/BusDevice.h" +#include "Hal/Pgm.h" +#include + +namespace Espfc::Device { + +const char ** BusDevice::getNames() +{ + static const char* busDevChoices[] = { PSTR("NONE"), PSTR("AUTO"), PSTR("I2C"), PSTR("SPI"), PSTR("SLV"), NULL }; + return busDevChoices; +} + +const char * BusDevice::getName(BusType type) +{ + if(type >= BUS_MAX) return PSTR("?"); + return getNames()[type]; +} + +} diff --git a/lib/Espfc/src/Device/BusDevice.h b/lib/Espfc/src/Device/BusDevice.h index 1f3a38cb..49cc0c19 100644 --- a/lib/Espfc/src/Device/BusDevice.h +++ b/lib/Espfc/src/Device/BusDevice.h @@ -1,9 +1,7 @@ -#ifndef _ESPFC_DEVICE_BUSDEVICE_H_ -#define _ESPFC_DEVICE_BUSDEVICE_H_ +#pragma once #include #include -#include "Hal/Pgm.h" #include "Utils/Bits.hpp" #define ESPFC_BUS_TIMEOUT 100 @@ -126,17 +124,8 @@ class BusDevice } } - static const char ** getNames() - { - static const char* busDevChoices[] = { PSTR("NONE"), PSTR("AUTO"), PSTR("I2C"), PSTR("SPI"), PSTR("SLV"), NULL }; - return busDevChoices; - } - - static const char * getName(BusType type) - { - if(type >= BUS_MAX) return PSTR("?"); - return getNames()[type]; - } + static const char ** getNames(); + static const char * getName(BusType type); std::function onError; @@ -147,5 +136,3 @@ class BusDevice } } - -#endif diff --git a/lib/Espfc/src/Device/GyroDevice.cpp b/lib/Espfc/src/Device/GyroDevice.cpp new file mode 100644 index 00000000..e56a8a52 --- /dev/null +++ b/lib/Espfc/src/Device/GyroDevice.cpp @@ -0,0 +1,19 @@ +#include "Device/GyroDevice.h" +#include "Hal/Pgm.h" +#include + +namespace Espfc::Device { + +const char ** GyroDevice::getNames() +{ + static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("MPU6000"), PSTR("MPU6050"), PSTR("MPU6500"), PSTR("MPU9250"), PSTR("LSM6DSO"), PSTR("ICM20602"),PSTR("BMI160"), NULL }; + return devChoices; +} + +const char * GyroDevice::getName(DeviceType type) +{ + if(type >= GYRO_MAX) return PSTR("?"); + return getNames()[type]; +} + +} diff --git a/lib/Espfc/src/Device/GyroDevice.h b/lib/Espfc/src/Device/GyroDevice.h index 1abf53e9..0f49d5ee 100644 --- a/lib/Espfc/src/Device/GyroDevice.h +++ b/lib/Espfc/src/Device/GyroDevice.h @@ -1,5 +1,4 @@ -#ifndef _ESPFC_DEVICE_GYRO_DEVICE_H_ -#define _ESPFC_DEVICE_GYRO_DEVICE_H_ +#pragma once #include #include "BusDevice.h" @@ -41,21 +40,10 @@ class GyroDevice: public BusAwareDevice virtual bool testConnection() = 0; - static const char ** getNames() - { - static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("MPU6000"), PSTR("MPU6050"), PSTR("MPU6500"), PSTR("MPU9250"), PSTR("LSM6DSO"), PSTR("ICM20602"),PSTR("BMI160"), NULL }; - return devChoices; - } - - static const char * getName(DeviceType type) - { - if(type >= GYRO_MAX) return PSTR("?"); - return getNames()[type]; - } + static const char ** getNames(); + static const char * getName(DeviceType type); }; } } - -#endif diff --git a/lib/Espfc/src/Device/MagDevice.cpp b/lib/Espfc/src/Device/MagDevice.cpp new file mode 100644 index 00000000..63a20384 --- /dev/null +++ b/lib/Espfc/src/Device/MagDevice.cpp @@ -0,0 +1,19 @@ +#include "Device/MagDevice.h" +#include "Hal/Pgm.h" +#include + +namespace Espfc::Device { + +const char ** MagDevice::getNames() +{ + static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("HMC5883L"), PSTR("AK8975"), PSTR("AK8963"), PSTR("QMC5883L"),NULL }; + return devChoices; +} + +const char * MagDevice::getName(DeviceType type) +{ + if(type >= MAG_MAX) return PSTR("?"); + return getNames()[type]; +} + +} \ No newline at end of file diff --git a/lib/Espfc/src/Device/MagDevice.h b/lib/Espfc/src/Device/MagDevice.h index 2c77bef3..6bb5823b 100644 --- a/lib/Espfc/src/Device/MagDevice.h +++ b/lib/Espfc/src/Device/MagDevice.h @@ -1,5 +1,4 @@ -#ifndef _ESPFC_DEVICE_MAG_DEVICE_H_ -#define _ESPFC_DEVICE_MAG_DEVICE_H_ +#pragma once #include "helper_3dmath.h" #include "BusAwareDevice.h" @@ -34,21 +33,10 @@ class MagDevice: public BusAwareDevice virtual bool testConnection() = 0; - static const char ** getNames() - { - static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("HMC5883L"), PSTR("AK8975"), PSTR("AK8963"), PSTR("QMC5883L"),NULL }; - return devChoices; - } - - static const char * getName(DeviceType type) - { - if(type >= MAG_MAX) return PSTR("?"); - return getNames()[type]; - } + static const char ** getNames(); + static const char * getName(DeviceType type); }; } } - -#endif From 6ca59b8f7c9ce7c9a4cbf0a82162311ada1044bd Mon Sep 17 00:00:00 2001 From: rtlopez Date: Tue, 14 Jan 2025 21:06:47 +0100 Subject: [PATCH 66/68] replace typedef with using --- lib/AHRS/src/helper_3dmath.h | 6 +++--- lib/EscDriver/src/EscDriverEsp32c3.h | 2 +- lib/EscDriver/src/EscDriverEsp8266.h | 2 +- lib/EscDriver/src/EscDriverRP2040.h | 2 +- lib/Espfc/src/Blackbox/BlackboxFlashfs.cpp | 3 ++- lib/Espfc/src/Hal/Gpio.h | 8 ++++---- lib/Espfc/src/Target/Queue.h | 8 ++++---- 7 files changed, 16 insertions(+), 15 deletions(-) diff --git a/lib/AHRS/src/helper_3dmath.h b/lib/AHRS/src/helper_3dmath.h index 829a71a9..c795d98c 100644 --- a/lib/AHRS/src/helper_3dmath.h +++ b/lib/AHRS/src/helper_3dmath.h @@ -511,6 +511,6 @@ class RotationMatrix }; }; -typedef VectorBase VectorFloat; -typedef VectorBase VectorInt16; -typedef RotationMatrix RotationMatrixFloat; +using VectorFloat = VectorBase; +using VectorInt16 = VectorBase; +using RotationMatrixFloat = RotationMatrix; diff --git a/lib/EscDriver/src/EscDriverEsp32c3.h b/lib/EscDriver/src/EscDriverEsp32c3.h index 325ad9b3..b4e15a54 100644 --- a/lib/EscDriver/src/EscDriverEsp32c3.h +++ b/lib/EscDriver/src/EscDriverEsp32c3.h @@ -39,7 +39,7 @@ class EscDriverEsp32c3: public EscDriverBase void reset() { *this = Item(); } }; - typedef uint32_t mask_t; + using mask_t = uint32_t; EscDriverEsp32c3(); diff --git a/lib/EscDriver/src/EscDriverEsp8266.h b/lib/EscDriver/src/EscDriverEsp8266.h index 81c449d0..34a5ed74 100644 --- a/lib/EscDriver/src/EscDriverEsp8266.h +++ b/lib/EscDriver/src/EscDriverEsp8266.h @@ -45,7 +45,7 @@ class EscDriverEsp8266: public EscDriverBase void reset() { *this = Item(); } }; - typedef uint32_t mask_t; + using mask_t = uint32_t; EscDriverEsp8266(); diff --git a/lib/EscDriver/src/EscDriverRP2040.h b/lib/EscDriver/src/EscDriverRP2040.h index bdbe9cc9..7f847be2 100644 --- a/lib/EscDriver/src/EscDriverRP2040.h +++ b/lib/EscDriver/src/EscDriverRP2040.h @@ -34,7 +34,7 @@ class EscDriverRP2040: public EscDriverBase inline bool active() const { return pin != -1; } }; - typedef uint32_t mask_t; + using mask_t = uint32_t; EscDriverRP2040(); diff --git a/lib/Espfc/src/Blackbox/BlackboxFlashfs.cpp b/lib/Espfc/src/Blackbox/BlackboxFlashfs.cpp index 04596ed5..41e1228a 100644 --- a/lib/Espfc/src/Blackbox/BlackboxFlashfs.cpp +++ b/lib/Espfc/src/Blackbox/BlackboxFlashfs.cpp @@ -9,7 +9,8 @@ static const uint32_t FLASHFS_ERASED_VAL = 0xffffffff; -typedef Espfc::Utils::RingBuf BufferType; +using BufferType = Espfc::Utils::RingBuf; + static BufferType buff; static FlashfsRuntime flashfs; diff --git a/lib/Espfc/src/Hal/Gpio.h b/lib/Espfc/src/Hal/Gpio.h index a0ad7e9a..d18ebb20 100644 --- a/lib/Espfc/src/Hal/Gpio.h +++ b/lib/Espfc/src/Hal/Gpio.h @@ -3,11 +3,11 @@ #include #if defined(ARCH_RP2040) - typedef PinStatus pin_status_t; - typedef PinMode pin_mode_t; + using pin_status_t = PinStatus; + using pin_mode_t = PinMode; #else - typedef uint8_t pin_status_t; - typedef uint8_t pin_mode_t; + using pin_status_t = uint8_t; + using pin_mode_t = uint8_t; #endif namespace Espfc { diff --git a/lib/Espfc/src/Target/Queue.h b/lib/Espfc/src/Target/Queue.h index 39de5214..69b02106 100644 --- a/lib/Espfc/src/Target/Queue.h +++ b/lib/Espfc/src/Target/Queue.h @@ -28,15 +28,15 @@ class Event #if defined(ARCH_RP2040) #include - typedef queue_t TargetQueueHandle; + using TargetQueueHandle = queue_t; #elif defined(ESPFC_FREE_RTOS_QUEUE) #include - typedef QueueHandle_t TargetQueueHandle; + using TargetQueueHandle = QueueHandle_t; #elif defined(ESPFC_ATOMIC_QUEUE) #include "QueueAtomic.h" - typedef Espfc::QueueAtomic TargetQueueHandle; + using TargetQueueHandle = Espfc::QueueAtomic; #elif defined(UNIT_TEST) || !defined(ESPFC_MULTI_CORE) - typedef int TargetQueueHandle; + using TargetQueueHandle = int; #else #error "Not yet implelented multicore queue" #endif From 9b22cf934e95975915fc06841c1db2a21606fd5e Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 16 Jan 2025 00:34:47 +0100 Subject: [PATCH 67/68] baro tuning --- lib/Espfc/src/Device/BaroBMP085.h | 16 +++++++++------- lib/Espfc/src/Device/BaroBMP280.h | 15 +++++++++++---- lib/Espfc/src/ModelConfig.h | 2 +- lib/Espfc/src/Sensor/BaroSensor.cpp | 18 +++++++++++------- 4 files changed, 32 insertions(+), 19 deletions(-) diff --git a/lib/Espfc/src/Device/BaroBMP085.h b/lib/Espfc/src/Device/BaroBMP085.h index 7985feea..9307a4b9 100644 --- a/lib/Espfc/src/Device/BaroBMP085.h +++ b/lib/Espfc/src/Device/BaroBMP085.h @@ -87,11 +87,10 @@ class BaroBMP085: public BaroDevice uint8_t buffer[3]; _bus->read(_addr, BMP085_MEASUREMENT_REG, 3, buffer); - uint8_t oss = (_mode & 0xC0) >> 6; + uint32_t up = ((uint32_t)buffer[0] << 16) | ((uint32_t)buffer[1] << 8) | buffer[2]; - uint32_t up = ((uint32_t)buffer[0] << 16) + ((uint16_t)buffer[1] << 8) + buffer[2]; + uint8_t oss = (_mode & 0xC0) >> 6; if(_mode & 0x34) up = up >> (8 - oss); - if(up == 0) return NAN; int32_t b6 = _t_fine - 4000; @@ -104,6 +103,7 @@ class BaroBMP085: public BaroDevice x3 = ((x1 + x2) + 2) >> 2; uint32_t b4 = ((uint32_t)_cal.ac4 * (uint32_t)(x3 + 32768)) >> 15; uint32_t b7 = ((uint32_t)up - b3) * (uint32_t)(50000UL >> oss); + int32_t p = 0; if (b7 < 0x80000000) { p = (b7 << 1) / b4; @@ -114,12 +114,14 @@ class BaroBMP085: public BaroDevice x1 = (x1 * 3038) >> 16; x2 = (-7357 * p) >> 16; - return p + ((x1 + x2 + (int32_t)3791) >> 4); + p = p + ((x1 + x2 + (int32_t)3791) >> 4); + + return p; } void setMode(BaroDeviceMode mode) { - _mode = mode == BARO_MODE_TEMP ? BMP085_MEASURE_T : BMP085_MEASURE_P2; + _mode = mode == BARO_MODE_TEMP ? BMP085_MEASURE_T : BMP085_MEASURE_P3; _bus->writeByte(_addr, BMP085_CONTROL_REG, _mode); } @@ -132,8 +134,8 @@ class BaroBMP085: public BaroDevice default: //return 4550; // press_0 //return 7550; // press_1 - return 13550; // press_2 - //return 25550; // press_3 + //return 13550; // press_2 + return 25550; // press_3 } } diff --git a/lib/Espfc/src/Device/BaroBMP280.h b/lib/Espfc/src/Device/BaroBMP280.h index a6a63921..394f7694 100644 --- a/lib/Espfc/src/Device/BaroBMP280.h +++ b/lib/Espfc/src/Device/BaroBMP280.h @@ -23,9 +23,14 @@ #define BMP280_PRESSURE_REG 0xF7 #define BMP280_TEMPERATURE_REG 0xFA -#define BMP280_FILTER_X2 (1 << 2) #define BMP280_MODE_NORMAL 0x03 +#define BMP280_FILTER_OFF 0x00 +#define BMP280_FILTER_X2 0x01 +#define BMP280_FILTER_X4 0x02 +#define BMP280_FILTER_X8 0x03 +#define BMP280_FILTER_X16 0x04 + #define BMP280_SAMPLING_X1 1 #define BMP280_SAMPLING_X2 2 #define BMP280_SAMPLING_X4 3 @@ -75,10 +80,10 @@ class BaroBMP280: public BaroDevice writeReg(BMP280_RESET_REG, BMP280_RESET_VAL); // device reset delay(2); - writeReg(BMP280_CONFIG_REG, BMP280_FILTER_X2); // set minimal standby and IIR filter X2 + writeReg(BMP280_CONFIG_REG, BMP280_FILTER_X4 << 2); // set minimal standby and IIR filter X2 //writeReg(BMP280_CONFIG_REG, 0); // set minimal standby and IIR filter off - writeReg(BMP280_CONTROL_REG, BMP280_SAMPLING_X1 << 5 | BMP280_SAMPLING_X4 << 2 | BMP280_MODE_NORMAL); // set sampling mode + writeReg(BMP280_CONTROL_REG, BMP280_SAMPLING_X2 << 5 | BMP280_SAMPLING_X8 << 2 | BMP280_MODE_NORMAL); // set sampling mode delay(20); @@ -145,7 +150,9 @@ class BaroBMP280: public BaroDevice default: //return 5500; // if sapling X1 //return 7500; // if sampling X2 - return 11500; // if sampling X4 + //return 11500; // if sampling X4 + return 19500; // if sampling X8 + //return 37500; // if sampling X16 } } diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index ffebb324..8c48b931 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -594,7 +594,7 @@ struct BaroConfig { int8_t bus = BUS_AUTO; int8_t dev = BARO_NONE; - FilterConfig filter{FILTER_BIQUAD, 5}; + FilterConfig filter{FILTER_BIQUAD, 3}; }; struct MagConfig diff --git a/lib/Espfc/src/Sensor/BaroSensor.cpp b/lib/Espfc/src/Sensor/BaroSensor.cpp index ffc0851b..1242b8dd 100644 --- a/lib/Espfc/src/Sensor/BaroSensor.cpp +++ b/lib/Espfc/src/Sensor/BaroSensor.cpp @@ -1,4 +1,5 @@ #include "BaroSensor.h" +#include namespace Espfc { @@ -22,8 +23,11 @@ int BaroSensor::begin() _model.state.baro.altitudeBiasSamples = 2 * _model.state.baro.rate; // TODO: move filters to BaroState - _temperatureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baro.rate * 0.1f), _model.state.baro.rate); - _pressureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baro.rate * 0.1f), _model.state.baro.rate); + auto internalFilter = FILTER_PT1; + auto internalCutoff = std::max(_model.state.baro.rate / 10, 1); + _temperatureFilter.begin(FilterConfig(internalFilter, internalCutoff), _model.state.baro.rate); + _pressureFilter.begin(FilterConfig(internalFilter, internalCutoff), _model.state.baro.rate); + _altitudeFilter.begin(_model.config.baro.filter, _model.state.baro.rate); _model.logger.info().log(F("BARO INIT")).log(FPSTR(Device::BaroDevice::getName(_baro->getType()))).log(_baro->getAddress()).log(toGyroRate).log(_model.state.baro.rate).logln(_model.config.baro.filter.freq); @@ -48,7 +52,7 @@ int BaroSensor::read() if(_model.config.debug.mode == DEBUG_BARO) { - _model.state.debug[0] = _state; + _model.state.debug[3] = _state; } switch(_state) @@ -104,19 +108,19 @@ void BaroSensor::readPressure() void BaroSensor::updateAltitude() { - _model.state.baro.altitudeRaw = _altitudeFilter.update(Utils::toAltitude(_model.state.baro.pressure)); + _model.state.baro.altitudeRaw = Utils::toAltitude(_model.state.baro.pressure); if(_model.state.baro.altitudeBiasSamples > 0) { _model.state.baro.altitudeBiasSamples--; - _model.state.baro.altitudeBias += (_model.state.baro.altitudeRaw - _model.state.baro.altitudeBias) * 0.2f; + _model.state.baro.altitudeBias += (_model.state.baro.altitudeRaw - _model.state.baro.altitudeBias) * (5.0f / _model.state.baro.rate); } - _model.state.baro.altitude = _model.state.baro.altitudeRaw - _model.state.baro.altitudeBias; + _model.state.baro.altitude = _altitudeFilter.update(_model.state.baro.altitudeRaw - _model.state.baro.altitudeBias); if(_model.config.debug.mode == DEBUG_BARO) { + _model.state.debug[0] = lrintf((_model.state.baro.altitudeRaw - _model.state.baro.altitudeBias) * 100.f); // cm _model.state.debug[1] = lrintf(_model.state.baro.pressureRaw * 0.1f); // hPa x 10 _model.state.debug[2] = lrintf(_model.state.baro.temperatureRaw * 100.f); // deg C x 100 - _model.state.debug[3] = lrintf(_model.state.baro.altitudeRaw * 10.f); } } From 05da2ce641a32b73e931077bdef9e1a07b9def93 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 16 Jan 2025 01:05:58 +0100 Subject: [PATCH 68/68] arg type fix --- lib/Espfc/src/Sensor/BaroSensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/Espfc/src/Sensor/BaroSensor.cpp b/lib/Espfc/src/Sensor/BaroSensor.cpp index 1242b8dd..75438b31 100644 --- a/lib/Espfc/src/Sensor/BaroSensor.cpp +++ b/lib/Espfc/src/Sensor/BaroSensor.cpp @@ -24,7 +24,7 @@ int BaroSensor::begin() // TODO: move filters to BaroState auto internalFilter = FILTER_PT1; - auto internalCutoff = std::max(_model.state.baro.rate / 10, 1); + auto internalCutoff = std::max(_model.state.baro.rate / 10, (int32_t)1); _temperatureFilter.begin(FilterConfig(internalFilter, internalCutoff), _model.state.baro.rate); _pressureFilter.begin(FilterConfig(internalFilter, internalCutoff), _model.state.baro.rate);