*

ODrive-fw-v0.5.6/Firmware/communication/can/can_helpers.hpp

@@ -0,0 +1,97 @@
+#pragma once
+
+#include <stdint.h>
+#include <algorithm>
+#include <cstring>
+#include <iterator>
+
+struct can_Message_t {
+    uint32_t id = 0x000;  // 11-bit max is 0x7ff, 29-bit max is 0x1FFFFFFF
+    bool isExt = false;
+    bool rtr = false;
+    uint8_t len = 8;
+    uint8_t buf[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+} ;
+
+struct can_Signal_t {
+    const uint8_t startBit;
+    const uint8_t length;
+    const bool isIntel;
+    const float factor;
+    const float offset;
+};
+
+struct can_Cyclic_t {
+    uint32_t cycleTime_ms;
+    uint32_t lastTime_ms;
+};
+
+#include <iterator>
+template <typename T>
+constexpr 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 = length < 64 ? (1ULL << length) - 1ULL : -1ULL;
+
+    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 retVal;
+}
+
+template <typename T>
+constexpr void can_setSignal(can_Message_t& msg, const T& val, const uint8_t startBit, const uint8_t length, const bool isIntel) {
+    uint64_t valAsBits = 0;
+    std::memcpy(&valAsBits, &val, sizeof(val));
+
+    uint64_t mask = length < 64 ? (1ULL << length) - 1ULL : -1ULL;
+
+    if (isIntel) {
+        uint64_t data = 0;
+        std::memcpy(&data, msg.buf, sizeof(data));
+
+        data &= ~(mask << startBit);
+        data |= valAsBits << startBit;
+
+        std::memcpy(msg.buf, &data, sizeof(data));
+    } else {
+        uint64_t data = 0;
+        std::reverse(std::begin(msg.buf), std::end(msg.buf));
+        std::memcpy(&data, msg.buf, sizeof(data));
+
+        data &= ~(mask << (64 - startBit - length));
+        data |= valAsBits << (64 - startBit - length);
+
+        std::memcpy(msg.buf, &data, sizeof(data));
+        std::reverse(std::begin(msg.buf), std::end(msg.buf));
+    }
+}
+
+template<typename T>
+void can_setSignal(can_Message_t& msg, const T& val, const uint8_t startBit, const uint8_t length, const bool isIntel, const float factor, const float offset) {
+    T scaledVal = static_cast<T>((val - offset) / factor);
+    can_setSignal<T>(msg, scaledVal, startBit, length, isIntel);
+}
+
+template<typename T>
+float can_getSignal(can_Message_t msg, const uint8_t startBit, const uint8_t length, const bool isIntel, const float factor, const float offset) {
+    T retVal = can_getSignal<T>(msg, startBit, length, isIntel);
+    return (retVal * factor) + offset;
+}
+
+template <typename T>
+float can_getSignal(can_Message_t msg, const can_Signal_t& signal) {
+    return can_getSignal<T>(msg, signal.startBit, signal.length, signal.isIntel, signal.factor, signal.offset);
+}
+
+template <typename T>
+void can_setSignal(can_Message_t& msg, const T& val, const can_Signal_t& signal) {
+    can_setSignal(msg, val, signal.startBit, signal.length, signal.isIntel, signal.factor, signal.offset);
+}

ODrive-fw-v0.5.6/Firmware/communication/can/can_simple.cpp

@@ -0,0 +1,467 @@
+
+#include "can_simple.hpp"
+
+#include <odrive_main.h>
+#include <functional>
+
+bool CANSimple::init() {
+    for (size_t i = 0; i < AXIS_COUNT; ++i) {
+        if (!renew_subscription(i)) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+bool CANSimple::renew_subscription(size_t i) {
+    Axis& axis = axes[i];
+
+    // TODO: remove these two lines (see comment in header)
+    node_ids_[i] = axis.config_.can.node_id;
+    extended_node_ids_[i] = axis.config_.can.is_extended;
+
+    MsgIdFilterSpecs filter = {
+        .id = {},
+        .mask = (uint32_t)(0xffffffff << NUM_CMD_ID_BITS)};
+    if (axis.config_.can.is_extended) {
+        filter.id = (uint32_t)(axis.config_.can.node_id << NUM_CMD_ID_BITS);
+    } else {
+        filter.id = (uint16_t)(axis.config_.can.node_id << NUM_CMD_ID_BITS);
+    }
+
+    if (subscription_handles_[i]) {
+        canbus_->unsubscribe(subscription_handles_[i]);
+    }
+
+    return canbus_->subscribe(
+        filter, [](void* ctx, const can_Message_t& msg) {
+            ((CANSimple*)ctx)->handle_can_message(msg);
+        },
+        this, &subscription_handles_[i]);
+}
+
+void CANSimple::handle_can_message(const can_Message_t& 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.is_extended == msg.isExt)) {
+            do_command(axis, msg);
+            return;
+        }
+    }
+}
+
+void CANSimple::do_command(Axis& axis, const can_Message_t& msg) {
+    const uint32_t cmd = get_cmd_id(msg.id);
+    axis.watchdog_feed();
+    switch (cmd) {
+        case MSG_CO_NMT_CTRL:
+            break;
+        case MSG_CO_HEARTBEAT_CMD:
+            break;
+        case MSG_ODRIVE_HEARTBEAT:
+            // We don't currently do anything to respond to ODrive heartbeat messages
+            break;
+        case MSG_ODRIVE_ESTOP:
+            estop_callback(axis, msg);
+            break;
+        case MSG_GET_MOTOR_ERROR:
+            if (msg.rtr || msg.len == 0)
+                get_motor_error_callback(axis);
+            break;
+        case MSG_GET_ENCODER_ERROR:
+            if (msg.rtr || msg.len == 0)
+                get_encoder_error_callback(axis);
+            break;
+        case MSG_GET_SENSORLESS_ERROR:
+            if (msg.rtr || msg.len == 0)
+                get_sensorless_error_callback(axis);
+            break;
+        case MSG_SET_AXIS_NODE_ID:
+            set_axis_nodeid_callback(axis, msg);
+            break;
+        case MSG_SET_AXIS_REQUESTED_STATE:
+            set_axis_requested_state_callback(axis, msg);
+            break;
+        case MSG_SET_AXIS_STARTUP_CONFIG:
+            set_axis_startup_config_callback(axis, msg);
+            break;
+        case MSG_GET_ENCODER_ESTIMATES:
+            if (msg.rtr || msg.len == 0)
+                get_encoder_estimates_callback(axis);
+            break;
+        case MSG_GET_ENCODER_COUNT:
+            if (msg.rtr || msg.len == 0)
+                get_encoder_count_callback(axis);
+            break;
+        case MSG_SET_INPUT_POS:
+            set_input_pos_callback(axis, msg);
+            break;
+        case MSG_SET_INPUT_VEL:
+            set_input_vel_callback(axis, msg);
+            break;
+        case MSG_SET_INPUT_TORQUE:
+            set_input_torque_callback(axis, msg);
+            break;
+        case MSG_SET_CONTROLLER_MODES:
+            set_controller_modes_callback(axis, msg);
+            break;
+        case MSG_SET_LIMITS:
+            set_limits_callback(axis, msg);
+            break;
+        case MSG_START_ANTICOGGING:
+            start_anticogging_callback(axis, msg);
+            break;
+        case MSG_SET_TRAJ_INERTIA:
+            set_traj_inertia_callback(axis, msg);
+            break;
+        case MSG_SET_TRAJ_ACCEL_LIMITS:
+            set_traj_accel_limits_callback(axis, msg);
+            break;
+        case MSG_SET_TRAJ_VEL_LIMIT:
+            set_traj_vel_limit_callback(axis, msg);
+            break;
+        case MSG_GET_IQ:
+            if (msg.rtr || msg.len == 0)
+                get_iq_callback(axis);
+            break;
+        case MSG_GET_SENSORLESS_ESTIMATES:
+            if (msg.rtr || msg.len == 0)
+                get_sensorless_estimates_callback(axis);
+            break;
+        case MSG_RESET_ODRIVE:
+            NVIC_SystemReset();
+            break;
+        case MSG_GET_BUS_VOLTAGE_CURRENT:
+            if (msg.rtr || msg.len == 0)
+                get_bus_voltage_current_callback(axis);
+            break;
+        case MSG_CLEAR_ERRORS:
+            clear_errors_callback(axis, msg);
+            break;
+        case MSG_SET_LINEAR_COUNT:
+            set_linear_count_callback(axis, msg);
+            break;
+        case MSG_SET_POS_GAIN:
+            set_pos_gain_callback(axis, msg);
+            break;
+        case MSG_SET_VEL_GAINS:
+            set_vel_gains_callback(axis, msg);
+            break;
+        case MSG_GET_ADC_VOLTAGE:
+            get_adc_voltage_callback(axis, msg);
+            break;
+        case MSG_GET_CONTROLLER_ERROR:
+            get_controller_error_callback(axis);
+            break;
+        default:
+            break;
+    }
+}
+
+void CANSimple::nmt_callback(const Axis& axis, const can_Message_t& msg) {
+    // Not implemented
+}
+
+void CANSimple::estop_callback(Axis& axis, const can_Message_t& msg) {
+    axis.error_ |= Axis::ERROR_ESTOP_REQUESTED;
+}
+
+bool 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, 64, true);
+
+    return canbus_->send_message(txmsg);
+}
+
+bool 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);
+
+    return canbus_->send_message(txmsg);
+}
+
+bool 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);
+
+    return canbus_->send_message(txmsg);
+}
+
+bool CANSimple::get_controller_error_callback(const Axis& axis) {
+    can_Message_t txmsg;
+    txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS;
+    txmsg.id += MSG_GET_CONTROLLER_ERROR;  // heartbeat ID
+    txmsg.isExt = axis.config_.can.is_extended;
+    txmsg.len = 8;
+
+    can_setSignal(txmsg, axis.controller_.error_, 0, 32, true);
+
+    return canbus_->send_message(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);
+}
+
+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, 32, true));
+}
+
+void CANSimple::set_axis_startup_config_callback(Axis& axis, const can_Message_t& msg) {
+    // Not Implemented
+}
+
+bool 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;
+
+    can_setSignal<float>(txmsg, axis.controller_.pos_estimate_linear_src_.any().value_or(0.0f), 0, 32, true);
+    can_setSignal<float>(txmsg, axis.controller_.vel_estimate_src_.any().value_or(0.0f), 32, 32, true);
+
+    return canbus_->send_message(txmsg);
+}
+
+bool 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_));
+
+    can_setSignal<float>(txmsg, axis.sensorless_estimator_.pll_pos_, 0, 32, true);
+    can_setSignal<float>(txmsg, axis.sensorless_estimator_.vel_estimate_.any().value_or(0.0f), 32, 32, true);
+
+    return canbus_->send_message(txmsg);
+}
+
+bool 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);
+    return canbus_->send_message(txmsg);
+}
+
+void CANSimple::set_input_pos_callback(Axis& axis, const can_Message_t& msg) {
+    axis.controller_.set_input_pos_and_steps(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) {
+    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) {
+    axis.controller_.input_torque_ = can_getSignal<float>(msg, 0, 32, true);
+}
+
+void CANSimple::set_controller_modes_callback(Axis& axis, const can_Message_t& msg) {
+    Controller::ControlMode const mode = static_cast<Controller::ControlMode>(can_getSignal<int32_t>(msg, 0, 32, true));
+    axis.controller_.config_.control_mode = static_cast<Controller::ControlMode>(mode);
+    axis.controller_.config_.input_mode = static_cast<Controller::InputMode>(can_getSignal<int32_t>(msg, 32, 32, true));
+    axis.controller_.control_mode_updated();
+}
+
+void CANSimple::set_limits_callback(Axis& axis, const can_Message_t& msg) {
+    axis.controller_.config_.vel_limit = can_getSignal<float>(msg, 0, 32, true);
+    axis.motor_.config_.current_lim = can_getSignal<float>(msg, 32, 32, true);
+}
+
+void CANSimple::start_anticogging_callback(const Axis& axis, const can_Message_t& msg) {
+    axis.controller_.start_anticogging_calibration();
+}
+
+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) {
+    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) {
+    axis.controller_.config_.inertia = can_getSignal<float>(msg, 0, 32, true);
+}
+
+void CANSimple::set_linear_count_callback(Axis& axis, const can_Message_t& msg) {
+    axis.encoder_.set_linear_count(can_getSignal<int32_t>(msg, 0, 32, true));
+}
+
+void CANSimple::set_pos_gain_callback(Axis& axis, const can_Message_t& msg) {
+    axis.controller_.config_.pos_gain = can_getSignal<float>(msg, 0, 32, true);
+}
+
+void CANSimple::set_vel_gains_callback(Axis& axis, const can_Message_t& msg) {
+    axis.controller_.config_.vel_gain = can_getSignal<float>(msg, 0, 32, true);
+    axis.controller_.config_.vel_integrator_gain = can_getSignal<float>(msg, 32, 32, true);
+}
+
+bool 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;
+
+    std::optional<float2D> Idq_setpoint = axis.motor_.current_control_.Idq_setpoint_;
+    if (!Idq_setpoint.has_value()) {
+        Idq_setpoint = {0.0f, 0.0f};
+    }
+    
+    static_assert(sizeof(float) == sizeof(Idq_setpoint->second));
+    static_assert(sizeof(float) == sizeof(axis.motor_.current_control_.Iq_measured_));
+    can_setSignal<float>(txmsg, Idq_setpoint->second, 0, 32, true);
+    can_setSignal<float>(txmsg, axis.motor_.current_control_.Iq_measured_, 32, 32, true);
+
+    return canbus_->send_message(txmsg);
+}
+
+bool CANSimple::get_bus_voltage_current_callback(const Axis& axis) {
+    can_Message_t txmsg;
+
+    txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS;
+    txmsg.id += MSG_GET_BUS_VOLTAGE_CURRENT;
+    txmsg.isExt = axis.config_.can.is_extended;
+    txmsg.len = 8;
+
+    static_assert(sizeof(float) == sizeof(vbus_voltage));
+    static_assert(sizeof(float) == sizeof(ibus_));
+    can_setSignal<float>(txmsg, vbus_voltage, 0, 32, true);
+    can_setSignal<float>(txmsg, ibus_, 32, 32, true);
+
+    return canbus_->send_message(txmsg);
+}
+
+bool CANSimple::get_adc_voltage_callback(const Axis& axis, const can_Message_t& msg) {
+    can_Message_t txmsg;
+
+    txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS;
+    txmsg.id += MSG_GET_ADC_VOLTAGE;
+    txmsg.isExt = axis.config_.can.is_extended;
+    txmsg.len = 8;
+
+    auto gpio_num = can_getSignal<uint8_t>(msg, 0, 8, true);
+    if (gpio_num < GPIO_COUNT) {
+        auto voltage = get_adc_voltage(get_gpio(gpio_num));
+        can_setSignal<float>(txmsg, voltage, 0, 32, true);
+        return canbus_->send_message(txmsg);
+    } else {
+        return false;
+    }
+}
+
+void CANSimple::clear_errors_callback(Axis& axis, const can_Message_t& msg) {
+    odrv.clear_errors();  // TODO: might want to clear axis errors only
+}
+
+uint32_t CANSimple::service_stack() {
+    uint32_t nextServiceTime = UINT32_MAX;
+    uint32_t now = HAL_GetTick();
+
+    // TODO: remove this polling loop and replace with protocol hook
+    for (size_t i = 0; i < AXIS_COUNT; ++i) {
+        bool node_id_changed = (axes[i].config_.can.node_id != node_ids_[i]) || (axes[i].config_.can.is_extended != extended_node_ids_[i]);
+        if (node_id_changed) {
+            renew_subscription(i);
+        }
+    }
+
+    struct periodic {
+        const uint32_t& rate;
+        uint32_t& last_time;
+        bool (CANSimple::* callback)(const Axis& axis);
+    };
+
+    for (auto& axis : axes) {
+        std::array<periodic, 10> periodics = {{
+            {axis.config_.can.heartbeat_rate_ms, axis.can_.last_heartbeat, &CANSimple::send_heartbeat},
+            {axis.config_.can.encoder_rate_ms, axis.can_.last_encoder, &CANSimple::get_encoder_estimates_callback},
+            {axis.config_.can.motor_error_rate_ms, axis.can_.last_motor_error, &CANSimple::get_motor_error_callback},
+            {axis.config_.can.encoder_error_rate_ms, axis.can_.last_encoder_error, &CANSimple::get_encoder_error_callback},
+            {axis.config_.can.controller_error_rate_ms, axis.can_.last_controller_error, &CANSimple::get_controller_error_callback},
+            {axis.config_.can.sensorless_error_rate_ms, axis.can_.last_sensorless_error, &CANSimple::get_sensorless_error_callback},
+            {axis.config_.can.encoder_count_rate_ms, axis.can_.last_encoder_count, &CANSimple::get_encoder_count_callback},
+            {axis.config_.can.iq_rate_ms, axis.can_.last_iq, &CANSimple::get_iq_callback},
+            {axis.config_.can.sensorless_rate_ms, axis.can_.last_sensorless, &CANSimple::get_sensorless_estimates_callback},
+            {axis.config_.can.bus_vi_rate_ms, axis.can_.last_bus_vi, &CANSimple::get_bus_voltage_current_callback},
+        }};
+
+        MEASURE_TIME(axis.task_times_.can_heartbeat) {
+            for (auto& msg : periodics) {
+                if (msg.rate > 0) {
+                    if ((now - msg.last_time) >= msg.rate) {
+                        if (std::invoke(msg.callback, this, axis)) {
+                            msg.last_time = now;
+                        }
+                    }
+
+                    int nextAxisService = msg.last_time + msg.rate - now;
+                    nextServiceTime = std::min(nextServiceTime, static_cast<uint32_t>(std::max(0, nextAxisService)));
+                }
+            }
+        }
+    }
+
+    return nextServiceTime;
+}
+
+bool CANSimple::send_heartbeat(const 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 = axis.config_.can.is_extended;
+    txmsg.len = 8;
+
+    can_setSignal(txmsg, axis.error_, 0, 32, true);
+    can_setSignal(txmsg, uint8_t(axis.current_state_), 32, 8, true);
+
+    // Motor flags
+    uint8_t motorFlags = axis.motor_.error_ != 0;
+
+    // Encoder flags
+    uint8_t encoderFlags = axis.encoder_.error_ != 0;
+
+    // Controller flags
+    uint8_t controllerFlags =axis.controller_.error_ != 0;
+    uint8_t trajDone = uint8_t(axis.controller_.trajectory_done_) << 7;
+    controllerFlags |= trajDone;
+
+    can_setSignal(txmsg, motorFlags, 40, 8, true);
+    can_setSignal(txmsg, encoderFlags, 48, 8, true);
+    can_setSignal(txmsg, controllerFlags, 56, 8, true);
+
+    return canbus_->send_message(txmsg);
+}

ODrive-fw-v0.5.6/Firmware/communication/can/can_simple.hpp

@@ -0,0 +1,113 @@
+#ifndef __CAN_SIMPLE_HPP_
+#define __CAN_SIMPLE_HPP_
+
+#include "canbus.hpp"
+#include "axis.hpp"
+
+class CANSimple {
+   public:
+    enum {
+        MSG_CO_NMT_CTRL = 0x000,  // CANOpen NMT Message REC
+        MSG_ODRIVE_HEARTBEAT,
+        MSG_ODRIVE_ESTOP,
+        MSG_GET_MOTOR_ERROR,  // Errors
+        MSG_GET_ENCODER_ERROR,
+        MSG_GET_SENSORLESS_ERROR,
+        MSG_SET_AXIS_NODE_ID,
+        MSG_SET_AXIS_REQUESTED_STATE,
+        MSG_SET_AXIS_STARTUP_CONFIG,
+        MSG_GET_ENCODER_ESTIMATES,
+        MSG_GET_ENCODER_COUNT,
+        MSG_SET_CONTROLLER_MODES,
+        MSG_SET_INPUT_POS,
+        MSG_SET_INPUT_VEL,
+        MSG_SET_INPUT_TORQUE,
+        MSG_SET_LIMITS,
+        MSG_START_ANTICOGGING,
+        MSG_SET_TRAJ_VEL_LIMIT,
+        MSG_SET_TRAJ_ACCEL_LIMITS,
+        MSG_SET_TRAJ_INERTIA,
+        MSG_GET_IQ,
+        MSG_GET_SENSORLESS_ESTIMATES,
+        MSG_RESET_ODRIVE,
+        MSG_GET_BUS_VOLTAGE_CURRENT,
+        MSG_CLEAR_ERRORS,
+        MSG_SET_LINEAR_COUNT,
+        MSG_SET_POS_GAIN,
+        MSG_SET_VEL_GAINS,
+        MSG_GET_ADC_VOLTAGE,
+        MSG_GET_CONTROLLER_ERROR,
+        MSG_CO_HEARTBEAT_CMD = 0x700,  // CANOpen NMT Heartbeat  SEND
+    };
+
+    CANSimple(CanBusBase* canbus) : canbus_(canbus) {}
+
+    bool init();
+    uint32_t service_stack();
+
+   private:
+
+    bool renew_subscription(size_t i);
+    bool send_heartbeat(const Axis& axis);
+
+    void handle_can_message(const can_Message_t& msg);
+
+    void do_command(Axis& axis, const can_Message_t& cmd);
+    
+    // Get functions (msg.rtr bit must be set)
+    bool get_motor_error_callback(const Axis& axis);
+    bool get_encoder_error_callback(const Axis& axis);
+    bool get_controller_error_callback(const Axis& axis);
+    bool get_sensorless_error_callback(const Axis& axis);
+    bool get_encoder_estimates_callback(const Axis& axis);
+    bool get_encoder_count_callback(const Axis& axis);
+    bool get_iq_callback(const Axis& axis);
+    bool get_sensorless_estimates_callback(const Axis& axis);
+    bool get_bus_voltage_current_callback(const Axis& axis);
+    // msg.rtr bit must NOT be set
+    bool get_adc_voltage_callback(const Axis& axis, const can_Message_t& msg);
+
+    // 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 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_limits_callback(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 set_linear_count_callback(Axis& axis, const can_Message_t& msg);
+    static void set_pos_gain_callback(Axis& axis, const can_Message_t& msg);
+    static void set_vel_gains_callback(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 constexpr uint32_t get_node_id(uint32_t msgID) {
+        return (msgID >> NUM_CMD_ID_BITS);  // Upper 6 or more bits
+    };
+
+    static constexpr uint8_t get_cmd_id(uint32_t msgID) {
+        return (msgID & 0x01F);  // Bottom 5 bits
+    }
+
+    CanBusBase* canbus_;
+    CanBusBase::CanSubscription* subscription_handles_[AXIS_COUNT];
+
+    // TODO: we this is a hack but actually we should use protocol hooks to
+    // renew our filter when the node ID changes
+    uint32_t node_ids_[AXIS_COUNT];
+    bool extended_node_ids_[AXIS_COUNT];
+};
+
+#endif

ODrive-fw-v0.5.6/Firmware/communication/can/canbus.hpp

@@ -0,0 +1,43 @@
+#ifndef __CANBUS_HPP
+#define __CANBUS_HPP
+
+#include "can_helpers.hpp"
+#include <variant>
+
+struct MsgIdFilterSpecs {
+    std::variant<uint16_t, uint32_t> id;
+    uint32_t mask;
+};
+
+class CanBusBase {
+public:
+    typedef void(*on_can_message_cb_t)(void* ctx, const can_Message_t& message);
+    struct CanSubscription {};
+
+    /**
+     * @brief Sends the specified CAN message.
+     * 
+     * @returns: true on success or false otherwise (e.g. if the send queue is
+     * full).
+     */
+    virtual bool send_message(const can_Message_t& message) = 0;
+
+    /**
+     * @brief Registers a callback that will be invoked for every incoming CAN
+     * message that matches the filter.
+     * 
+     * @param handle: On success this handle is set to an opaque pointer that
+     *        can be used to cancel the subscription.
+     * 
+     * @returns: true on success or false otherwise (e.g. if the maximum number
+     * of subscriptions has been reached).
+     */
+    virtual bool subscribe(const MsgIdFilterSpecs& filter, on_can_message_cb_t callback, void* ctx, CanSubscription** handle) = 0;
+
+    /**
+     * @brief Deregisters a callback that was previously registered with subscribe().
+     */
+    virtual bool unsubscribe(CanSubscription* handle) = 0;
+};
+
+#endif // __CANBUS_HPP

ODrive-fw-v0.5.6/Firmware/communication/can/odrive_can.cpp

@@ -0,0 +1,238 @@
+#include "odrive_can.hpp"
+
+#include <can.h>
+#include <cmsis_os.h>
+
+#include "freertos_vars.h"
+#include "utils.hpp"
+
+// Safer context handling via maps instead of arrays
+// #include <unordered_map>
+// std::unordered_map<CAN_HandleTypeDef *, ODriveCAN *> ctxMap;
+
+
+bool ODriveCAN::apply_config() {
+    config_.parent = this;
+    set_baud_rate(config_.baud_rate);
+    return true;
+}
+
+bool ODriveCAN::reinit() {
+    HAL_CAN_Stop(handle_);
+    HAL_CAN_ResetError(handle_);
+    return (HAL_CAN_Init(handle_) == HAL_OK)
+        && (HAL_CAN_Start(handle_) == HAL_OK)
+        && (HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_RX_FIFO1_MSG_PENDING | CAN_IT_TX_MAILBOX_EMPTY) == HAL_OK);
+}
+
+bool ODriveCAN::start_server(CAN_HandleTypeDef* handle) {
+    handle_ = handle;
+
+    handle_->Init.Prescaler = CAN_FREQ / config_.baud_rate;
+    if (!reinit()) {
+        return false;
+    }
+
+    auto wrapper = [](void* ctx) {
+        ((ODriveCAN*)ctx)->can_server_thread();
+    };
+    osThreadDef(can_server_thread_def, wrapper, osPriorityNormal, 0, stack_size_ / sizeof(StackType_t));
+    thread_id_ = osThreadCreate(osThread(can_server_thread_def), this);
+
+    return true;
+}
+
+void ODriveCAN::can_server_thread() {
+    Protocol protocol = config_.protocol;
+
+    if (protocol & PROTOCOL_SIMPLE) {
+        can_simple_.init();
+    }
+
+    for (;;) {
+        uint32_t status = HAL_CAN_GetError(handle_);
+        if (status == HAL_CAN_ERROR_NONE) {
+            uint32_t next_service_time = UINT32_MAX;
+
+            if (protocol & PROTOCOL_SIMPLE) {
+                next_service_time = std::min(can_simple_.service_stack(), next_service_time);
+            }
+
+            process_rx_fifo(CAN_RX_FIFO0);
+            process_rx_fifo(CAN_RX_FIFO1);
+            HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_RX_FIFO1_MSG_PENDING | CAN_IT_TX_MAILBOX_EMPTY);
+
+            // wait at least 1ms to prevent busy-spin on failed sends
+            osSemaphoreWait(sem_can, std::max(next_service_time, 1UL));
+        } else if (status == HAL_CAN_ERROR_TIMEOUT) {
+            HAL_CAN_ResetError(handle_);
+            status = HAL_CAN_Start(handle_);
+            if (status == HAL_OK)
+                status = HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_TX_MAILBOX_EMPTY);
+        }
+    }
+}
+
+// Set one of only a few common baud rates.  CAN doesn't do arbitrary baud rates well due to the time-quanta issue.
+// 21 TQ allows for easy sampling at exactly 80% (recommended by Vector Informatik GmbH for high reliability systems)
+// Conveniently, the CAN peripheral's 42MHz clock lets us easily create 21TQs for all common baud rates
+bool ODriveCAN::set_baud_rate(uint32_t baud_rate) {
+    uint32_t prescaler = CAN_FREQ / baud_rate;
+    if (prescaler * baud_rate == CAN_FREQ) {
+        // valid baud rate
+        config_.baud_rate = baud_rate;
+        if (handle_) {
+            handle_->Init.Prescaler = prescaler;
+            return reinit();
+        }
+        return true;
+    } else {
+        // invalid baud rate - ignore
+        return false;
+    }
+}
+
+void ODriveCAN::process_rx_fifo(uint32_t fifo) {
+    while (HAL_CAN_GetRxFifoFillLevel(handle_, fifo)) {
+        CAN_RxHeaderTypeDef header;
+        can_Message_t rxmsg;
+        HAL_CAN_GetRxMessage(handle_, fifo, &header, rxmsg.buf);
+
+        rxmsg.isExt = header.IDE;
+        rxmsg.id = rxmsg.isExt ? header.ExtId : header.StdId;  // If it's an extended message, pass the extended ID
+        rxmsg.len = header.DLC;
+        rxmsg.rtr = header.RTR;
+
+        // TODO: this could be optimized with an ahead-of-time computed
+        // index-to-filter map
+
+        size_t fifo0_idx = 0;
+        size_t fifo1_idx = 0;
+
+        // Find the triggered subscription item based on header.FilterMatchIndex
+        auto it = std::find_if(subscriptions_.begin(), subscriptions_.end(), [&](auto& s) {
+            size_t current_idx = (s.fifo == 0 ? fifo0_idx : fifo1_idx)++;
+            return (header.FilterMatchIndex == current_idx) && (s.fifo == fifo);
+        });
+
+        if (it == subscriptions_.end()) {
+            continue;
+        }
+
+        it->callback(it->ctx, rxmsg);
+    }
+}
+
+// Send a CAN message on the bus
+bool ODriveCAN::send_message(const can_Message_t &txmsg) {
+    if (HAL_CAN_GetError(handle_) != HAL_CAN_ERROR_NONE) {
+        return false;
+    }
+
+    CAN_TxHeaderTypeDef header;
+    header.StdId = txmsg.id;
+    header.ExtId = txmsg.id;
+    header.IDE = txmsg.isExt ? CAN_ID_EXT : CAN_ID_STD;
+    header.RTR = CAN_RTR_DATA;
+    header.DLC = txmsg.len;
+    header.TransmitGlobalTime = FunctionalState::DISABLE;
+
+    uint32_t retTxMailbox = 0;
+    if (!HAL_CAN_GetTxMailboxesFreeLevel(handle_)) {
+        return false;
+    }
+    
+    return HAL_CAN_AddTxMessage(handle_, &header, (uint8_t*)txmsg.buf, &retTxMailbox) == HAL_OK;
+}
+
+//void ODriveCAN::set_error(Error error) {
+//    error_ |= error;
+//}
+
+bool ODriveCAN::subscribe(const MsgIdFilterSpecs& filter, on_can_message_cb_t callback, void* ctx, CanSubscription** handle) {
+    auto it = std::find_if(subscriptions_.begin(), subscriptions_.end(), [](auto& subscription) {
+        return subscription.fifo == kCanFifoNone;
+    });
+
+    if (it == subscriptions_.end()) {
+        return false; // all subscription slots in use
+    }
+
+    it->callback = callback;
+    it->ctx = ctx;
+    it->fifo = CAN_RX_FIFO0; // TODO: make customizable
+    if (handle) {
+        *handle = &*it;
+    }
+
+    bool is_extended = filter.id.index() == 1;
+    uint32_t id = is_extended ?
+                  ((std::get<1>(filter.id) << 3) | (1 << 2)) :
+                  (std::get<0>(filter.id) << 21);
+    uint32_t mask = (is_extended ? (filter.mask << 3) : (filter.mask << 21))
+                  | (1 << 2); // care about the is_extended bit
+
+    CAN_FilterTypeDef hal_filter;
+    hal_filter.FilterActivation = ENABLE;
+    hal_filter.FilterBank = &*it - &subscriptions_[0];
+    hal_filter.FilterFIFOAssignment = it->fifo;
+    hal_filter.FilterIdHigh = (id >> 16) & 0xffff;
+    hal_filter.FilterIdLow = id & 0xffff;
+    hal_filter.FilterMaskIdHigh = (mask >> 16) & 0xffff;
+    hal_filter.FilterMaskIdLow = mask & 0xffff;
+    hal_filter.FilterMode = CAN_FILTERMODE_IDMASK;
+    hal_filter.FilterScale = CAN_FILTERSCALE_32BIT;
+
+    if (HAL_CAN_ConfigFilter(handle_, &hal_filter) != HAL_OK) {
+        return false;
+    }
+    return true;
+}
+
+bool ODriveCAN::unsubscribe(CanSubscription* handle) {
+    ODriveCanSubscription* subscription = static_cast<ODriveCanSubscription*>(handle);
+    if (subscription < subscriptions_.begin() || subscription >= subscriptions_.end()) {
+        return false;
+    }
+    if (subscription->fifo != kCanFifoNone) {
+        return false; // not in use
+    }
+
+    subscription->fifo = kCanFifoNone;
+
+    CAN_FilterTypeDef hal_filter = {};
+    hal_filter.FilterActivation = DISABLE;
+    return HAL_CAN_ConfigFilter(handle_, &hal_filter) == HAL_OK;
+}
+
+void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) {
+    HAL_CAN_DeactivateNotification(hcan, CAN_IT_TX_MAILBOX_EMPTY);
+    osSemaphoreRelease(sem_can);
+}
+void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) {
+    HAL_CAN_DeactivateNotification(hcan, CAN_IT_TX_MAILBOX_EMPTY);
+    osSemaphoreRelease(sem_can);
+}
+void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) {
+    HAL_CAN_DeactivateNotification(hcan, CAN_IT_TX_MAILBOX_EMPTY);
+    osSemaphoreRelease(sem_can);
+}
+void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef *hcan) {}
+void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef *hcan) {}
+void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef *hcan) {}
+void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
+    HAL_CAN_DeactivateNotification(hcan, CAN_IT_RX_FIFO0_MSG_PENDING);
+    osSemaphoreRelease(sem_can);
+}
+void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef *hcan) {
+    HAL_CAN_DeactivateNotification(hcan, CAN_IT_RX_FIFO1_MSG_PENDING);
+    osSemaphoreRelease(sem_can);
+}
+void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) {}
+void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef *hcan) {}
+void HAL_CAN_SleepCallback(CAN_HandleTypeDef *hcan) {}
+void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef *hcan) {}
+
+void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan) {
+    //HAL_CAN_ResetError(hcan);
+}

ODrive-fw-v0.5.6/Firmware/communication/can/odrive_can.hpp

@@ -0,0 +1,68 @@
+#ifndef __ODRIVE_CAN_HPP
+#define __ODRIVE_CAN_HPP
+
+#include <cmsis_os.h>
+
+#include "canbus.hpp"
+#include "can_simple.hpp"
+#include <autogen/interfaces.hpp>
+
+#define CAN_CLK_HZ (42000000)
+#define CAN_CLK_MHZ (42)
+
+// Anonymous enum for defining the most common CAN baud rates
+enum {
+    CAN_BAUD_125K = 125000,
+    CAN_BAUD_250K = 250000,
+    CAN_BAUD_500K = 500000,
+    CAN_BAUD_1000K = 1000000,
+    CAN_BAUD_1M = 1000000
+};
+
+class ODriveCAN : public CanBusBase, public ODriveIntf::CanIntf {
+public:
+    struct Config_t {
+        uint32_t baud_rate = CAN_BAUD_250K;
+        Protocol protocol = PROTOCOL_SIMPLE;
+
+        ODriveCAN* parent = nullptr; // set in apply_config()
+        void set_baud_rate(uint32_t value) { parent->set_baud_rate(value); }
+    };
+
+    ODriveCAN() {}
+
+    bool apply_config();
+    bool start_server(CAN_HandleTypeDef* handle);
+
+    Error error_ = ERROR_NONE;
+
+    Config_t config_;
+    CANSimple can_simple_{this};
+
+    osThreadId thread_id_;
+    const uint32_t stack_size_ = 1024;  // Bytes
+
+private:
+    static const uint8_t kCanFifoNone = 0xff;
+
+    struct ODriveCanSubscription : CanSubscription {
+        uint8_t fifo = kCanFifoNone;
+        on_can_message_cb_t callback;
+        void* ctx;
+    };
+
+    bool reinit();
+    void can_server_thread();
+    bool set_baud_rate(uint32_t baud_rate);
+    void process_rx_fifo(uint32_t fifo);
+    bool send_message(const can_Message_t& message) final;
+    bool subscribe(const MsgIdFilterSpecs& filter, on_can_message_cb_t callback, void* ctx, CanSubscription** handle) final;
+    bool unsubscribe(CanSubscription* handle) final;
+
+    // Hardware supports at most 28 filters unless we do optimizations. For now
+    // we don't need that many.
+    std::array<ODriveCanSubscription, 8> subscriptions_;
+    CAN_HandleTypeDef *handle_ = nullptr;
+};
+
+#endif  // __ODRIVE_CAN_HPP