From 2bb990937a533b2749edcdfdc019804e3d61981b Mon Sep 17 00:00:00 2001 From: Unknown Date: Tue, 25 Aug 2020 21:40:24 -0400 Subject: [PATCH] Add TaskTimer class --- Firmware/MotorControl/axis.cpp | 23 +++++++---- Firmware/MotorControl/axis.hpp | 62 ++++++++++++++-------------- Firmware/MotorControl/controller.cpp | 2 - Firmware/MotorControl/low_level.cpp | 16 +++---- Firmware/MotorControl/odrive_main.h | 3 +- Firmware/MotorControl/taskTimer.hpp | 27 ++++++++++++ Firmware/MotorControl/utils.cpp | 4 -- Firmware/MotorControl/utils.hpp | 1 - Firmware/odrive-interface.yaml | 40 +++++++++++------- 9 files changed, 106 insertions(+), 72 deletions(-) create mode 100644 Firmware/MotorControl/taskTimer.hpp diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index a69fc140..b44f0b8c 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/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; }); diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 3cf5dd91..13c631d8 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/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 @@ -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 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; } diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 1524dce6..73e5fc00 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -2,8 +2,6 @@ #include "odrive_main.h" #include -#include - bool Controller::apply_config() { config_.parent = this; update_filter_gains(); diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 0f9da174..c4495e5e 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/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(); } diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 2a5c9f5c..41fecc58 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/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(reinterpret_cast&>(a) ^= static_cast>(b)); } \ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_cast>(a)); } - - #include "autogen/interfaces.hpp" +#include // ODrive specific includes #include diff --git a/Firmware/MotorControl/taskTimer.hpp b/Firmware/MotorControl/taskTimer.hpp new file mode 100644 index 00000000..b9363aa8 --- /dev/null +++ b/Firmware/MotorControl/taskTimer.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include +#include +#include + +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); + } +}; \ No newline at end of file diff --git a/Firmware/MotorControl/utils.cpp b/Firmware/MotorControl/utils.cpp index 3dada0c8..a267aeea 100644 --- a/Firmware/MotorControl/utils.cpp +++ b/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 -} \ No newline at end of file diff --git a/Firmware/MotorControl/utils.hpp b/Firmware/MotorControl/utils.hpp index 8dcf09aa..49f9434d 100644 --- a/Firmware/MotorControl/utils.hpp +++ b/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); diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 3f991961..c1ff609b 100644 --- a/Firmware/odrive-interface.yaml +++ b/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