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
Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/communication/ascii_protocol.cpp
+313
View File
@@ -0,0 +1,313 @@
/*
* 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"
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
/* Private constant data -----------------------------------------------------*/
#define MAX_LINE_LENGTH 256
#define TO_STR_INNER(s) #s
#define TO_STR(s) TO_STR_INNER(s)
/* Private variables ---------------------------------------------------------*/
static Introspectable root_obj = ODriveTypeInfo<ODrive>::make_introspectable(odrv);
/* Private function prototypes -----------------------------------------------*/
/* Function implementations --------------------------------------------------*/
// @brief Sends a line on the specified output.
template<typename ... TArgs>
void respond(StreamSink& output, bool include_checksum, const char * fmt, TArgs&& ... args) {
char response[64]; // Hardcoded max buffer size. We silently truncate the output if it's too long for the buffer.
size_t len = snprintf(response, sizeof(response), fmt, std::forward<TArgs>(args)...);
len = std::min(len, sizeof(response));
output.process_bytes((uint8_t*)response, len, nullptr); // TODO: use process_all instead
if (include_checksum) {
uint8_t checksum = 0;
for (size_t i = 0; i < len; ++i)
checksum ^= response[i];
len = snprintf(response, sizeof(response), "*%u", checksum);
len = std::min(len, sizeof(response));
output.process_bytes((uint8_t*)response, len, nullptr);
}
output.process_bytes((const uint8_t*)"\r\n", 2, nullptr);
}
// @brief Executes an ASCII protocol command
// @param buffer buffer of ASCII encoded characters
// @param len size of the buffer
void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& response_channel) {
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 < len; ++i) {
if (buffer[i] == ';') { // ';' is the comment start char
len = 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];
if (len > MAX_LINE_LENGTH) len = MAX_LINE_LENGTH;
memcpy(cmd, buffer, 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((const char *)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
if (cmd[0] == 'p') { // position control
unsigned motor_number;
float pos_setpoint, vel_feed_forward, torque_feed_forward;
int numscan = sscanf(cmd, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &torque_feed_forward);
if (numscan < 2) {
respond(response_channel, use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(response_channel, 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();
}
} else if (cmd[0] == 'q') { // position control with limits
unsigned motor_number;
float pos_setpoint, vel_limit, torque_lim;
int numscan = sscanf(cmd, "q %u %f %f %f", &motor_number, &pos_setpoint, &vel_limit, &torque_lim);
if (numscan < 2) {
respond(response_channel, use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(response_channel, 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();
}
} else if (cmd[0] == 'v') { // velocity control
unsigned motor_number;
float vel_setpoint, torque_feed_forward;
int numscan = sscanf(cmd, "v %u %f %f", &motor_number, &vel_setpoint, &torque_feed_forward);
if (numscan < 2) {
respond(response_channel, use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(response_channel, 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();
}
} else if (cmd[0] == 'c') { // torque control
unsigned motor_number;
float torque_setpoint;
int numscan = sscanf(cmd, "c %u %f", &motor_number, &torque_setpoint);
if (numscan < 2) {
respond(response_channel, use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(response_channel, 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();
}
} else if (cmd[0] == 't') { // trapezoidal trajectory
unsigned motor_number;
float goal_point;
int numscan = sscanf(cmd, "t %u %f", &motor_number, &goal_point);
if (numscan < 2) {
respond(response_channel, use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(response_channel, 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();
}
} else if (cmd[0] == 'f') { // feedback
unsigned motor_number;
int numscan = sscanf(cmd, "f %u", &motor_number);
if (numscan < 1) {
respond(response_channel, use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(response_channel, use_checksum, "invalid motor %u", motor_number);
} else {
respond(response_channel, use_checksum, "%f %f",
(double)axes[motor_number]->encoder_.pos_estimate_,
(double)axes[motor_number]->encoder_.vel_estimate_);
}
} else if (cmd[0] == 'h') { // Help
respond(response_channel, use_checksum, "Please see documentation for more details");
respond(response_channel, use_checksum, "");
respond(response_channel, use_checksum, "Available commands syntax reference:");
respond(response_channel, use_checksum, "Position: q axis pos vel-lim I-lim");
respond(response_channel, use_checksum, "Position: p axis pos vel-ff I-ff");
respond(response_channel, use_checksum, "Velocity: v axis vel I-ff");
respond(response_channel, use_checksum, "Torque: c axis T");
respond(response_channel, use_checksum, "");
respond(response_channel, use_checksum, "Properties start at odrive root, such as axis0.requested_state");
respond(response_channel, use_checksum, "Read: r property");
respond(response_channel, use_checksum, "Write: w property value");
respond(response_channel, use_checksum, "");
respond(response_channel, use_checksum, "Save config: ss");
respond(response_channel, use_checksum, "Erase config: se");
respond(response_channel, use_checksum, "Reboot: sr");
} else if (cmd[0] == 'i'){ // Dump device info
// respond(response_channel, use_checksum, "Signature: %#x", STM_ID_GetSignature());
// respond(response_channel, use_checksum, "Revision: %#x", STM_ID_GetRevision());
// respond(response_channel, use_checksum, "Flash Size: %#x KiB", STM_ID_GetFlashSize());
respond(response_channel, use_checksum, "Hardware version: %d.%d-%dV", odrv.hw_version_major_, odrv.hw_version_minor_, odrv.hw_version_variant_);
respond(response_channel, use_checksum, "Firmware version: %d.%d.%d", odrv.fw_version_major_, odrv.fw_version_minor_, odrv.fw_version_revision_);
respond(response_channel, use_checksum, "Serial number: %s", serial_number_str);
} else if (cmd[0] == 's'){ // System
if(cmd[1] == 's') { // Save config
odrv.save_configuration();
} else if (cmd[1] == 'e'){ // Erase config
odrv.erase_configuration();
} else if (cmd[1] == 'r'){ // Reboot
odrv.reboot();
}
} else if (cmd[0] == 'r') { // read property
char name[MAX_LINE_LENGTH];
int numscan = sscanf(cmd, "r %255s", name);
if (numscan < 1) {
respond(response_channel, 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(response_channel, use_checksum, "invalid property");
} else {
char response[10];
bool success = type_info->get_string(property, response, sizeof(response));
if (!success)
respond(response_channel, use_checksum, "not implemented");
else
respond(response_channel, use_checksum, response);
}
}
} else if (cmd[0] == 'w') { // write property
char name[MAX_LINE_LENGTH];
char value[MAX_LINE_LENGTH];
int numscan = sscanf(cmd, "w %255s %255s", name, value);
if (numscan < 1) {
respond(response_channel, 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(response_channel, use_checksum, "invalid property");
} else {
bool success = type_info->set_string(property, value, sizeof(value));
if (!success)
respond(response_channel, use_checksum, "not implemented");
}
}
} else if (cmd[0] == 'u') { // Update axis watchdog.
unsigned motor_number;
int numscan = sscanf(cmd, "u %u", &motor_number);
if(numscan < 1){
respond(response_channel, use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(response_channel, use_checksum, "invalid motor %u", motor_number);
}else {
axes[motor_number]->watchdog_feed();
}
} else if (cmd[0] != 0) {
respond(response_channel, use_checksum, "unknown command");
}
}
void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink& response_channel) {
static uint8_t parse_buffer[MAX_LINE_LENGTH];
static bool read_active = true;
static uint32_t parse_buffer_idx = 0;
while (len--) {
// if the line becomes too long, reset buffer and wait for the next line
if (parse_buffer_idx >= MAX_LINE_LENGTH) {
read_active = false;
parse_buffer_idx = 0;
}
// Fetch the next char
uint8_t c = *(buffer++);
bool is_end_of_line = (c == '\r' || c == '\n' || c == '!');
if (is_end_of_line) {
if (read_active)
ASCII_protocol_process_line(parse_buffer, parse_buffer_idx, response_channel);
parse_buffer_idx = 0;
read_active = true;
} else {
if (read_active) {
parse_buffer[parse_buffer_idx++] = c;
}
}
}
}
Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/communication/ascii_protocol.hpp
+22
View File
@@ -0,0 +1,22 @@
#ifndef __ASCII_PROTOCOL_H
#define __ASCII_PROTOCOL_H
/* Includes ------------------------------------------------------------------*/
#include <fibre/protocol.hpp>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
/* Exported variables --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink& response_channel);
#endif /* __ASCII_PROTOCOL_H */
Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/communication/can_helpers.hpp
+88
View File
@@ -0,0 +1,88 @@
#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;
};
#include <iterator>
template <typename T>
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 = (1ULL << length) - 1;
if (isIntel) {
std::memcpy(&tempVal, msg.buf, sizeof(tempVal));
tempVal = (tempVal >> startBit) & mask;
} else {
std::reverse(std::begin(msg.buf), std::end(msg.buf));
std::memcpy(&tempVal, msg.buf, sizeof(tempVal));
tempVal = (tempVal >> (64 - startBit - length)) & mask;
}
T retVal;
std::memcpy(&retVal, &tempVal, sizeof(T));
return retVal;
}
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>
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 = (val - offset) / factor;
uint64_t valAsBits = 0;
std::memcpy(&valAsBits, &scaledVal, sizeof(scaledVal));
uint64_t mask = (1ULL << length) - 1;
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>
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);
}
Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/communication/can_simple.cpp
+412
View File
@@ -0,0 +1,412 @@
#include "can_simple.hpp"
#include <odrive_main.h>
#include <cstring>
static constexpr uint8_t NUM_NODE_ID_BITS = 6;
static constexpr uint8_t NUM_CMD_ID_BITS = 11 - NUM_NODE_ID_BITS;
void CANSimple::handle_can_message(can_Message_t& msg) {
// This functional way of handling the messages is neat and is much cleaner from
// a data security point of view, but it will require some tweaking to fix the syntax.
//
// auto func = callback_map.find(msg.id);
// if(func != callback_map.end()){
// func->second(msg);
// }
// Frame
// nodeID | CMD
// 6 bits | 5 bits
uint32_t nodeID = get_node_id(msg.id);
uint32_t cmd = get_cmd_id(msg.id);
Axis* axis = nullptr;
bool validAxis = false;
for (uint8_t i = 0; i < AXIS_COUNT; i++) {
if ((axes[i]->config_.can_node_id == nodeID) && (axes[i]->config_.can_node_id_extended == msg.isExt)) {
axis = axes[i];
if (!validAxis) {
validAxis = true;
} else {
// Duplicate can IDs, don't assign to any axis
odCAN->set_error(ODriveCAN::ERROR_DUPLICATE_CAN_IDS);
validAxis = false;
break;
}
}
}
if (validAxis) {
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:
get_motor_error_callback(axis, msg);
break;
case MSG_GET_ENCODER_ERROR:
get_encoder_error_callback(axis, msg);
break;
case MSG_GET_SENSORLESS_ERROR:
get_sensorless_error_callback(axis, msg);
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:
get_encoder_estimates_callback(axis, msg);
break;
case MSG_GET_ENCODER_COUNT:
get_encoder_count_callback(axis, msg);
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_VEL_LIMIT:
set_vel_limit_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:
get_iq_callback(axis, msg);
break;
case MSG_GET_SENSORLESS_ESTIMATES:
get_sensorless_estimates_callback(axis, msg);
break;
case MSG_RESET_ODRIVE:
NVIC_SystemReset();
break;
case MSG_GET_VBUS_VOLTAGE:
get_vbus_voltage_callback(axis, msg);
break;
case MSG_CLEAR_ERRORS:
clear_errors_callback(axis, msg);
break;
default:
break;
}
}
}
void CANSimple::nmt_callback(Axis* axis, can_Message_t& msg) {
// Not implemented
}
void CANSimple::estop_callback(Axis* axis, can_Message_t& msg) {
axis->error_ |= Axis::ERROR_ESTOP_REQUESTED;
}
void CANSimple::get_motor_error_callback(Axis* axis, can_Message_t& msg) {
if (msg.rtr) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_MOTOR_ERROR; // heartbeat ID
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
txmsg.buf[0] = axis->motor_.error_;
txmsg.buf[1] = axis->motor_.error_ >> 8;
txmsg.buf[2] = axis->motor_.error_ >> 16;
txmsg.buf[3] = axis->motor_.error_ >> 24;
odCAN->write(txmsg);
}
}
void CANSimple::get_encoder_error_callback(Axis* axis, can_Message_t& msg) {
if (msg.rtr) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_ENCODER_ERROR; // heartbeat ID
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
txmsg.buf[0] = axis->encoder_.error_;
txmsg.buf[1] = axis->encoder_.error_ >> 8;
txmsg.buf[2] = axis->encoder_.error_ >> 16;
txmsg.buf[3] = axis->encoder_.error_ >> 24;
odCAN->write(txmsg);
}
}
void CANSimple::get_sensorless_error_callback(Axis* axis, can_Message_t& msg) {
if (msg.rtr) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_SENSORLESS_ERROR; // heartbeat ID
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
txmsg.buf[0] = axis->sensorless_estimator_.error_;
txmsg.buf[1] = axis->sensorless_estimator_.error_ >> 8;
txmsg.buf[2] = axis->sensorless_estimator_.error_ >> 16;
txmsg.buf[3] = axis->sensorless_estimator_.error_ >> 24;
odCAN->write(txmsg);
}
}
void CANSimple::set_axis_nodeid_callback(Axis* axis, 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, can_Message_t& msg) {
axis->requested_state_ = static_cast<Axis::AxisState>(can_getSignal<int32_t>(msg, 0, 16, true));
}
void CANSimple::set_axis_startup_config_callback(Axis* axis, can_Message_t& msg) {
// Not Implemented
}
void CANSimple::get_encoder_estimates_callback(Axis* axis, can_Message_t& msg) {
if (msg.rtr) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_ENCODER_ESTIMATES; // heartbeat ID
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
// Undefined behaviour!
// uint32_t floatBytes = *(reinterpret_cast<int32_t*>(&(axis->encoder_.pos_estimate_)));
uint32_t floatBytes;
static_assert(sizeof axis->encoder_.pos_estimate_ == sizeof floatBytes);
std::memcpy(&floatBytes, &axis->encoder_.pos_estimate_, sizeof floatBytes);
txmsg.buf[0] = floatBytes;
txmsg.buf[1] = floatBytes >> 8;
txmsg.buf[2] = floatBytes >> 16;
txmsg.buf[3] = floatBytes >> 24;
static_assert(sizeof floatBytes == sizeof axis->encoder_.vel_estimate_);
std::memcpy(&floatBytes, &axis->encoder_.vel_estimate_, sizeof floatBytes);
txmsg.buf[4] = floatBytes;
txmsg.buf[5] = floatBytes >> 8;
txmsg.buf[6] = floatBytes >> 16;
txmsg.buf[7] = floatBytes >> 24;
odCAN->write(txmsg);
}
}
void CANSimple::get_sensorless_estimates_callback(Axis* axis, can_Message_t& msg) {
if (msg.rtr) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_SENSORLESS_ESTIMATES; // heartbeat ID
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
// Undefined behaviour!
// uint32_t floatBytes = *(reinterpret_cast<int32_t*>(&(axis->encoder_.pos_estimate_)));
uint32_t floatBytes;
static_assert(sizeof axis->sensorless_estimator_.pll_pos_ == sizeof floatBytes);
std::memcpy(&floatBytes, &axis->sensorless_estimator_.pll_pos_, sizeof floatBytes);
txmsg.buf[0] = floatBytes;
txmsg.buf[1] = floatBytes >> 8;
txmsg.buf[2] = floatBytes >> 16;
txmsg.buf[3] = floatBytes >> 24;
static_assert(sizeof floatBytes == sizeof axis->sensorless_estimator_.vel_estimate_);
std::memcpy(&floatBytes, &axis->sensorless_estimator_.vel_estimate_, sizeof floatBytes);
txmsg.buf[4] = floatBytes;
txmsg.buf[5] = floatBytes >> 8;
txmsg.buf[6] = floatBytes >> 16;
txmsg.buf[7] = floatBytes >> 24;
odCAN->write(txmsg);
}
}
void CANSimple::get_encoder_count_callback(Axis* axis, can_Message_t& msg) {
if (msg.rtr) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_ENCODER_COUNT;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
txmsg.buf[0] = axis->encoder_.shadow_count_;
txmsg.buf[1] = axis->encoder_.shadow_count_ >> 8;
txmsg.buf[2] = axis->encoder_.shadow_count_ >> 16;
txmsg.buf[3] = axis->encoder_.shadow_count_ >> 24;
txmsg.buf[4] = axis->encoder_.count_in_cpr_;
txmsg.buf[5] = axis->encoder_.count_in_cpr_ >> 8;
txmsg.buf[6] = axis->encoder_.count_in_cpr_ >> 16;
txmsg.buf[7] = axis->encoder_.count_in_cpr_ >> 24;
odCAN->write(txmsg);
}
}
void CANSimple::set_input_pos_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.input_pos_ = can_getSignal<float>(msg, 0, 32, true);
axis->controller_.input_vel_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.001f, 0);
axis->controller_.input_torque_ = can_getSignal<int16_t>(msg, 48, 16, true, 0.001f, 0);
axis->controller_.input_pos_updated();
}
void CANSimple::set_input_vel_callback(Axis* axis, 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, can_Message_t& msg) {
axis->controller_.input_torque_ = can_getSignal<float>(msg, 0, 32, true);
}
void CANSimple::set_controller_modes_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.config_.control_mode = static_cast<Controller::ControlMode>(can_getSignal<int32_t>(msg, 0, 32, true));
axis->controller_.config_.input_mode = static_cast<Controller::InputMode>(can_getSignal<int32_t>(msg, 32, 32, true));
}
void CANSimple::set_vel_limit_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.config_.vel_limit = can_getSignal<float>(msg, 0, 32, true);
}
void CANSimple::start_anticogging_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.start_anticogging_calibration();
}
void CANSimple::set_traj_vel_limit_callback(Axis* axis, can_Message_t& msg) {
axis->trap_traj_.config_.vel_limit = can_getSignal<float>(msg, 0, 32, true);
}
void CANSimple::set_traj_accel_limits_callback(Axis* axis, 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, can_Message_t& msg) {
axis->controller_.config_.inertia = can_getSignal<float>(msg, 0, 32, true);
}
void CANSimple::get_iq_callback(Axis* axis, can_Message_t& msg) {
if (msg.rtr) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_IQ;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
uint32_t floatBytes;
static_assert(sizeof axis->motor_.current_control_.Iq_setpoint == sizeof floatBytes);
std::memcpy(&floatBytes, &axis->motor_.current_control_.Iq_setpoint, sizeof floatBytes);
txmsg.buf[0] = floatBytes;
txmsg.buf[1] = floatBytes >> 8;
txmsg.buf[2] = floatBytes >> 16;
txmsg.buf[3] = floatBytes >> 24;
static_assert(sizeof floatBytes == sizeof axis->motor_.current_control_.Iq_measured);
std::memcpy(&floatBytes, &axis->motor_.current_control_.Iq_measured, sizeof floatBytes);
txmsg.buf[4] = floatBytes;
txmsg.buf[5] = floatBytes >> 8;
txmsg.buf[6] = floatBytes >> 16;
txmsg.buf[7] = floatBytes >> 24;
odCAN->write(txmsg);
}
}
void CANSimple::get_vbus_voltage_callback(Axis* axis, can_Message_t& msg) {
if (msg.rtr) {
can_Message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_VBUS_VOLTAGE;
txmsg.isExt = axis->config_.can_node_id_extended;
txmsg.len = 8;
uint32_t floatBytes;
static_assert(sizeof vbus_voltage == sizeof floatBytes);
std::memcpy(&floatBytes, &vbus_voltage, sizeof floatBytes);
// This also works in principle, but I don't have hardware to verify endianness
// std::memcpy(&txmsg.buf[0], &vbus_voltage, sizeof vbus_voltage);
txmsg.buf[0] = floatBytes;
txmsg.buf[1] = floatBytes >> 8;
txmsg.buf[2] = floatBytes >> 16;
txmsg.buf[3] = floatBytes >> 24;
txmsg.buf[4] = 0;
txmsg.buf[5] = 0;
txmsg.buf[6] = 0;
txmsg.buf[7] = 0;
odCAN->write(txmsg);
}
}
void CANSimple::clear_errors_callback(Axis* axis, can_Message_t& msg) {
axis->clear_errors();
}
void CANSimple::send_heartbeat(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_node_id_extended;
txmsg.len = 8;
// Axis errors in 1st 32-bit value
txmsg.buf[0] = axis->error_;
txmsg.buf[1] = axis->error_ >> 8;
txmsg.buf[2] = axis->error_ >> 16;
txmsg.buf[3] = axis->error_ >> 24;
// Current state of axis in 2nd 32-bit value
txmsg.buf[4] = axis->current_state_;
txmsg.buf[5] = axis->current_state_ >> 8;
txmsg.buf[6] = axis->current_state_ >> 16;
txmsg.buf[7] = axis->current_state_ >> 24;
odCAN->write(txmsg);
}
uint32_t CANSimple::get_node_id(uint32_t msgID) {
return (msgID >> NUM_CMD_ID_BITS); // Upper 6 or more bits
}
uint8_t CANSimple::get_cmd_id(uint32_t msgID) {
return (msgID & 0x01F); // Bottom 5 bits
}
Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/communication/can_simple.hpp
+84
View File
@@ -0,0 +1,84 @@
#ifndef __CAN_SIMPLE_HPP_
#define __CAN_SIMPLE_HPP_
#include "interface_can.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_VEL_LIMIT,
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_VBUS_VOLTAGE,
MSG_CLEAR_ERRORS,
MSG_CO_HEARTBEAT_CMD = 0x700, // CANOpen NMT Heartbeat SEND
};
static void handle_can_message(can_Message_t& msg);
static void send_heartbeat(Axis* axis);
private:
static void nmt_callback(Axis* axis, can_Message_t& msg);
static void estop_callback(Axis* axis, can_Message_t& msg);
static void get_motor_error_callback(Axis* axis, can_Message_t& msg);
static void get_encoder_error_callback(Axis* axis, can_Message_t& msg);
static void get_controller_error_callback(Axis* axis, can_Message_t& msg);
static void get_sensorless_error_callback(Axis* axis, can_Message_t& msg);
static void set_axis_nodeid_callback(Axis* axis, can_Message_t& msg);
static void set_axis_requested_state_callback(Axis* axis, can_Message_t& msg);
static void set_axis_startup_config_callback(Axis* axis, can_Message_t& msg);
static void get_encoder_estimates_callback(Axis* axis, can_Message_t& msg);
static void get_encoder_count_callback(Axis* axis, can_Message_t& msg);
static void set_input_pos_callback(Axis* axis, can_Message_t& msg);
static void set_input_vel_callback(Axis* axis, can_Message_t& msg);
static void set_input_torque_callback(Axis* axis, can_Message_t& msg);
static void set_controller_modes_callback(Axis* axis, can_Message_t& msg);
static void set_vel_limit_callback(Axis* axis, can_Message_t& msg);
static void start_anticogging_callback(Axis* axis, can_Message_t& msg);
static void set_traj_vel_limit_callback(Axis* axis, can_Message_t& msg);
static void set_traj_accel_limits_callback(Axis* axis, can_Message_t& msg);
static void set_traj_inertia_callback(Axis* axis, can_Message_t& msg);
static void get_iq_callback(Axis* axis, can_Message_t& msg);
static void get_sensorless_estimates_callback(Axis* axis, can_Message_t& msg);
static void get_vbus_voltage_callback(Axis* axis, can_Message_t& msg);
static void clear_errors_callback(Axis* axis, can_Message_t& msg);
// Utility functions
static uint32_t get_node_id(uint32_t msgID);
static uint8_t get_cmd_id(uint32_t msgID);
// Fetch a specific signal from the message
// This functional way of handling the messages is neat and is much cleaner from
// a data security point of view, but it will require some tweaking
//
// const std::map<uint32_t, std::function<void(can_Message_t&)>> callback_map = {
// {0x000, std::bind(&CANSimple::heartbeat_callback, this, _1)}
// };
};
#endif
Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/communication/communication.cpp
+98
View File
@@ -0,0 +1,98 @@
/* 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 "gpio_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 ---------------------------------------------------------*/
osThreadId comm_thread;
const uint32_t stack_size_comm_thread = 4096; // Bytes
volatile bool endpoint_list_valid = false;
/* Private function prototypes -----------------------------------------------*/
/* Function implementations --------------------------------------------------*/
void init_communication(void) {
printf("hi!\r\n");
// Start command handling thread
osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, stack_size_comm_thread / sizeof(StackType_t));
comm_thread = osThreadCreate(osThread(task_cmd_parse), NULL);
while (!endpoint_list_valid)
osDelay(1);
}
float oscilloscope[OSCILLOSCOPE_SIZE] = {0};
size_t oscilloscope_pos = 0;
// Thread to handle deffered processing of USB interrupt, and
// read commands out of the UART DMA circular buffer
void communication_task(void * ctx) {
(void) ctx; // unused parameter
// Allow main init to continue
endpoint_list_valid = true;
start_uart_server();
start_usb_server();
if (odrv.config_.enable_i2c_instead_of_can) {
start_i2c_server();
} else {
odCAN->start_can_server();
}
for (;;) {
osDelay(1000); // nothing to do
}
}
extern "C" {
int _write(int file, const char* data, int len);
}
// @brief This is what printf calls internally
int _write(int file, const char* data, int len) {
#ifdef USB_PROTOCOL_STDOUT
usb_stream_output_ptr->process_bytes((const uint8_t *)data, len, nullptr);
#endif
#ifdef UART_PROTOCOL_STDOUT
uart4_stream_output_ptr->process_bytes((const uint8_t *)data, len, nullptr);
#endif
return len;
}
#include "../autogen/function_stubs.hpp"
ODrive& ep_root = odrv;
#include "../autogen/endpoints.hpp"
Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/communication/communication.h
+28
View File
@@ -0,0 +1,28 @@
#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>
extern osThreadId comm_thread;
extern const uint32_t stack_size_comm_thread;
void init_communication(void);
void initTree();
void communication_task(void * ctx);
#ifdef __cplusplus
}
#endif
#endif /* COMMANDS_H */
Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/communication/interface_can.cpp
+214
View File
@@ -0,0 +1,214 @@
#include "interface_can.hpp"
#include "fibre/crc.hpp"
#include "freertos_vars.h"
#include "utils.hpp"
#include <can.h>
#include <cmsis_os.h>
#include <stm32f4xx_hal.h>
// Specific CAN Protocols
#include "can_simple.hpp"
// Safer context handling via maps instead of arrays
// #include <unordered_map>
// std::unordered_map<CAN_HandleTypeDef *, ODriveCAN *> ctxMap;
// Constructor is called by communication.cpp and the handle is assigned appropriately
ODriveCAN::ODriveCAN(ODriveCAN::Config_t &config, CAN_HandleTypeDef *handle)
: config_{config},
handle_{handle} {
// ctxMap[handle_] = this;
}
void ODriveCAN::can_server_thread() {
for (;;) {
uint32_t status = HAL_CAN_GetError(handle_);
if (status == HAL_CAN_ERROR_NONE) {
can_Message_t rxmsg;
osSemaphoreWait(sem_can, 10); // Poll every 10ms regardless of sempahore status
while (available()) {
read(rxmsg);
switch (config_.protocol) {
case PROTOCOL_SIMPLE:
CANSimple::handle_can_message(rxmsg);
break;
}
}
HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING);
} 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);
}
}
}
}
static void can_server_thread_wrapper(void *ctx) {
reinterpret_cast<ODriveCAN *>(ctx)->can_server_thread();
reinterpret_cast<ODriveCAN *>(ctx)->thread_id_valid_ = false;
}
bool ODriveCAN::start_can_server() {
HAL_StatusTypeDef status;
set_baud_rate(config_.baud_rate);
status = HAL_CAN_Init(handle_);
CAN_FilterTypeDef filter;
filter.FilterActivation = ENABLE;
filter.FilterBank = 0;
filter.FilterFIFOAssignment = CAN_RX_FIFO0;
filter.FilterIdHigh = 0x0000;
filter.FilterIdLow = 0x0000;
filter.FilterMaskIdHigh = 0x0000;
filter.FilterMaskIdLow = 0x0000;
filter.FilterMode = CAN_FILTERMODE_IDMASK;
filter.FilterScale = CAN_FILTERSCALE_32BIT;
status = HAL_CAN_ConfigFilter(handle_, &filter);
status = HAL_CAN_Start(handle_);
if (status == HAL_OK)
status = HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING);
osThreadDef(can_server_thread_def, can_server_thread_wrapper, osPriorityNormal, 0, stack_size_ / sizeof(StackType_t));
thread_id_ = osThreadCreate(osThread(can_server_thread_def), this);
thread_id_valid_ = true;
return status;
}
// Send a CAN message on the bus
uint32_t ODriveCAN::write(can_Message_t &txmsg) {
if (HAL_CAN_GetError(handle_) == HAL_CAN_ERROR_NONE) {
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_) > 0)
HAL_CAN_AddTxMessage(handle_, &header, txmsg.buf, &retTxMailbox);
return retTxMailbox;
} else {
return -1;
}
}
uint32_t ODriveCAN::available() {
return (HAL_CAN_GetRxFifoFillLevel(handle_, CAN_RX_FIFO0) + HAL_CAN_GetRxFifoFillLevel(handle_, CAN_RX_FIFO1));
}
bool ODriveCAN::read(can_Message_t &rxmsg) {
CAN_RxHeaderTypeDef header;
bool validRead = false;
if (HAL_CAN_GetRxFifoFillLevel(handle_, CAN_RX_FIFO0) > 0) {
HAL_CAN_GetRxMessage(handle_, CAN_RX_FIFO0, &header, rxmsg.buf);
validRead = true;
} else if (HAL_CAN_GetRxFifoFillLevel(handle_, CAN_RX_FIFO1) > 0) {
HAL_CAN_GetRxMessage(handle_, CAN_RX_FIFO1, &header, rxmsg.buf);
validRead = true;
}
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;
return validRead;
}
// 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
void ODriveCAN::set_baud_rate(uint32_t baudRate) {
switch (baudRate) {
case CAN_BAUD_125K:
handle_->Init.Prescaler = 16; // 21 TQ's
config_.baud_rate = baudRate;
reinit_can();
break;
case CAN_BAUD_250K:
handle_->Init.Prescaler = 8; // 21 TQ's
config_.baud_rate = baudRate;
reinit_can();
break;
case CAN_BAUD_500K:
handle_->Init.Prescaler = 4; // 21 TQ's
config_.baud_rate = baudRate;
reinit_can();
break;
case CAN_BAUD_1000K:
handle_->Init.Prescaler = 2; // 21 TQ's
config_.baud_rate = baudRate;
reinit_can();
break;
default:
// baudRate is invalid, so don't accept it.
break;
}
}
void ODriveCAN::reinit_can() {
HAL_CAN_Stop(handle_);
HAL_CAN_Init(handle_);
auto status = HAL_CAN_Start(handle_);
if (status == HAL_OK)
status = HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING);
}
void ODriveCAN::set_error(Error error) {
error_ |= error;
}
// This function is called by each axis.
// It provides an abstraction from the specific CAN protocol in use
void ODriveCAN::send_heartbeat(Axis *axis) {
// Handle heartbeat message
if (axis->config_.can_heartbeat_rate_ms > 0) {
uint32_t now = osKernelSysTick();
if ((now - axis->last_heartbeat_) >= axis->config_.can_heartbeat_rate_ms) {
switch (config_.protocol) {
case PROTOCOL_SIMPLE:
CANSimple::send_heartbeat(axis);
break;
}
axis->last_heartbeat_ = now;
}
}
}
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) {}
void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) {}
void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) {}
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) {
// 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);
}
Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/communication/interface_can.hpp
+57
View File
@@ -0,0 +1,57 @@
#ifndef __INTERFACE_CAN_HPP
#define __INTERFACE_CAN_HPP
#include <cmsis_os.h>
#include <stm32f4xx_hal.h>
#include "fibre/protocol.hpp"
#include "odrive_main.h"
#include "can_helpers.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 ODriveIntf::CanIntf {
public:
struct Config_t {
uint32_t baud_rate = CAN_BAUD_250K;
Protocol protocol = PROTOCOL_SIMPLE;
};
ODriveCAN(ODriveCAN::Config_t &config, CAN_HandleTypeDef *handle);
// Thread Relevant Data
osThreadId thread_id_;
const uint32_t stack_size_ = 1024; // Bytes
Error error_ = ERROR_NONE;
volatile bool thread_id_valid_ = false;
bool start_can_server();
void can_server_thread();
void send_heartbeat(Axis *axis);
void reinit_can();
void set_error(Error error);
// I/O Functions
uint32_t available();
uint32_t write(can_Message_t &txmsg);
bool read(can_Message_t &rxmsg);
ODriveCAN::Config_t &config_;
private:
CAN_HandleTypeDef *handle_ = nullptr;
void set_baud_rate(uint32_t baudRate);
};
#endif // __INTERFACE_CAN_HPP
Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/communication/interface_i2c.cpp
+86
View File
@@ -0,0 +1,86 @@
#include "interface_i2c.h"
#include "fibre/protocol.hpp"
#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_;
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);
}
Makerbase_ODrive/ODrive-fw-v0.5.1/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
Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/communication/interface_uart.cpp
+112
View File
@@ -0,0 +1,112 @@
#include "interface_uart.h"
#include "ascii_protocol.hpp"
#include <MotorControl/utils.hpp>
#include <fibre/protocol.hpp>
#include <usart.h>
#include <cmsis_os.h>
#include <freertos_vars.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;
// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable
// static thread_local uint32_t deadline_ms = 0;
osThreadId uart_thread;
const uint32_t stack_size_uart_thread = 4096; // Bytes
class UART4Sender : public StreamSink {
public:
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) {
// Loop to ensure all bytes get sent
while (length) {
size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
// wait for USB interface to become ready
// TODO: implement ring buffer to get a more continuous stream of data
// if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
if (osSemaphoreWait(sem_uart_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
return -1;
// transmit chunk
memcpy(tx_buf_, buffer, chunk);
if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK)
return -1;
buffer += chunk;
length -= chunk;
if (processed_bytes)
*processed_bytes += chunk;
}
return 0;
}
size_t get_free_space() { return SIZE_MAX; }
private:
uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
} uart4_stream_output;
StreamSink* uart4_stream_output_ptr = &uart4_stream_output;
StreamBasedPacketSink uart4_packet_output(uart4_stream_output);
BidirectionalPacketBasedChannel uart4_channel(uart4_packet_output);
StreamToPacketSegmenter uart4_stream_input(uart4_channel);
static void uart_server_thread(void * ctx) {
(void) ctx;
for (;;) {
osDelay(1);
// Check for UART errors and restart recieve DMA transfer if required
if (huart4.RxState != HAL_UART_STATE_BUSY_RX) {
HAL_UART_AbortReceive(&huart4);
HAL_UART_Receive_DMA(&huart4, 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 - huart4.hdmarx->Instance->NDTR;
if (new_rcv_idx > UART_RX_BUFFER_SIZE) { // defensive programming
continue;
}
// deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
// Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < dma_last_rcv_idx) {
uart4_stream_input.process_bytes(dma_rx_buffer + dma_last_rcv_idx,
UART_RX_BUFFER_SIZE - dma_last_rcv_idx, nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer + dma_last_rcv_idx,
UART_RX_BUFFER_SIZE - dma_last_rcv_idx, uart4_stream_output);
dma_last_rcv_idx = 0;
}
if (new_rcv_idx > dma_last_rcv_idx) {
uart4_stream_input.process_bytes(dma_rx_buffer + dma_last_rcv_idx,
new_rcv_idx - dma_last_rcv_idx, nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer + dma_last_rcv_idx,
new_rcv_idx - dma_last_rcv_idx, uart4_stream_output);
dma_last_rcv_idx = new_rcv_idx;
}
};
}
void start_uart_server() {
// DMA is set up to recieve 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(&huart4, 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 HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) {
osSemaphoreRelease(sem_uart_dma);
}
Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/communication/interface_uart.h
+22
View File
@@ -0,0 +1,22 @@
#ifndef __INTERFACE_UART_HPP
#define __INTERFACE_UART_HPP
#ifdef __cplusplus
#include "fibre/protocol.hpp"
extern StreamSink* uart4_stream_output_ptr;
extern "C" {
#endif
#include <cmsis_os.h>
extern osThreadId uart_thread;
extern const uint32_t stack_size_uart_thread;
void start_uart_server(void);
#ifdef __cplusplus
}
#endif
#endif // __INTERFACE_UART_HPP
Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/communication/interface_usb.cpp
+183
View File
@@ -0,0 +1,183 @@
#include "interface_usb.h"
#include "ascii_protocol.hpp"
#include <MotorControl/utils.hpp>
#include <fibre/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_;
class USBSender : public PacketSink {
public:
USBSender(uint8_t endpoint_pair, const osSemaphoreId& sem_usb_tx)
: endpoint_pair_(endpoint_pair), sem_usb_tx_(sem_usb_tx) {}
int process_packet(const uint8_t* buffer, size_t length) {
// cannot send partial packets
if (length > USB_TX_DATA_SIZE)
return -1;
// wait for USB interface to become ready
if (osSemaphoreWait(sem_usb_tx_, PROTOCOL_SERVER_TIMEOUT_MS) != osOK) {
// If the host resets the device it might be that the TX-complete handler is never called
// and the sem_usb_tx_ semaphore is never released. To handle this we just override the
// TX buffer if this wait times out. The implication is that the channel is no longer lossless.
// TODO: handle endpoint reset properly
usb_stats_.tx_overrun_cnt++;
}
// transmit packet
uint8_t status = CDC_Transmit_FS(
const_cast<uint8_t*>(buffer) /* casting this const away is safe because...
well... it's not actually. Stupid STM. */, length, endpoint_pair_);
if (status != USBD_OK) {
osSemaphoreRelease(sem_usb_tx_);
return -1;
}
usb_stats_.tx_cnt++;
return 0;
}
private:
uint8_t endpoint_pair_;
const osSemaphoreId& sem_usb_tx_;
};
// Note we could have independent semaphores here to allow concurrent transmission
USBSender usb_packet_output_cdc(CDC_OUT_EP, sem_usb_tx);
USBSender usb_packet_output_native(ODRIVE_OUT_EP, sem_usb_tx);
class TreatPacketSinkAsStreamSink : public StreamSink {
public:
TreatPacketSinkAsStreamSink(PacketSink& output) : output_(output) {}
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) {
// Loop to ensure all bytes get sent
while (length) {
size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE;
if (output_.process_packet(buffer, chunk) != 0)
return -1;
buffer += chunk;
length -= chunk;
if (processed_bytes)
*processed_bytes += chunk;
}
return 0;
}
size_t get_free_space() { return SIZE_MAX; }
private:
PacketSink& output_;
} usb_stream_output(usb_packet_output_cdc);
// This is used by the printf feature. Hence the above statics, and below seemingly random ptr (it's externed)
// TODO: less spaghetti code
StreamSink* usb_stream_output_ptr = &usb_stream_output;
#if defined(USB_PROTOCOL_NATIVE)
BidirectionalPacketBasedChannel usb_channel(usb_packet_output_native);
#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED)
StreamBasedPacketSink usb_packetized_output(usb_stream_output);
BidirectionalPacketBasedChannel usb_channel(usb_packetized_output);
StreamToPacketSegmenter usb_native_stream_input(usb_channel);
#endif
struct USBInterface {
uint8_t* rx_buf = nullptr;
uint32_t rx_len = 0;
bool data_pending = false;
uint8_t out_ep;
uint8_t in_ep;
USBSender& usb_sender;
};
// Note: statics make this less modular.
// Note: we use a single rx semaphore and loop over data_pending to allow a single pump loop thread
static USBInterface CDC_interface = {
.rx_buf = nullptr,
.rx_len = 0,
.data_pending = false,
.out_ep = CDC_OUT_EP,
.in_ep = CDC_IN_EP,
.usb_sender = usb_packet_output_cdc,
};
static USBInterface ODrive_interface = {
.rx_buf = nullptr,
.rx_len = 0,
.data_pending = false,
.out_ep = ODRIVE_OUT_EP,
.in_ep = ODRIVE_IN_EP,
.usb_sender = usb_packet_output_native,
};
static void usb_server_thread(void * ctx) {
(void) ctx;
for (;;) {
// const uint32_t usb_check_timeout = 1; // ms
osStatus sem_stat = osSemaphoreWait(sem_usb_rx, osWaitForever);
if (sem_stat == osOK) {
usb_stats_.rx_cnt++;
// CDC Interface
if (CDC_interface.data_pending) {
CDC_interface.data_pending = false;
if (odrv.config_.enable_ascii_protocol_on_usb) {
ASCII_protocol_parse_stream(CDC_interface.rx_buf,
CDC_interface.rx_len, usb_stream_output);
} else {
#if defined(USB_PROTOCOL_NATIVE)
usb_channel.process_packet(CDC_interface.rx_buf, CDC_interface.rx_len);
#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED)
usb_native_stream_input.process_bytes(
CDC_interface.rx_buf, CDC_interface.rx_len, nullptr);
#endif
}
USBD_CDC_ReceivePacket(&hUsbDeviceFS, CDC_interface.out_ep); // Allow next packet
}
// Native Interface
if (ODrive_interface.data_pending) {
ODrive_interface.data_pending = false;
#if defined(USB_PROTOCOL_NATIVE)
usb_channel.process_packet(ODrive_interface.rx_buf, ODrive_interface.rx_len);
#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED)
usb_native_stream_input.process_bytes(
ODrive_interface.rx_buf, ODrive_interface.rx_len, nullptr);
#endif
USBD_CDC_ReceivePacket(&hUsbDeviceFS, ODrive_interface.out_ep); // Allow next packet
}
}
}
}
// 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) {
USBInterface* usb_iface;
if (endpoint_pair == CDC_interface.out_ep) {
usb_iface = &CDC_interface;
} else if (endpoint_pair == ODrive_interface.out_ep) {
usb_iface = &ODrive_interface;
} else {
return;
}
// We don't allow the next USB packet until the previous one has been processed completely.
// Therefore it's safe to write to these vars directly since we know previous processing is complete.
usb_iface->rx_buf = buf;
usb_iface->rx_len = len;
usb_iface->data_pending = true;
osSemaphoreRelease(sem_usb_rx);
}
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);
}
Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/communication/interface_usb.h
+32
View File
@@ -0,0 +1,32 @@
#ifndef __INTERFACE_USB_HPP
#define __INTERFACE_USB_HPP
#ifdef __cplusplus
#include "fibre/protocol.hpp"
extern StreamSink* usb_stream_output_ptr;
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
#endif // __INTERFACE_USB_HPP