Skip to content

Commit

Permalink
controller debug pin and serial config refactor
Browse files Browse the repository at this point in the history
  • Loading branch information
@rtlopez
rtlopez committed Oct 28, 2024
1 parent 893c374 commit 3adc103
Show file tree
Hide file tree
Showing 14 changed files with 135 additions and 150 deletions.
2 changes: 1 addition & 1 deletion lib/Espfc/src/Actuator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
14 changes: 7 additions & 7 deletions lib/Espfc/src/Blackbox/Blackbox.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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++)
Expand All @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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++)
{
Expand Down
8 changes: 4 additions & 4 deletions lib/Espfc/src/Cli.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down Expand Up @@ -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),
Expand Down
18 changes: 9 additions & 9 deletions lib/Espfc/src/Controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
Expand All @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -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)
{
Expand All @@ -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);
Expand All @@ -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()
Expand Down
2 changes: 1 addition & 1 deletion lib/Espfc/src/Fusion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
16 changes: 8 additions & 8 deletions lib/Espfc/src/Input.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand All @@ -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;
}
Expand All @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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;
}
Expand All @@ -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;
Expand All @@ -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);
Expand All @@ -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;
}
Expand Down
2 changes: 1 addition & 1 deletion lib/Espfc/src/Model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down
Loading

0 comments on commit 3adc103

Please sign in to comment.