diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h index f3d22c13..c054898c 100644 --- a/lib/Espfc/src/Actuator.h +++ b/lib/Espfc/src/Actuator.h @@ -42,7 +42,7 @@ class Actuator updateDynLpf(); updateRescueConfig(); - if(_model.config.debugMode == DEBUG_PIDLOOP) + if(_model.config.debug.mode == DEBUG_PIDLOOP) { _model.state.debug[4] = micros() - startTime; } diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index a7250eca..a2c40e83 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -38,7 +38,7 @@ int Blackbox::begin() } systemConfigMutable()->activeRateProfile = 0; - systemConfigMutable()->debug_mode = debugMode = _model.config.debugMode; + systemConfigMutable()->debug_mode = debugMode = _model.config.debug.mode; controlRateConfig_t *rp = controlRateProfilesMutable(systemConfig()->activeRateProfile); for(int i = 0; i <= AXIS_YAW; i++) @@ -50,8 +50,8 @@ int Blackbox::begin() } rp->thrMid8 = 50; rp->thrExpo8 = 0; - rp->dynThrPID = _model.config.tpaScale; - rp->tpa_breakpoint = _model.config.tpaBreakpoint; + rp->dynThrPID = _model.config.controller.tpaScale; + rp->tpa_breakpoint = _model.config.controller.tpaBreakpoint; rp->rates_type = _model.config.input.rateType; pidProfile_s * cp = currentPidProfile = &_pidProfile; @@ -82,8 +82,8 @@ int Blackbox::begin() cp->ff_boost = 0; cp->feedForwardTransition = 0; cp->tpa_mode = 0; // PD - cp->tpa_rate = _model.config.tpaScale; - cp->tpa_breakpoint = _model.config.tpaBreakpoint; + cp->tpa_rate = _model.config.controller.tpaScale; + cp->tpa_breakpoint = _model.config.controller.tpaBreakpoint; cp->motor_output_limit = _model.config.output.motorLimit; cp->throttle_boost = 0; cp->throttle_boost_cutoff = 100; @@ -225,7 +225,7 @@ int FAST_CODE_ATTR Blackbox::update() } //PIN_DEBUG(LOW); - if(_model.config.debugMode == DEBUG_PIDLOOP) + if(_model.config.debug.mode == DEBUG_PIDLOOP) { _model.state.debug[5] = micros() - startTime; } @@ -263,7 +263,7 @@ void FAST_CODE_ATTR Blackbox::updateData() motor[i] = PWM_TO_DSHOT(motor[i]); } } - if(_model.config.debugMode != DEBUG_NONE && _model.config.debugMode != DEBUG_BLACKBOX_OUTPUT) + if(_model.config.debug.mode != DEBUG_NONE && _model.config.debug.mode != DEBUG_BLACKBOX_OUTPUT) { for(size_t i = 0; i < 8; i++) { diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 2df27caa..2d2c949f 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -429,8 +429,8 @@ class Cli Param(PSTR("feature_soft_serial"), &c.featureMask, 6), Param(PSTR("feature_telemetry"), &c.featureMask, 10), - Param(PSTR("debug_mode"), &c.debugMode, debugModeChoices), - Param(PSTR("debug_axis"), &c.debugAxis), + Param(PSTR("debug_mode"), &c.debug.mode, debugModeChoices), + Param(PSTR("debug_axis"), &c.debug.axis), Param(PSTR("gyro_bus"), &c.gyro.bus, busDevChoices), Param(PSTR("gyro_dev"), &c.gyro.dev, gyroDevChoices), @@ -631,8 +631,8 @@ class Cli Param(PSTR("pid_iterm_zero"), &c.iterm.lowThrottleZeroIterm), Param(PSTR("pid_iterm_relax"), &c.iterm.relax, inputItermRelaxChoices), Param(PSTR("pid_iterm_relax_cutoff"), &c.iterm.relaxCutoff), - Param(PSTR("pid_tpa_scale"), &c.tpaScale), - Param(PSTR("pid_tpa_breakpoint"), &c.tpaBreakpoint), + Param(PSTR("pid_tpa_scale"), &c.controller.tpaScale), + Param(PSTR("pid_tpa_breakpoint"), &c.controller.tpaBreakpoint), Param(PSTR("mixer_sync"), &c.mixerSync), Param(PSTR("mixer_type"), &c.mixer.type, mixerTypeChoices), diff --git a/lib/Espfc/src/Controller.cpp b/lib/Espfc/src/Controller.cpp index d44fb634..afa8d52e 100644 --- a/lib/Espfc/src/Controller.cpp +++ b/lib/Espfc/src/Controller.cpp @@ -15,7 +15,7 @@ int Controller::begin() int FAST_CODE_ATTR Controller::update() { uint32_t startTime = 0; - if(_model.config.debugMode == DEBUG_PIDLOOP) + if(_model.config.debug.mode == DEBUG_PIDLOOP) { startTime = micros(); _model.state.debug[0] = startTime - _model.state.loopTimer.last; @@ -46,7 +46,7 @@ int FAST_CODE_ATTR Controller::update() } } - if(_model.config.debugMode == DEBUG_PIDLOOP) + if(_model.config.debug.mode == DEBUG_PIDLOOP) { _model.state.debug[2] = micros() - startTime; } @@ -72,7 +72,7 @@ void Controller::outerLoopRobot() _model.state.desiredAngle.set(AXIS_PITCH, angle); _model.state.desiredRate[AXIS_YAW] = _model.state.input[AXIS_YAW] * Math::toRad(_model.config.level.rateLimit); - if(_model.config.debugMode == DEBUG_ANGLERATE) + if(_model.config.debug.mode == DEBUG_ANGLERATE) { _model.state.debug[0] = speed * 1000; _model.state.debug[1] = lrintf(degrees(angle) * 10); @@ -99,7 +99,7 @@ void Controller::innerLoopRobot() _model.state.output[AXIS_YAW] = 0.f; } - if(_model.config.debugMode == DEBUG_ANGLERATE) + if(_model.config.debug.mode == DEBUG_ANGLERATE) { _model.state.debug[2] = lrintf(degrees(_model.state.angle[AXIS_PITCH]) * 10); _model.state.debug[3] = lrintf(_model.state.output[AXIS_PITCH] * 1000); @@ -129,7 +129,7 @@ void FAST_CODE_ATTR Controller::outerLoop() _model.state.desiredRate[AXIS_YAW] = calculateSetpointRate(AXIS_YAW, _model.state.input[AXIS_YAW]); _model.state.desiredRate[AXIS_THRUST] = _model.state.input[AXIS_THRUST]; - if(_model.config.debugMode == DEBUG_ANGLERATE) + if(_model.config.debug.mode == DEBUG_ANGLERATE) { for(size_t i = 0; i < 3; ++i) { @@ -148,7 +148,7 @@ void FAST_CODE_ATTR Controller::innerLoop() } _model.state.output[AXIS_THRUST] = _model.state.desiredRate[AXIS_THRUST]; - if(_model.config.debugMode == DEBUG_ITERM_RELAX) + if(_model.config.debug.mode == DEBUG_ITERM_RELAX) { _model.state.debug[0] = lrintf(Math::toDeg(_model.state.innerPid[0].itermRelaxBase)); _model.state.debug[1] = lrintf(_model.state.innerPid[0].itermRelaxFactor * 100.0f); @@ -159,9 +159,9 @@ void FAST_CODE_ATTR Controller::innerLoop() float Controller::getTpaFactor() const { - if(_model.config.tpaScale == 0) return 1.f; - float t = Math::clamp(_model.state.inputUs[AXIS_THRUST], (float)_model.config.tpaBreakpoint, 2000.f); - return Math::map(t, (float)_model.config.tpaBreakpoint, 2000.f, 1.f, 1.f - ((float)_model.config.tpaScale * 0.01f)); + if(_model.config.controller.tpaScale == 0) return 1.f; + float t = Math::clamp(_model.state.inputUs[AXIS_THRUST], (float)_model.config.controller.tpaBreakpoint, 2000.f); + return Math::map(t, (float)_model.config.controller.tpaBreakpoint, 2000.f, 1.f, 1.f - ((float)_model.config.controller.tpaScale * 0.01f)); } void Controller::resetIterm() diff --git a/lib/Espfc/src/Fusion.cpp b/lib/Espfc/src/Fusion.cpp index 99be1d3b..8fe3b804 100644 --- a/lib/Espfc/src/Fusion.cpp +++ b/lib/Espfc/src/Fusion.cpp @@ -70,7 +70,7 @@ int FAST_CODE_ATTR Fusion::update() } //else madgwickFusion1(); - if(_model.config.debugMode == DEBUG_ALTITUDE) + if(_model.config.debug.mode == DEBUG_ALTITUDE) { _model.state.debug[0] = lrintf(degrees(_model.state.angle[0]) * 10); _model.state.debug[1] = lrintf(degrees(_model.state.angle[1]) * 10); diff --git a/lib/Espfc/src/Input.cpp b/lib/Espfc/src/Input.cpp index 95654fbd..c9199313 100644 --- a/lib/Espfc/src/Input.cpp +++ b/lib/Espfc/src/Input.cpp @@ -88,7 +88,7 @@ int FAST_CODE_ATTR Input::update() filterInputs(status); } - if(_model.config.debugMode == DEBUG_PIDLOOP) + if(_model.config.debug.mode == DEBUG_PIDLOOP) { _model.state.debug[1] = micros() - startTime; } @@ -103,7 +103,7 @@ InputStatus FAST_CODE_ATTR Input::readInputs() InputStatus status = _device->update(); - if(_model.config.debugMode == DEBUG_RX_TIMING) + if(_model.config.debug.mode == DEBUG_RX_TIMING) { _model.state.debug[0] = micros() - startTime; } @@ -118,7 +118,7 @@ InputStatus FAST_CODE_ATTR Input::readInputs() processInputs(); - if(_model.config.debugMode == DEBUG_RX_SIGNAL_LOSS) + if(_model.config.debug.mode == DEBUG_RX_SIGNAL_LOSS) { _model.state.debug[0] = !_model.state.inputRxLoss; _model.state.debug[1] = _model.state.inputRxFailSafe; @@ -175,7 +175,7 @@ void FAST_CODE_ATTR Input::processInputs() _model.state.inputBuffer[c] = v; } - if(_model.config.debugMode == DEBUG_RX_TIMING) + if(_model.config.debug.mode == DEBUG_RX_TIMING) { _model.state.debug[2] = micros() - startTime; } @@ -279,7 +279,7 @@ void FAST_CODE_ATTR Input::filterInputs(InputStatus status) setInput((Axis)c, v, newFrame); } - if(_model.config.debugMode == DEBUG_RX_TIMING) + if(_model.config.debug.mode == DEBUG_RX_TIMING) { _model.state.debug[3] = micros() - startTime; } @@ -300,7 +300,7 @@ void FAST_CODE_ATTR Input::updateFrameRate() _model.state.inputInterpolationStep = _model.state.loopTimer.intervalf / _model.state.inputInterpolationDelta; } - if(_model.config.debugMode == DEBUG_RC_SMOOTHING_RATE) + if(_model.config.debug.mode == DEBUG_RC_SMOOTHING_RATE) { _model.state.debug[0] = _model.state.inputFrameDelta / 10; _model.state.debug[1] = _model.state.inputFrameRate; @@ -311,7 +311,7 @@ void FAST_CODE_ATTR Input::updateFrameRate() if(freq > _model.state.inputAutoFreq * 1.1f || freq < _model.state.inputAutoFreq * 0.9f) { _model.state.inputAutoFreq += 0.25f * (freq - _model.state.inputAutoFreq); - if(_model.config.debugMode == DEBUG_RC_SMOOTHING_RATE) + if(_model.config.debug.mode == DEBUG_RC_SMOOTHING_RATE) { _model.state.debug[2] = lrintf(freq); _model.state.debug[3] = lrintf(_model.state.inputAutoFreq); @@ -331,7 +331,7 @@ void FAST_CODE_ATTR Input::updateFrameRate() } } - if(_model.config.debugMode == DEBUG_RX_TIMING) + if(_model.config.debug.mode == DEBUG_RX_TIMING) { _model.state.debug[1] = micros() - now; } diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 3a21d101..669aa962 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -212,7 +212,7 @@ class Model void inline setDebug(DebugMode mode, size_t index, int16_t value) { if(index >= 8) return; - if(config.debugMode != mode) return; + if(config.debug.mode != mode) return; state.debug[index] = value; } diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 4e54bc95..fb6a201f 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -532,6 +532,12 @@ struct BlackboxConfig int8_t mode = 0; }; +struct DebugConfig +{ + int8_t mode = DEBUG_NONE; + uint8_t axis = 1; +}; + struct RpmFilterConfig { uint8_t harmonics = 3; @@ -635,23 +641,31 @@ struct MixerConfiguration bool yawReverse = 0; }; +struct ControllerConfig +{ + int8_t tpaScale = 10; + int16_t tpaBreakpoint = 1650; +}; + // persistent data class ModelConfig { public: + // inputs and sensors GyroConfig gyro; AccelConfig accel; BaroConfig baro; MagConfig mag; - InputConfig input; FailsafeConfig failsafe; + FusionConfig fusion; + VBatConfig vbat; + IBatConfig ibat; ActuatorCondition conditions[ACTUATOR_CONDITIONS]; ScalerConfig scaler[SCALER_COUNT]; - OutputConfig output; - + // pid controller PidConfig pid[FC_PID_ITEM_COUNT] = { [FC_PID_ROLL] = { .P = 42, .I = 85, .D = 24, .F = 72 }, // ROLL [FC_PID_PITCH] = { .P = 46, .I = 90, .D = 26, .F = 76 }, // PITCH @@ -664,137 +678,108 @@ class ModelConfig [FC_PID_MAG] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // MAG [FC_PID_VEL] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // VEL }; - - MixerConfiguration mixer; YawConfig yaw; LevelConfig level; DtermConfig dterm; ItermConfig iterm; + ControllerConfig controller; - int8_t loopSync = 8; // MPU 1000Hz - int8_t mixerSync = 1; - - int32_t featureMask = ESPFC_FEATURE_MASK; - bool telemetry = 0; - int32_t telemetryInterval = 1000; - - BlackboxConfig blackbox; - - SerialPortConfig serial[SERIAL_UART_COUNT]; - - FusionConfig fusion; - - char modelName[MODEL_NAME_LEN + 1]; - - VBatConfig vbat; - IBatConfig ibat; - - int8_t debugMode = DEBUG_NONE; - uint8_t debugAxis = 1; - - BuzzerConfig buzzer; - - int8_t pin[PIN_COUNT]; - int16_t i2cSpeed = 800; - - int8_t tpaScale = 10; - int16_t tpaBreakpoint = 1650; - - int8_t customMixerCount = 0; - MixerEntry customMixes[MIXER_RULE_MAX]; - - WirelessConfig wireless; - - uint8_t rescueConfigDelay = 30; - - int16_t boardAlignment[3] = {0, 0, 0}; - - ModelConfig() - { + // hardware + int8_t pin[PIN_COUNT] = { #ifdef ESPFC_INPUT - pin[PIN_INPUT_RX] = ESPFC_INPUT_PIN; + [PIN_INPUT_RX] = ESPFC_INPUT_PIN, #endif - pin[PIN_OUTPUT_0] = ESPFC_OUTPUT_0; - pin[PIN_OUTPUT_1] = ESPFC_OUTPUT_1; - pin[PIN_OUTPUT_2] = ESPFC_OUTPUT_2; - pin[PIN_OUTPUT_3] = ESPFC_OUTPUT_3; + [PIN_OUTPUT_0] = ESPFC_OUTPUT_0, + [PIN_OUTPUT_1] = ESPFC_OUTPUT_1, + [PIN_OUTPUT_2] = ESPFC_OUTPUT_2, + [PIN_OUTPUT_3] = ESPFC_OUTPUT_3, #if ESPFC_OUTPUT_COUNT > 4 - pin[PIN_OUTPUT_4] = ESPFC_OUTPUT_4; + [PIN_OUTPUT_4] = ESPFC_OUTPUT_4, #endif #if ESPFC_OUTPUT_COUNT > 5 - pin[PIN_OUTPUT_5] = ESPFC_OUTPUT_5; + [PIN_OUTPUT_5] = ESPFC_OUTPUT_5, #endif #if ESPFC_OUTPUT_COUNT > 6 - pin[PIN_OUTPUT_6] = ESPFC_OUTPUT_6; + [PIN_OUTPUT_6] = ESPFC_OUTPUT_6, #endif #if ESPFC_OUTPUT_COUNT > 7 - pin[PIN_OUTPUT_7] = ESPFC_OUTPUT_7; + [PIN_OUTPUT_7] = ESPFC_OUTPUT_7, #endif - pin[PIN_BUZZER] = ESPFC_BUZZER_PIN; + [PIN_BUZZER] = ESPFC_BUZZER_PIN, #ifdef ESPFC_SERIAL_0 - pin[PIN_SERIAL_0_TX] = ESPFC_SERIAL_0_TX; - pin[PIN_SERIAL_0_RX] = ESPFC_SERIAL_0_RX; + [PIN_SERIAL_0_TX] = ESPFC_SERIAL_0_TX, + [PIN_SERIAL_0_RX] = ESPFC_SERIAL_0_RX, #endif #ifdef ESPFC_SERIAL_1 - pin[PIN_SERIAL_1_TX] = ESPFC_SERIAL_1_TX; - pin[PIN_SERIAL_1_RX] = ESPFC_SERIAL_1_RX; + [PIN_SERIAL_1_TX] = ESPFC_SERIAL_1_TX, + [PIN_SERIAL_1_RX] = ESPFC_SERIAL_1_RX, #endif #ifdef ESPFC_SERIAL_2 - pin[PIN_SERIAL_2_TX] = ESPFC_SERIAL_2_TX; - pin[PIN_SERIAL_2_RX] = ESPFC_SERIAL_2_RX; + [PIN_SERIAL_2_TX] = ESPFC_SERIAL_2_TX, + [PIN_SERIAL_2_RX] = ESPFC_SERIAL_2_RX, #endif #ifdef ESPFC_I2C_0 - pin[PIN_I2C_0_SCL] = ESPFC_I2C_0_SCL; - pin[PIN_I2C_0_SDA] = ESPFC_I2C_0_SDA; + [PIN_I2C_0_SCL] = ESPFC_I2C_0_SCL, + [PIN_I2C_0_SDA] = ESPFC_I2C_0_SDA, #endif #ifdef ESPFC_ADC_0 - pin[PIN_INPUT_ADC_0] = ESPFC_ADC_0_PIN; + [PIN_INPUT_ADC_0] = ESPFC_ADC_0_PIN, #endif #ifdef ESPFC_ADC_1 - pin[PIN_INPUT_ADC_1] = ESPFC_ADC_1_PIN; + [PIN_INPUT_ADC_1] = ESPFC_ADC_1_PIN, #endif #ifdef ESPFC_SPI_0 - pin[PIN_SPI_0_SCK] = ESPFC_SPI_0_SCK; - pin[PIN_SPI_0_MOSI] = ESPFC_SPI_0_MOSI; - pin[PIN_SPI_0_MISO] = ESPFC_SPI_0_MISO; + [PIN_SPI_0_SCK] = ESPFC_SPI_0_SCK, + [PIN_SPI_0_MOSI] = ESPFC_SPI_0_MOSI, + [PIN_SPI_0_MISO] = ESPFC_SPI_0_MISO, #endif #ifdef ESPFC_SPI_0 - pin[PIN_SPI_CS0] = ESPFC_SPI_CS_GYRO; - pin[PIN_SPI_CS1] = ESPFC_SPI_CS_BARO; - pin[PIN_SPI_CS2] = -1; + [PIN_SPI_CS0] = ESPFC_SPI_CS_GYRO, + [PIN_SPI_CS1] = ESPFC_SPI_CS_BARO, + [PIN_SPI_CS2] = -1, +#endif + }; + SerialPortConfig serial[SERIAL_UART_COUNT] = { +#ifdef ESPFC_SERIAL_USB + [SERIAL_USB] = { .id = SERIAL_ID_USB_VCP, .functionMask = ESPFC_SERIAL_USB_FN, .baud = SERIAL_SPEED_115200, .blackboxBaud = SERIAL_SPEED_NONE }, #endif - #ifdef ESPFC_SERIAL_0 - serial[SERIAL_UART_0].id = SERIAL_ID_UART_1; - serial[SERIAL_UART_0].functionMask = ESPFC_SERIAL_0_FN; - serial[SERIAL_UART_0].baud = ESPFC_SERIAL_0_BAUD; - serial[SERIAL_UART_0].blackboxBaud = ESPFC_SERIAL_0_BBAUD; + [SERIAL_UART_0] = { .id = SERIAL_ID_UART_1, .functionMask = ESPFC_SERIAL_0_FN, .baud = ESPFC_SERIAL_0_BAUD, .blackboxBaud = ESPFC_SERIAL_0_BBAUD }, #endif #ifdef ESPFC_SERIAL_1 - serial[SERIAL_UART_1].id = SERIAL_ID_UART_2; - serial[SERIAL_UART_1].functionMask = ESPFC_SERIAL_1_FN; - serial[SERIAL_UART_1].baud = ESPFC_SERIAL_1_BAUD; - serial[SERIAL_UART_1].blackboxBaud = ESPFC_SERIAL_1_BBAUD; + [SERIAL_UART_1] = { .id = SERIAL_ID_UART_2, .functionMask = ESPFC_SERIAL_1_FN, .baud = ESPFC_SERIAL_1_BAUD, .blackboxBaud = ESPFC_SERIAL_1_BBAUD }, #endif #ifdef ESPFC_SERIAL_2 - serial[SERIAL_UART_2].id = SERIAL_ID_UART_3; - serial[SERIAL_UART_2].functionMask = ESPFC_SERIAL_2_FN; - serial[SERIAL_UART_2].baud = ESPFC_SERIAL_2_BAUD; - serial[SERIAL_UART_2].blackboxBaud = ESPFC_SERIAL_2_BBAUD; -#endif -#ifdef ESPFC_SERIAL_USB - serial[SERIAL_USB].id = SERIAL_ID_USB_VCP; - serial[SERIAL_USB].functionMask = ESPFC_SERIAL_USB_FN; - serial[SERIAL_USB].baud = SERIAL_SPEED_115200; - serial[SERIAL_USB].blackboxBaud = SERIAL_SPEED_NONE; + [SERIAL_UART_2] = { .id = SERIAL_ID_UART_3, .functionMask = ESPFC_SERIAL_2_FN, .baud = ESPFC_SERIAL_2_BAUD, .blackboxBaud = ESPFC_SERIAL_2_BBAUD }, #endif #ifdef ESPFC_SERIAL_SOFT_0 - serial[SERIAL_SOFT_0].id = SERIAL_ID_SOFTSERIAL_1; - serial[SERIAL_SOFT_0].functionMask = ESPFC_SERIAL_SOFT_0_FN; - serial[SERIAL_SOFT_0].baud = SERIAL_SPEED_115200; - serial[SERIAL_SOFT_0].blackboxBaud = SERIAL_SPEED_NONE; + [SERIAL_SOFT_0] = { .id = SERIAL_ID_SOFTSERIAL_1, .functionMask = ESPFC_SERIAL_SOFT_0_FN, .baud = SERIAL_SPEED_115200, .blackboxBaud = SERIAL_SPEED_NONE }, #endif + }; + BuzzerConfig buzzer; + WirelessConfig wireless; + // mixer and outputs + int8_t customMixerCount = 0; + MixerEntry customMixes[MIXER_RULE_MAX]; + MixerConfiguration mixer; + OutputConfig output; + BlackboxConfig blackbox; + DebugConfig debug; + + // not classified yet + int16_t i2cSpeed = 800; + int8_t loopSync = 8; // MPU 1000Hz + int8_t mixerSync = 1; + int32_t featureMask = ESPFC_FEATURE_MASK; + bool telemetry = 0; + int32_t telemetryInterval = 1000; + uint8_t rescueConfigDelay = 30; + int16_t boardAlignment[3] = {0, 0, 0}; + char modelName[MODEL_NAME_LEN + 1]; + + ModelConfig() + { for(size_t i = 0; i < INPUT_CHANNELS; i++) { input.channel[i].map = i; @@ -826,7 +811,7 @@ class ModelConfig { #ifdef ESPFC_DEV_PRESET_BLACKBOX blackbox.dev = BLACKBOX_DEV_SERIAL; // serial - debugMode = DEBUG_GYRO_SCALED; + debug.mode = DEBUG_GYRO_SCALED; serial[ESPFC_DEV_PRESET_BLACKBOX].functionMask |= SERIAL_FUNCTION_BLACKBOX; serial[ESPFC_DEV_PRESET_BLACKBOX].blackboxBaud = SERIAL_SPEED_250000; serial[ESPFC_DEV_PRESET_BLACKBOX].baud = SERIAL_SPEED_250000; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 89e61aea..fc1dbc23 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -910,10 +910,10 @@ class MspProcessor { r.writeU8(_model.config.input.superRate[i]); } - r.writeU8(_model.config.tpaScale); // dyn thr pid + r.writeU8(_model.config.controller.tpaScale); // dyn thr pid r.writeU8(50); // thrMid8 r.writeU8(0); // thr expo - r.writeU16(_model.config.tpaBreakpoint); // tpa breakpoint + r.writeU16(_model.config.controller.tpaBreakpoint); // tpa breakpoint r.writeU8(_model.config.input.expo[AXIS_YAW]); // yaw expo r.writeU8(_model.config.input.rate[AXIS_YAW]); // yaw rate r.writeU8(_model.config.input.rate[AXIS_PITCH]); // pitch rate @@ -951,10 +951,10 @@ class MspProcessor { _model.config.input.superRate[i] = m.readU8(); } - _model.config.tpaScale = Math::clamp(m.readU8(), (uint8_t)0, (uint8_t)90); // dyn thr pid + _model.config.controller.tpaScale = Math::clamp(m.readU8(), (uint8_t)0, (uint8_t)90); // dyn thr pid m.readU8(); // thrMid8 m.readU8(); // thr expo - _model.config.tpaBreakpoint = Math::clamp(m.readU16(), (uint16_t)1000, (uint16_t)2000); // tpa breakpoint + _model.config.controller.tpaBreakpoint = Math::clamp(m.readU16(), (uint16_t)1000, (uint16_t)2000); // tpa breakpoint if(m.remain() >= 1) { _model.config.input.expo[AXIS_YAW] = m.readU8(); // yaw expo @@ -1012,7 +1012,7 @@ class MspProcessor r.writeU16(125); // gyro cal duration (1.25s) r.writeU16(0); // gyro offset yaw r.writeU8(0); // check overflow - r.writeU8(_model.config.debugMode); + r.writeU8(_model.config.debug.mode); r.writeU8(DEBUG_COUNT); break; @@ -1040,7 +1040,7 @@ class MspProcessor m.readU8(); // check overflow } if(m.remain()) { - _model.config.debugMode = m.readU8(); + _model.config.debug.mode = m.readU8(); } _model.reload(); break; diff --git a/lib/Espfc/src/Output/Mixer.cpp b/lib/Espfc/src/Output/Mixer.cpp index eccbed01..c612f9e5 100644 --- a/lib/Espfc/src/Output/Mixer.cpp +++ b/lib/Espfc/src/Output/Mixer.cpp @@ -90,14 +90,14 @@ int FAST_CODE_ATTR Mixer::update() updateMixer(mixer, outputs); writeOutput(mixer, outputs); - if(_model.config.debugMode == DEBUG_PIDLOOP) + if(_model.config.debug.mode == DEBUG_PIDLOOP) { _model.state.debug[3] = micros() - startTime; } _model.state.stats.loopTick(); - if(_model.config.debugMode == DEBUG_CYCLETIME) + if(_model.config.debug.mode == DEBUG_CYCLETIME) { _model.state.debug[0] = _model.state.stats.loopTime(); _model.state.debug[1] = lrintf(_model.state.stats.getCpuLoad()); @@ -350,7 +350,7 @@ void FAST_CODE_ATTR Mixer::readTelemetry() if(_model.state.outputTelemetryErrorsCount[i]) { _model.state.outputTelemetryErrors[i] = _model.state.outputTelemetryErrorsSum[i] * 10000 / _model.state.outputTelemetryErrorsCount[i]; - if(_model.config.debugMode == DEBUG_DSHOT_RPM_ERRORS) + if(_model.config.debug.mode == DEBUG_DSHOT_RPM_ERRORS) { _model.state.debug[i] = _model.state.outputTelemetryErrors[i]; } diff --git a/lib/Espfc/src/Sensor/AccelSensor.h b/lib/Espfc/src/Sensor/AccelSensor.h index 8e9d1005..5ef22037 100644 --- a/lib/Espfc/src/Sensor/AccelSensor.h +++ b/lib/Espfc/src/Sensor/AccelSensor.h @@ -69,7 +69,7 @@ class AccelSensor: public BaseSensor for(size_t i = 0; i < 3; i++) { - if(_model.config.debugMode == DEBUG_ACCELEROMETER) + if(_model.config.debug.mode == DEBUG_ACCELEROMETER) { _model.state.debug[i] = _model.state.accelRaw[i]; } diff --git a/lib/Espfc/src/Sensor/BaroSensor.h b/lib/Espfc/src/Sensor/BaroSensor.h index 05704c10..351b4873 100644 --- a/lib/Espfc/src/Sensor/BaroSensor.h +++ b/lib/Espfc/src/Sensor/BaroSensor.h @@ -60,7 +60,7 @@ class BaroSensor: public BaseSensor Stats::Measure measure(_model.state.stats, COUNTER_BARO); - if(_model.config.debugMode == DEBUG_BARO) + if(_model.config.debug.mode == DEBUG_BARO) { _model.state.debug[0] = _state; } @@ -127,7 +127,7 @@ class BaroSensor: public BaseSensor } _model.state.baroAltitude = _model.state.baroAltitudeRaw - _model.state.baroAltitudeBias; - if(_model.config.debugMode == DEBUG_BARO) + if(_model.config.debug.mode == DEBUG_BARO) { _model.state.debug[1] = lrintf(_model.state.baroPressureRaw * 0.1f); // hPa x 10 _model.state.debug[2] = lrintf(_model.state.baroTemperatureRaw * 100.f); // deg C x 100 diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index 92753d79..b002620c 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -31,7 +31,7 @@ int GyroSensor::begin() _dyn_notch_denom = std::max((uint32_t)1, _model.state.loopTimer.rate / 1000); _dyn_notch_sma.begin(_dyn_notch_denom); _dyn_notch_enabled = _model.isActive(FEATURE_DYNAMIC_FILTER) && _model.config.gyro.dynamicFilter.width > 0 && _model.state.loopTimer.rate >= DynamicFilterConfig::MIN_FREQ; - _dyn_notch_debug = _model.config.debugMode == DEBUG_FFT_FREQ || _model.config.debugMode == DEBUG_FFT_TIME; + _dyn_notch_debug = _model.config.debug.mode == DEBUG_FFT_FREQ || _model.config.debug.mode == DEBUG_FFT_TIME; _rpm_enabled = _model.config.gyro.rpmFilter.harmonics > 0 && _model.config.output.dshotTelemetry; _rpm_motor_index = 0; @@ -101,11 +101,11 @@ int FAST_CODE_ATTR GyroSensor::filter() _model.setDebug(DEBUG_GYRO_SCALED, i, lrintf(degrees(_model.state.gyroScaled[i]))); } - _model.setDebug(DEBUG_GYRO_SAMPLE, 0, lrintf(degrees(_model.state.gyro[_model.config.debugAxis]))); + _model.setDebug(DEBUG_GYRO_SAMPLE, 0, lrintf(degrees(_model.state.gyro[_model.config.debug.axis]))); _model.state.gyro = Utils::applyFilter(_model.state.gyroFilter2, _model.state.gyro); - _model.setDebug(DEBUG_GYRO_SAMPLE, 1, lrintf(degrees(_model.state.gyro[_model.config.debugAxis]))); + _model.setDebug(DEBUG_GYRO_SAMPLE, 1, lrintf(degrees(_model.state.gyro[_model.config.debug.axis]))); if (_rpm_enabled) { @@ -118,13 +118,13 @@ int FAST_CODE_ATTR GyroSensor::filter() } } - _model.setDebug(DEBUG_GYRO_SAMPLE, 2, lrintf(degrees(_model.state.gyro[_model.config.debugAxis]))); + _model.setDebug(DEBUG_GYRO_SAMPLE, 2, lrintf(degrees(_model.state.gyro[_model.config.debug.axis]))); _model.state.gyro = Utils::applyFilter(_model.state.gyroNotch1Filter, _model.state.gyro); _model.state.gyro = Utils::applyFilter(_model.state.gyroNotch2Filter, _model.state.gyro); _model.state.gyro = Utils::applyFilter(_model.state.gyroFilter, _model.state.gyro); - _model.setDebug(DEBUG_GYRO_SAMPLE, 3, lrintf(degrees(_model.state.gyro[_model.config.debugAxis]))); + _model.setDebug(DEBUG_GYRO_SAMPLE, 3, lrintf(degrees(_model.state.gyro[_model.config.debug.axis]))); if (_dyn_notch_enabled || _dyn_notch_debug) { @@ -212,13 +212,13 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() { uint32_t startTime = micros(); int status = _fft[i].update(_model.state.gyroDynNotch[i]); - if (_model.config.debugMode == DEBUG_FFT_TIME) + if (_model.config.debug.mode == DEBUG_FFT_TIME) { if (i == 0) _model.state.debug[0] = status; _model.state.debug[i + 1] = micros() - startTime; } - if (_model.config.debugMode == DEBUG_FFT_FREQ && i == _model.config.debugAxis) + if (_model.config.debug.mode == DEBUG_FFT_FREQ && i == _model.config.debug.axis) { _model.state.debug[0] = lrintf(_fft[i].peaks[0].freq); _model.state.debug[1] = lrintf(_fft[i].peaks[1].freq); @@ -243,17 +243,17 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() uint32_t startTime = micros(); _freqAnalyzer[i].update(_model.state.gyroDynNotch[i]); float freq = _freqAnalyzer[i].freq; - if (_model.config.debugMode == DEBUG_FFT_TIME) + if (_model.config.debug.mode == DEBUG_FFT_TIME) { if (i == 0) _model.state.debug[0] = update; _model.state.debug[i + 1] = micros() - startTime; } - if (_model.config.debugMode == DEBUG_FFT_FREQ) + if (_model.config.debug.mode == DEBUG_FFT_FREQ) { if (update) _model.state.debug[i] = lrintf(freq); - if (i == _model.config.debugAxis) + if (i == _model.config.debug.axis) _model.state.debug[3] = lrintf(degrees(_model.state.gyroDynNotch[i])); } if (_dyn_notch_enabled && update) diff --git a/lib/Espfc/src/Sensor/VoltageSensor.h b/lib/Espfc/src/Sensor/VoltageSensor.h index ba7a63a4..3e4af7ad 100644 --- a/lib/Espfc/src/Sensor/VoltageSensor.h +++ b/lib/Espfc/src/Sensor/VoltageSensor.h @@ -80,7 +80,7 @@ class VoltageSensor: public BaseSensor _model.state.battery.cellVoltage = _model.state.battery.voltage / constrain(_model.state.battery.cells, 1, 6); _model.state.battery.percentage = Math::clamp(Math::map(_model.state.battery.cellVoltage, 3.4f, 4.2f, 0.0f, 100.0f), 0.0f, 100.0f); - if(_model.config.debugMode == DEBUG_BATTERY) + if(_model.config.debug.mode == DEBUG_BATTERY) { _model.state.debug[0] = constrain(lrintf(_model.state.battery.voltageUnfiltered * 100.0f), 0, 32000); _model.state.debug[1] = constrain(lrintf(_model.state.battery.voltage * 100.0f), 0, 32000); @@ -106,7 +106,7 @@ class VoltageSensor: public BaseSensor _model.state.battery.currentUnfiltered = volts; _model.state.battery.current = _iFilter.update(_model.state.battery.currentUnfiltered); - if(_model.config.debugMode == DEBUG_CURRENT_SENSOR) + if(_model.config.debug.mode == DEBUG_CURRENT_SENSOR) { _model.state.debug[0] = lrintf(milivolts); _model.state.debug[1] = constrain(lrintf(_model.state.battery.currentUnfiltered * 100.0f), 0, 32000);