commander add loop times

Author: askuric

src/BLDCMotor.cpp

@@ -335,6 +335,9 @@ int BLDCMotor::absoluteZeroSearch() {
 // Iterative function looping FOC algorithm, setting Uq on the Motor
 // The faster it can be run the better
 void BLDCMotor::loopFOC() {
+  // update loop time measurement
+  updateLoopTime();
+
   // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track
   //                 of full rotations otherwise.
   if (sensor) sensor->update();

src/HybridStepperMotor.cpp

@@ -313,6 +313,8 @@ int HybridStepperMotor::absoluteZeroSearch()
 // Iterative function looping FOC algorithm, setting Uq on the Motor
 // The faster it can be run the better
 void HybridStepperMotor::loopFOC() {
+  // update loop time measurement
+  updateLoopTime();
 
   // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track
   //                 of full rotations otherwise.

src/StepperMotor.cpp

@@ -294,6 +294,8 @@ int StepperMotor::absoluteZeroSearch() {
 // Iterative function looping FOC algorithm, setting Uq on the Motor
 // The faster it can be run the better
 void StepperMotor::loopFOC() {
+  // update loop time measurement
+  updateLoopTime();
 
   // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track
   //                 of full rotations otherwise.

src/common/base_classes/FOCMotor.h

@@ -256,9 +256,23 @@ class FOCMotor
 
     // monitoring functions
     Print* monitor_port; //!< Serial terminal variable if provided
+
+    //!< time between two loopFOC executions in microseconds
+    uint32_t loop_time_us = 0; //!< filtered loop times
+    // last loop time measurement update
+    void updateLoopTime() {
+      uint32_t now = _micros();
+      last_loop_time_us = now - last_loop_timestamp_us;
+      loop_time_us = 0.9f * loop_time_us + 0.1f * last_loop_time_us;
+      last_loop_timestamp_us = now;
+    }
+
   private:
     // monitor counting variable
     unsigned int monitor_cnt = 0 ; //!< counting variable
+
+    uint32_t last_loop_timestamp_us = 0; //!< timestamp of the last loopFOC execution in microseconds
+    uint32_t last_loop_time_us = 0; //!< time between two loopFOC executions in microseconds    
 };
 
 

src/communication/Commander.cpp

@@ -266,6 +266,23 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
           break;
        }
       break;
+    case CMD_FOC_PARAMS:
+      printVerbose(F("FOC | "));
+      switch (sub_cmd){
+        case SCMD_LOOPFOC_TIME:      // loopFOC execution time
+          printVerbose(F("loop time: "));
+          println((int)motor->loop_time_us);
+          break;
+        case SCMD_REINIT_FOC:
+          printVerbose(F("Reinit!"));
+          motor->initFOC();
+          println(F("done"));
+          break;
+        default:
+          printError();
+          break;
+      }
+      break;
     case CMD_MONITOR:     // get current values of the state variables
       printVerbose(F("Monitor | "));
       switch (sub_cmd){

src/communication/commands.h

@@ -18,6 +18,7 @@
  #define CMD_INDUCTANCE    'I' //!< motor phase inductance
  #define CMD_KV_RATING 'K' //!< motor kv rating
  #define CMD_PWMMOD   'W' //!< pwm modulation
+ #define CMD_FOC_PARAMS 'F'  //!< time parameters
 
  // commander configuration
  #define CMD_SCAN    '?' //!< command scaning the network - only for commander
@@ -48,5 +49,7 @@
  #define SCMD_PWMMOD_TYPE   'T'  //!<< Pwm modulation type
  #define SCMD_PWMMOD_CENTER 'C'  //!<< Pwm modulation center flag
 
+ #define SCMD_LOOPFOC_TIME   'L' //!< loopFOC execution time
+ #define SCMD_REINIT_FOC    'R' //!< reinitialize FOC
 
 #endif

src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp

@@ -148,7 +148,7 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin)
             SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M);
 
             // wait for conversion
-            while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0);
+            while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); 
             // read the value
             value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S);
             break;