Add TaskTimer class

Firmware/MotorControl/axis.cpp

@@ -198,25 +198,29 @@ bool Axis::do_checks() {
 // @brief Update all esitmators
 bool Axis::do_updates() {
     // Sub-components should use set_error which will propegate to this error_
+    task_times_.thermistor_update.beginTimer();
     for (ThermistorCurrentLimiter* thermistor : thermistors_) {
         thermistor->update();
     }
-    task_times_.thermistor_update = sample_TIM13();
+    task_times_.thermistor_update.stopTimer();
 
+    task_times_.encoder_update.beginTimer();
     encoder_.update();
-    task_times_.encoder_update = sample_TIM13();
+    task_times_.encoder_update.stopTimer();
 
+    task_times_.sensorless_update.beginTimer();
     sensorless_estimator_.update();
-    task_times_.sensorless_update = sample_TIM13();
+    task_times_.sensorless_update.stopTimer();
 
+    task_times_.min_endstop_update.beginTimer();
     min_endstop_.update();
-    task_times_.min_endstop_update = sample_TIM13();
+    task_times_.min_endstop_update.stopTimer();
 
+    task_times_.max_endstop_update.beginTimer();
     max_endstop_.update();
-    task_times_.max_endstop_update = sample_TIM13();
+    task_times_.max_endstop_update.stopTimer();
 
     bool ret = check_for_errors();
-    task_times_.axis_error_check = sample_TIM13();
 
     odCAN->send_heartbeat(this);
     return ret;
@@ -356,15 +360,18 @@ bool Axis::run_closed_loop_control_loop() {
     set_step_dir_active(config_.enable_step_dir);
     run_control_loop([this](){
         // Note that all estimators are updated in the loop prefix in run_control_loop
+        
+        task_times_.controller_update.beginTimer();
         float torque_setpoint;
         if (!controller_.update(&torque_setpoint))
             return error_ |= ERROR_CONTROLLER_FAILED, false;
-        task_times_.controller_update = sample_TIM13();
+        task_times_.controller_update.stopTimer();
 
+        task_times_.motor_update.beginTimer();
         float phase_vel = (2*M_PI) * encoder_.vel_estimate_ * motor_.config_.pole_pairs;
         if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel))
             return false; // set_error should update axis.error_
-        task_times_.motor_update = sample_TIM13();
+        task_times_.motor_update.stopTimer();
 
         return true;
     });

Firmware/MotorControl/axis.hpp

@@ -11,6 +11,7 @@ class Axis;
 #include "low_level.h"
 #include "utils.hpp"
 #include "communication/interface_uart.h" // TODO: remove once uart_poll() is gone
+#include "taskTimer.hpp"
 
 #include <array>
 
@@ -29,22 +30,24 @@ public:
     };
 
     struct TaskTimes_t {
-        uint16_t thermistor_update = 0;
-        uint16_t encoder_update = 0;
-        uint16_t sensorless_update = 0;
-        uint16_t min_endstop_update = 0;
-        uint16_t max_endstop_update = 0;
-        uint16_t axis_update = 0;
-        uint16_t axis_error_check = 0;
+        TaskTimer thermistor_update;
+        TaskTimer encoder_update;
+        TaskTimer sensorless_update;
+        TaskTimer min_endstop_update;
+        TaskTimer max_endstop_update;
+        TaskTimer axis_update;
+        TaskTimer axis_error_check;
 
-        uint16_t controller_update = 0;
-        uint16_t motor_update = 0;
-        uint16_t update_handler = 0;
+        TaskTimer controller_update;
+        TaskTimer motor_update;
+        TaskTimer update_handler;
 
-        uint16_t brake_update = 0;
-        uint16_t adc_cb = 0;
-        uint16_t control_loop = 0;
-        uint32_t total = 0;
+        TaskTimer brake_update;
+        TaskTimer adc_cb;
+        TaskTimer control_loop;
+        TaskTimer total;
+
+        TaskTimer uart_poll;
     };
 
     static LockinConfig_t default_calibration();
@@ -166,18 +169,21 @@ public:
     // go to zero.
     //
     // @tparam T Must be a callable type that takes no arguments and returns a bool
-    uint16_t start = 0;
     template<typename T>
     void run_control_loop(const T& update_handler) {
         while (requested_state_ == AXIS_STATE_UNDEFINED) {
-            start = sample_TIM13();
+            task_times_.control_loop.beginTimer();
+
             // look for errors at axis level and also all subcomponents
+            task_times_.axis_error_check.beginTimer();
             bool checks_ok = do_checks();
+            task_times_.axis_error_check.stopTimer();
 
             // Update all estimators
             // Note: updates run even if checks fail
+            task_times_.axis_update.beginTimer();
             bool updates_ok = do_updates();
-            task_times_.axis_update = sample_TIM13();
+            task_times_.axis_update.stopTimer();
 
             // make sure the watchdog is being fed. 
             bool watchdog_ok = watchdog_check();
@@ -191,29 +197,21 @@ public:
 
             // Run main loop function, defer quitting for after wait
             // TODO: change arming logic to arm after waiting
+            task_times_.update_handler.beginTimer();
             bool main_continue = update_handler();
-            task_times_.update_handler = sample_TIM13();
+            task_times_.update_handler.stopTimer();
 
             if (axis_num_ == 0) {
+                task_times_.uart_poll.beginTimer();
                 uart_poll(); // TODO: move to board-level control loop once it exists
+                task_times_.uart_poll.stopTimer();
             }
 
             // Check we meet deadlines after queueing
             ++loop_counter_;
 
-            task_times_.control_loop = sample_TIM13() - task_times_.update_handler;
-            task_times_.update_handler -= task_times_.axis_update;
-            task_times_.axis_update -= start;
-            task_times_.motor_update -= task_times_.controller_update;
-            task_times_.controller_update -= task_times_.axis_error_check;
-            task_times_.axis_error_check -= task_times_.max_endstop_update;
-            task_times_.max_endstop_update -= task_times_.min_endstop_update;
-            task_times_.min_endstop_update -= task_times_.sensorless_update;
-            task_times_.sensorless_update -= task_times_.encoder_update;
-            task_times_.encoder_update -= task_times_.thermistor_update;
-            task_times_.thermistor_update -= start;
-            task_times_.total = sample_TIM13() - start;
-
+            task_times_.control_loop.stopTimer();
+            task_times_.total.stopTimer();
 
             // Wait until the current measurement interrupt fires
             if (!wait_for_current_meas()) {
@@ -225,6 +223,8 @@ public:
                 break;
             }
 
+            task_times_.total.beginTimer();
+
             if (!main_continue)
                 break;
         }

Firmware/MotorControl/controller.cpp

@@ -2,8 +2,6 @@
 #include "odrive_main.h"
 #include <algorithm>
 
-#include <algorithm>
-
 bool Controller::apply_config() {
     config_.parent = this;
     update_filter_gains();

Firmware/MotorControl/low_level.cpp

@@ -382,7 +382,8 @@ void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
 // This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion.
 // TODO: Document how the phasing is done, link to timing diagram
 void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
-    auto start = sample_TIM13();
+    axes[0].task_times_.adc_cb.beginTimer();
+    axes[1].task_times_.adc_cb.beginTimer();
 #define calib_tau 0.2f  //@TOTO make more easily configurable
     constexpr float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau;
 
@@ -478,15 +479,15 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
             axis.motor_.DC_calib_.phC += (current - axis.motor_.DC_calib_.phC) * calib_filter_k;
         }
     }
-    auto end = (sample_TIM13() - start);
-    axes[0]->task_times_.adc_cb = end;
-    axes[1]->task_times_.adc_cb = end;
+    axes[0].task_times_.adc_cb.stopTimer();
+    axes[1].task_times_.adc_cb.stopTimer();
 }
 
 // @brief Sums up the Ibus contribution of each motor and updates the
 // brake resistor PWM accordingly.
 void update_brake_current() {
-    auto start = sample_TIM13();
+    axes[0].task_times_.brake_update.beginTimer();
+    axes[1].task_times_.brake_update.beginTimer();
     float Ibus_sum = 0.0f;
     for (size_t i = 0; i < AXIS_COUNT; ++i) {
         if (axes[i].motor_.armed_state_ == Motor::ARMED_STATE_ARMED) {
@@ -533,9 +534,8 @@ void update_brake_current() {
     int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS;
     if (low_off < 0) low_off = 0;
     safety_critical_apply_brake_resistor_timings(low_off, high_on);
-    
-    auto end = sample_TIM13() - start;
-    axes[0]->task_times_.brake_update = end;
+    axes[0].task_times_.brake_update.stopTimer();
+    axes[1].task_times_.brake_update.stopTimer();
 }
 
 

Firmware/MotorControl/odrive_main.h

@@ -128,9 +128,8 @@ inline ENUMTYPE &operator &= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast
 inline ENUMTYPE &operator ^= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast<ENUMTYPE&>(reinterpret_cast<std::underlying_type_t<ENUMTYPE>&>(a) ^= static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
 inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast<ENUMTYPE>(~static_cast<std::underlying_type_t<ENUMTYPE>>(a)); }
 
-
-
 #include "autogen/interfaces.hpp"
+#include <taskTimer.hpp>
 
 // ODrive specific includes
 #include <utils.hpp>

Firmware/MotorControl/taskTimer.hpp

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <tim.h>
+#include <stdint.h>
+#include <algorithm>
+
+inline uint16_t sample_TIM13(){
+    constexpr uint16_t clocks_per_cnt = (uint16_t)((float)TIM_1_8_CLOCK_HZ / (float)TIM_APB1_CLOCK_HZ);
+    return clocks_per_cnt * htim13.Instance->CNT; // TODO: Use a hw_config
+}
+
+struct TaskTimer {
+    uint32_t startTime = 0;
+    uint32_t endTime = 0;
+    uint32_t length = 0;
+    uint32_t maxLength = 0;
+
+    void beginTimer(){
+        startTime = sample_TIM13();
+    }
+
+    void stopTimer(){
+        endTime = sample_TIM13();
+        length = endTime - startTime;
+        maxLength = std::max(maxLength, length);
+    }
+};

Firmware/MotorControl/utils.cpp

@@ -206,7 +206,3 @@ void delay_us(uint32_t us)
     }
 }
 
-uint16_t sample_TIM13(){
-    constexpr uint16_t clocks_per_cnt = (uint16_t)((float)TIM_1_8_CLOCK_HZ / (float)TIM_APB1_CLOCK_HZ);
-    return clocks_per_cnt * htim13.Instance->CNT; // TODO: Use a hw_config
-}

Firmware/MotorControl/utils.hpp

@@ -121,7 +121,6 @@ int is_in_the_future(uint32_t time_ms);
 
 uint32_t micros(void);
 void delay_us(uint32_t us);
-uint16_t sample_TIM13();
 
 float our_arm_sin_f32(float x);
 float our_arm_cos_f32(float x);

Firmware/odrive-interface.yaml

@@ -454,30 +454,38 @@ interfaces:
       trap_traj: TrapezoidalTrajectory
       min_endstop: Endstop
       max_endstop: Endstop
-      task_times: TaskTime
+      task_times: TaskTimes
     functions:
       watchdog_feed:
         doc: Feed the watchdog to prevent watchdog timeouts.
       clear_errors:
         doc: Check the watchdog timer for expiration. Also sets the watchdog error bit if expired.
 
-  ODrive.Axis.TaskTime:
+  ODrive.Axis.TaskTimes:
     c_is_class: False
     attributes:
-      thermistor_update: uint16
-      encoder_update: uint16
-      sensorless_update: uint16
-      min_endstop_update: uint16
-      max_endstop_update: uint16
-      axis_update: uint16
-      axis_error_check: uint16
-      controller_update: uint16
-      motor_update: uint16
-      control_loop: uint16
-      update_handler: uint16
-      brake_update: uint16
-      adc_cb: uint16
-      total: uint32
+      thermistor_update: TaskTimer
+      encoder_update: TaskTimer
+      sensorless_update: TaskTimer
+      min_endstop_update: TaskTimer
+      max_endstop_update: TaskTimer
+      axis_update: TaskTimer
+      axis_error_check: TaskTimer
+      controller_update: TaskTimer
+      motor_update: TaskTimer
+      control_loop: TaskTimer
+      update_handler: TaskTimer
+      brake_update: TaskTimer
+      adc_cb: TaskTimer
+      total: TaskTimer
+
+  ODrive.TaskTimer:
+    c_is_class: False
+    attributes:
+      startTime: readonly uint32
+      endTime: readonly uint32
+      length: readonly uint32
+      maxLength: readonly uint32
 
   ODrive.Axis.LockinConfig:
     c_is_class: False