Merge remote-tracking branch 'origin/devel' into feature/interface_autogen_merged

This commit is contained in:
Samuel Sadok
2020-05-22 21:27:58 +02:00
22 changed files with 156 additions and 115 deletions
+1
View File
@@ -5,6 +5,7 @@ Please add a note of your changes below this heading if you make a Pull Request.
* AC Induction Motor support.
* Tracking of rotor flux through rotor time constant
* Automatic d axis current for Maximum Torque Per Amp (MTPA)
* ASCII "w" commands now execute write hooks.
* Simplified control interface ("Input Filter" branch)
* New input variables: `input_pos`, `input_vel`, and `input_current`
* New setting `input_mode` to switch between different input behaviours
+16 -2
View File
@@ -9,7 +9,7 @@
"type": "cortex-debug",
"servertype": "openocd",
"request": "launch",
"name": "Debug ODrive",
"name": "Debug ODrive - ST-Link",
"executable": "${workspaceRoot}/build/ODriveFirmware.elf",
"configFiles": [
"interface/stlink-v2.cfg",
@@ -23,7 +23,7 @@
"type": "cortex-debug",
"servertype": "openocd",
"request": "launch",
"name": "Debug ODrive - FreeRTOS",
"name": "Debug ODrive - ST-Link - FreeRTOS",
"executable": "${workspaceRoot}/build/ODriveFirmware.elf",
"rtos": "FreeRTOS",
"configFiles": [
@@ -52,5 +52,19 @@
"svdFile": "${workspaceRoot}/Board/v3/STM32F40x.svd",
"cwd": "${workspaceRoot}"
},
{
// For the Cortex-Debug extensions
"type": "cortex-debug",
"servertype": "bmp",
"request": "launch",
"name": "Debug ODrive - Black Magic Probe",
"executable": "${workspaceRoot}/build/ODriveFirmware.elf",
"device": "STM32F4xx",
"BMPGDBSerialPort": "${env:BMP_PORT}",
"interface": "swd",
"targetId": 1,
"armToolchainPath": "${env:ARM_GCC_ROOT}/bin/",
"cwd": "${workspaceRoot}"
}
]
}
+7 -1
View File
@@ -19,11 +19,17 @@
]
},
{
"label": "flash",
"label": "flash - ST-Link",
"type": "shell",
"command": "make flash",
"problemMatcher": []
},
{
"label": "flash - Black Magic Probe",
"type": "shell",
"command": "make flashbmp",
"problemMatcher": []
},
{
"label": "openocd",
"type": "shell",
+9
View File
@@ -20,6 +20,15 @@ flash: all
-c 'reset run' \
-c exit
flashbmp: all
arm-none-eabi-gdb --ex 'target extended-remote $(BMP_PORT)' \
--ex 'monitor swdp_scan' \
--ex 'attach 1' \
--ex 'load' \
--ex 'detach' \
--ex 'quit' \
$(FIRMWARE)
gdb: all
arm-none-eabi-gdb $(FIRMWARE) -x openocd.gdbinit
+2 -4
View File
@@ -87,7 +87,7 @@ static void run_state_machine_loop_wrapper(void* ctx) {
// @brief Starts run_state_machine_loop in a new thread
void Axis::start_thread() {
osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, stack_size_ / sizeof(StackType_t));
thread_id_ = osThreadCreate(osThread(thread_def), this);
thread_id_ = osThreadCreate(osThread(thread_def), this);
thread_id_valid_ = true;
}
@@ -202,9 +202,7 @@ void Axis::watchdog_feed() {
// @brief Check the watchdog timer for expiration. Also sets the watchdog error bit if expired.
bool Axis::watchdog_check() {
// reset value = 0 means watchdog disabled.
if (!config_.enable_watchdog) return true;
if (get_watchdog_reset() == 0) return true;
// explicit check here to ensure that we don't underflow back to UINT32_MAX
if (watchdog_current_value_ > 0) {
@@ -379,7 +377,7 @@ bool Axis::run_homing() {
controller_.vel_setpoint_ = 0.0f; // Change directions without decelerating
// Set our current position in encoder counts to make control more logical
encoder_.set_linear_count(static_cast<int32_t>(controller_.pos_setpoint_));
encoder_.set_linear_count((int32_t)controller_.pos_setpoint_);
controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL;
controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ;
+6 -5
View File
@@ -41,7 +41,7 @@ public:
float counts_per_step = 2.0f;
float watchdog_timeout = 0.0f; // [s] (0 disables watchdog)
float watchdog_timeout = 0.0f; // [s]
bool enable_watchdog = false;
// Defaults loaded from hw_config in load_configuration in main.cpp
@@ -51,7 +51,8 @@ public:
LockinConfig_t calibration_lockin = default_calibration();
LockinConfig_t sensorless_ramp = default_sensorless();
LockinConfig_t general_lockin;
uint8_t can_node_id = 0; // Both axes will have the same id to start
uint32_t can_node_id = 0; // Both axes will have the same id to start
bool can_node_id_extended = false;
uint32_t can_heartbeat_rate_ms = 100;
// custom setters
@@ -101,10 +102,10 @@ public:
bool watchdog_check();
void clear_errors() {
motor_.error_ = Motor::ERROR_NONE;
controller_.error_ = Controller::ERROR_NONE;
motor_.error_ = Motor::ERROR_NONE;
controller_.error_ = Controller::ERROR_NONE;
sensorless_estimator_.error_ = SensorlessEstimator::ERROR_NONE;
encoder_.error_ = Encoder::ERROR_NONE;
encoder_.error_ = Encoder::ERROR_NONE;
error_ = ERROR_NONE;
}
+3 -3
View File
@@ -165,8 +165,8 @@ bool Controller::update(float* current_setpoint_output) {
} break;
case INPUT_MODE_CURRENT_RAMP: {
float max_step_size = std::abs(current_meas_period * config_.current_ramp_rate);
float full_step = input_current_ - current_setpoint_;
float step = std::clamp(full_step, -max_step_size, max_step_size);
float full_step = input_current_ - current_setpoint_;
float step = std::clamp(full_step, -max_step_size, max_step_size);
current_setpoint_ += step;
} break;
@@ -293,7 +293,7 @@ bool Controller::update(float* current_setpoint_output) {
// We get the current position and apply a current feed-forward
// ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1)
if (anticogging_valid_ && config_.anticogging.anticogging_enabled) {
Iq += config_.anticogging.cogging_map[std::clamp(mod(static_cast<int>(anticogging_pos), 3600), 0, 3600)];
Iq += config_.anticogging.cogging_map[std::clamp(mod((int)anticogging_pos, 3600), 0, 3600)];
}
float v_err = 0.0f;
+24 -24
View File
@@ -14,34 +14,34 @@ public:
bool calib_anticogging = false;
float calib_pos_threshold = 1.0f;
float calib_vel_threshold = 1.0f;
float cogging_ratio = 1.0f;
bool anticogging_enabled = true;
float cogging_ratio = 1.0f;
bool anticogging_enabled = true;
} Anticogging_t;
struct Config_t {
ControlMode control_mode = CONTROL_MODE_POSITION_CONTROL; //see: ControlMode
InputMode input_mode = INPUT_MODE_PASSTHROUGH; //see: InputMode
float pos_gain = 20.0f; // [(counts/s) / counts]
float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)]
// float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] <sensorless example>
float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)]
float vel_limit = 20000.0f; // [counts/s] Infinity to disable.
float vel_limit_tolerance = 1.2f; // ratio to vel_lim. Infinity to disable.
float vel_ramp_rate = 10000.0f; // [(counts/s) / s]
float current_ramp_rate = 1.0f; // A / sec
bool setpoints_in_cpr = false;
float inertia = 0.0f; // [A/(count/s^2)]
float input_filter_bandwidth = 2.0f; // [1/s]
float homing_speed = 2000.0f; // [counts/s]
ControlMode control_mode = CONTROL_MODE_POSITION_CONTROL; //see: ControlMode_t
InputMode input_mode = INPUT_MODE_PASSTHROUGH; //see: InputMode_t
float pos_gain = 20.0f; // [(counts/s) / counts]
float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)]
// float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] <sensorless example>
float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)]
float vel_limit = 20000.0f; // [counts/s] Infinity to disable.
float vel_limit_tolerance = 1.2f; // ratio to vel_lim. Infinity to disable.
float vel_ramp_rate = 10000.0f; // [(counts/s) / s]
float current_ramp_rate = 1.0f; // A / sec
bool setpoints_in_cpr = false;
float inertia = 0.0f; // [A/(count/s^2)]
float input_filter_bandwidth = 2.0f; // [1/s]
float homing_speed = 2000.0f; // [counts/s]
Anticogging_t anticogging;
float gain_scheduling_width = 10.0f;
bool enable_gain_scheduling = false;
bool enable_vel_limit = true;
bool enable_overspeed_error = true;
bool enable_current_mode_vel_limit = true; // enable velocity limit in current control mode (requires a valid velocity estimator)
uint8_t axis_to_mirror = -1;
float mirror_ratio = 1.0f;
uint8_t load_encoder_axis = -1; // default depends on Axis number and is set in load_configuration()
float gain_scheduling_width = 10.0f;
bool enable_gain_scheduling = false;
bool enable_vel_limit = true;
bool enable_overspeed_error = true;
bool enable_current_mode_vel_limit = true; // enable velocity limit in current control mode (requires a valid velocity estimator)
uint8_t axis_to_mirror = -1;
float mirror_ratio = 1.0f;
uint8_t load_encoder_axis = -1; // default depends on Axis number and is set in load_configuration()
// custom setters
Controller* parent;
+23 -23
View File
@@ -109,8 +109,8 @@ void Encoder::set_linear_count(int32_t count) {
uint32_t prim = cpu_enter_critical();
// Update states
shadow_count_ = count;
pos_estimate_ = static_cast<float>(count);
shadow_count_ = count;
pos_estimate_ = (float)count;
tim_cnt_sample_ = count;
//Write hardware last
@@ -132,7 +132,7 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) {
// Update states
count_in_cpr_ = mod(count, config_.cpr);
pos_cpr_ = static_cast<float>(count_in_cpr_);
pos_cpr_ = (float)count_in_cpr_;
cpu_exit_critical(prim);
}
@@ -182,7 +182,7 @@ bool Encoder::run_direction_find() {
// TODO: Do the scan with current, not voltage!
bool Encoder::run_offset_calibration() {
static const float start_lock_duration = 1.0f;
static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * static_cast<float>(current_meas_hz));
static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * (float)current_meas_hz);
// Require index found if enabled
if (config_.use_index && !index_found_) {
@@ -219,7 +219,7 @@ bool Encoder::run_offset_calibration() {
// scan forward
i = 0;
axis_->run_control_loop([&]() {
float phase = wrap_pm_pi(config_.calib_scan_distance * static_cast<float>(i) / static_cast<float>(num_steps) - config_.calib_scan_distance / 2.0f);
float phase = wrap_pm_pi(config_.calib_scan_distance * (float)i / (float)num_steps - config_.calib_scan_distance / 2.0f);
float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
@@ -248,9 +248,9 @@ bool Encoder::run_offset_calibration() {
//TODO avoid recomputing elec_rad_per_enc every time
// Check CPR
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / static_cast<float>(config_.cpr));
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc;
calib_scan_response_ = std::abs(shadow_count_ - init_enc_val);
calib_scan_response_ = std::abs(shadow_count_ - init_enc_val);
if (std::abs(calib_scan_response_ - expected_encoder_delta) / expected_encoder_delta > config_.calib_range) {
set_error(ERROR_CPR_POLEPAIRS_MISMATCH);
return false;
@@ -259,7 +259,7 @@ bool Encoder::run_offset_calibration() {
// scan backwards
i = 0;
axis_->run_control_loop([&]() {
float phase = wrap_pm_pi(-config_.calib_scan_distance * static_cast<float>(i) / static_cast<float>(num_steps) + config_.calib_scan_distance / 2.0f);
float phase = wrap_pm_pi(-config_.calib_scan_distance * (float)i / (float)num_steps + config_.calib_scan_distance / 2.0f);
float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
@@ -273,9 +273,9 @@ bool Encoder::run_offset_calibration() {
if (axis_->error_ != Axis::ERROR_NONE)
return false;
config_.offset = encvaluesum / (num_steps * 2);
int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2));
config_.offset_float = static_cast<float>(residual) / static_cast<float>(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase
config_.offset = encvaluesum / (num_steps * 2);
int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2));
config_.offset_float = (float)residual / (float)(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase
is_ready_ = true;
return true;
@@ -334,12 +334,12 @@ bool Encoder::abs_spi_init(){
spi->Init.CLKPhase = SPI_PHASE_2EDGE;
spi->Init.NSS = SPI_NSS_SOFT;
spi->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32;
spi->Init.FirstBit = SPI_FIRSTBIT_MSB;
spi->Init.TIMode = SPI_TIMODE_DISABLE;
spi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
spi->Init.CRCPolynomial = 10;
spi->Init.FirstBit = SPI_FIRSTBIT_MSB;
spi->Init.TIMode = SPI_TIMODE_DISABLE;
spi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
spi->Init.CRCPolynomial = 10;
if (mode_ == MODE_SPI_ABS_AEAT) {
spi->Init.CLKPolarity = SPI_POLARITY_HIGH;
spi->Init.CLKPolarity = SPI_POLARITY_HIGH;
}
HAL_SPI_DeInit(spi);
HAL_SPI_Init(spi);
@@ -508,17 +508,17 @@ bool Encoder::update() {
pos_estimate_ += current_meas_period * vel_estimate_;
pos_cpr_ += current_meas_period * vel_estimate_;
// discrete phase detector
float delta_pos = static_cast<float>(shadow_count_) - static_cast<int32_t>(std::floor(pos_estimate_));
float delta_pos_cpr = static_cast<float>(count_in_cpr_) - static_cast<int32_t>(std::floor(pos_cpr_));
delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * static_cast<float>(config_.cpr));
float delta_pos = (float)(shadow_count_ - (int32_t)std::floor(pos_estimate_));
float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)std::floor(pos_cpr_));
delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr));
// pll feedback
pos_estimate_ += current_meas_period * pll_kp_ * delta_pos;
pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr;
pos_cpr_ = fmodf_pos(pos_cpr_, static_cast<float>(config_.cpr));
pos_cpr_ = fmodf_pos(pos_cpr_, (float)(config_.cpr));
vel_estimate_ += current_meas_period * pll_ki_ * delta_pos_cpr;
bool snap_to_zero_vel = false;
if (std::abs(vel_estimate_) < 0.5f * current_meas_period * pll_ki_) {
vel_estimate_ = 0.0f; //align delta-sigma on zero to prevent jitter
vel_estimate_ = 0.0f; //align delta-sigma on zero to prevent jitter
snap_to_zero_vel = true;
}
@@ -543,8 +543,8 @@ bool Encoder::update() {
//// compute electrical phase
//TODO avoid recomputing elec_rad_per_enc every time
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / static_cast<float>(config_.cpr));
float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float);
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float);
// ph = fmodf(ph, 2*M_PI);
phase_ = wrap_pm_pi(ph);
+2 -2
View File
@@ -33,8 +33,8 @@ class Endstop {
bool endstop_state_ = false;
private:
bool pin_state_ = false;
float pos_when_pressed_ = 0.0f;
bool pin_state_ = false;
float pos_when_pressed_ = 0.0f;
Timer<float> debounceTimer_;
};
#endif
+1 -1
View File
@@ -647,7 +647,7 @@ void update_brake_current() {
return;
}
int high_on = static_cast<int>(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty));
int high_on = (int)(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty));
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);
+3 -3
View File
@@ -216,7 +216,7 @@ float Motor::phase_current_from_adcval(uint32_t ADCValue) {
// TODO check Ibeta balance to verify good motor connection
bool Motor::measure_phase_resistance(float test_current, float max_voltage) {
static const float kI = 10.0f; // [(V/s)/A]
static const int num_test_cycles = static_cast<int>(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s
static const int num_test_cycles = (int)(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s
float test_voltage = 0.0f;
size_t i = 0;
@@ -329,7 +329,7 @@ bool Motor::FOC_voltage(float v_d, float v_q, float pwm_phase) {
float c = our_arm_cos_f32(pwm_phase);
float s = our_arm_sin_f32(pwm_phase);
float v_alpha = c*v_d - s*v_q;
float v_beta = c*v_q + s*v_d;
float v_beta = c*v_q + s*v_d;
return enqueue_voltage_timings(v_alpha, v_beta);
}
@@ -400,7 +400,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha
float c_p = our_arm_cos_f32(pwm_phase);
float s_p = our_arm_sin_f32(pwm_phase);
float mod_alpha = c_p * mod_d - s_p * mod_q;
float mod_beta = c_p * mod_q + s_p * mod_d;
float mod_beta = c_p * mod_q + s_p * mod_d;
// Report final applied voltage in stationary frame (for sensorles estimator)
ictrl.final_v_alpha = mod_to_V * mod_alpha;
+1 -1
View File
@@ -159,7 +159,7 @@ public:
} gate_driver_exported_;
DRV_SPI_8301_Vars_t gate_driver_regs_; //Local view of DRV registers (initialized by DRV8301_setup)
float thermal_current_lim_ = 10.0f; //[A]
float inverter_temp_ = 20.0f;
float inverter_temp_ = NAN; // [°C] NaN while the ODrive is initializing.
};
#endif // __MOTOR_HPP
-1
View File
@@ -86,7 +86,6 @@ static inline float wrap_pm(float x, float pm_range) {
return fmodf_pos(x + pm_range, 2.0f * pm_range) - pm_range;
}
//beware of inserting large angles!
static inline float wrap_pm_pi(float theta) {
return wrap_pm(theta, M_PI);
}
+2 -2
View File
@@ -26,7 +26,7 @@ struct can_Signal_t {
template <typename T>
T can_getSignal(can_Message_t msg, const uint8_t startBit, const uint8_t length, const bool isIntel) {
uint64_t tempVal = 0;
uint64_t mask = (1ULL << length) - 1;
uint64_t mask = (1ULL << length) - 1;
if (isIntel) {
std::memcpy(&tempVal, msg.buf, sizeof(tempVal));
@@ -50,7 +50,7 @@ float can_getSignal(can_Message_t msg, const uint8_t startBit, const uint8_t len
template <typename T>
void can_setSignal(can_Message_t& msg, const T& val, const uint8_t startBit, const uint8_t length, const bool isIntel, const float factor, const float offset) {
T scaledVal = (val - offset) / factor;
T scaledVal = (val - offset) / factor;
uint64_t valAsBits = 0;
std::memcpy(&valAsBits, &scaledVal, sizeof(scaledVal));
+16 -16
View File
@@ -26,7 +26,7 @@ void CANSimple::handle_can_message(can_Message_t& msg) {
bool validAxis = false;
for (uint8_t i = 0; i < AXIS_COUNT; i++) {
if (axes[i]->config_.can_node_id == nodeID) {
if ((axes[i]->config_.can_node_id == nodeID) && (axes[i]->config_.can_node_id_extended == msg.isExt)) {
axis = axes[i];
if (!validAxis) {
validAxis = true;
@@ -137,7 +137,7 @@ void CANSimple::get_motor_error_callback(Axis* axis, can_Message_t& msg) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_MOTOR_ERROR; // heartbeat ID
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
txmsg.buf[0] = axis->motor_.error_;
@@ -154,7 +154,7 @@ void CANSimple::get_encoder_error_callback(Axis* axis, can_Message_t& msg) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_ENCODER_ERROR; // heartbeat ID
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
txmsg.buf[0] = axis->encoder_.error_;
@@ -171,7 +171,7 @@ void CANSimple::get_sensorless_error_callback(Axis* axis, can_Message_t& msg) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_SENSORLESS_ERROR; // heartbeat ID
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
txmsg.buf[0] = axis->sensorless_estimator_.error_;
@@ -184,7 +184,7 @@ void CANSimple::get_sensorless_error_callback(Axis* axis, can_Message_t& msg) {
}
void CANSimple::set_axis_nodeid_callback(Axis* axis, can_Message_t& msg) {
axis->config_.can_node_id = msg.buf[0] & 0x3F; // Node ID bitmask
axis->config_.can_node_id = can_getSignal<uint32_t>(msg, 0, 32, true);
}
void CANSimple::set_axis_requested_state_callback(Axis* axis, can_Message_t& msg) {
@@ -199,7 +199,7 @@ void CANSimple::get_encoder_estimates_callback(Axis* axis, can_Message_t& msg) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_ENCODER_ESTIMATES; // heartbeat ID
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
// Undefined behaviour!
@@ -230,7 +230,7 @@ void CANSimple::get_sensorless_estimates_callback(Axis* axis, can_Message_t& msg
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_SENSORLESS_ESTIMATES; // heartbeat ID
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
// Undefined behaviour!
@@ -261,7 +261,7 @@ void CANSimple::get_encoder_count_callback(Axis* axis, can_Message_t& msg) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_ENCODER_COUNT;
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
txmsg.buf[0] = axis->encoder_.shadow_count_;
@@ -279,14 +279,14 @@ void CANSimple::get_encoder_count_callback(Axis* axis, can_Message_t& msg) {
}
void CANSimple::set_input_pos_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.input_pos_ = can_getSignal<int32_t>(msg, 0, 32, true);
axis->controller_.input_vel_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.1f, 0);
axis->controller_.input_pos_ = can_getSignal<int32_t>(msg, 0, 32, true);
axis->controller_.input_vel_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.1f, 0);
axis->controller_.input_current_ = can_getSignal<int16_t>(msg, 48, 16, true, 0.01f, 0);
axis->controller_.input_pos_updated();
}
void CANSimple::set_input_vel_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.input_vel_ = can_getSignal<int32_t>(msg, 0, 32, true, 0.01f, 0.0f);
axis->controller_.input_vel_ = can_getSignal<int32_t>(msg, 0, 32, true, 0.01f, 0.0f);
axis->controller_.input_current_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.01f, 0.0f);
}
@@ -325,7 +325,7 @@ void CANSimple::get_iq_callback(Axis* axis, can_Message_t& msg) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_IQ;
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
uint32_t floatBytes;
@@ -354,7 +354,7 @@ void CANSimple::get_vbus_voltage_callback(Axis* axis, can_Message_t& msg) {
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_VBUS_VOLTAGE;
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
uint32_t floatBytes;
@@ -386,7 +386,7 @@ void CANSimple::send_heartbeat(Axis* axis) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_ODRIVE_HEARTBEAT; // heartbeat ID
txmsg.isExt = false;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
// Axis errors in 1st 32-bit value
@@ -403,8 +403,8 @@ void CANSimple::send_heartbeat(Axis* axis) {
odCAN->write(txmsg);
}
uint8_t CANSimple::get_node_id(uint32_t msgID) {
return ((msgID >> NUM_CMD_ID_BITS) & 0x03F); // Upper 6 bits
uint32_t CANSimple::get_node_id(uint32_t msgID) {
return (msgID >> NUM_CMD_ID_BITS); // Upper 6 or more bits
}
uint8_t CANSimple::get_cmd_id(uint32_t msgID) {
+1 -1
View File
@@ -64,7 +64,7 @@ class CANSimple {
static void clear_errors_callback(Axis* axis, can_Message_t& msg);
// Utility functions
static uint8_t get_node_id(uint32_t msgID);
static uint32_t get_node_id(uint32_t msgID);
static uint8_t get_cmd_id(uint32_t msgID);
// Fetch a specific signal from the message
+6 -2
View File
@@ -275,8 +275,9 @@ interfaces:
sensorless_ramp: LockinState
general_lockin: LockinState
can_node_id:
type: uint8
type: uint32
doc: Both axes will have the same id to start
can_node_id_extended: bool
can_heartbeat_rate_ms: uint32
motor: Motor
controller: Controller
@@ -353,7 +354,10 @@ interfaces:
DC_calib_phC: {type: float32, c_name: DC_calib_.phC}
phase_current_rev_gain: float32
thermal_current_lim: readonly float32
inverter_temp: readonly float32
inverter_temp:
type: readonly float32
unit: °C
doc: NaN while the ODrive is initializing.
current_control:
c_is_class: False
attributes:
+3 -3
View File
@@ -16,7 +16,7 @@ We've implemented a very basic CAN protocol that we call "CAN Simple" to get use
### CAN Frame
At its most basic, the CAN Simple frame looks like this:
* Upper 6 bits - Node ID - max 0x3F
* Upper 6 bits - Node ID - max 0x3F (or 0xFFFFFF when using extended CAN IDs)
* Lower 5 bits - Command ID - max 0x1F
To understand how the Node ID and Command ID interact, let's look at an example
@@ -40,7 +40,7 @@ CMD ID | Name | Sender | Signals | Start byte | Signal Type | Bits | Factor | Of
0x003 | Get Motor Error\* | Axis | Motor Error | 0 | Unsigned Int | 32 | 1 | 0 | Intel
0x004 | Get Encoder Error\* | Axis | Encoder Error | 0 | Unsigned Int | 32 | 1 | 0 | Intel
0x005 | Get Sensorless Error\* | Axis | Sensorless Error | 0 | Unsigned Int | 32 | 1 | 0 | Intel
0x006 | Set Axis Node ID | Master | Axis CAN Node ID | 0 | Unsigned Int | 16 | 1 | 0 | Intel
0x006 | Set Axis Node ID | Master | Axis CAN Node ID | 0 | Unsigned Int | 32 | 1 | 0 | Intel
0x007 | Set Axis Requested State | Master | Axis Requested State | 0 | Unsigned Int | 32 | 1 | 0 | Intel
0x008 | Set Axis Startup Config | Master | - Not yet implemented - | - | - | - | - | - | -
0x009 | Get Encoder Estimates\* | Master | Encoder Pos Estimate<br>Encoder Vel Estimate | 0<br>4 | IEEE 754 Float<br>IEEE 754 Float | 32<br>32 | 1<br>1 | 0<br>0 | Intel<br>Intel
@@ -72,7 +72,7 @@ Configuration of the CAN parameters should be done via USB before putting the de
To set the desired baud rate, use `<odrv>.can.set_baud_rate(<value>)`. The baud rate can be done without rebooting the device. If you'd like to keep the baud rate, simply call `<odrv>.save_configuration()` before rebooting.
Each axis looks like a separate node on the bus. Thus, they've inherited a new configuration property: `can_node_id`. This ID can be from 0 to 63 (0x3F) inclusive.
Each axis looks like a separate node on the bus. Thus, they both have the two properties `can_node_id` and `can_node_id_extended`. The node ID can be from 0 to 63 (0x3F) inclusive, or, if extended CAN IDs are used, from 0 to 16777215 (0xFFFFFF).
### Example Configuration
+27 -18
View File
@@ -17,8 +17,8 @@ command_set = {
'estop': (0x002, []), # tested
'get_motor_error': (0x003, [('motor_error', 'I', 1)]), # untested
'get_encoder_error': (0x004, [('encoder_error', 'I', 1)]), # untested
'get_sensorless_error': (0x004, [('sensorless_error', 'I', 1)]), # untested
'set_node_id': (0x006, [('node_id', 'H', 1)]), # tested
'get_sensorless_error': (0x005, [('sensorless_error', 'I', 1)]), # untested
'set_node_id': (0x006, [('node_id', 'I', 1)]), # tested
'set_requested_state': (0x007, [('requested_state', 'I', 1)]), # tested
# 0x008 not yet implemented
'get_encoder_estimates': (0x009, [('encoder_pos_estimate', 'f', 1), ('encoder_vel_estimate', 'f', 1)]), # partially tested
@@ -39,7 +39,7 @@ command_set = {
'clear_errors': (0x018, []), # partially tested
}
def command(bus, node_id_, cmd_name, **kwargs):
def command(bus, node_id_, extended_id, cmd_name, **kwargs):
cmd_spec = command_set[cmd_name]
cmd_id = cmd_spec[0]
fmt = '<' + ''.join([f for (n, f, s) in cmd_spec[1]]) # all little endian
@@ -49,10 +49,10 @@ def command(bus, node_id_, cmd_name, **kwargs):
fields = [((kwargs[n] / s) if f == 'f' else int(kwargs[n] / s)) for (n, f, s) in cmd_spec[1]]
data = struct.pack(fmt, *fields)
msg = can.Message(arbitration_id=((node_id_ << 5) | cmd_id), data=data)
msg = can.Message(arbitration_id=((node_id_ << 5) | cmd_id), extended_id=extended_id, data=data)
bus.send(msg)
async def record_messages(bus, node_id, cmd_name, timeout = 5.0):
async def record_messages(bus, node_id, extended_id, cmd_name, timeout = 5.0):
"""
Returns an async generator that yields a dictionary for each CAN message that
is received, provided that the CAN ID matches the expected value.
@@ -71,7 +71,7 @@ async def record_messages(bus, node_id, cmd_name, timeout = 5.0):
start = time.monotonic()
while True:
msg = await reader.get_message()
if ((msg.arbitration_id == ((node_id << 5) | cmd_id)) and not msg.is_remote_frame):
if ((msg.arbitration_id == ((node_id << 5) | cmd_id)) and (msg.is_extended_id == extended_id) and not msg.is_remote_frame):
fields = struct.unpack(fmt, msg.data[:(struct.calcsize(fmt))])
res = {n: (fields[i] * s) for (i, (n, f, s)) in enumerate(cmd_spec[1])}
res['t'] = time.monotonic()
@@ -81,13 +81,13 @@ async def record_messages(bus, node_id, cmd_name, timeout = 5.0):
finally:
notifier.stop()
async def request(bus, node_id, cmd_name, timeout = 1.0):
async def request(bus, node_id, extended_id, cmd_name, timeout = 1.0):
cmd_spec = command_set[cmd_name]
cmd_id = cmd_spec[0]
msg_generator = record_messages(bus, node_id, cmd_name, timeout)
msg_generator = record_messages(bus, node_id, extended_id, cmd_name, timeout)
msg = can.Message(arbitration_id=((node_id << 5) | cmd_id), data=[], is_remote_frame=True)
msg = can.Message(arbitration_id=((node_id << 5) | cmd_id), extended_id=extended_id, data=[], is_remote_frame=True)
bus.send(msg)
async for msg in msg_generator:
@@ -102,34 +102,43 @@ async def get_all(async_iterator):
class TestSimpleCAN():
def get_test_cases(self, testrig: TestRig):
for odrive in testrig.get_components(ODriveComponent):
can_interfaces = testrig.get_connected_components(odrive.can, CanInterfaceComponent)
yield (odrive, list(can_interfaces))
can_interfaces = list(testrig.get_connected_components(odrive.can, CanInterfaceComponent))
yield (odrive, can_interfaces, 0, False) # standard ID
yield (odrive, can_interfaces, 0xfedcba, True) # extended ID
def run_test(self, odrive: ODriveComponent, canbus: CanInterfaceComponent, logger: Logger):
def run_test(self, odrive: ODriveComponent, canbus: CanInterfaceComponent, node_id: int, extended_id: bool, logger: Logger):
# make sure no gpio input is overwriting our values
odrive.unuse_gpios()
node_id = 0
axis = odrive.handle.axis0
axis.clear_errors()
axis.config.can_node_id = node_id
axis.config.can_node_id_extended = extended_id
time.sleep(0.1)
def my_cmd(cmd_name, **kwargs): command(canbus.handle, node_id, cmd_name, **kwargs)
def my_req(cmd_name, **kwargs): return asyncio.run(request(canbus.handle, node_id, cmd_name, **kwargs))
def my_cmd(cmd_name, **kwargs): command(canbus.handle, node_id, extended_id, cmd_name, **kwargs)
def my_req(cmd_name, **kwargs): return asyncio.run(request(canbus.handle, node_id, extended_id, cmd_name, **kwargs))
def fence(): my_req('get_vbus_voltage') # fence to ensure the CAN command was sent
test_assert_eq(my_req('get_vbus_voltage')['vbus_voltage'], odrive.handle.vbus_voltage, accuracy=0.01)
my_cmd('set_node_id', node_id=node_id+20)
asyncio.run(request(canbus.handle, node_id+20, 'get_vbus_voltage'))
asyncio.run(request(canbus.handle, node_id+20, extended_id, 'get_vbus_voltage'))
test_assert_eq(axis.config.can_node_id, node_id+20)
# Reset node ID to default value
command(canbus.handle, node_id+20, 'set_node_id', node_id=node_id)
command(canbus.handle, node_id+20, extended_id, 'set_node_id', node_id=node_id)
fence()
test_assert_eq(axis.config.can_node_id, node_id)
# Check that extended node IDs are not carelessly projected to 6-bit IDs
extended_id = not extended_id
my_cmd('estop') # should not be accepted
extended_id = not extended_id
fence()
test_assert_eq(axis.error, errors.axis.ERROR_NONE)
axis.encoder.set_linear_count(123)
test_assert_eq(my_req('get_encoder_estimates')['encoder_pos_estimate'], 123.0, accuracy=0.01)
test_assert_eq(my_req('get_encoder_count')['encoder_shadow_count'], 123.0, accuracy=0.01)
@@ -205,7 +214,7 @@ class TestSimpleCAN():
logger.debug('testing heartbeat...')
# note that this will include the heartbeats that were received during the
# watchdog test (which takes 4.8s).
heartbeats = asyncio.run(get_all(record_messages(canbus.handle, node_id, 'heartbeat', timeout = 1.0)))
heartbeats = asyncio.run(get_all(record_messages(canbus.handle, node_id, extended_id, 'heartbeat', timeout = 1.0)))
test_assert_eq(len(heartbeats), 5.8 / 0.1, accuracy=0.05)
test_assert_eq([msg['error'] for msg in heartbeats[0:35]], [0] * 35) # before watchdog expiry
test_assert_eq([msg['error'] for msg in heartbeats[-10:]], [errors.axis.ERROR_WATCHDOG_TIMER_EXPIRED] * 10) # after watchdog expiry
+1 -1
View File
@@ -81,7 +81,7 @@ class TestPwmInput():
full_scale = max_val - min_val
slope, offset, fitted_curve = fit_sawtooth(data, min_val, max_val)
test_assert_eq(slope, full_scale / 1.0, accuracy=0.001)
test_curve_fit(data, fitted_curve, max_mean_err = full_scale * 0.02, inlier_range = full_scale * 0.03, max_outliers = len(data[:,0]) * 0.01)
test_curve_fit(data, fitted_curve, max_mean_err = full_scale * 0.02, inlier_range = full_scale * 0.05, max_outliers = len(data[:,0]) * 0.01)
+2 -2
View File
@@ -679,7 +679,7 @@ def select_params(param_options):
# Select parameters from the resource list
# (this could be arbitrarily complex to improve parallelization of the tests)
for combination in get_combinations(param_options):
if all_unique(combination):
if all_unique([x for x in combination if isinstance(x, Component)]):
return list(combination)
return None
@@ -708,7 +708,7 @@ def run(tests):
test_cases = list(test.get_test_cases(testrig))
if len(test_cases) == 0:
logger.warn('no resources are available to conduct the test {}'.format(type(test).__name__))
logger.warn('no test cases are available to conduct the test {}'.format(type(test).__name__))
continue
for test_case in test_cases: