Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-21 07:14:22 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Formatting pass on CAN files

Browse Source
This commit is contained in:
Paul Guenette
2019-05-19 21:19:30 +02:00
parent b09e835647
commit 9503a9ce29
5 changed files with 109 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/communication/can_simple.cpp
+42 -65
View File
@@ -7,7 +7,7 @@
static const 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(CAN_message_t& msg) {
void CANSimple::handle_can_message(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.
//
@@ -121,17 +121,17 @@ void CANSimple::handle_can_message(CAN_message_t& msg) {
}
}
void CANSimple::nmt_callback(Axis* axis, CAN_message_t& msg) {
void CANSimple::nmt_callback(Axis* axis, can_Message_t& msg) {
// Not implemented
}
void CANSimple::estop_callback(Axis* axis, CAN_message_t& msg) {
void CANSimple::estop_callback(Axis* axis, can_Message_t& msg) {
axis->error_ |= Axis::ERROR_ESTOP_REQUESTED;
}
void CANSimple::get_motor_error_callback(Axis* axis, CAN_message_t& msg) {
void CANSimple::get_motor_error_callback(Axis* axis, can_Message_t& msg) {
if (msg.rtr) {
CAN_message_t txmsg;
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;
@@ -146,9 +146,9 @@ void CANSimple::get_motor_error_callback(Axis* axis, CAN_message_t& msg) {
}
}
void CANSimple::get_encoder_error_callback(Axis* axis, CAN_message_t& msg) {
void CANSimple::get_encoder_error_callback(Axis* axis, can_Message_t& msg) {
if (msg.rtr) {
CAN_message_t txmsg;
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;
@@ -163,9 +163,9 @@ void CANSimple::get_encoder_error_callback(Axis* axis, CAN_message_t& msg) {
}
}
void CANSimple::get_sensorless_error_callback(Axis* axis, CAN_message_t& msg) {
void CANSimple::get_sensorless_error_callback(Axis* axis, can_Message_t& msg) {
if (msg.rtr) {
CAN_message_t txmsg;
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;
@@ -180,20 +180,20 @@ void CANSimple::get_sensorless_error_callback(Axis* axis, CAN_message_t& msg) {
}
}
void CANSimple::set_axis_nodeid_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
}
void CANSimple::set_axis_requested_state_callback(Axis* axis, CAN_message_t& msg) {
axis->requested_state_ = static_cast<Axis::State_t>(get_16bit_val(msg, 0));
void CANSimple::set_axis_requested_state_callback(Axis* axis, can_Message_t& msg) {
axis->requested_state_ = static_cast<Axis::State_t>(can_getSignal<int16_t>(msg, 0, 16, true, 1, 0));
}
void CANSimple::set_axis_startup_config_callback(Axis* axis, CAN_message_t& msg) {
void CANSimple::set_axis_startup_config_callback(Axis* axis, can_Message_t& msg) {
// Not Implemented
}
void CANSimple::get_encoder_estimates_callback(Axis* axis, CAN_message_t& msg) {
void CANSimple::get_encoder_estimates_callback(Axis* axis, can_Message_t& msg) {
if (msg.rtr) {
CAN_message_t txmsg;
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;
@@ -222,9 +222,9 @@ void CANSimple::get_encoder_estimates_callback(Axis* axis, CAN_message_t& msg) {
}
}
void CANSimple::get_sensorless_estimates_callback(Axis* axis, CAN_message_t& msg) {
void CANSimple::get_sensorless_estimates_callback(Axis* axis, can_Message_t& msg) {
if (msg.rtr) {
CAN_message_t txmsg;
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;
@@ -253,9 +253,9 @@ void CANSimple::get_sensorless_estimates_callback(Axis* axis, CAN_message_t& msg
}
}
void CANSimple::get_encoder_count_callback(Axis* axis, CAN_message_t& msg) {
void CANSimple::get_encoder_count_callback(Axis* axis, can_Message_t& msg) {
if (msg.rtr) {
CAN_message_t txmsg;
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_ENCODER_COUNT;
txmsg.isExt = false;
@@ -275,46 +275,46 @@ void CANSimple::get_encoder_count_callback(Axis* axis, CAN_message_t& msg) {
}
}
void CANSimple::move_to_pos_callback(Axis* axis, CAN_message_t& msg) {
axis->controller_.move_to_pos(get_32bit_val(msg, 0));
void CANSimple::move_to_pos_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.move_to_pos(can_getSignal<int32_t>(msg, 0, 32, true, 1, 0));
}
void CANSimple::set_pos_setpoint_callback(Axis* axis, CAN_message_t& msg) {
axis->controller_.set_pos_setpoint(get_32bit_val(msg, 0), get_16bit_val(msg, 4) * 0.1f, get_16bit_val(msg, 6) * 0.01f);
void CANSimple::set_pos_setpoint_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.set_pos_setpoint(can_getSignal<int32_t>(msg, 0, 32, true, 1, 0), can_getSignal<int16_t>(msg, 32, 16, true, 0.1f, 0), can_getSignal<int16_t>(msg, 48, 16, true, 0.01f, 0));
}
void CANSimple::set_vel_setpoint_callback(Axis* axis, CAN_message_t& msg) {
axis->controller_.set_vel_setpoint(get_32bit_val(msg, 0) * 0.01f, get_32bit_val(msg, 4) * 0.01f);
void CANSimple::set_vel_setpoint_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.set_vel_setpoint(can_getSignal<int32_t>(msg, 0, 32, true, 0.01f, 0.0f), can_getSignal<int32_t>(msg, 4, 32, true, 0.01f, 0.0f));
}
void CANSimple::set_current_setpoint_callback(Axis* axis, CAN_message_t& msg) {
axis->controller_.set_current_setpoint(get_32bit_val(msg, 0) * 0.01f);
void CANSimple::set_current_setpoint_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.set_current_setpoint(can_getSignal<int32_t>(msg, 0, 32, true, 0.01f, 0));
}
void CANSimple::set_vel_limit_callback(Axis* axis, CAN_message_t& msg) {
axis->controller_.config_.vel_limit = get_float(msg, 0);
void CANSimple::set_vel_limit_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.config_.vel_limit = can_getSignal<float>(msg, 0, 32, true, 1, 0);
}
void CANSimple::start_anticogging_callback(Axis* axis, CAN_message_t& msg) {
void CANSimple::start_anticogging_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.start_anticogging_calibration();
}
void CANSimple::set_traj_vel_limit_callback(Axis* axis, CAN_message_t& msg) {
axis->trap_.config_.vel_limit = get_float(msg, 0);
void CANSimple::set_traj_vel_limit_callback(Axis* axis, can_Message_t& msg) {
axis->trap_.config_.vel_limit = can_getSignal<float>(msg, 0, 32, true, 1, 0);
}
void CANSimple::set_traj_accel_limits_callback(Axis* axis, CAN_message_t& msg) {
axis->trap_.config_.accel_limit = get_float(msg, 0);
axis->trap_.config_.decel_limit = get_float(msg, 4);
void CANSimple::set_traj_accel_limits_callback(Axis* axis, can_Message_t& msg) {
axis->trap_.config_.accel_limit = can_getSignal<float>(msg, 0, 32, true, 1, 0);
axis->trap_.config_.decel_limit = can_getSignal<float>(msg, 32, 32, true, 1, 0);
}
void CANSimple::set_traj_A_per_css_callback(Axis* axis, CAN_message_t& msg) {
axis->trap_.config_.A_per_css = get_float(msg, 0);
void CANSimple::set_traj_A_per_css_callback(Axis* axis, can_Message_t& msg) {
axis->trap_.config_.A_per_css = can_getSignal<float>(msg, 0, 32, true, 1, 0);
}
void CANSimple::get_iq_callback(Axis* axis, CAN_message_t& msg) {
void CANSimple::get_iq_callback(Axis* axis, can_Message_t& msg) {
if (msg.rtr) {
CAN_message_t txmsg;
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_IQ;
txmsg.isExt = false;
@@ -340,9 +340,9 @@ void CANSimple::get_iq_callback(Axis* axis, CAN_message_t& msg) {
}
}
void CANSimple::get_vbus_voltage_callback(Axis* axis, CAN_message_t& msg) {
void CANSimple::get_vbus_voltage_callback(Axis* axis, can_Message_t& msg) {
if (msg.rtr) {
CAN_message_t txmsg;
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_VBUS_VOLTAGE;
@@ -371,7 +371,7 @@ void CANSimple::get_vbus_voltage_callback(Axis* axis, CAN_message_t& msg) {
}
void CANSimple::send_heartbeat(Axis* axis) {
CAN_message_t txmsg;
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;
@@ -397,27 +397,4 @@ uint8_t CANSimple::get_node_id(uint32_t msgID) {
uint8_t CANSimple::get_cmd_id(uint32_t msgID) {
return (msgID & 0x01F); // Bottom 5 bits
}
int16_t CANSimple::get_16bit_val(CAN_message_t& msg, uint8_t start_byte) {
int16_t retVal = 0;
if (msg.len >= start_byte && size_t(msg.len - start_byte) >= sizeof(retVal))
std::memcpy(&retVal, &(msg.buf[start_byte]), sizeof(retVal));
return retVal;
}
int32_t CANSimple::get_32bit_val(CAN_message_t& msg, uint8_t start_byte) {
int32_t retVal = 0;
if (msg.len >= start_byte && size_t(msg.len - start_byte) >= sizeof(retVal))
std::memcpy(&retVal, &(msg.buf[start_byte]), sizeof(retVal));
return retVal;
}
float CANSimple::get_float(CAN_message_t& msg, uint8_t start_byte) {
int32_t val = get_32bit_val(msg, start_byte);
float retVal;
static_assert(sizeof retVal == sizeof val);
std::memcpy(&retVal, &val, sizeof val); // Sexier int32_t -> float cast that isn't UB
return retVal;
}
Firmware/communication/can_simple.hpp
+49 -31
View File
@@ -6,8 +6,8 @@
class CANSimple {
public:
enum {
MSG_CO_NMT_CTRL = 0x000, // CANOpen NMT Message REC
MSG_CO_HEARTBEAT_CMD = 0x700, // CANOpen NMT Heartbeat SEND
MSG_CO_NMT_CTRL = 0x000, // CANOpen NMT Message REC
MSG_CO_HEARTBEAT_CMD = 0x700, // CANOpen NMT Heartbeat SEND
MSG_ODRIVE_HEARTBEAT = 0x001,
MSG_ODRIVE_ESTOP,
MSG_GET_MOTOR_ERROR, // Errors
@@ -33,49 +33,67 @@ class CANSimple {
MSG_GET_VBUS_VOLTAGE,
};
static void handle_can_message(CAN_message_t& msg);
static void handle_can_message(can_Message_t& msg);
static void send_heartbeat(Axis* axis);
private:
static void nmt_callback(Axis* axis, CAN_message_t& msg);
static void estop_callback(Axis* axis, CAN_message_t& msg);
static void get_motor_error_callback(Axis* axis, CAN_message_t& msg);
static void get_encoder_error_callback(Axis* axis, CAN_message_t& msg);
static void get_controller_error_callback(Axis* axis, CAN_message_t& msg);
static void get_sensorless_error_callback(Axis* axis, CAN_message_t& msg);
static void set_axis_nodeid_callback(Axis* axis, CAN_message_t& msg);
static void set_axis_requested_state_callback(Axis* axis, CAN_message_t& msg);
static void set_axis_startup_config_callback(Axis* axis, CAN_message_t& msg);
static void get_encoder_estimates_callback(Axis* axis, CAN_message_t& msg);
static void get_encoder_count_callback(Axis* axis, CAN_message_t& msg);
static void move_to_pos_callback(Axis* axis, CAN_message_t& msg);
static void set_pos_setpoint_callback(Axis* axis, CAN_message_t& msg);
static void set_vel_setpoint_callback(Axis* axis, CAN_message_t& msg);
static void set_current_setpoint_callback(Axis* axis, CAN_message_t& msg);
static void set_vel_limit_callback(Axis* axis, CAN_message_t& msg);
static void start_anticogging_callback(Axis* axis, CAN_message_t& msg);
static void set_traj_vel_limit_callback(Axis* axis, CAN_message_t& msg);
static void set_traj_accel_limits_callback(Axis* axis, CAN_message_t& msg);
static void set_traj_A_per_css_callback(Axis* axis, CAN_message_t& msg);
static void get_iq_callback(Axis* axis, CAN_message_t& msg);
static void get_sensorless_estimates_callback(Axis* axis, CAN_message_t& msg);
static void get_vbus_voltage_callback(Axis* axis, CAN_message_t& msg);
static void nmt_callback(Axis* axis, can_Message_t& msg);
static void estop_callback(Axis* axis, can_Message_t& msg);
static void get_motor_error_callback(Axis* axis, can_Message_t& msg);
static void get_encoder_error_callback(Axis* axis, can_Message_t& msg);
static void get_controller_error_callback(Axis* axis, can_Message_t& msg);
static void get_sensorless_error_callback(Axis* axis, can_Message_t& msg);
static void set_axis_nodeid_callback(Axis* axis, can_Message_t& msg);
static void set_axis_requested_state_callback(Axis* axis, can_Message_t& msg);
static void set_axis_startup_config_callback(Axis* axis, can_Message_t& msg);
static void get_encoder_estimates_callback(Axis* axis, can_Message_t& msg);
static void get_encoder_count_callback(Axis* axis, can_Message_t& msg);
static void move_to_pos_callback(Axis* axis, can_Message_t& msg);
static void set_pos_setpoint_callback(Axis* axis, can_Message_t& msg);
static void set_vel_setpoint_callback(Axis* axis, can_Message_t& msg);
static void set_current_setpoint_callback(Axis* axis, can_Message_t& msg);
static void set_vel_limit_callback(Axis* axis, can_Message_t& msg);
static void start_anticogging_callback(Axis* axis, can_Message_t& msg);
static void set_traj_vel_limit_callback(Axis* axis, can_Message_t& msg);
static void set_traj_accel_limits_callback(Axis* axis, can_Message_t& msg);
static void set_traj_A_per_css_callback(Axis* axis, can_Message_t& msg);
static void get_iq_callback(Axis* axis, can_Message_t& msg);
static void get_sensorless_estimates_callback(Axis* axis, can_Message_t& msg);
static void get_vbus_voltage_callback(Axis* axis, can_Message_t& msg);
// Utility functions
static uint8_t get_node_id(uint32_t msgID);
static uint8_t get_cmd_id(uint32_t msgID);
static int16_t get_16bit_val(CAN_message_t& msg, uint8_t start_byte);
static int32_t get_32bit_val(CAN_message_t& msg, uint8_t start_byte);
static float get_float(CAN_message_t& msg, uint8_t start_byte);
// 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 = {
// const std::map<uint32_t, std::function<void(can_Message_t&)>> callback_map = {
// {0x000, std::bind(&CANSimple::heartbeat_callback, this, _1)}
// };
};
#include <iterator>
template <typename T>
T can_getSignal(can_Message_t msg, uint8_t startBit, uint8_t length, bool isIntel, float factor, float offset) {
uint64_t tempVal = 0;
uint64_t mask = (1ULL << length) - 1;
if (isIntel) {
std::memcpy(&tempVal, msg.buf, sizeof(tempVal));
tempVal = (tempVal >> startBit) & mask;
} else {
std::reverse(std::begin(msg.buf), std::end(msg.buf));
std::memcpy(&tempVal, msg.buf, sizeof(tempVal));
tempVal = (tempVal >> (64 - startBit - length)) & mask;
}
T retVal;
std::memcpy(&retVal, &tempVal, sizeof(T));
return static_cast<T>((retVal * factor) + offset);
}
#endif
Firmware/communication/interface_can.cpp
+3 -3
View File
@@ -26,7 +26,7 @@ void ODriveCAN::can_server_thread() {
for (;;) {
uint32_t status = HAL_CAN_GetError(handle_);
if (status == HAL_CAN_ERROR_NONE) {
CAN_message_t rxmsg;
can_Message_t rxmsg;
osSemaphoreWait(sem_can, 10); // Poll every 10ms regardless of sempahore status
while (available()) {
@@ -86,7 +86,7 @@ bool ODriveCAN::start_can_server() {
}
// Send a CAN message on the bus
uint32_t ODriveCAN::write(CAN_message_t &txmsg) {
uint32_t ODriveCAN::write(can_Message_t &txmsg) {
if (HAL_CAN_GetError(handle_) == HAL_CAN_ERROR_NONE) {
CAN_TxHeaderTypeDef header;
header.StdId = txmsg.id;
@@ -110,7 +110,7 @@ uint32_t ODriveCAN::available() {
return (HAL_CAN_GetRxFifoFillLevel(handle_, CAN_RX_FIFO0) + HAL_CAN_GetRxFifoFillLevel(handle_, CAN_RX_FIFO1));
}
bool ODriveCAN::read(CAN_message_t &rxmsg) {
bool ODriveCAN::read(can_Message_t &rxmsg) {
CAN_RxHeaderTypeDef header;
bool validRead = false;
if (HAL_CAN_GetRxFifoFillLevel(handle_, CAN_RX_FIFO0) > 0) {
Firmware/communication/interface_can.hpp
+3 -3
View File
@@ -15,7 +15,7 @@ typedef struct {
bool rtr = false;
uint8_t len = 8;
uint8_t buf[8] = {0, 0, 0, 0, 0, 0, 0, 0};
} CAN_message_t;
} can_Message_t;
// Anonymous enum for defining the most common CAN baud rates
@@ -59,8 +59,8 @@ class ODriveCAN {
// I/O Functions
uint32_t available();
uint32_t write(CAN_message_t &txmsg);
bool read(CAN_message_t &rxmsg);
uint32_t write(can_Message_t &txmsg);
bool read(can_Message_t &rxmsg);
// Communication Protocol Handling
auto make_protocol_definitions() {
ODrive_Workspace.code-workspace
+12 -1
View File
@@ -53,7 +53,18 @@
"chrono": "cpp",
"condition_variable": "cpp",
"future": "cpp",
"arm_math.h": "c"
"arm_math.h": "c",
"iostream": "cpp",
"cmath": "cpp",
"csignal": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"ctime": "cpp",
"unordered_map": "cpp",
"fstream": "cpp",
"iomanip": "cpp",
"optional": "cpp",
"sstream": "cpp"
}
}
}