diff --git a/Firmware/communication/can_simple.cpp b/Firmware/communication/can_simple.cpp index d6327449..81502c8e 100644 --- a/Firmware/communication/can_simple.cpp +++ b/Firmware/communication/can_simple.cpp @@ -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(get_16bit_val(msg, 0)); +void CANSimple::set_axis_requested_state_callback(Axis* axis, can_Message_t& msg) { + axis->requested_state_ = static_cast(can_getSignal(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(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(msg, 0, 32, true, 1, 0), can_getSignal(msg, 32, 16, true, 0.1f, 0), can_getSignal(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(msg, 0, 32, true, 0.01f, 0.0f), can_getSignal(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(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(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(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(msg, 0, 32, true, 1, 0); + axis->trap_.config_.decel_limit = can_getSignal(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(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; } \ No newline at end of file diff --git a/Firmware/communication/can_simple.hpp b/Firmware/communication/can_simple.hpp index fd078c5a..9138e109 100644 --- a/Firmware/communication/can_simple.hpp +++ b/Firmware/communication/can_simple.hpp @@ -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> callback_map = { + // const std::map> callback_map = { // {0x000, std::bind(&CANSimple::heartbeat_callback, this, _1)} // }; }; + +#include +template +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((retVal * factor) + offset); +} + #endif \ No newline at end of file diff --git a/Firmware/communication/interface_can.cpp b/Firmware/communication/interface_can.cpp index 44cd7e2d..251dec43 100644 --- a/Firmware/communication/interface_can.cpp +++ b/Firmware/communication/interface_can.cpp @@ -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) { diff --git a/Firmware/communication/interface_can.hpp b/Firmware/communication/interface_can.hpp index 68479a61..a19c953a 100644 --- a/Firmware/communication/interface_can.hpp +++ b/Firmware/communication/interface_can.hpp @@ -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() { diff --git a/ODrive_Workspace.code-workspace b/ODrive_Workspace.code-workspace index d9644123..9c333d91 100644 --- a/ODrive_Workspace.code-workspace +++ b/ODrive_Workspace.code-workspace @@ -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" } } }