Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-08-18 01:18:52 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge branch 'timing' into devel_fast

Browse Source
This commit is contained in:
Paul Guenette
2020-10-30 21:26:31 -04:00
parent 53a976fe6b f9fa06c86f
commit a428d08ecd
13 changed files with 189 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/axis.cpp
+23
View File
@@ -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();
motor_.fet_thermistor_.update();
motor_.motor_thermistor_.update();
task_times_.sensorless_update.stopTimer();
task_times_.min_endstop_update.beginTimer();
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;
});
Firmware/MotorControl/axis.hpp
+42 -1
View File
@@ -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 <array>
@@ -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<typename T>
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
Firmware/MotorControl/controller.cpp
-2
View File
@@ -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
+24 -2
View File
@@ -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 -----------------------------------------------------*/
@@ -385,6 +386,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;
@@ -405,10 +408,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) {
@@ -453,6 +458,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.
@@ -485,6 +503,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) {
@@ -533,6 +553,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();
}
Firmware/MotorControl/low_level.h
+1
View File
@@ -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];
Firmware/MotorControl/motor.cpp
+2
View File
@@ -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;
}
Firmware/MotorControl/odrive_main.h
+2 -2
View File
@@ -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>
@@ -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
Firmware/MotorControl/taskTimer.cpp
+4
View File
@@ -0,0 +1,4 @@
#include "taskTimer.hpp"
bool TaskTimer::sample_next = false;
volatile uint32_t adc_timestamp = 0;
Firmware/MotorControl/taskTimer.hpp
+36
View File
@@ -0,0 +1,36 @@
#pragma once
#include <stdint.h>
#include <tim.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;
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;
Firmware/MotorControl/utils.cpp
+1
View File
@@ -187,3 +187,4 @@ void delay_us(uint32_t us)
asm volatile ("nop");
}
}
Firmware/Tupfile.lua
+1
View File
@@ -204,6 +204,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',
Firmware/odrive-interface.yaml
+29
View File
@@ -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:
Firmware/sampler.py
+24 -10
View File
@@ -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))