Skip to content

Commit

Permalink
Merge pull request #54 from rtlopez/debug
Browse files Browse the repository at this point in the history 
Debug
  • Loading branch information
@rtlopez
rtlopez authored Aug 8, 2023
2 parents 8d1e614 + fc95cd4 commit d317dcf
Show file tree
Hide file tree
Showing 12 changed files with 186 additions and 61 deletions.
7 changes: 7 additions & 0 deletions lib/Espfc/src/Actuator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@ class Actuator

int update()
{
uint32_t startTime = micros();
Stats::Measure(_model.state.stats, COUNTER_ACTUATOR);
updateArmingDisabled();
updateModeMask();
Expand All @@ -28,6 +29,12 @@ class Actuator
updateScaler();
updateBuzzer();
updateDynLpf();

if(_model.config.debugMode == DEBUG_PIDLOOP)
{
_model.state.debug[4] = micros() - startTime;
}

return 1;
}

Expand Down
21 changes: 17 additions & 4 deletions lib/Espfc/src/Blackbox.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -266,7 +266,7 @@ class Blackbox
if(_model.magActive()) enabledSensors |= SENSOR_MAG;
if(_model.baroActive()) enabledSensors |= SENSOR_BARO;

gyro.sampleLooptime = 125; //_model.state.gyroTimer.interval;
gyro.sampleLooptime = _model.state.gyroTimer.interval;
targetPidLooptime = _model.state.loopTimer.interval;
activePidLoopDenom = _model.config.loopSync;

Expand Down Expand Up @@ -316,11 +316,19 @@ class Blackbox
if(!_model.blackboxEnabled()) return 0;
if(!blackboxSerial) return 0;
Stats::Measure measure(_model.state.stats, COUNTER_BLACKBOX);

uint32_t startTime = micros();
updateArmed();
updateMode();
updateData();
blackboxUpdate(_model.state.loopTimer.last);
_buffer.flush();

if(_model.config.debugMode == DEBUG_PIDLOOP)
{
_model.state.debug[5] = micros() - startTime;
}

return 1;
}

Expand All @@ -330,7 +338,7 @@ class Blackbox
for(size_t i = 0; i < 3; i++)
{
gyro.gyroADCf[i] = degrees(_model.state.gyro[i]);
gyro.gyroADC[i] = degrees(_model.state.gyroSampled[i]);
gyro.gyroADC[i] = degrees(_model.state.gyroScaled[i]);
pidData[i].P = _model.state.innerPid[i].pTerm * 1000.f;
pidData[i].I = _model.state.innerPid[i].iTerm * 1000.f;
pidData[i].D = _model.state.innerPid[i].dTerm * 1000.f;
Expand All @@ -350,10 +358,15 @@ class Blackbox
for(size_t i = 0; i < 4; i++)
{
motor[i] = Math::clamp(_model.state.outputUs[i], (int16_t)1000, (int16_t)2000);
if(_model.state.digitalOutput) {
if(_model.state.digitalOutput)
{
motor[i] = PWM_TO_DSHOT(motor[i]);
}
if(_model.config.debugMode != DEBUG_NONE && _model.config.debugMode != DEBUG_BLACKBOX_OUTPUT) {
}
if(_model.config.debugMode != DEBUG_NONE && _model.config.debugMode != DEBUG_BLACKBOX_OUTPUT)
{
for(size_t i = 0; i < 8; i++)
{
debug[i] = _model.state.debug[i];
}
}
Expand Down
12 changes: 12 additions & 0 deletions lib/Espfc/src/Controller.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,13 @@ class Controller

int update()
{
uint32_t startTime = 0;
if(_model.config.debugMode == DEBUG_PIDLOOP)
{
startTime = micros();
_model.state.debug[0] = startTime - _model.state.loopTimer.last;
}

{
Stats::Measure(_model.state.stats, COUNTER_OUTER_PID);
resetIterm();
Expand All @@ -67,6 +74,11 @@ class Controller
}
}

if(_model.config.debugMode == DEBUG_PIDLOOP)
{
_model.state.debug[2] = micros() - startTime;
}

return 1;
}

Expand Down
6 changes: 3 additions & 3 deletions lib/Espfc/src/Espfc.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,10 +59,10 @@ class Espfc
int update()
{
#if defined(ESPFC_MULTI_CORE)
if(!_model.state.appQueue.isEmpty())
/*if(!_model.state.appQueue.isEmpty())
{
return 0;
}
}*/

if(!_model.state.gyroTimer.check())
{
Expand All @@ -82,7 +82,7 @@ class Espfc
_buzzer.update();
_model.state.stats.update();
}
_model.state.appQueue.send(Event(EVENT_IDLE));
//_model.state.appQueue.send(Event(EVENT_IDLE));

return 1;
#else
Expand Down
43 changes: 38 additions & 5 deletions lib/Espfc/src/Input.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,21 +100,35 @@ class Input
{
if(!_device) return 0;

uint32_t startTime = micros();

InputStatus status = readInputs();

if(failsafe(status)) return 1;
if(!failsafe(status))
{
filterInputs(status);
}

filterInputs(status);
if(_model.config.debugMode == DEBUG_PIDLOOP)
{
_model.state.debug[1] = micros() - startTime;
}

return 1;
}

InputStatus readInputs()
{
Stats::Measure readMeasure(_model.state.stats, COUNTER_INPUT_READ);
uint32_t startTime = micros();

InputStatus status = _device->update();

if(_model.config.debugMode == DEBUG_RX_TIMING)
{
_model.state.debug[0] = micros() - startTime;
}

if(status == INPUT_IDLE) return status;

_model.state.inputRxLoss = (status == INPUT_LOST || status == INPUT_FAILSAFE);
Expand All @@ -140,6 +154,8 @@ class Input
{
if(_model.state.inputFrameCount < 5) return; // ignore few first frames that might be garbage

uint32_t startTime = micros();

uint16_t channels[INPUT_CHANNELS];
_device->get(channels, _model.state.inputChannelCount);

Expand Down Expand Up @@ -179,6 +195,11 @@ class Input
_model.state.inputBufferPrevious[c] = _model.state.inputBuffer[c];
_model.state.inputBuffer[c] = v;
}

if(_model.config.debugMode == DEBUG_RX_TIMING)
{
_model.state.debug[2] = micros() - startTime;
}
}

bool failsafe(InputStatus status)
Expand Down Expand Up @@ -251,6 +272,7 @@ class Input
void filterInputs(InputStatus status)
{
Stats::Measure filterMeasure(_model.state.stats, COUNTER_INPUT_FILTER);
uint32_t startTime = micros();

const bool newFrame = status != INPUT_IDLE;
const bool interpolation = _model.config.input.interpolationMode != INPUT_INTERPOLATION_OFF && _model.config.input.filterType == INPUT_INTERPOLATION;
Expand All @@ -276,6 +298,11 @@ class Input
}
setInput((Axis)c, v, newFrame);
}

if(_model.config.debugMode == DEBUG_RX_TIMING)
{
_model.state.debug[3] = micros() - startTime;
}
}

void updateFrameRate()
Expand All @@ -295,7 +322,8 @@ class Input

if(_model.config.debugMode == DEBUG_RC_SMOOTHING_RATE)
{
_model.state.debug[0] = _model.state.inputFrameRate;
_model.state.debug[0] = _model.state.inputFrameDelta / 10;
_model.state.debug[1] = _model.state.inputFrameRate;
}

// auto cutoff input freq
Expand All @@ -305,8 +333,8 @@ class Input
_model.state.inputAutoFreq += 0.25f * (freq - _model.state.inputAutoFreq);
if(_model.config.debugMode == DEBUG_RC_SMOOTHING_RATE)
{
_model.state.debug[1] = lrintf(freq);
_model.state.debug[2] = lrintf(_model.state.inputAutoFreq);
_model.state.debug[2] = lrintf(freq);
_model.state.debug[3] = lrintf(_model.state.inputAutoFreq);
}
FilterConfig conf((FilterType)_model.config.input.filter.type, _model.state.inputAutoFreq);
FilterConfig confDerivative((FilterType)_model.config.input.filterDerivative.type, _model.state.inputAutoFreq);
Expand All @@ -322,6 +350,11 @@ class Input
}
}
}

if(_model.config.debugMode == DEBUG_RX_TIMING)
{
_model.state.debug[1] = micros() - now;
}
}

Device::InputDevice * getInputDevice()
Expand Down
Loading

0 comments on commit d317dcf

Please sign in to comment.