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

Add cyclic encoder estimate message

Browse Source
This commit is contained in:
Unknown
2020-08-31 21:08:58 -04:00
parent 083b23b8c7
commit ff6c18baea
8 changed files with 192 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/axis.cpp
+2 -2
View File
@@ -94,7 +94,7 @@ void Axis::clear_config() {
config_ = {};
config_.step_gpio_pin = default_step_gpio_pin_;
config_.dir_gpio_pin = default_dir_gpio_pin_;
config_.can_node_id = axis_num_;
config_.can.node_id = axis_num_;
}
// @brief Sets up all components of the axis,
@@ -206,7 +206,7 @@ bool Axis::do_updates() {
min_endstop_.update();
max_endstop_.update();
bool ret = check_for_errors();
odCAN->send_heartbeat(*this);
odCAN->send_cyclic(*this);
return ret;
}
Firmware/MotorControl/axis.hpp
+17 -5
View File
@@ -32,6 +32,13 @@ public:
static LockinConfig_t default_sensorless();
static LockinConfig_t default_lockin();
struct CANConfig_t {
uint32_t node_id = 0;
bool is_extended = false;
uint32_t heartbeat_rate_ms = 100;
uint32_t encoder_rate_ms = 10;
};
struct Config_t {
bool startup_motor_calibration = false; //<! run motor calibration at startup, skip otherwise
bool startup_encoder_index_search = false; //<! run encoder index search after startup, skip otherwise
@@ -60,9 +67,8 @@ public:
LockinConfig_t calibration_lockin = default_calibration();
LockinConfig_t sensorless_ramp = default_sensorless();
LockinConfig_t general_lockin;
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;
CANConfig_t can;
// custom setters
Axis* parent = nullptr;
@@ -74,6 +80,11 @@ public:
bool is_homed = false;
};
struct CAN_t {
uint32_t last_heartbeat = 0;
uint32_t last_encoder = 0;
};
enum thread_signals {
M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
};
@@ -243,8 +254,9 @@ public:
AxisState& current_state_ = task_chain_.front();
uint32_t loop_counter_ = 0;
LockinState lockin_state_ = LOCKIN_STATE_INACTIVE;
Homing_t homing_;
uint32_t last_heartbeat_ = 0;
Homing_t homing_;
CAN_t can_;
// watchdog
uint32_t watchdog_current_value_= 0;
Firmware/communication/can_helpers.hpp
+4
View File
@@ -21,6 +21,10 @@ struct can_Signal_t {
const float offset;
};
struct can_Cyclic_t {
uint32_t cycleTime_ms;
uint32_t lastTime_ms;
};
#include <iterator>
template <typename T>
Firmware/communication/can_simple.cpp
+123 -140
View File
@@ -3,25 +3,14 @@
#include <odrive_main.h>
static constexpr uint8_t NUM_NODE_ID_BITS = 6;
static constexpr uint8_t NUM_CMD_ID_BITS = 11 - NUM_NODE_ID_BITS;
void CANSimple::handle_can_message(const can_Message_t& msg) {
// This functional way of handling the messages is neat and is much cleaner from
// a data security point of view, but it will require some tweaking to fix the syntax.
//
// auto func = callback_map.find(msg.id);
// if(func != callback_map.end()){
// func->second(msg);
// }
// Frame
// nodeID | CMD
// 6 bits | 5 bits
uint32_t nodeID = get_node_id(msg.id);
for (auto& axis : axes) {
if ((axis.config_.can_node_id == nodeID) && (axis.config_.can_node_id_extended == msg.isExt)) {
if ((axis.config_.can.node_id == nodeID) && (axis.config_.can.is_extended == msg.isExt)) {
doCommand(axis, msg);
return;
}
@@ -43,13 +32,16 @@ void CANSimple::doCommand(Axis& axis, const can_Message_t& msg) {
estop_callback(axis, msg);
break;
case MSG_GET_MOTOR_ERROR:
get_motor_error_callback(axis, msg);
if (msg.rtr)
get_motor_error_callback(axis);
break;
case MSG_GET_ENCODER_ERROR:
get_encoder_error_callback(axis, msg);
if (msg.rtr)
get_encoder_error_callback(axis);
break;
case MSG_GET_SENSORLESS_ERROR:
get_sensorless_error_callback(axis, msg);
if (msg.rtr)
get_sensorless_error_callback(axis);
break;
case MSG_SET_AXIS_NODE_ID:
set_axis_nodeid_callback(axis, msg);
@@ -61,10 +53,12 @@ void CANSimple::doCommand(Axis& axis, const can_Message_t& msg) {
set_axis_startup_config_callback(axis, msg);
break;
case MSG_GET_ENCODER_ESTIMATES:
get_encoder_estimates_callback(axis, msg);
if (msg.rtr)
get_encoder_estimates_callback(axis);
break;
case MSG_GET_ENCODER_COUNT:
get_encoder_count_callback(axis, msg);
if (msg.rtr)
get_encoder_count_callback(axis);
break;
case MSG_SET_INPUT_POS:
set_input_pos_callback(axis, msg);
@@ -94,16 +88,19 @@ void CANSimple::doCommand(Axis& axis, const can_Message_t& msg) {
set_traj_vel_limit_callback(axis, msg);
break;
case MSG_GET_IQ:
get_iq_callback(axis, msg);
if (msg.rtr)
get_iq_callback(axis);
break;
case MSG_GET_SENSORLESS_ESTIMATES:
get_sensorless_estimates_callback(axis, msg);
if (msg.rtr)
get_sensorless_estimates_callback(axis);
break;
case MSG_RESET_ODRIVE:
NVIC_SystemReset();
break;
case MSG_GET_VBUS_VOLTAGE:
get_vbus_voltage_callback(axis, msg);
if (msg.rtr)
get_vbus_voltage_callback(axis);
break;
case MSG_CLEAR_ERRORS:
clear_errors_callback(axis, msg);
@@ -113,7 +110,6 @@ void CANSimple::doCommand(Axis& axis, const can_Message_t& msg) {
}
}
void CANSimple::nmt_callback(const Axis& axis, const can_Message_t& msg) {
// Not implemented
}
@@ -122,131 +118,120 @@ void CANSimple::estop_callback(Axis& axis, const can_Message_t& msg) {
axis.error_ |= Axis::ERROR_ESTOP_REQUESTED;
}
void CANSimple::get_motor_error_callback(const Axis& axis, const can_Message_t& msg) {
if (msg.rtr) {
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 = axis.config_.can_node_id_extended;
txmsg.len = 8;
void CANSimple::get_motor_error_callback(const Axis& axis) {
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 = axis.config_.can.is_extended;
txmsg.len = 8;
can_setSignal(txmsg, axis.motor_.error_, 0, 32, true);
can_setSignal(txmsg, axis.motor_.error_, 0, 32, true);
odCAN->write(txmsg);
}
odCAN->write(txmsg);
}
void CANSimple::get_encoder_error_callback(const Axis& axis, const can_Message_t& msg) {
if (msg.rtr) {
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 = axis.config_.can_node_id_extended;
txmsg.len = 8;
void CANSimple::get_encoder_error_callback(const Axis& axis) {
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 = axis.config_.can.is_extended;
txmsg.len = 8;
can_setSignal(txmsg, axis.encoder_.error_, 0, 32, true);
can_setSignal(txmsg, axis.encoder_.error_, 0, 32, true);
odCAN->write(txmsg);
}
odCAN->write(txmsg);
}
void CANSimple::get_sensorless_error_callback(const Axis& axis, const can_Message_t& msg) {
if (msg.rtr) {
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 = axis.config_.can_node_id_extended;
txmsg.len = 8;
void CANSimple::get_sensorless_error_callback(const Axis& axis) {
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 = axis.config_.can.is_extended;
txmsg.len = 8;
can_setSignal(txmsg, axis.sensorless_estimator_.error_, 0, 32, true);
can_setSignal(txmsg, axis.sensorless_estimator_.error_, 0, 32, true);
odCAN->write(txmsg);
}
odCAN->write(txmsg);
}
void CANSimple::set_axis_nodeid_callback(Axis& axis, const can_Message_t& msg) {
axis.config_.can_node_id = can_getSignal<uint32_t>(msg, 0, 32, true);
axis.config_.can.node_id = can_getSignal<uint32_t>(msg, 0, 32, true);
}
void CANSimple::set_axis_requested_state_callback(Axis& axis, const can_Message_t& msg) {
axis.requested_state_ = static_cast<Axis::AxisState>(can_getSignal<int32_t>(msg, 0, 16, true));
}
void CANSimple::set_axis_startup_config_callback(Axis& axis, const can_Message_t& msg) {
// Not Implemented
}
void CANSimple::get_encoder_estimates_callback(const Axis& axis, const can_Message_t& msg) {
if (msg.rtr) {
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 = axis.config_.can_node_id_extended;
txmsg.len = 8;
void CANSimple::get_encoder_estimates_callback(const Axis& axis) {
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 = axis.config_.can.is_extended;
txmsg.len = 8;
static_assert(sizeof(float) == sizeof(axis.encoder_.pos_estimate_));
static_assert(sizeof(float) == sizeof(axis.encoder_.vel_estimate_));
static_assert(sizeof(float) == sizeof(axis.encoder_.pos_estimate_));
static_assert(sizeof(float) == sizeof(axis.encoder_.vel_estimate_));
can_setSignal<float>(txmsg, axis.encoder_.pos_estimate_, 0, 32, true);
can_setSignal<float>(txmsg, axis.encoder_.vel_estimate_, 32, 32, true);
can_setSignal<float>(txmsg, axis.encoder_.pos_estimate_, 0, 32, true);
can_setSignal<float>(txmsg, axis.encoder_.vel_estimate_, 32, 32, true);
odCAN->write(txmsg);
}
odCAN->write(txmsg);
}
void CANSimple::get_sensorless_estimates_callback(const Axis& axis, const can_Message_t& msg) {
if (msg.rtr) {
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 = axis.config_.can_node_id_extended;
txmsg.len = 8;
void CANSimple::get_sensorless_estimates_callback(const Axis& axis) {
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 = axis.config_.can.is_extended;
txmsg.len = 8;
static_assert(sizeof(float) == sizeof(axis.sensorless_estimator_.pll_pos_));
static_assert(sizeof(float) == sizeof(axis.sensorless_estimator_.vel_estimate_));
static_assert(sizeof(float) == sizeof(axis.sensorless_estimator_.pll_pos_));
static_assert(sizeof(float) == sizeof(axis.sensorless_estimator_.vel_estimate_));
can_setSignal<float>(txmsg, axis.sensorless_estimator_.pll_pos_, 0, 32, true);
can_setSignal<float>(txmsg, axis.sensorless_estimator_.vel_estimate_, 32, 32, true);
can_setSignal<float>(txmsg, axis.sensorless_estimator_.pll_pos_, 0, 32, true);
can_setSignal<float>(txmsg, axis.sensorless_estimator_.vel_estimate_, 32, 32, true);
odCAN->write(txmsg);
}
odCAN->write(txmsg);
}
void CANSimple::get_encoder_count_callback(const Axis& axis, const can_Message_t& msg) {
if (msg.rtr) {
can_Message_t txmsg;
txmsg.id = axis.config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_ENCODER_COUNT;
txmsg.isExt = axis.config_.can_node_id_extended;
txmsg.len = 8;
void CANSimple::get_encoder_count_callback(const Axis& axis) {
can_Message_t txmsg;
txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_ENCODER_COUNT;
txmsg.isExt = axis.config_.can.is_extended;
txmsg.len = 8;
can_setSignal<int32_t>(txmsg, axis.encoder_.shadow_count_, 0, 32, true);
can_setSignal<int32_t>(txmsg, axis.encoder_.count_in_cpr_, 32, 32, true);
odCAN->write(txmsg);
}
can_setSignal<int32_t>(txmsg, axis.encoder_.shadow_count_, 0, 32, true);
can_setSignal<int32_t>(txmsg, axis.encoder_.count_in_cpr_, 32, 32, true);
odCAN->write(txmsg);
}
void CANSimple::set_input_pos_callback( Axis& axis, const can_Message_t& msg) {
void CANSimple::set_input_pos_callback(Axis& axis, const can_Message_t& msg) {
axis.controller_.input_pos_ = can_getSignal<float>(msg, 0, 32, true);
axis.controller_.input_vel_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.001f, 0);
axis.controller_.input_torque_ = can_getSignal<int16_t>(msg, 48, 16, true, 0.001f, 0);
axis.controller_.input_pos_updated();
}
void CANSimple::set_input_vel_callback( Axis& axis, const can_Message_t& msg) {
void CANSimple::set_input_vel_callback(Axis& axis, const can_Message_t& msg) {
axis.controller_.input_vel_ = can_getSignal<float>(msg, 0, 32, true);
axis.controller_.input_torque_ = can_getSignal<float>(msg, 32, 32, true);
}
void CANSimple::set_input_torque_callback( Axis& axis, const can_Message_t& msg) {
void CANSimple::set_input_torque_callback(Axis& axis, const can_Message_t& msg) {
axis.controller_.input_torque_ = can_getSignal<float>(msg, 0, 32, true);
}
void CANSimple::set_controller_modes_callback( Axis& axis, const can_Message_t& msg) {
void CANSimple::set_controller_modes_callback(Axis& axis, const can_Message_t& msg) {
axis.controller_.config_.control_mode = static_cast<Controller::ControlMode>(can_getSignal<int32_t>(msg, 0, 32, true));
axis.controller_.config_.input_mode = static_cast<Controller::InputMode>(can_getSignal<int32_t>(msg, 32, 32, true));
}
void CANSimple::set_vel_limit_callback( Axis& axis, const can_Message_t& msg) {
void CANSimple::set_vel_limit_callback(Axis& axis, const can_Message_t& msg) {
axis.controller_.config_.vel_limit = can_getSignal<float>(msg, 0, 32, true);
}
@@ -254,51 +239,47 @@ void CANSimple::start_anticogging_callback(const Axis& axis, const can_Message_t
axis.controller_.start_anticogging_calibration();
}
void CANSimple::set_traj_vel_limit_callback( Axis& axis, const can_Message_t& msg) {
void CANSimple::set_traj_vel_limit_callback(Axis& axis, const can_Message_t& msg) {
axis.trap_traj_.config_.vel_limit = can_getSignal<float>(msg, 0, 32, true);
}
void CANSimple::set_traj_accel_limits_callback( Axis& axis, const can_Message_t& msg) {
void CANSimple::set_traj_accel_limits_callback(Axis& axis, const can_Message_t& msg) {
axis.trap_traj_.config_.accel_limit = can_getSignal<float>(msg, 0, 32, true);
axis.trap_traj_.config_.decel_limit = can_getSignal<float>(msg, 32, 32, true);
}
void CANSimple::set_traj_inertia_callback( Axis& axis, const can_Message_t& msg) {
void CANSimple::set_traj_inertia_callback(Axis& axis, const can_Message_t& msg) {
axis.controller_.config_.inertia = can_getSignal<float>(msg, 0, 32, true);
}
void CANSimple::get_iq_callback(const Axis& axis, const can_Message_t& msg) {
if (msg.rtr) {
can_Message_t txmsg;
txmsg.id = axis.config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_IQ;
txmsg.isExt = axis.config_.can_node_id_extended;
txmsg.len = 8;
void CANSimple::get_iq_callback(const Axis& axis) {
can_Message_t txmsg;
txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_IQ;
txmsg.isExt = axis.config_.can.is_extended;
txmsg.len = 8;
static_assert(sizeof(float) == sizeof(axis.motor_.current_control_.Iq_setpoint));
static_assert(sizeof(float) == sizeof(axis.motor_.current_control_.Iq_measured));
can_setSignal<float>(txmsg, axis.motor_.current_control_.Iq_setpoint, 0, 32, true);
can_setSignal<float>(txmsg, axis.motor_.current_control_.Iq_measured, 32, 32, true);
static_assert(sizeof(float) == sizeof(axis.motor_.current_control_.Iq_setpoint));
static_assert(sizeof(float) == sizeof(axis.motor_.current_control_.Iq_measured));
can_setSignal<float>(txmsg, axis.motor_.current_control_.Iq_setpoint, 0, 32, true);
can_setSignal<float>(txmsg, axis.motor_.current_control_.Iq_measured, 32, 32, true);
odCAN->write(txmsg);
}
odCAN->write(txmsg);
}
void CANSimple::get_vbus_voltage_callback(const Axis& axis, const can_Message_t& msg) {
if (msg.rtr) {
can_Message_t txmsg;
void CANSimple::get_vbus_voltage_callback(const Axis& axis) {
can_Message_t txmsg;
txmsg.id = axis.config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_VBUS_VOLTAGE;
txmsg.isExt = axis.config_.can_node_id_extended;
txmsg.len = 8;
txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_VBUS_VOLTAGE;
txmsg.isExt = axis.config_.can.is_extended;
txmsg.len = 8;
uint32_t floatBytes;
static_assert(sizeof(vbus_voltage) == sizeof(floatBytes));
can_setSignal<float>(txmsg, vbus_voltage, 0, 32, true);
uint32_t floatBytes;
static_assert(sizeof(vbus_voltage) == sizeof(floatBytes));
can_setSignal<float>(txmsg, vbus_voltage, 0, 32, true);
odCAN->write(txmsg);
}
odCAN->write(txmsg);
}
void CANSimple::clear_errors_callback(Axis& axis, const can_Message_t& msg) {
@@ -307,29 +288,31 @@ void CANSimple::clear_errors_callback(Axis& axis, const can_Message_t& msg) {
void CANSimple::send_heartbeat(const Axis& axis) {
can_Message_t txmsg;
txmsg.id = axis.config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_ODRIVE_HEARTBEAT; // heartbeat ID
txmsg.isExt = axis.config_.can_node_id_extended;
txmsg.isExt = axis.config_.can.is_extended;
txmsg.len = 8;
// Axis errors in 1st 32-bit value
txmsg.buf[0] = axis.error_;
txmsg.buf[1] = axis.error_ >> 8;
txmsg.buf[2] = axis.error_ >> 16;
txmsg.buf[3] = axis.error_ >> 24;
can_setSignal(txmsg, axis.error_, 0, 32, true);
can_setSignal(txmsg, axis.current_state_, 32, 32, true);
// Current state of axis in 2nd 32-bit value
txmsg.buf[4] = axis.current_state_;
txmsg.buf[5] = axis.current_state_ >> 8;
txmsg.buf[6] = axis.current_state_ >> 16;
txmsg.buf[7] = axis.current_state_ >> 24;
odCAN->write(txmsg);
}
uint32_t CANSimple::get_node_id(uint32_t msgID) {
return (msgID >> NUM_CMD_ID_BITS); // Upper 6 or more bits
}
void CANSimple::send_cyclic(Axis& axis) {
const uint32_t now = osKernelSysTick();
uint8_t CANSimple::get_cmd_id(uint32_t msgID) {
return (msgID & 0x01F); // Bottom 5 bits
}
if (axis.config_.can.heartbeat_rate_ms > 0) {
if ((now - axis.can_.last_heartbeat) >= axis.config_.can.heartbeat_rate_ms) {
send_heartbeat(axis);
axis.can_.last_heartbeat = now;
}
}
if (axis.config_.can.encoder_rate_ms > 0) {
if ((now - axis.can_.last_encoder) >= axis.config_.can.encoder_rate_ms) {
get_encoder_estimates_callback(axis);
axis.can_.last_encoder = now;
}
}
}
Firmware/communication/can_simple.hpp
+30 -23
View File
@@ -35,47 +35,54 @@ class CANSimple {
};
static void handle_can_message(const can_Message_t& msg);
static void send_heartbeat(const Axis& axis);
static void doCommand(Axis& axis, const can_Message_t& cmd);
// Cyclic Senders
static void send_heartbeat(const Axis& axis);
static void send_cyclic(Axis& axis);
private:
static void nmt_callback(const Axis& axis, const can_Message_t& msg);
static void estop_callback(Axis& axis, const can_Message_t& msg);
static void get_motor_error_callback(const Axis& axis, const can_Message_t& msg);
static void get_encoder_error_callback(const Axis& axis, const can_Message_t& msg);
static void get_controller_error_callback(const Axis& axis, const can_Message_t& msg);
static void get_sensorless_error_callback(const Axis& axis, const can_Message_t& msg);
// Get functions (msg.rtr bit must be set)
static void get_motor_error_callback(const Axis& axis);
static void get_encoder_error_callback(const Axis& axis);
static void get_controller_error_callback(const Axis& axis);
static void get_sensorless_error_callback(const Axis& axis);
static void get_encoder_estimates_callback(const Axis& axis);
static void get_encoder_count_callback(const Axis& axis);
static void get_iq_callback(const Axis& axis);
static void get_sensorless_estimates_callback(const Axis& axis);
static void get_vbus_voltage_callback(const Axis& axis);
// Set functions
static void set_axis_nodeid_callback(Axis& axis, const can_Message_t& msg);
static void set_axis_requested_state_callback(Axis& axis, const can_Message_t& msg);
static void set_axis_startup_config_callback(Axis& axis, const can_Message_t& msg);
static void get_encoder_estimates_callback(const Axis& axis, const can_Message_t& msg);
static void get_encoder_count_callback(const Axis& axis, const can_Message_t& msg);
static void set_input_pos_callback(Axis& axis, const can_Message_t& msg);
static void set_input_vel_callback(Axis& axis, const can_Message_t& msg);
static void set_input_torque_callback(Axis& axis, const can_Message_t& msg);
static void set_controller_modes_callback(Axis& axis, const can_Message_t& msg);
static void set_vel_limit_callback(Axis& axis, const can_Message_t& msg);
static void start_anticogging_callback(const Axis& axis, const can_Message_t& msg);
static void set_traj_vel_limit_callback(Axis& axis, const can_Message_t& msg);
static void set_traj_accel_limits_callback(Axis& axis, const can_Message_t& msg);
static void set_traj_inertia_callback(Axis& axis, const can_Message_t& msg);
static void get_iq_callback(const Axis& axis, const can_Message_t& msg);
static void get_sensorless_estimates_callback(const Axis& axis, const can_Message_t& msg);
static void get_vbus_voltage_callback(const Axis& axis, const can_Message_t& msg);
// Other functions
static void nmt_callback(const Axis& axis, const can_Message_t& msg);
static void estop_callback(Axis& axis, const can_Message_t& msg);
static void clear_errors_callback(Axis& axis, const can_Message_t& msg);
static void start_anticogging_callback(const Axis& axis, const can_Message_t& msg);
static constexpr uint8_t NUM_NODE_ID_BITS = 6;
static constexpr uint8_t NUM_CMD_ID_BITS = 11 - NUM_NODE_ID_BITS;
// Utility functions
static uint32_t get_node_id(uint32_t msgID);
static uint8_t get_cmd_id(uint32_t msgID);
static constexpr uint32_t get_node_id(uint32_t msgID) {
return (msgID >> NUM_CMD_ID_BITS); // Upper 6 or more bits
};
// Fetch a specific signal from the message
// This functional way of handling the messages is neat and is much cleaner from
// a data security point of view, but it will require some tweaking
//
// const std::map<uint32_t, std::function<void(can_Message_t&)>> callback_map = {
// {0x000, std::bind(&CANSimple::heartbeat_callback, this, _1)}
// };
static constexpr uint8_t get_cmd_id(uint32_t msgID) {
return (msgID & 0x01F); // Bottom 5 bits
}
};
#endif
Firmware/communication/interface_can.cpp
+6 -11
View File
@@ -174,20 +174,15 @@ void ODriveCAN::reinit_can() {
void ODriveCAN::set_error(Error error) {
error_ |= error;
}
// This function is called by each axis.
// It provides an abstraction from the specific CAN protocol in use
void ODriveCAN::send_heartbeat(Axis& axis) {
void ODriveCAN::send_cyclic(Axis &axis) {
// Handle heartbeat message
if (axis.config_.can_heartbeat_rate_ms > 0) {
uint32_t now = osKernelSysTick();
if ((now - axis.last_heartbeat_) >= axis.config_.can_heartbeat_rate_ms) {
switch (config_.protocol) {
case PROTOCOL_SIMPLE:
CANSimple::send_heartbeat(axis);
break;
}
axis.last_heartbeat_ = now;
}
switch (config_.protocol) {
case PROTOCOL_SIMPLE:
CANSimple::send_cyclic(axis);
break;
}
}
Firmware/communication/interface_can.hpp
+1 -1
View File
@@ -35,7 +35,7 @@ class ODriveCAN : public ODriveIntf::CanIntf {
volatile bool thread_id_valid_ = false;
bool start_can_server();
void can_server_thread();
void send_heartbeat(Axis& axis);
void send_cyclic(Axis& axis);
void reinit_can();
void set_error(Error error);
Firmware/odrive-interface.yaml
+9 -5
View File
@@ -417,11 +417,7 @@ interfaces:
vel: float32
sensorless_ramp: LockinConfig
general_lockin: LockinConfig
can_node_id:
type: uint32
doc: Both axes will have the same id to start
can_node_id_extended: bool
can_heartbeat_rate_ms: uint32
can: CANConfig
gate_driver:
c_name: gate_driver_exported_
c_is_class: False
@@ -485,6 +481,14 @@ interfaces:
finish_on_distance: bool
finish_on_enc_idx: bool
ODrive.Axis.CANConfig:
c_is_class: False
attributes:
node_id: uint32
is_extended: bool
heartbeat_rate_ms: uint32
encoder_rate_ms: uint32
ODrive.ThermistorCurrentLimiter:
c_is_class: False