diff --git a/CHANGELOG.md b/CHANGELOG.md index e2b4f6be..2ae19953 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -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 diff --git a/Firmware/.vscode/launch.json b/Firmware/.vscode/launch.json index 5107086e..cc8662d3 100644 --- a/Firmware/.vscode/launch.json +++ b/Firmware/.vscode/launch.json @@ -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}" + } ] } \ No newline at end of file diff --git a/Firmware/.vscode/tasks.json b/Firmware/.vscode/tasks.json index d5b7d969..c82adc83 100644 --- a/Firmware/.vscode/tasks.json +++ b/Firmware/.vscode/tasks.json @@ -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", diff --git a/Firmware/Makefile b/Firmware/Makefile index 91754f5a..a40ae89f 100644 --- a/Firmware/Makefile +++ b/Firmware/Makefile @@ -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 diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 71edd5f1..be15ae3e 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -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(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; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 31cc118e..598da2a8 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -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; } diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 695fedb3..6f918c8b 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -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(anticogging_pos), 3600), 0, 3600)]; + Iq += config_.anticogging.cogging_map[std::clamp(mod((int)anticogging_pos, 3600), 0, 3600)]; } float v_err = 0.0f; diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index ce3cf53f..66f2dbb1 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -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)] - 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)] + 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; diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 743ae591..8f2c7eb8 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -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(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(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(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(i) / static_cast(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(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(i) / static_cast(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(residual) / static_cast(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(shadow_count_) - static_cast(std::floor(pos_estimate_)); - float delta_pos_cpr = static_cast(count_in_cpr_) - static_cast(std::floor(pos_cpr_)); - delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * static_cast(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(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(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); diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp index a87af4f6..f108dffe 100644 --- a/Firmware/MotorControl/endstop.hpp +++ b/Firmware/MotorControl/endstop.hpp @@ -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 debounceTimer_; }; #endif \ No newline at end of file diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index ccfe6aa5..8aafca4e 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -647,7 +647,7 @@ void update_brake_current() { return; } - int high_on = static_cast(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); diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index f924f13f..76cdfa5e 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -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(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; diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 9b6632cb..a7e87139 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -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 diff --git a/Firmware/MotorControl/utils.hpp b/Firmware/MotorControl/utils.hpp index 224f3716..0c191dfe 100644 --- a/Firmware/MotorControl/utils.hpp +++ b/Firmware/MotorControl/utils.hpp @@ -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); } diff --git a/Firmware/communication/can_helpers.hpp b/Firmware/communication/can_helpers.hpp index b5e40385..779b6a7a 100644 --- a/Firmware/communication/can_helpers.hpp +++ b/Firmware/communication/can_helpers.hpp @@ -26,7 +26,7 @@ struct can_Signal_t { template 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 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)); diff --git a/Firmware/communication/can_simple.cpp b/Firmware/communication/can_simple.cpp index 4ec91660..a6953a41 100644 --- a/Firmware/communication/can_simple.cpp +++ b/Firmware/communication/can_simple.cpp @@ -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(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(msg, 0, 32, true); - axis->controller_.input_vel_ = can_getSignal(msg, 32, 16, true, 0.1f, 0); + axis->controller_.input_pos_ = can_getSignal(msg, 0, 32, true); + axis->controller_.input_vel_ = can_getSignal(msg, 32, 16, true, 0.1f, 0); axis->controller_.input_current_ = can_getSignal(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(msg, 0, 32, true, 0.01f, 0.0f); + axis->controller_.input_vel_ = can_getSignal(msg, 0, 32, true, 0.01f, 0.0f); axis->controller_.input_current_ = can_getSignal(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) { diff --git a/Firmware/communication/can_simple.hpp b/Firmware/communication/can_simple.hpp index 4f98f0a8..c4b6d6ed 100644 --- a/Firmware/communication/can_simple.hpp +++ b/Firmware/communication/can_simple.hpp @@ -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 diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 854f9588..d545f412 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -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: diff --git a/docs/can-protocol.md b/docs/can-protocol.md index f316e73e..4fc06275 100644 --- a/docs/can-protocol.md +++ b/docs/can-protocol.md @@ -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
Encoder Vel Estimate | 0
4 | IEEE 754 Float
IEEE 754 Float | 32
32 | 1
1 | 0
0 | Intel
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 `.can.set_baud_rate()`. The baud rate can be done without rebooting the device. If you'd like to keep the baud rate, simply call `.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 diff --git a/tools/odrive/tests/can_test.py b/tools/odrive/tests/can_test.py index 164dd251..db08e47c 100644 --- a/tools/odrive/tests/can_test.py +++ b/tools/odrive/tests/can_test.py @@ -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 diff --git a/tools/odrive/tests/pwm_input_test.py b/tools/odrive/tests/pwm_input_test.py index a37cc155..f871b4dd 100644 --- a/tools/odrive/tests/pwm_input_test.py +++ b/tools/odrive/tests/pwm_input_test.py @@ -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) diff --git a/tools/odrive/tests/test_runner.py b/tools/odrive/tests/test_runner.py index 80bb363d..a6dbffc0 100644 --- a/tools/odrive/tests/test_runner.py +++ b/tools/odrive/tests/test_runner.py @@ -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: