l-proger/MotorDriver.Research
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

*

Browse Source
This commit is contained in:
l-proger
2025-05-13 01:34:53 +03:00
parent 427735e23d
commit 83f3f1c7d4
945 changed files with 633484 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ODrive-fw-v0.5.6/Firmware/communication/ascii_protocol.cpp
+454
View File
@@ -0,0 +1,454 @@
/*
* The ASCII protocol is a simpler, human readable alternative to the main native
* protocol.
* In the future this protocol might be extended to support selected GCode commands.
* For a list of supported commands see doc/ascii-protocol.md
*/
/* Includes ------------------------------------------------------------------*/
#include "odrive_main.h"
#include "communication.h"
#include "ascii_protocol.hpp"
#include <utils.hpp>
#include <fibre/cpp_utils.hpp>
#include "autogen/type_info.hpp"
#include "communication/interface_can.hpp"
using namespace fibre;
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
/* Private constant data -----------------------------------------------------*/
#define TO_STR_INNER(s) #s
#define TO_STR(s) TO_STR_INNER(s)
/* Private variables ---------------------------------------------------------*/
#if HW_VERSION_MAJOR == 3
static Introspectable root_obj = ODrive3TypeInfo<ODrive>::make_introspectable(odrv);
#elif HW_VERSION_MAJOR == 4
static Introspectable root_obj = ODrive4TypeInfo<ODrive>::make_introspectable(odrv);
#endif
/* Private function prototypes -----------------------------------------------*/
/* Function implementations --------------------------------------------------*/
// @brief Sends a line on the specified output.
template<typename ... TArgs>
void AsciiProtocol::respond(bool include_checksum, const char * fmt, TArgs&& ... args) {
char tx_buf[64];
size_t len = snprintf(tx_buf, sizeof(tx_buf), fmt, std::forward<TArgs>(args)...);
// Silently truncate the output if it's too long for the buffer.
len = std::min(len, sizeof(tx_buf));
if (include_checksum) {
uint8_t checksum = 0;
for (size_t i = 0; i < len; ++i)
checksum ^= tx_buf[i];
len += snprintf(tx_buf + len, sizeof(tx_buf) - len, "*%u\r\n", checksum);
} else {
len += snprintf(tx_buf + len, sizeof(tx_buf) - len, "\r\n");
}
// Silently truncate the output if it's too long for the buffer.
len = std::min(len, sizeof(tx_buf));
sink_.write({(const uint8_t*)tx_buf, len});
sink_.maybe_start_async_write();
}
// @brief Executes an ASCII protocol command
// @param buffer buffer of ASCII encoded characters
// @param len size of the buffer
void AsciiProtocol::process_line(cbufptr_t buffer) {
static_assert(sizeof(char) == sizeof(uint8_t));
// scan line to find beginning of checksum and prune comment
uint8_t checksum = 0;
size_t checksum_start = SIZE_MAX;
for (size_t i = 0; i < buffer.size(); ++i) {
if (buffer.begin()[i] == ';') { // ';' is the comment start char
buffer = buffer.take(i);
break;
}
if (checksum_start > i) {
if (buffer[i] == '*') {
checksum_start = i + 1;
} else {
checksum ^= buffer[i];
}
}
}
// copy everything into a local buffer so we can insert null-termination
char cmd[MAX_LINE_LENGTH + 1];
size_t len = std::min(buffer.size(), MAX_LINE_LENGTH);
memcpy(cmd, buffer.begin(), len);
cmd[len] = 0; // null-terminate
// optional checksum validation
bool use_checksum = (checksum_start < len);
if (use_checksum) {
unsigned int received_checksum;
int numscan = sscanf(&cmd[checksum_start], "%u", &received_checksum);
if ((numscan < 1) || (received_checksum != checksum))
return;
len = checksum_start - 1; // prune checksum and asterisk
cmd[len] = 0; // null-terminate
}
// check incoming packet type
switch(cmd[0]) {
case 'p': cmd_set_position(cmd, use_checksum); break; // position control
case 'q': cmd_set_position_wl(cmd, use_checksum); break; // position control with limits
case 'v': cmd_set_velocity(cmd, use_checksum); break; // velocity control
case 'c': cmd_set_torque(cmd, use_checksum); break; // current control
case 't': cmd_set_trapezoid_trajectory(cmd, use_checksum); break; // trapezoidal trajectory
case 'f': cmd_get_feedback(cmd, use_checksum); break; // feedback
case 'h': cmd_help(cmd, use_checksum); break; // Help
case 'i': cmd_info_dump(cmd, use_checksum); break; // Dump device info
case 's': cmd_system_ctrl(cmd, use_checksum); break; // System
case 'r': cmd_read_property(cmd, use_checksum); break; // read property
case 'w': cmd_write_property(cmd, use_checksum); break; // write property
case 'u': cmd_update_axis_wdg(cmd, use_checksum); break; // Update axis watchdog.
case 'e': cmd_encoder(cmd, use_checksum); break; // Encoder commands
default : cmd_unknown(nullptr, use_checksum); break;
}
}
// @brief Executes the set position command
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void AsciiProtocol::cmd_set_position(char * pStr, bool use_checksum) {
unsigned motor_number;
float pos_setpoint, vel_feed_forward, torque_feed_forward;
int numscan = sscanf(pStr, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &torque_feed_forward);
if (numscan < 2) {
respond(use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(use_checksum, "invalid motor %u", motor_number);
} else {
Axis& axis = axes[motor_number];
axis.controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL;
axis.controller_.input_pos_ = pos_setpoint;
if (numscan >= 3)
axis.controller_.input_vel_ = vel_feed_forward;
if (numscan >= 4)
axis.controller_.input_torque_ = torque_feed_forward;
axis.controller_.input_pos_updated();
axis.watchdog_feed();
}
}
// @brief Executes the set position with current and velocity limit command
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void AsciiProtocol::cmd_set_position_wl(char * pStr, bool use_checksum) {
unsigned motor_number;
float pos_setpoint, vel_limit, torque_lim;
int numscan = sscanf(pStr, "q %u %f %f %f", &motor_number, &pos_setpoint, &vel_limit, &torque_lim);
if (numscan < 2) {
respond(use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(use_checksum, "invalid motor %u", motor_number);
} else {
Axis& axis = axes[motor_number];
axis.controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL;
axis.controller_.input_pos_ = pos_setpoint;
if (numscan >= 3)
axis.controller_.config_.vel_limit = vel_limit;
if (numscan >= 4)
axis.motor_.config_.torque_lim = torque_lim;
axis.controller_.input_pos_updated();
axis.watchdog_feed();
}
}
// @brief Executes the set velocity command
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void AsciiProtocol::cmd_set_velocity(char * pStr, bool use_checksum) {
unsigned motor_number;
float vel_setpoint, torque_feed_forward;
int numscan = sscanf(pStr, "v %u %f %f", &motor_number, &vel_setpoint, &torque_feed_forward);
if (numscan < 2) {
respond(use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(use_checksum, "invalid motor %u", motor_number);
} else {
Axis& axis = axes[motor_number];
axis.controller_.config_.control_mode = Controller::CONTROL_MODE_VELOCITY_CONTROL;
axis.controller_.input_vel_ = vel_setpoint;
if (numscan >= 3)
axis.controller_.input_torque_ = torque_feed_forward;
axis.watchdog_feed();
}
}
// @brief Executes the set torque control command
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void AsciiProtocol::cmd_set_torque(char * pStr, bool use_checksum) {
unsigned motor_number;
float torque_setpoint;
if (sscanf(pStr, "c %u %f", &motor_number, &torque_setpoint) < 2) {
respond(use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(use_checksum, "invalid motor %u", motor_number);
} else {
Axis& axis = axes[motor_number];
axis.controller_.config_.control_mode = Controller::CONTROL_MODE_TORQUE_CONTROL;
axis.controller_.input_torque_ = torque_setpoint;
axis.watchdog_feed();
}
}
// @brief Sets the encoder linear count
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void AsciiProtocol::cmd_encoder(char * pStr, bool use_checksum) {
if (pStr[1] == 's') {
pStr += 2; // Substring two characters to the right (ok because we have guaranteed null termination after all chars)
unsigned motor_number;
int encoder_count;
if (sscanf(pStr, "l %u %i", &motor_number, &encoder_count) < 2) {
respond(use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(use_checksum, "invalid motor %u", motor_number);
} else {
Axis& axis = axes[motor_number];
axis.encoder_.set_linear_count(encoder_count);
axis.watchdog_feed();
respond(use_checksum, "encoder set to %u", encoder_count);
}
} else {
respond(use_checksum, "invalid command format");
}
}
// @brief Executes the set trapezoid trajectory command
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void AsciiProtocol::cmd_set_trapezoid_trajectory(char* pStr, bool use_checksum) {
unsigned motor_number;
float goal_point;
if (sscanf(pStr, "t %u %f", &motor_number, &goal_point) < 2) {
respond(use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(use_checksum, "invalid motor %u", motor_number);
} else {
Axis& axis = axes[motor_number];
axis.controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ;
axis.controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL;
axis.controller_.input_pos_ = goal_point;
axis.controller_.input_pos_updated();
axis.watchdog_feed();
}
}
// @brief Executes the get position and velocity feedback command
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void AsciiProtocol::cmd_get_feedback(char * pStr, bool use_checksum) {
unsigned motor_number;
if (sscanf(pStr, "f %u", &motor_number) < 1) {
respond(use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(use_checksum, "invalid motor %u", motor_number);
} else {
Axis& axis = axes[motor_number];
respond(use_checksum, "%f %f",
(double)axis.encoder_.pos_estimate_.any().value_or(0.0f),
(double)axis.encoder_.vel_estimate_.any().value_or(0.0f));
}
}
// @brief Shows help text
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void AsciiProtocol::cmd_help(char * pStr, bool use_checksum) {
(void)pStr;
respond(use_checksum, "Please see documentation for more details");
respond(use_checksum, "");
respond(use_checksum, "Available commands syntax reference:");
respond(use_checksum, "Position: q axis pos vel-lim I-lim");
respond(use_checksum, "Position: p axis pos vel-ff I-ff");
respond(use_checksum, "Velocity: v axis vel I-ff");
respond(use_checksum, "Torque: c axis T");
respond(use_checksum, "");
respond(use_checksum, "Properties start at odrive root, such as axis0.requested_state");
respond(use_checksum, "Read: r property");
respond(use_checksum, "Write: w property value");
respond(use_checksum, "");
respond(use_checksum, "Save config: ss");
respond(use_checksum, "Erase config: se");
respond(use_checksum, "Reboot: sr");
}
// @brief Gets the hardware, firmware and serial details
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void AsciiProtocol::cmd_info_dump(char * pStr, bool use_checksum) {
// respond(use_checksum, "Signature: %#x", STM_ID_GetSignature());
// respond(use_checksum, "Revision: %#x", STM_ID_GetRevision());
// respond(use_checksum, "Flash Size: %#x KiB", STM_ID_GetFlashSize());
respond(use_checksum, "Hardware version: %d.%d-%dV", odrv.hw_version_major_, odrv.hw_version_minor_, odrv.hw_version_variant_);
respond(use_checksum, "Firmware version: %d.%d.%d", odrv.fw_version_major_, odrv.fw_version_minor_, odrv.fw_version_revision_);
respond(use_checksum, "Serial number: %s", serial_number_str);
}
// @brief Executes the system control command
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void AsciiProtocol::cmd_system_ctrl(char * pStr, bool use_checksum) {
switch (pStr[1])
{
case 's': odrv.save_configuration(); break; // Save config
case 'e': odrv.erase_configuration(); break; // Erase config
case 'r': odrv.reboot(); break; // Reboot
case 'c': odrv.clear_errors(); break; // clear all errors and rearm brake resistor if necessary
default: /* default */ break;
}
}
// @brief Executes the read parameter command
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void AsciiProtocol::cmd_read_property(char * pStr, bool use_checksum) {
char name[MAX_LINE_LENGTH];
if (sscanf(pStr, "r %255s", name) < 1) {
respond(use_checksum, "invalid command format");
} else {
Introspectable property = root_obj.get_child(name, sizeof(name));
const StringConvertibleTypeInfo* type_info = dynamic_cast<const StringConvertibleTypeInfo*>(property.get_type_info());
if (!type_info) {
respond(use_checksum, "invalid property");
} else {
char response[10];
bool success = type_info->get_string(property, response, sizeof(response));
respond(use_checksum, success ? response : "not implemented");
}
}
}
// @brief Executes the set write position command
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void AsciiProtocol::cmd_write_property(char * pStr, bool use_checksum) {
char name[MAX_LINE_LENGTH];
char value[MAX_LINE_LENGTH];
if (sscanf(pStr, "w %255s %255s", name, value) < 1) {
respond(use_checksum, "invalid command format");
} else {
Introspectable property = root_obj.get_child(name, sizeof(name));
const StringConvertibleTypeInfo* type_info = dynamic_cast<const StringConvertibleTypeInfo*>(property.get_type_info());
if (!type_info) {
respond(use_checksum, "invalid property");
} else {
bool success = type_info->set_string(property, value, sizeof(value));
if (!success) {
respond(use_checksum, "not implemented");
}
}
}
}
// @brief Executes the motor watchdog update command
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void AsciiProtocol::cmd_update_axis_wdg(char * pStr, bool use_checksum) {
unsigned motor_number;
if (sscanf(pStr, "u %u", &motor_number) < 1) {
respond(use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(use_checksum, "invalid motor %u", motor_number);
} else {
axes[motor_number].watchdog_feed();
}
}
// @brief Sends the unknown command response
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void AsciiProtocol::cmd_unknown(char * pStr, bool use_checksum) {
(void)pStr;
respond(use_checksum, "unknown command");
}
void AsciiProtocol::on_read_finished(ReadResult result) {
if (result.status != kStreamOk) {
return;
}
for (;;) {
uint8_t* end_of_line = std::find_if(rx_buf_, result.end, [](uint8_t c) {
return c == '\r' || c == '\n' || c == '!';
});
if (end_of_line >= result.end) {
break;
}
if (read_active_) {
process_line({rx_buf_, end_of_line});
} else {
// Ignoring this line cause it didn't start at a new-line character
read_active_ = true;
}
// Discard the processed bytes and shift the remainder to the beginning of the buffer
size_t n_remaining = result.end - end_of_line - 1;
memmove(rx_buf_, end_of_line + 1, n_remaining);
result.end = rx_buf_ + n_remaining;
}
// No more new-line characters in buffer
if (result.end >= rx_buf_ + sizeof(rx_buf_)) {
// If the line becomes too long, reset buffer and wait for the next line
result.end = rx_buf_;
read_active_ = false;
}
TransferHandle dummy;
rx_channel_->start_read({result.end, rx_buf_ + sizeof(rx_buf_)}, &dummy, MEMBER_CB(this, on_read_finished));
}
void AsciiProtocol::start() {
TransferHandle dummy;
rx_channel_->start_read(rx_buf_, &dummy, MEMBER_CB(this, on_read_finished));
}
ODrive-fw-v0.5.6/Firmware/communication/ascii_protocol.hpp
+46
View File
@@ -0,0 +1,46 @@
#ifndef __ASCII_PROTOCOL_HPP
#define __ASCII_PROTOCOL_HPP
#include <fibre/async_stream.hpp>
#include <fibre/../../stream_utils.hpp>
#define MAX_LINE_LENGTH ((size_t)256)
class AsciiProtocol {
public:
AsciiProtocol(fibre::AsyncStreamSource* rx_channel, fibre::AsyncStreamSink* tx_channel)
: rx_channel_(rx_channel), sink_(*tx_channel) {}
void start();
private:
void cmd_set_position(char * pStr, bool use_checksum);
void cmd_set_position_wl(char * pStr, bool use_checksum);
void cmd_set_velocity(char * pStr, bool use_checksum);
void cmd_set_torque(char * pStr, bool use_checksum);
void cmd_set_trapezoid_trajectory(char * pStr, bool use_checksum);
void cmd_get_feedback(char * pStr, bool use_checksum);
void cmd_help(char * pStr, bool use_checksum);
void cmd_info_dump(char * pStr, bool use_checksum);
void cmd_system_ctrl(char * pStr, bool use_checksum);
void cmd_read_property(char * pStr, bool use_checksum);
void cmd_write_property(char * pStr, bool use_checksum);
void cmd_update_axis_wdg(char * pStr, bool use_checksum);
void cmd_unknown(char * pStr, bool use_checksum);
void cmd_encoder(char * pStr, bool use_checksum);
template<typename ... TArgs> void respond(bool include_checksum, const char * fmt, TArgs&& ... args);
void process_line(fibre::cbufptr_t buffer);
void on_write_finished(fibre::WriteResult result);
void on_read_finished(fibre::ReadResult result);
fibre::AsyncStreamSource* rx_channel_ = nullptr;
uint8_t* rx_end_ = nullptr; // non-zero if an RX operation has finished but wasn't handled yet because the TX channel was busy
uint8_t rx_buf_[MAX_LINE_LENGTH];
bool read_active_ = true;
fibre::BufferedStreamSink<512> sink_;
};
#endif // __ASCII_PROTOCOL_HPP
ODrive-fw-v0.5.6/Firmware/communication/can/can_helpers.hpp
+97
View File
@@ -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
+467
View File
@@ -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
+113
View File
@@ -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
+43
View File
@@ -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
+238
View File
@@ -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
+68
View File
@@ -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
ODrive-fw-v0.5.6/Firmware/communication/communication.cpp
+99
View File
@@ -0,0 +1,99 @@
/* Includes ------------------------------------------------------------------*/
#include "communication.h"
#include "interface_usb.h"
#include "interface_uart.h"
#include "interface_can.hpp"
#include "interface_i2c.h"
#include "odrive_main.h"
#include "freertos_vars.h"
#include "utils.hpp"
#include <cmsis_os.h>
#include <memory>
//#include <usbd_cdc_if.h>
//#include <usb_device.h>
//#include <usart.h>
#include <gpio.h>
#include <type_traits>
/* Private defines -----------------------------------------------------------*/
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
uint64_t serial_number;
char serial_number_str[13]; // 12 digits + null termination
/* Private constant data -----------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/* Function implementations --------------------------------------------------*/
void init_communication(void) {
//printf("hi!\r\n");
// Dual UART operation not supported yet
if (odrv.config_.enable_uart_a && odrv.config_.enable_uart_b) {
odrv.misconfigured_ = true;
}
if (odrv.config_.enable_uart_a && uart_a) {
start_uart_server(uart_a);
} else if (odrv.config_.enable_uart_b && uart_b) {
start_uart_server(uart_b);
}
start_usb_server();
if (odrv.config_.enable_i2c_a) {
start_i2c_server();
}
if (odrv.config_.enable_can_a) {
odrv.can_.start_server(&hcan1);
}
}
#include <fibre/async_stream.hpp>
extern "C" {
int _write(int file, const char* data, int len) __attribute__((used));
}
// @brief This is what printf calls internally
int _write(int file, const char* data, int len) {
fibre::cbufptr_t buf{(const uint8_t*)data, (const uint8_t*)data + len};
if (odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_STDOUT ||
odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT) {
uart0_stdout_sink.write(buf);
if (!uart0_stdout_pending) {
uart0_stdout_pending = true;
osMessagePut(uart_event_queue, 3, 0);
}
}
if (odrv.config_.usb_cdc_protocol == ODrive::STREAM_PROTOCOL_TYPE_STDOUT ||
odrv.config_.usb_cdc_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT) {
usb_cdc_stdout_sink.write(buf);
if (!usb_cdc_stdout_pending) {
usb_cdc_stdout_pending = true;
osMessagePut(usb_event_queue, 7, 0);
}
}
return len; // Always pretend that we processed everything
}
#include "../autogen/function_stubs.hpp"
ODrive& ep_root = odrv;
#include "../autogen/endpoints.hpp"
ODrive-fw-v0.5.6/Firmware/communication/communication.h
+23
View File
@@ -0,0 +1,23 @@
#ifndef COMMANDS_H
#define COMMANDS_H
// TODO: resolve assert
#define assert(expr)
#ifdef __cplusplus
#include <functional>
#include <limits>
extern "C" {
#endif
#include <cmsis_os.h>
void init_communication(void);
#ifdef __cplusplus
}
#endif
#endif /* COMMANDS_H */
ODrive-fw-v0.5.6/Firmware/communication/interface_can.hpp
+10
View File
@@ -0,0 +1,10 @@
#ifndef __INTERFACE_CAN_HPP
#define __INTERFACE_CAN_HPP
//#include <cmsis_os.h>
//#include "odrive_main.h"
//#include "can_helpers.hpp"
//#include <communication/can/can_simple.hpp>
//// Other protocol implementations here
#endif
ODrive-fw-v0.5.6/Firmware/communication/interface_i2c.cpp
+88
View File
@@ -0,0 +1,88 @@
#include "interface_i2c.h"
#include <i2c.h>
#define I2C_RX_BUFFER_SIZE 128
#define I2C_RX_BUFFER_PREAMBLE_SIZE 4
#define I2C_TX_BUFFER_SIZE 128
I2CStats_t i2c_stats_;
/*
TODO: add support back
static uint8_t i2c_rx_buffer[I2C_RX_BUFFER_PREAMBLE_SIZE + I2C_RX_BUFFER_SIZE];
static uint8_t i2c_tx_buffer[I2C_TX_BUFFER_SIZE];
class I2CSender : public PacketSink {
public:
int process_packet(const uint8_t* buffer, size_t length) {
if (length >= 2 && (length - 2) <= sizeof(i2c_tx_buffer))
memcpy(i2c_tx_buffer, buffer + 2, length - 2);
return 0;
}
size_t get_free_space() { return SIZE_MAX; }
} i2c1_packet_output;
BidirectionalPacketBasedChannel i2c1_channel(i2c1_packet_output);
*/
void start_i2c_server() {
// CAN H = SDA
// CAN L = SCL
//HAL_I2C_EnableListen_IT(&hi2c1);
}
/*
void i2c_handle_packet(I2C_HandleTypeDef *hi2c) {
size_t received = sizeof(i2c_rx_buffer) - hi2c->XferCount;
if (received > I2C_RX_BUFFER_PREAMBLE_SIZE) {
i2c_stats_.rx_cnt++;
write_le<uint16_t>(0, i2c_rx_buffer); // hallucinate seq-no (not needed for I2C)
i2c_rx_buffer[2] = i2c_rx_buffer[4]; // endpoint-id = I2C register address
i2c_rx_buffer[3] = i2c_rx_buffer[5] | 0x80; // MSB must be 1
size_t expected_bytes = (TX_BUF_SIZE - 2) < I2C_TX_BUFFER_SIZE ? (TX_BUF_SIZE - 2) : I2C_TX_BUFFER_SIZE;
write_le<uint16_t>(expected_bytes, i2c_rx_buffer + 4); // hallucinate maximum number of expected response bytes
i2c1_channel.process_packet(i2c_rx_buffer, received);
// reset receive buffer
hi2c->pBuffPtr = I2C_RX_BUFFER_PREAMBLE_SIZE + i2c_rx_buffer;
hi2c->XferCount = sizeof(i2c_rx_buffer) - I2C_RX_BUFFER_PREAMBLE_SIZE;
}
if (hi2c->State == HAL_I2C_STATE_BUSY_RX_LISTEN)
hi2c->State = HAL_I2C_STATE_LISTEN;
}
void HAL_I2C_ListenCpltCallback(I2C_HandleTypeDef *hi2c) {
i2c_handle_packet(hi2c);
// restart listening for address
HAL_I2C_EnableListen_IT(hi2c);
}
void HAL_I2C_AddrCallback(I2C_HandleTypeDef *hi2c, uint8_t TransferDirection, uint16_t AddrMatchCode) {
i2c_stats_.addr_match_cnt += 1;
i2c_handle_packet(hi2c);
if (TransferDirection == I2C_DIRECTION_TRANSMIT) {
HAL_I2C_Slave_Sequential_Receive_IT(hi2c,
I2C_RX_BUFFER_PREAMBLE_SIZE + i2c_rx_buffer,
sizeof(i2c_rx_buffer) - I2C_RX_BUFFER_PREAMBLE_SIZE, I2C_FIRST_AND_LAST_FRAME);
} else {
HAL_I2C_Slave_Sequential_Transmit_IT(hi2c, i2c_tx_buffer, sizeof(i2c_tx_buffer), I2C_FIRST_AND_LAST_FRAME);
}
}
void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c) {
// ignore NACK errors
if (!(hi2c->ErrorCode & (~HAL_I2C_ERROR_AF)))
return;
i2c_stats_.error_cnt += 1;
// Continue listening
HAL_I2C_EnableListen_IT(hi2c);
}
*/
ODrive-fw-v0.5.6/Firmware/communication/interface_i2c.h
+25
View File
@@ -0,0 +1,25 @@
#ifndef __INTERFACE_I2C_HPP
#define __INTERFACE_I2C_HPP
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
struct I2CStats_t {
uint8_t addr;
uint32_t addr_match_cnt;
uint32_t rx_cnt;
uint32_t error_cnt;
};
extern I2CStats_t i2c_stats_;
void start_i2c_server(void);
#ifdef __cplusplus
}
#endif
#endif // __INTERFACE_I2C_HPP
ODrive-fw-v0.5.6/Firmware/communication/interface_uart.cpp
+206
View File
@@ -0,0 +1,206 @@
#include "interface_uart.h"
#include "ascii_protocol.hpp"
#include <MotorControl/utils.hpp>
#include <fibre/async_stream.hpp>
#include <fibre/../../legacy_protocol.hpp>
#include <usart.h>
#include <cmsis_os.h>
#include <freertos_vars.h>
#include <odrive_main.h>
#define UART_TX_BUFFER_SIZE 64
#define UART_RX_BUFFER_SIZE 64
// DMA open loop continous circular buffer
// 1ms delay periodic, chase DMA ptr around
static uint8_t dma_rx_buffer[UART_RX_BUFFER_SIZE];
static uint32_t dma_last_rcv_idx;
osThreadId uart_thread = 0;
static UART_HandleTypeDef* huart_ = nullptr;
const uint32_t stack_size_uart_thread = 4096; // Bytes
namespace fibre {
class Stm32UartTxStream : public AsyncStreamSink {
public:
Stm32UartTxStream(UART_HandleTypeDef* huart) : huart_(huart) {}
void start_write(cbufptr_t buffer, TransferHandle* handle, Callback<void, WriteResult> completer) final;
void cancel_write(TransferHandle transfer_handle) final;
void did_finish();
UART_HandleTypeDef *huart_;
Callback<void, WriteResult> completer_;
const uint8_t* tx_end_ = nullptr;
};
class Stm32UartRxStream : public AsyncStreamSource {
public:
void start_read(bufptr_t buffer, TransferHandle* handle, Callback<void, ReadResult> completer) final;
void cancel_read(TransferHandle transfer_handle) final;
void did_receive(uint8_t* buffer, size_t length);
Callback<void, ReadResult> completer_;
bufptr_t rx_buf_ = {nullptr, nullptr};
};
}
using namespace fibre;
void Stm32UartTxStream::start_write(cbufptr_t buffer, TransferHandle* handle, Callback<void, WriteResult> completer) {
size_t chunk = std::min(buffer.size(), (size_t)UART_TX_BUFFER_SIZE);
completer_ = completer;
tx_end_ = buffer.begin() + chunk;
if (handle) {
*handle = reinterpret_cast<TransferHandle>(this);
}
if (HAL_UART_Transmit_DMA(huart_, const_cast<uint8_t*>(buffer.begin()), chunk) != HAL_OK) {
completer_ = nullptr;
tx_end_ = nullptr;
completer.invoke({kStreamError, buffer.begin()});
}
}
void Stm32UartTxStream::cancel_write(TransferHandle transfer_handle) {
// not implemented
}
void Stm32UartTxStream::did_finish() {
const uint8_t* tx_end = tx_end_;
tx_end_ = nullptr;
completer_.invoke_and_clear({kStreamOk, tx_end});
}
void Stm32UartRxStream::start_read(bufptr_t buffer, TransferHandle* handle, Callback<void, ReadResult> completer) {
completer_ = completer;
rx_buf_ = buffer;
if (handle) {
*handle = reinterpret_cast<TransferHandle>(this);
}
}
void Stm32UartRxStream::cancel_read(TransferHandle transfer_handle) {
// not implemented
}
void Stm32UartRxStream::did_receive(uint8_t* buffer, size_t length) {
// This can be called even if there was no RX operation in progress
bufptr_t rx_buf = rx_buf_;
if (completer_ && rx_buf.begin()) {
rx_buf_ = {nullptr, nullptr};
size_t chunk = std::min(length, rx_buf.size());
memcpy(rx_buf.begin(), buffer, chunk);
completer_.invoke_and_clear({kStreamOk, rx_buf.begin() + chunk});
}
}
Stm32UartTxStream uart_tx_stream(huart_);
Stm32UartRxStream uart_rx_stream;
LegacyProtocolStreamBased fibre_over_uart(&uart_rx_stream, &uart_tx_stream);
fibre::AsyncStreamSinkMultiplexer<2> uart_tx_multiplexer(uart_tx_stream);
fibre::BufferedStreamSink<64> uart0_stdout_sink(uart_tx_multiplexer); // Used in communication.cpp
AsciiProtocol ascii_over_uart(&uart_rx_stream, &uart_tx_multiplexer);
bool uart0_stdout_pending = false;
static void uart_server_thread(void * ctx) {
(void) ctx;
if (odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_FIBRE) {
fibre_over_uart.start({});
} else if (odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII
|| odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT) {
ascii_over_uart.start();
}
for (;;) {
osEvent event = osMessageGet(uart_event_queue, osWaitForever);
if (event.status != osEventMessage) {
continue;
}
switch (event.value.v) {
case 1: {
// This event is triggered by the control loop at 8kHz. This should be
// enough for most applications.
// At 1Mbaud/s that corresponds to at most 12.5 bytes which can arrive
// during the sleep period.
// Check for UART errors and restart receive DMA transfer if required
if (huart_->RxState != HAL_UART_STATE_BUSY_RX) {
HAL_UART_AbortReceive(huart_);
HAL_UART_Receive_DMA(huart_, dma_rx_buffer, sizeof(dma_rx_buffer));
dma_last_rcv_idx = 0;
}
// Fetch the circular buffer "write pointer", where it would write next
uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart_->hdmarx->Instance->NDTR;
if (new_rcv_idx > UART_RX_BUFFER_SIZE) { // defensive programming
continue;
}
// Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < dma_last_rcv_idx) {
uart_rx_stream.did_receive(dma_rx_buffer + dma_last_rcv_idx,
UART_RX_BUFFER_SIZE - dma_last_rcv_idx);
dma_last_rcv_idx = 0;
}
if (new_rcv_idx > dma_last_rcv_idx) {
uart_rx_stream.did_receive(dma_rx_buffer + dma_last_rcv_idx,
new_rcv_idx - dma_last_rcv_idx);
dma_last_rcv_idx = new_rcv_idx;
}
} break;
case 2: {
uart_tx_stream.did_finish();
} break;
case 3: { // stdout has data
uart0_stdout_pending = false;
uart0_stdout_sink.maybe_start_async_write();
} break;
}
}
}
// TODO: allow multiple UART server instances
void start_uart_server(UART_HandleTypeDef* huart) {
huart_ = huart;
uart_tx_stream.huart_ = huart;
// DMA is set up to receive in a circular buffer forever.
// We dont use interrupts to fetch the data, instead we periodically read
// data out of the circular buffer into a parse buffer, controlled by a state machine
HAL_UART_Receive_DMA(huart_, dma_rx_buffer, sizeof(dma_rx_buffer));
dma_last_rcv_idx = 0;
// Start UART communication thread
osThreadDef(uart_server_thread_def, uart_server_thread, osPriorityNormal, 0, stack_size_uart_thread / sizeof(StackType_t) /* the ascii protocol needs considerable stack space */);
uart_thread = osThreadCreate(osThread(uart_server_thread_def), NULL);
}
void uart_poll() {
if (uart_thread) { // the thread is only started if UART is enabled
osMessagePut(uart_event_queue, 1, 0);
}
}
void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) {
if (huart == huart_) {
osMessagePut(uart_event_queue, 2, 0);
}
}
ODrive-fw-v0.5.6/Firmware/communication/interface_uart.h
+29
View File
@@ -0,0 +1,29 @@
#ifndef __INTERFACE_UART_HPP
#define __INTERFACE_UART_HPP
#ifdef __cplusplus
extern "C" {
#endif
#include <cmsis_os.h>
#include "usart.h"
extern osThreadId uart_thread;
extern const uint32_t stack_size_uart_thread;
void start_uart_server(UART_HandleTypeDef* huart);
void uart_poll(void);
#ifdef __cplusplus
}
#endif
#ifdef __cplusplus
#include <fibre/../../stream_utils.hpp>
extern fibre::BufferedStreamSink<64> uart0_stdout_sink;
extern bool uart0_stdout_pending;
#endif
#endif // __INTERFACE_UART_HPP
ODrive-fw-v0.5.6/Firmware/communication/interface_usb.cpp
+237
View File
@@ -0,0 +1,237 @@
#include "interface_usb.h"
#include "ascii_protocol.hpp"
#include <MotorControl/utils.hpp>
#include <fibre/async_stream.hpp>
#include <fibre/../../legacy_protocol.hpp>
#include <usbd_cdc.h>
#include <usbd_cdc_if.h>
#include <usb_device.h>
#include <cmsis_os.h>
#include <freertos_vars.h>
#include <odrive_main.h>
osThreadId usb_thread;
const uint32_t stack_size_usb_thread = 4096; // Bytes
USBStats_t usb_stats_;
namespace fibre {
class Stm32UsbTxStream : public AsyncStreamSink {
public:
Stm32UsbTxStream(uint8_t endpoint_num) : endpoint_num_(endpoint_num) {}
void start_write(cbufptr_t buffer, TransferHandle* handle, Callback<void, WriteResult> completer) final;
void cancel_write(TransferHandle transfer_handle) final;
void did_finish();
const uint8_t endpoint_num_;
bool connected_ = false;
Callback<void, WriteResult> completer_;
const uint8_t* tx_end_ = nullptr;
};
class Stm32UsbRxStream : public AsyncStreamSource {
public:
Stm32UsbRxStream(uint8_t endpoint_num) : endpoint_num_(endpoint_num) {}
void start_read(bufptr_t buffer, TransferHandle* handle, Callback<void, ReadResult> completer) final;
void cancel_read(TransferHandle transfer_handle) final;
void did_finish();
const uint8_t endpoint_num_;
bool connected_ = false;
Callback<void, ReadResult> completer_;
uint8_t* rx_end_ = nullptr;
};
}
using namespace fibre;
void Stm32UsbTxStream::start_write(cbufptr_t buffer, TransferHandle* handle, Callback<void, WriteResult> completer) {
if (handle) {
*handle = reinterpret_cast<TransferHandle>(this);
}
if (!connected_) {
completer.invoke({kStreamClosed, buffer.begin()});
return;
}
// Note on MTU: on the physical layer, a full speed device can transmit up
// to 64 bytes of payload per bulk package. However a single logical
// transfer can consist of multiple 64 byte packets terminated by a 0 byte
// packet. Currently we don't implement this segmentation. Therefore we
// must ensure that all packets are < 64 bytes, otherwise the host will wait
// for more.
if (buffer.size() >= USB_TX_DATA_SIZE) {
completer.invoke({kStreamError, buffer.begin()});
return;
}
if (completer_ || tx_end_) {
completer.invoke({kStreamError, buffer.begin()});
return;
}
completer_ = completer;
tx_end_ = buffer.end();
if (
#if HW_VERSION_MAJOR == 3 // TODO: remove preprocessor switch
CDC_Transmit_FS
#elif HW_VERSION_MAJOR == 4
CDC_Transmit_HS
#else
#error "not supported"
#endif
(const_cast<uint8_t*>(buffer.begin()), buffer.size(), endpoint_num_) != USBD_OK) {
tx_end_ = nullptr;
completer_.invoke_and_clear({kStreamError, buffer.begin()});
}
}
void Stm32UsbTxStream::cancel_write(TransferHandle transfer_handle) {
// not implemented
}
void Stm32UsbTxStream::did_finish() {
const uint8_t* tx_end = tx_end_;
tx_end_ = nullptr;
completer_.invoke_and_clear({connected_ ? kStreamOk : kStreamClosed, tx_end});
}
void Stm32UsbRxStream::start_read(bufptr_t buffer, TransferHandle* handle, Callback<void, ReadResult> completer) {
if (handle) {
*handle = reinterpret_cast<TransferHandle>(this);
}
if (!connected_) {
completer.invoke({kStreamClosed, buffer.begin()});
return;
}
if (completer_ || rx_end_) {
completer.invoke({kStreamError, buffer.begin()});
return;
}
completer_ = completer;
rx_end_ = buffer.begin(); // the pointer is updated at the end of the transfer
if (USBD_CDC_ReceivePacket(&usb_dev_handle, buffer.begin(), buffer.size(), endpoint_num_) != USBD_OK) {
rx_end_ = nullptr;
completer_.invoke_and_clear({kStreamError, buffer.begin()});
return;
}
}
void Stm32UsbRxStream::cancel_read(TransferHandle transfer_handle) {
// not implemented
}
void Stm32UsbRxStream::did_finish() {
uint8_t* rx_end = rx_end_;
rx_end_ = nullptr;
completer_.invoke_and_clear({connected_ ? kStreamOk : kStreamClosed, rx_end});
}
Stm32UsbTxStream usb_cdc_tx_stream(CDC_IN_EP);
Stm32UsbTxStream usb_native_tx_stream(ODRIVE_IN_EP);
Stm32UsbRxStream usb_cdc_rx_stream(CDC_OUT_EP);
Stm32UsbRxStream usb_native_rx_stream(ODRIVE_OUT_EP);
LegacyProtocolStreamBased fibre_over_cdc(&usb_cdc_rx_stream, &usb_cdc_tx_stream);
LegacyProtocolPacketBased fibre_over_usb(&usb_native_rx_stream, &usb_native_tx_stream, USB_TX_DATA_SIZE - 1); // See note on MTU above
fibre::AsyncStreamSinkMultiplexer<2> usb_cdc_tx_multiplexer(usb_cdc_tx_stream);
fibre::BufferedStreamSink<64> usb_cdc_stdout_sink(usb_cdc_tx_multiplexer); // Used in communication.cpp
AsciiProtocol ascii_over_cdc(&usb_cdc_rx_stream, &usb_cdc_tx_multiplexer);
bool usb_cdc_stdout_pending = false;
static void usb_server_thread(void * ctx) {
(void) ctx;
for (;;) {
osEvent event = osMessageGet(usb_event_queue, osWaitForever);
if (event.status != osEventMessage) {
continue;
}
usb_stats_.rx_cnt++;
switch (event.value.v) {
case 1: { // USB connected event
usb_cdc_tx_stream.connected_ = true;
usb_native_tx_stream.connected_ = true;
usb_cdc_rx_stream.connected_ = true;
usb_native_rx_stream.connected_ = true;
fibre_over_usb.start({});
if (odrv.config_.usb_cdc_protocol == ODrive::STREAM_PROTOCOL_TYPE_FIBRE) {
fibre_over_cdc.start({});
} else if (odrv.config_.usb_cdc_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII
|| odrv.config_.usb_cdc_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT) {
ascii_over_cdc.start();
}
} break;
case 2: { // USB disconnected event
usb_cdc_tx_stream.connected_ = false;
usb_native_tx_stream.connected_ = false;
usb_cdc_rx_stream.connected_ = false;
usb_native_rx_stream.connected_ = false;
usb_cdc_tx_stream.did_finish();
usb_native_tx_stream.did_finish();
usb_cdc_rx_stream.did_finish();
usb_native_rx_stream.did_finish();
} break;
case 3: { // TX on CDC interface done
usb_cdc_tx_stream.did_finish();
} break;
case 4: { // TX on custom interface done
usb_native_tx_stream.did_finish();
} break;
case 5: { // RX on CDC interface done
usb_cdc_rx_stream.did_finish();
} break;
case 6: { // RX on custom interface done
usb_native_rx_stream.did_finish();
} break;
case 7: { // stdout has data
usb_cdc_stdout_pending = false;
usb_cdc_stdout_sink.maybe_start_async_write();
} break;
}
}
}
// Called from CDC_Receive_FS callback function, this allows the communication
// thread to handle the incoming data
void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair) {
if (endpoint_pair == CDC_OUT_EP && usb_cdc_rx_stream.rx_end_) {
usb_cdc_rx_stream.rx_end_ += len;
osMessagePut(usb_event_queue, 5, 0);
} else if (endpoint_pair == ODRIVE_OUT_EP && usb_native_rx_stream.rx_end_) {
usb_native_rx_stream.rx_end_ += len;
osMessagePut(usb_event_queue, 6, 0);
}
}
void start_usb_server() {
// Start USB communication thread
osThreadDef(usb_server_thread_def, usb_server_thread, osPriorityNormal, 0, stack_size_usb_thread / sizeof(StackType_t));
usb_thread = osThreadCreate(osThread(usb_server_thread_def), NULL);
}
ODrive-fw-v0.5.6/Firmware/communication/interface_usb.h
+37
View File
@@ -0,0 +1,37 @@
#ifndef __INTERFACE_USB_HPP
#define __INTERFACE_USB_HPP
#ifdef __cplusplus
extern "C" {
#endif
#include <cmsis_os.h>
#include <stdint.h>
extern osThreadId usb_thread;
extern const uint32_t stack_size_usb_thread;
typedef struct {
uint32_t rx_cnt;
uint32_t tx_cnt;
uint32_t tx_overrun_cnt;
} USBStats_t;
extern USBStats_t usb_stats_;
void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair);
void start_usb_server(void);
#ifdef __cplusplus
}
#endif
#ifdef __cplusplus
#include <fibre/../../stream_utils.hpp>
extern fibre::BufferedStreamSink<64> usb_cdc_stdout_sink;
extern bool usb_cdc_stdout_pending;
#endif
#endif // __INTERFACE_USB_HPP