From e1d9e3e430a88282f43ae95f4eee1598dc3d695b Mon Sep 17 00:00:00 2001 From: rtlopez Date: Wed, 30 Oct 2024 23:40:44 +0100 Subject: [PATCH 01/29] 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 e661042..d57f984 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 e24c678..8743215 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 43fa0e7..da90dab 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 e8d7d6f..3ea650e 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 116de4df0dbb46bbb3241b03b079cb86bcaf3ad0 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 31 Oct 2024 00:08:38 +0100 Subject: [PATCH 02/29] 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 5c2eae1..ab057c2 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 dc2ead1..61aedc8 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 5e5de83..a1fbcee 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 b8662826334680511c1c542fcb66ad5aa732a562 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 31 Oct 2024 00:26:05 +0100 Subject: [PATCH 03/29] 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 6765742..0000000 --- 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 0000000..0cff63f --- /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 0000000..5d80c97 --- /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 da90dab..59700fc 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 3ea650e..2dca3cf 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 b034b67a340e3a7c2deb09326edde0227def48fa Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 31 Oct 2024 00:38:25 +0100 Subject: [PATCH 04/29] 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 0000000..432f8f0 --- /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 f9ea1bc..c67301f 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 e58669bddbcd755b78428c46c3a9430368ca460a Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 31 Oct 2024 22:54:42 +0100 Subject: [PATCH 05/29] 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 | 163 ++----------------------- lib/Espfc/src/Sensor/VoltageSensor.cpp | 115 +++++++++++++++++ lib/Espfc/src/Sensor/VoltageSensor.h | 114 ++--------------- 6 files changed, 429 insertions(+), 372 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 0000000..cba071b --- /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 dbeda6d..d5f8201 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 0000000..f98cf63 --- /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 01f00c7..0c42417 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,156 +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; @@ -171,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 0000000..6718ba0 --- /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 3e4af7a..8c8688b 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 3cd04217a19f73735316882573534a40c2b9d8ce Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 2 Nov 2024 22:01:01 +0100 Subject: [PATCH 06/29] 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 26bba42..806497c 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 5f24b99..a0ad7e9 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 0000000..021c818 --- /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 e32c0f3..c853948 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 4ca5b0c5a965d99607f05b5d7543732e03a35caa Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 2 Nov 2024 23:39:52 +0100 Subject: [PATCH 07/29] 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 0000000..eb453c8 --- /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 7b184fc..3033145 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 dbdd69dc93ef54aa3198e787d45aa3de4ea0843f Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 2 Nov 2024 23:51:47 +0100 Subject: [PATCH 08/29] 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 eb453c8..81c71a4 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 42aad6ac8984bdc5afdf8e7bd36d9646d7dc50b8 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 3 Nov 2024 00:06:10 +0100 Subject: [PATCH 09/29] 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 8743215..1f386e1 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 deb3711..3c75721 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 68730a1..4f014cc 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 31749b2..e9293a8 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 a76e156..7b5be7f 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 31b6df0..b09c7c9 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 412ef0b..4575c6e 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 c67301f..970d698 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 d5f8201..adb31b1 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 8c8688b..cc93c4d 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 0746dfb..7a10f11 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 cf4b01c..e37dc71 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 76834c9..a90b54b 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 23c150c..60ebc4e 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 a87f8a9c53a76243dec7cd55530da5251a979b07 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 3 Nov 2024 22:10:31 +0100 Subject: [PATCH 10/29] 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 503e22d..f4c0809 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 97f0e3d..e2b6e3f 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 b2191ba..585333e 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 3831dd6..868bbf5 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 4575c6e..fa19020 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 20f3d10..0c1994b 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 7169645..c82fd59 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 189587d..3de7a95 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 2dca3cf..cfac4eb 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 54adccfbe3a1af72fab692a6b56ca12b79140511 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 3 Nov 2024 22:25:31 +0100 Subject: [PATCH 11/29] 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 d37ecc7..fd58058 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 0cff63f..f476b6b 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 d57f984..2dd6eb9 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 ab057c2..06715bf 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 1714b30..7d996f2 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 38f0461..b1fba03 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 fa19020..d930b7c 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 561353f..e4bdda5 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 432f8f0..72b7882 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 cba071b..5751ac6 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 4413f09..6125aa5 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); @@ -88,7 +88,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; @@ -163,7 +163,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++) @@ -199,7 +199,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 f98cf63..75a2308 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 6718ba0..3b3f9ed 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 8070490..c5a5fe8 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 0c1994b..0000000 --- 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 84fe58c..f7968e6 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 0000000..2df9175 --- /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 0000000..26666f4 --- /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 96e9e55..0092c6c 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 ecad8bf1727d2ba7f2487d9d7e01a3c43a7f6492 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 4 Nov 2024 20:22:54 +0100 Subject: [PATCH 12/29] 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 7328a4e..2f1d932 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 8ab11be..c43116c 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 00a53bb..f90549f 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 ff35547..3959739 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 d930b7c..73cc3f9 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 e93e041..c18551b 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 78d7ed1..0c4eba2 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 c5a5fe8..6828f40 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 b025f8c..3bc121e 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 c724a7c..834fca9 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 f7968e6..219ff61 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 501001f..3d25b11 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 aee123b..1d705dc 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 638aa8478ea4d71dcfb025891d7051ce7ebda959 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 4 Nov 2024 20:25:53 +0100 Subject: [PATCH 13/29] 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 a1d4632..6c3f4ac 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: @@ -1555,4 +1557,6 @@ class Cli } +} + #endif diff --git a/lib/Espfc/src/SerialManager.h b/lib/Espfc/src/SerialManager.h index 3bc121e..7e7f6c6 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 a7507c9645df01d88a8a1259fe52182ffe4e8411 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 4 Nov 2024 21:02:06 +0100 Subject: [PATCH 14/29] 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 a95dcb5..7d89e94 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 f7a279c..0000000 --- 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 0000000..c82fe0e --- /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 0000000..c2b90f1 --- /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 d2f4978acd86a460415a58628783c73d7759569f Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 4 Nov 2024 23:20:43 +0100 Subject: [PATCH 15/29] 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 fd58058..ed7651e 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 6c3f4ac..a8df2e6 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" @@ -1074,12 +1072,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 f90549f..7e176df 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 2dd6eb9..f3b3004 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 f90b3a9..1382380 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 7d89e94..0c120e3 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 e4bdda5..014ca23 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 6125aa5..c5235b2 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 3e79b81..f037eb4 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 ae4a90a3addebcdd519ba300c72048d71d761f6c Mon Sep 17 00:00:00 2001 From: rtlopez Date: Tue, 5 Nov 2024 21:52:00 +0100 Subject: [PATCH 16/29] 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 0000000..6ebce6a --- /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 219ff61..881ed9c 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 0000000..a027e6c --- /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 0092c6c..136810a 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 244de0ef68f9216bf29f50b3f76473ae0944702d Mon Sep 17 00:00:00 2001 From: rtlopez Date: Tue, 5 Nov 2024 22:34:49 +0100 Subject: [PATCH 17/29] 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 f6353bd..aa14bdf 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 6e908b6..d571195 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 8ada32f..e6e9c7e 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 97f2908..77755f2 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 81c71a4..0035fe8 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 1df5d87..160f790 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 73cc3f9..950efc6 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 7ab4ebe..c9b1665 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 658b91029cb5b302d1fd94d8d3f853b43c81b221 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 7 Nov 2024 23:46:18 +0100 Subject: [PATCH 18/29] 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 e9293a8..0000000 --- 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 7b5be7f..0000000 --- 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 0e5c091..0000000 --- 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 950efc6..4990a04 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 c5235b2..5e2e3ce 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 57f7a61..ea38857 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 0000000..7f004bc --- /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 0000000..7ae71cc --- /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 0000000..f7f5d14 --- /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 0000000..1d3e811 --- /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 0000000..6f682f0 --- /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 0000000..1a3517f --- /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 ca7f1146b7fb7b9fca6acc2bc914f072ff5377ef Mon Sep 17 00:00:00 2001 From: rtlopez Date: Fri, 8 Nov 2024 22:40:36 +0100 Subject: [PATCH 19/29] 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 dd3c3e6..1f3a38c 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 624a3ac..f613b19 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 5eca168..c350a52 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 60ebc4e..72116c2 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 60357044aed1861e36f406771b43b490e542c155 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Fri, 8 Nov 2024 22:52:53 +0100 Subject: [PATCH 20/29] 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 2f1d932..855a906 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 c43116c..f156fef 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 c18551b..e4fef0c 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 0c4eba2..e8bc2d2 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 5c8ced3..d3f734e 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 809ce18..a6abdb5 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 89f7505eef4cbc36cf4aae47a9bde8f4ebc26f4d Mon Sep 17 00:00:00 2001 From: rtlopez Date: Fri, 8 Nov 2024 23:06:49 +0100 Subject: [PATCH 21/29] 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 ed7651e..991b276 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 823d6dc..15fff80 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 8e810a0..04596ed 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 a8df2e6..688cf4d 100644 --- a/lib/Espfc/src/Connect/Cli.h +++ b/lib/Espfc/src/Connect/Cli.h @@ -1042,9 +1042,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(' '); @@ -1200,7 +1200,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(' '); @@ -1445,7 +1445,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 7e176df..9e70258 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 f476b6b..41fc701 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 f3b3004..6288fe3 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 06715bf..5bcde08 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 9a0c522..8fb0f74 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 3c75721..3f8930a 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; // +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; // 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 585333e..405b8f9 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 868bbf5..26579aa 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 af87b5c..0d1d123 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 b1fba03..bef0f9d 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 4f014cc..95d549f 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 0c120e3..254bf6d 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 014ca23..739d8ea 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 e4fef0c..147cc26 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 5751ac6..ffc0851 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 5e2e3ce..58aa31d 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++) { @@ -103,14 +103,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) { @@ -123,13 +123,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) { @@ -146,7 +146,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()) @@ -172,7 +172,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) @@ -255,7 +255,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) { @@ -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); + 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 3b3f9ed..4e5eb94 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 834fca9..728ba98 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 7f004bc..5e3528b 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 7ae71cc..d521986 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 7a10f11..56daf69 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 f7f5d14..d04f456 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 2c2f464..bd6fb1f 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 72116c2..bef4775 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 76a3492da2b34c99b99fbc41655b9ffb1f050ace Mon Sep 17 00:00:00 2001 From: rtlopez Date: Wed, 13 Nov 2024 22:27:32 +0100 Subject: [PATCH 22/29] 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 0035fe8..2ce8c66 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 3033145..3b84d7c 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 59700fc..5da2181 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 8cf71a2ea5b06b84cd18b1dadbf1d25acccd03f5 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 14 Nov 2024 00:09:29 +0100 Subject: [PATCH 23/29] 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 688cf4d..b668314 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" @@ -1480,7 +1481,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)); @@ -1489,7 +1490,7 @@ class Cli s.println(); } - void printVersion(Stream& s) + void printVersion(Stream& s) const { s.print(boardIdentifier); s.print(' '); @@ -1512,7 +1513,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 0000000..ea90375 --- /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 855a906..0000000 --- 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 0000000..39ac263 --- /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 0000000..03ccdb8 --- /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 f156fef..0000000 --- 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 0000000..751853b --- /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 0000000..1f87b3c --- /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 9e70258..0000000 --- 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 0000000..d63eb11 --- /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 f613b19..1f1590e 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 4990a04..0f38d7c 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 e8bc2d2..a3768ba 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 7e7f6c6..fff321b 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 881ed9c..f06db65 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 1d705dc..68688bc 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 d3ef8101c1486f986df1d1a581a85627da956ffc Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 14 Nov 2024 00:10:49 +0100 Subject: [PATCH 24/29] 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 751853b..f48e3ad 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 630c1f98d9f6294c24b72053515a8870b517ee9f Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 14 Nov 2024 21:21:25 +0100 Subject: [PATCH 25/29] split cli module --- lib/Espfc/src/Connect/Cli.cpp | 1490 +++++++++++++++++++++++++++++++ lib/Espfc/src/Connect/Cli.h | 1561 --------------------------------- lib/Espfc/src/Connect/Cli.hpp | 107 +++ lib/Espfc/src/SerialManager.h | 2 +- 4 files changed, 1598 insertions(+), 1562 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 0000000..020d9ae --- /dev/null +++ b/lib/Espfc/src/Connect/Cli.cpp @@ -0,0 +1,1490 @@ +#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), + +#ifdef ESPFC_SERIAL_SOFT_0_WIFI + Param(PSTR("wifi_ssid"), PARAM_STRING, &c.wireless.ssid[0], NULL, 32), + Param(PSTR("wifi_pass"), PARAM_STRING, &c.wireless.pass[0], NULL, 32), + Param(PSTR("wifi_tcp_port"), &c.wireless.port), +#endif + + 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.print(F("# ")); + //printVersion(s); + //s.println(); + for(size_t i = 0; _params[i].name; ++i) + { + print(_params[i], s); + } + } + 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("OK")); + } + 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 b668314..0000000 --- a/lib/Espfc/src/Connect/Cli.h +++ /dev/null @@ -1,1561 +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.max = String(args[6]).toInt(); - if(args[7]) ac.max = 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), - -#ifdef ESPFC_SERIAL_SOFT_0_WIFI - Param(PSTR("wifi_ssid"), PARAM_STRING, &c.wireless.ssid[0], NULL, 32), - Param(PSTR("wifi_pass"), PARAM_STRING, &c.wireless.pass[0], NULL, 32), - Param(PSTR("wifi_tcp_port"), &c.wireless.port), -#endif - - 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(); - for(size_t i = 0; _params[i].name; ++i) - { - print(_params[i], s); - } - } - 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("OK")); - } - 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 0000000..384cd70 --- /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 fff321b..9ab7777 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 ef22026bfe1adb5f726931dd7468fc9b40566edf Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 14 Nov 2024 21:34:16 +0100 Subject: [PATCH 26/29] 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 020d9ae..e2be605 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 a3a6492811759f4c0cc9264c1fd1d82947bde64c Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 14 Nov 2024 21:46:14 +0100 Subject: [PATCH 27/29] 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 9c3100d..0000000 --- a/lib/Kalman/library.json +++ /dev/null @@ -1,4 +0,0 @@ -{ - "name": "Kalman", - "version": "2.0.0" -} From e893e85319c91b5a768a4a992f0fccd0f9d78085 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 23 Nov 2024 00:15:44 +0100 Subject: [PATCH 28/29] 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 21f7881..50c256c 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 0000000..c3cf4ae --- /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 0000000..c8c590a --- /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 7e169de8e43483a64550754b04b3ac36b31bc2df Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 23 Nov 2024 01:52:39 +0100 Subject: [PATCH 29/29] 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 1e2d4e2..439b104 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 93134dd..325ad9b 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 06ec944..4768419 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 b0bc9c6..81c449d 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 fbc13fb..6b9ae67 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 75abfef..bdbe9cc 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 0000000..bcfcf2a --- /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 e94f327..f322779 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 0000000..a9ca598 --- /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 1f3a38c..49cc0c1 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 0000000..e56a8a5 --- /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 1abf53e..0f49d5e 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 0000000..63a2038 --- /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 2c77bef..6bb5823 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