From b1961e8a2944b4b3a917970ea410beb20d8366df Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 24 Aug 2020 00:50:23 -0400 Subject: [PATCH] Add timers for sections --- Firmware/MotorControl/axis.cpp | 14 ++++++++++ Firmware/MotorControl/axis.hpp | 42 +++++++++++++++++++++++++++-- Firmware/MotorControl/low_level.cpp | 8 ++++++ Firmware/MotorControl/utils.cpp | 7 +++++ Firmware/MotorControl/utils.hpp | 1 + Firmware/odrive-interface.yaml | 19 +++++++++++++ 6 files changed, 89 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 04a802c6..2f211a03 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -201,11 +201,23 @@ bool Axis::do_updates() { for (ThermistorCurrentLimiter* thermistor : thermistors_) { thermistor->update(); } + task_times_.thermistor_update = sample_TIM13(); + encoder_.update(); + task_times_.encoder_update = sample_TIM13(); + sensorless_estimator_.update(); + task_times_.sensorless_update = sample_TIM13(); + min_endstop_.update(); + task_times_.min_endstop_update = sample_TIM13(); + max_endstop_.update(); + task_times_.max_endstop_update = sample_TIM13(); + bool ret = check_for_errors(); + task_times_.axis_error_check = sample_TIM13(); + odCAN->send_heartbeat(this); return ret; } @@ -347,10 +359,12 @@ bool Axis::run_closed_loop_control_loop() { float torque_setpoint; if (!controller_.update(&torque_setpoint)) return error_ |= ERROR_CONTROLLER_FAILED, false; + task_times_.controller_update = sample_TIM13(); 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(); return true; }); diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 446958d6..71610259 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -21,6 +21,25 @@ public: bool finish_on_enc_idx = false; }; + 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; + + uint16_t controller_update = 0; + uint16_t motor_update = 0; + uint16_t update_handler = 0; + + uint16_t brake_update = 0; + uint16_t adc_cb = 0; + uint16_t control_loop = 0; + uint32_t total = 0; + }; + static LockinConfig_t default_calibration(); static LockinConfig_t default_sensorless(); static LockinConfig_t default_lockin(); @@ -140,14 +159,18 @@ 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(); // look for errors at axis level and also all subcomponents bool checks_ok = do_checks(); + // Update all estimators // Note: updates run even if checks fail - bool updates_ok = do_updates(); + bool updates_ok = do_updates(); + task_times_.axis_update = sample_TIM13(); // make sure the watchdog is being fed. bool watchdog_ok = watchdog_check(); @@ -162,10 +185,24 @@ public: // Run main loop function, defer quitting for after wait // TODO: change arming logic to arm after waiting bool main_continue = update_handler(); - + task_times_.update_handler = sample_TIM13(); // 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; + + // Wait until the current measurement interrupt fires if (!wait_for_current_meas()) { // maybe the interrupt handler is dead, let's be @@ -206,6 +243,7 @@ public: TrapezoidalTrajectory& trap_traj_; Endstop& min_endstop_; Endstop& max_endstop_; + TaskTimes_t task_times_; // List of current_limiters and thermistors to // provide easy iteration. diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index d447539d..1402fbc7 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -492,6 +492,7 @@ static void decode_hall_samples(Encoder& enc, uint16_t GPIO_samples[num_GPIO]) { // 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(); #define calib_tau 0.2f //@TOTO make more easily configurable constexpr float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau; @@ -587,6 +588,9 @@ 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; } void tim_update_cb(TIM_HandleTypeDef* htim) { @@ -620,6 +624,7 @@ void tim_update_cb(TIM_HandleTypeDef* htim) { // @brief Sums up the Ibus contribution of each motor and updates the // brake resistor PWM accordingly. void update_brake_current() { + auto start = sample_TIM13(); float Ibus_sum = 0.0f; for (size_t i = 0; i < AXIS_COUNT; ++i) { if (axes[i]->motor_.armed_state_ == Motor::ARMED_STATE_ARMED) { @@ -666,6 +671,9 @@ 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; } diff --git a/Firmware/MotorControl/utils.cpp b/Firmware/MotorControl/utils.cpp index 579a47a7..f089ba4e 100644 --- a/Firmware/MotorControl/utils.cpp +++ b/Firmware/MotorControl/utils.cpp @@ -5,6 +5,8 @@ #include #include +#include + int SVM(float alpha, float beta, float* tA, float* tB, float* tC) { int Sextant; @@ -203,3 +205,8 @@ void delay_us(uint32_t us) __ASM("nop"); } } + +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 49f9434d..8dcf09aa 100644 --- a/Firmware/MotorControl/utils.hpp +++ b/Firmware/MotorControl/utils.hpp @@ -121,6 +121,7 @@ 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 2c9060eb..df4365d6 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -364,12 +364,31 @@ interfaces: trap_traj: TrapezoidalTrajectory min_endstop: Endstop max_endstop: Endstop + task_times: TaskTime 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: + 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 + ODrive.Axis.LockinConfig: c_is_class: False attributes: