diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 5b685d87..79af1d3d 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -186,12 +186,30 @@ 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.stopTimer(); + + task_times_.encoder_update.beginTimer(); encoder_.update(); + task_times_.encoder_update.stopTimer(); + + task_times_.sensorless_update.beginTimer(); sensorless_estimator_.update(); + task_times_.sensorless_update.stopTimer(); + + task_times_.min_endstop_update.beginTimer(); motor_.fet_thermistor_.update(); motor_.motor_thermistor_.update(); min_endstop_.update(); + task_times_.min_endstop_update.stopTimer(); + + task_times_.max_endstop_update.beginTimer(); max_endstop_.update(); + task_times_.max_endstop_update.stopTimer(); + bool ret = check_for_errors(); odCAN->send_cyclic(*this); return ret; @@ -331,13 +349,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.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.stopTimer(); return true; }); diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index a128c688..4a002e84 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -12,6 +12,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,6 +30,28 @@ public: bool finish_on_enc_idx = false; }; + struct TaskTimes_t { + TaskTimer 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 update_handler; + + TaskTimer brake_update; + TaskTimer adc_cb; + TaskTimer control_loop; + TaskTimer total; + + TaskTimer uart_poll; + TaskTimer FOC_Current; + }; + static LockinConfig_t default_calibration(); static LockinConfig_t default_sensorless(); static LockinConfig_t default_lockin(); @@ -161,11 +184,18 @@ public: template void run_control_loop(const T& update_handler) { while (requested_state_ == AXIS_STATE_UNDEFINED) { + 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 - bool updates_ok = do_updates(); + task_times_.axis_update.beginTimer(); + bool updates_ok = do_updates(); + task_times_.axis_update.stopTimer(); // make sure the watchdog is being fed. bool watchdog_ok = watchdog_check(); @@ -179,15 +209,24 @@ 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.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.stopTimer(); + task_times_.total.stopTimer(); + if(axis_num_ == 1) + TaskTimer::sample_next = false; + // Wait until the current measurement interrupt fires if (!wait_for_current_meas()) { // maybe the interrupt handler is dead, let's be @@ -197,6 +236,7 @@ public: error_ |= ERROR_CURRENT_MEASUREMENT_TIMEOUT; break; } + task_times_.total.beginTimer(); if (!main_continue) break; @@ -230,6 +270,7 @@ public: Endstop& min_endstop_; Endstop& max_endstop_; MechanicalBrake& mechanical_brake_; + TaskTimes_t task_times_; osThreadId thread_id_; const uint32_t stack_size_ = 2048; // Bytes 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 3a611971..3418e468 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -31,6 +31,7 @@ constexpr float adc_ref_voltage = 3.3f; // Arbitrary non-zero inital value to avoid division by zero if ADC reading is late float vbus_voltage = 12.0f; float ibus_ = 0.0f; // exposed for monitoring only +bool task_timers_armed = false; bool brake_resistor_armed = false; bool brake_resistor_saturated = false; /* Private constant data -----------------------------------------------------*/ @@ -382,6 +383,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. // Timing diagram: Firmware/timing_diagram_v3.png void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { + + adc_timestamp = sample_TIM13(); #define calib_tau 0.2f //@TOTO make more easily configurable constexpr float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau; @@ -402,10 +405,12 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { bool current_meas_not_DC_CAL = !counting_down; // Check the timing of the sequencing - if (current_meas_not_DC_CAL) + if (current_meas_not_DC_CAL) { axis.motor_.log_timing(TIMING_LOG_ADC_CB_I); - else + } + else { axis.motor_.log_timing(TIMING_LOG_ADC_CB_DC); + } bool update_timings = false; if (hadc == &hadc2) { @@ -450,6 +455,19 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { } float current = axis.motor_.phase_current_from_adcval(ADCValue); + + if(current_meas_not_DC_CAL && axis_num == 0 && hadc == &hadc2){ + if (task_timers_armed) { + TaskTimer::sample_next = true; + task_timers_armed = false; + axes[0].task_times_.adc_cb.startTime = adc_timestamp; // Start of ADC2 + } + } + + if(current_meas_not_DC_CAL && axis_num == 0 && hadc == &hadc3){ + axes[0].task_times_.adc_cb.stopTimer(); // End of ADC3 + } + if (current_meas_not_DC_CAL) { // ADC2 and ADC3 record the phB and phC currents concurrently, // and their interrupts should arrive on the same clock cycle. @@ -482,6 +500,8 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // @brief Sums up the Ibus contribution of each motor and updates the // brake resistor PWM accordingly. void update_brake_current() { + 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) { @@ -528,6 +548,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); + axes[0].task_times_.brake_update.stopTimer(); + axes[1].task_times_.brake_update.stopTimer(); } diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index e02ef5c2..a1e5a470 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -19,6 +19,7 @@ extern const float adc_ref_voltage; /* Exported variables --------------------------------------------------------*/ extern float vbus_voltage; extern float ibus_; +extern bool task_timers_armed; extern bool brake_resistor_armed; extern bool brake_resistor_saturated; extern uint16_t adc_measurements_[ADC_CHANNEL_COUNT]; diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 42ac5ccf..c19ce0b0 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -306,6 +306,7 @@ bool Motor::FOC_voltage(float v_d, float v_q, float pwm_phase) { } bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_phase, float phase_vel) { + axis_->task_times_.FOC_Current.beginTimer(); // Syntactic sugar CurrentControl_t& ictrl = current_control_; @@ -418,6 +419,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha } } + axis_->task_times_.FOC_Current.stopTimer(); return true; } diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index a3c33a1d..4291bbfa 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 @@ -219,6 +218,7 @@ public: const uint8_t fw_version_revision_ = ::fw_version_revision_; const uint8_t fw_version_unreleased_ = ::fw_version_unreleased_; // 0 for official releases, 1 otherwise + bool& task_timers_armed_ = ::task_timers_armed; bool& brake_resistor_armed_ = ::brake_resistor_armed; // TODO: make this the actual variable bool& brake_resistor_saturated_ = ::brake_resistor_saturated; // TODO: make this the actual variable diff --git a/Firmware/MotorControl/taskTimer.cpp b/Firmware/MotorControl/taskTimer.cpp new file mode 100644 index 00000000..bc55573b --- /dev/null +++ b/Firmware/MotorControl/taskTimer.cpp @@ -0,0 +1,4 @@ +#include "taskTimer.hpp" + +bool TaskTimer::sample_next = false; +volatile uint32_t adc_timestamp = 0; diff --git a/Firmware/MotorControl/taskTimer.hpp b/Firmware/MotorControl/taskTimer.hpp new file mode 100644 index 00000000..8a2e17e4 --- /dev/null +++ b/Firmware/MotorControl/taskTimer.hpp @@ -0,0 +1,36 @@ +#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; + + static bool sample_next; + + void beginTimer() { + if (sample_next) + startTime = sample_TIM13(); + } + + void stopTimer() { + if (sample_next) { + endTime = sample_TIM13(); + length = endTime - startTime; + maxLength = std::max(maxLength, length); + } + } +}; + +extern volatile uint32_t adc_timestamp; \ No newline at end of file diff --git a/Firmware/MotorControl/utils.cpp b/Firmware/MotorControl/utils.cpp index f4dc212d..a267aeea 100644 --- a/Firmware/MotorControl/utils.cpp +++ b/Firmware/MotorControl/utils.cpp @@ -5,6 +5,9 @@ #include #include +#include + + int SVM(float alpha, float beta, float* tA, float* tB, float* tC) { int Sextant; @@ -202,3 +205,4 @@ void delay_us(uint32_t us) __ASM("nop"); } } + diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index 84412357..efd0d047 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -196,6 +196,7 @@ sources = { 'MotorControl/trapTraj.cpp', 'MotorControl/pwm_input.cpp', 'MotorControl/main.cpp', + 'MotorControl/taskTimer.cpp', 'Drivers/STM32/stm32_system.cpp', 'Drivers/STM32/stm32_gpio.cpp', 'Drivers/STM32/stm32_nvm.c', diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index a5d04289..49ca512f 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -13,6 +13,7 @@ interfaces: The odrv0, odrv1, ... objects that appear in odrivetool implement this toplevel interface. attributes: + task_timers_armed: bool vbus_voltage: type: readonly float32 unit: V @@ -449,12 +450,40 @@ interfaces: min_endstop: Endstop max_endstop: Endstop mechanical_brake: MechanicalBrake + task_times: TaskTimes functions: watchdog_feed: doc: Feed the watchdog to prevent watchdog timeouts. clear_errors: doc: Clear all the errors of this axis including all contained submodules. + ODrive.Axis.TaskTimes: + c_is_class: False + attributes: + 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 + FOC_Current: 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 attributes: diff --git a/Firmware/sampler.py b/Firmware/sampler.py index 349ea7b9..4fc25a97 100644 --- a/Firmware/sampler.py +++ b/Firmware/sampler.py @@ -45,17 +45,22 @@ class OpenOCDCMSampler(object): return 0 - def initSymbols(self, elf, readelf='arm-none-eabi-readelf'): - proc = subprocess.Popen([readelf, '-s', elf], stdout=subprocess.PIPE) + def initSymbols(self, elf, symbol_dump_cmd='arm-none-eabi-nm'): + proc = subprocess.Popen([symbol_dump_cmd, '-CS', '--size-sort', elf], stdout=subprocess.PIPE) for line in proc.stdout.readlines(): field = line.split() - # for i,txt in enumerate(field): - # print("{}, {}".format(i, txt)) + try: - if field[3] == b'FUNC': - addr = int(field[1], 16) - 1 # For some reason readelf dumps the func addr off by 1 - func = field[7] - size = int(field[2]) + # For using nm -CS + if field[2] in (b't', b'T', b'w', b'W'): + addr = int(field[0], 16) + func = b' '.join(field[3:]) + size = int(field[1], 16) + # # For using readelf -s + # if field[3] == b'FUNC': + # addr = int(field[1], 16) - 1 # For some reason readelf dumps the func addr off by 1 + # func = field[7] + # size = int(field[2]) if addr not in self.indexes: self.table.append((addr, func, size)) self.indexes.add(addr) @@ -88,6 +93,7 @@ if __name__ == '__main__': total = 0 countmap = { } pcmap = { } + funcmap = { } start = time.time() try: @@ -101,6 +107,9 @@ if __name__ == '__main__': func, addr = sampler.func(pc) + if(func == 'ADC_IRQ_Dispatch'): + funcmap[pc] = 1 + if not addr: continue @@ -112,10 +121,15 @@ if __name__ == '__main__': total += 1 cur = time.time() - if cur - start > 1.0: + if cur - start > 5.0: + + # tmp = sorted(funcmap) + # for k in tmp: + # print(hex(k)) + tmp = sorted(countmap.items(), key=operator.itemgetter(1)) #, reverse=True) for k, v in tmp: - print('{:05.2f}% {}'.format((v * 100.) / total, k)) + print('{:05.2f}% {}'.format((v * 100.) / total, k.decode('UTF-8'))) # print('{:06.2f} clocks : {}'.format((v * 10500) / total, k)) start = cur print('{} Samples'.format(total))