handle more debug modes + debug x8 fix

rtlopez · Aug 1, 2023 · 04bd9e2 · 04bd9e2
1 parent 8d1e614
commit 04bd9e2
Show file tree

Hide file tree

Showing 9 changed files with 64 additions and 8 deletions.
diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h
@@ -20,6 +20,7 @@ class Actuator
 
     int update()
     {
+      uint32_t startTime = micros();
       Stats::Measure(_model.state.stats, COUNTER_ACTUATOR);
       updateArmingDisabled();
       updateModeMask();
@@ -28,6 +29,12 @@ class Actuator
       updateScaler();
       updateBuzzer();
       updateDynLpf();
+
+      if(_model.config.debugMode == DEBUG_PIDLOOP)
+      {
+        _model.state.debug[4] = micros() - startTime;
+      }
+
       return 1;
     }
 

diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h
@@ -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;
     }
 
@@ -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;
@@ -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];
         }
       }

diff --git a/lib/Espfc/src/Controller.h b/lib/Espfc/src/Controller.h
@@ -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();
@@ -67,6 +74,11 @@ class Controller
         }
       }
 
+      if(_model.config.debugMode == DEBUG_PIDLOOP)
+      {
+        _model.state.debug[2] = micros() - startTime;
+      }
+
       return 1;
     }
 

diff --git a/lib/Espfc/src/Input.h b/lib/Espfc/src/Input.h
@@ -100,11 +100,19 @@ 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;
     }

diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h
@@ -139,6 +139,7 @@ struct ModelState
 
   VectorInt16 gyroRaw;
   VectorFloat gyroSampled;
+  VectorFloat gyroScaled;
   VectorFloat gyroDynNotch;
   VectorFloat gyroImu;
 

diff --git a/lib/Espfc/src/Output/Mixer.h b/lib/Espfc/src/Output/Mixer.h
@@ -76,12 +76,25 @@ class Mixer
 
     int update()
     {
+      uint32_t startTime = micros();
+
       float outputs[OUTPUT_CHANNELS];
       const MixerConfig& mixer = _model.state.currentMixer;
 
       updateMixer(mixer, outputs);
       writeOutput(mixer, outputs);
 
+      if(_model.config.debugMode == DEBUG_PIDLOOP)
+      {
+        _model.state.debug[3] = micros() - startTime;
+      }
+
+      if(_model.config.debugMode == DEBUG_CYCLETIME)
+      {
+        _model.state.debug[0] = _model.state.stats.loopTime();
+        _model.state.debug[1] = lrintf(_model.state.stats.getCpuLoad());
+      }
+
       return 1;
     }
 

diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h
@@ -103,6 +103,7 @@ class GyroSensor: public BaseSensor
 
       calibrate();
 
+      _model.state.gyroScaled = _model.state.gyro;
 
       // filtering
       for(size_t i = 0; i < 3; ++i)
@@ -113,7 +114,7 @@ class GyroSensor: public BaseSensor
         }
         if(_model.config.debugMode == DEBUG_GYRO_SCALED)
         {
-          _model.state.debug[i] = lrintf(degrees(_model.state.gyro[i]));
+          _model.state.debug[i] = lrintf(degrees(_model.state.gyroScaled[i]));
         }
         if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == _model.config.debugAxis)
         {

diff --git a/lib/Espfc/src/Stats.h b/lib/Espfc/src/Stats.h
@@ -80,7 +80,8 @@ class Stats
     {
       uint32_t now = micros();
       uint32_t diff = now - _loop_last;
-      _loop_time += (((int32_t)diff - _loop_time + 8) >> 4);
+      _loop_time = diff;
+      //_loop_time += (((int32_t)diff - _loop_time + 8) >> 4);
       _loop_last = now;
     }
 

diff --git a/lib/betaflight/src/platform.h b/lib/betaflight/src/platform.h
@@ -69,7 +69,7 @@ extern const char * boardIdentifier;
 #define CONCAT3(_1,_2,_3)  CONCAT(CONCAT(_1, _2), _3)
 #define CONCAT4(_1,_2,_3,_4)  CONCAT(CONCAT3(_1, _2, _3), _4)
 #define XYZ_AXIS_COUNT 3
-#define DEBUG16_VALUE_COUNT 4
+#define DEBUG16_VALUE_COUNT 8
 #define DEBUG_SET(mode, index, value) {if (debugMode == (mode)) {debug[(index)] = (value);}}
 
 #define LOG2_8BIT(v)  (8 - 90/(((v)/4+14)|1) - 2/((v)/2+1))