Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-08-18 01:18:52 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

refactor CAN

Browse Source
This commit is contained in:
Samuel Sadok
2020-12-03 12:53:54 +01:00
parent 520783d4b6
commit 4e350823b4
14 changed files with 362 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CHANGELOG.md
+1
View File
@@ -61,6 +61,7 @@ Please add a note of your changes below this heading if you make a Pull Request.
* `<axis>.encoder.config.offset` was renamed to ``<axis>.encoder.config.phase_offset`
* `<axis>.encoder.config.offset_float` was renamed to ``<axis>.encoder.config.phase_offset_float`
* `<odrv>.config.brake_resistance == 0.0` is no longer a valid way to disable the brake resistor. Use `<odrv>.config.enable_brake_resistor` instead.
* `<odrv>.can.set_baud_rate()` was removed. The baudrate is now automatically updated when writing to `<odrv>.can.config.baud_rate`.
# Releases
## [0.5.1] - 2020-09-27
Firmware/Board/v3/Src/can.c
+38 -23
View File
@@ -56,31 +56,46 @@
/* USER CODE END 0 */
CAN_HandleTypeDef hcan1;
CAN_HandleTypeDef hcan1 = {
.Instance = CAN1,
.Init = {
.Prescaler = 8,
.Mode = CAN_MODE_NORMAL,
.SyncJumpWidth = CAN_SJW_4TQ,
.TimeSeg1 = CAN_BS1_16TQ,
.TimeSeg2 = CAN_BS2_4TQ,
.TimeTriggeredMode = DISABLE,
.AutoBusOff = ENABLE,
.AutoWakeUp = ENABLE,
.AutoRetransmission = ENABLE,
.ReceiveFifoLocked = DISABLE,
.TransmitFifoPriority = DISABLE,
}
};
/* CAN1 init function */
void MX_CAN1_Init(void)
{
hcan1.Instance = CAN1;
hcan1.Init.Prescaler = 8;
hcan1.Init.Mode = CAN_MODE_NORMAL;
hcan1.Init.SyncJumpWidth = CAN_SJW_4TQ;
hcan1.Init.TimeSeg1 = CAN_BS1_16TQ;
hcan1.Init.TimeSeg2 = CAN_BS2_4TQ;
hcan1.Init.TimeTriggeredMode = DISABLE;
hcan1.Init.AutoBusOff = ENABLE;
hcan1.Init.AutoWakeUp = ENABLE;
hcan1.Init.AutoRetransmission = ENABLE;
hcan1.Init.ReceiveFifoLocked = DISABLE;
hcan1.Init.TransmitFifoPriority = DISABLE;
if (HAL_CAN_Init(&hcan1) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
}
//void MX_CAN1_Init(void)
//{
//
// hcan1.Instance = CAN1;
// hcan1.Init.Prescaler = 8;
// hcan1.Init.Mode = CAN_MODE_NORMAL;
// hcan1.Init.SyncJumpWidth = CAN_SJW_4TQ;
// hcan1.Init.TimeSeg1 = CAN_BS1_16TQ;
// hcan1.Init.TimeSeg2 = CAN_BS2_4TQ;
// hcan1.Init.TimeTriggeredMode = DISABLE;
// hcan1.Init.AutoBusOff = ENABLE;
// hcan1.Init.AutoWakeUp = ENABLE;
// hcan1.Init.AutoRetransmission = ENABLE;
// hcan1.Init.ReceiveFifoLocked = DISABLE;
// hcan1.Init.TransmitFifoPriority = DISABLE;
// if (HAL_CAN_Init(&hcan1) != HAL_OK)
// {
// _Error_Handler(__FILE__, __LINE__);
// }
//
//}
//
void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
{
Firmware/Board/v3/board.cpp
-2
View File
@@ -357,8 +357,6 @@ bool board_init() {
// mode initialization won't override the CAN mode.
if (odrv.config_.gpio_modes[15] != ODriveIntf::GPIO_MODE_CAN_A || odrv.config_.gpio_modes[16] != ODriveIntf::GPIO_MODE_CAN_A) {
odrv.misconfigured_ = true;
} else {
MX_CAN1_Init();
}
}
Firmware/MotorControl/encoder.hpp
+3 -1
View File
@@ -1,7 +1,9 @@
#ifndef __ENCODER_HPP
#define __ENCODER_HPP
#include <arm_math.h>
class Encoder;
#include <board.h> // needed for arm_math.h
#include <Drivers/STM32/stm32_spi_arbiter.hpp>
#include "utils.hpp"
#include <autogen/interfaces.hpp>
Firmware/MotorControl/main.cpp
+5 -16
View File
@@ -28,7 +28,7 @@ extern char _estack; // provided by the linker script
ODriveCAN::Config_t can_config;
ODriveCAN *odCAN = nullptr;
CANSimple *can_simple = nullptr;
ODrive odrv{};
@@ -146,7 +146,7 @@ static void config_clear_all() {
}
static bool config_apply_all() {
bool success = true;
bool success = odrv.can_.apply_config();
for (size_t i = 0; (i < AXIS_COUNT) && success; ++i) {
success = encoders[i].apply_config(motors[i].config_.motor_type)
&& axes[i].controller_.apply_config()
@@ -266,19 +266,19 @@ void vApplicationIdleHook(void) {
odrv.system_stats_.max_stack_usage_usb = stack_size_usb_thread - uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t);
odrv.system_stats_.max_stack_usage_uart = stack_size_uart_thread - uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t);
odrv.system_stats_.max_stack_usage_startup = stack_size_default_task - uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t);
odrv.system_stats_.max_stack_usage_can = odCAN ? (odCAN->stack_size_ - uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t)) : 0;
odrv.system_stats_.max_stack_usage_can = odrv.can_.stack_size_ - uxTaskGetStackHighWaterMark(odrv.can_.thread_id_) * sizeof(StackType_t);
odrv.system_stats_.stack_size_axis = axes[0].stack_size_;
odrv.system_stats_.stack_size_usb = stack_size_usb_thread;
odrv.system_stats_.stack_size_uart = stack_size_uart_thread;
odrv.system_stats_.stack_size_startup = stack_size_default_task;
odrv.system_stats_.stack_size_can = odCAN ? odCAN->stack_size_ : 0;
odrv.system_stats_.stack_size_can = odrv.can_.stack_size_;
odrv.system_stats_.prio_axis = osThreadGetPriority(axes[0].thread_id_);
odrv.system_stats_.prio_usb = osThreadGetPriority(usb_thread);
odrv.system_stats_.prio_uart = osThreadGetPriority(uart_thread);
odrv.system_stats_.prio_startup = osThreadGetPriority(defaultTaskHandle);
odrv.system_stats_.prio_can = odCAN ? osThreadGetPriority(odCAN->thread_id_) : 0;
odrv.system_stats_.prio_can = osThreadGetPriority(odrv.can_.thread_id_);
status_led_controller.update();
}
@@ -829,17 +829,6 @@ extern "C" int main(void) {
sem_can = osSemaphoreCreate(osSemaphore(sem_can), 1);
osSemaphoreWait(sem_can, 0);
// Construct all objects.
if (odrv.config_.enable_can_a) {
switch (can_config.protocol) {
case ODriveIntf::CanIntf::Protocol::PROTOCOL_SIMPLE:
odCAN = new CANSimple(can_config, &hcan1);
break;
default:
break;
}
}
// Create main thread
osThreadDef(defaultTask, rtos_main, osPriorityNormal, 0, stack_size_default_task / sizeof(StackType_t));
defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);
Firmware/MotorControl/odrive_main.h
+3 -4
View File
@@ -120,9 +120,6 @@ struct TaskTimes {
// Forward Declarations
class Axis;
class Motor;
class ODriveCAN;
extern ODriveCAN *odCAN;
// TODO: move
// this is technically not thread-safe but practically it might be
@@ -151,6 +148,7 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast<ENUMTYPE>(~static_c
#include <axis.hpp>
#include <oscilloscope.hpp>
#include <communication/communication.h>
#include <communication/can/odrive_can.hpp>
// Defined in autogen/version.c based on git-derived version numbers
extern "C" {
@@ -188,7 +186,6 @@ public:
void control_loop_cb(uint32_t timestamp);
Axis& get_axis(int num) { return axes[num]; }
ODriveCAN& get_can() { return *odCAN; }
uint32_t get_interrupt_status(int32_t irqn);
uint32_t get_dma_status(uint8_t stream_num);
@@ -227,6 +224,8 @@ public:
nullptr // data_src TODO: change data type
};
ODriveCAN can_;
BoardConfig_t config_;
uint32_t user_config_loaded_ = 0;
bool misconfigured_ = false;
Firmware/communication/can/can_simple.cpp
+66 -40
View File
@@ -3,6 +3,41 @@
#include <odrive_main.h>
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 = {
.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
@@ -11,13 +46,13 @@ void CANSimple::handle_can_message(const can_Message_t& msg) {
for (auto& axis : axes) {
if ((axis.config_.can.node_id == nodeID) && (axis.config_.can.is_extended == msg.isExt)) {
doCommand(axis, msg);
do_command(axis, msg);
return;
}
}
}
void CANSimple::doCommand(Axis& axis, const can_Message_t& msg) {
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) {
@@ -118,7 +153,7 @@ void CANSimple::estop_callback(Axis& axis, const can_Message_t& msg) {
axis.error_ |= Axis::ERROR_ESTOP_REQUESTED;
}
int32_t CANSimple::get_motor_error_callback(const Axis& axis) {
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
@@ -127,10 +162,10 @@ int32_t CANSimple::get_motor_error_callback(const Axis& axis) {
can_setSignal(txmsg, axis.motor_.error_, 0, 32, true);
return odCAN->write(txmsg);
return canbus_->send_message(txmsg);
}
int32_t CANSimple::get_encoder_error_callback(const Axis& axis) {
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
@@ -139,10 +174,10 @@ int32_t CANSimple::get_encoder_error_callback(const Axis& axis) {
can_setSignal(txmsg, axis.encoder_.error_, 0, 32, true);
return odCAN->write(txmsg);
return canbus_->send_message(txmsg);
}
int32_t CANSimple::get_sensorless_error_callback(const Axis& axis) {
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
@@ -151,7 +186,7 @@ int32_t CANSimple::get_sensorless_error_callback(const Axis& axis) {
can_setSignal(txmsg, axis.sensorless_estimator_.error_, 0, 32, true);
return odCAN->write(txmsg);
return canbus_->send_message(txmsg);
}
void CANSimple::set_axis_nodeid_callback(Axis& axis, const can_Message_t& msg) {
@@ -166,7 +201,7 @@ void CANSimple::set_axis_startup_config_callback(Axis& axis, const can_Message_t
// Not Implemented
}
int32_t CANSimple::get_encoder_estimates_callback(const Axis& axis) {
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
@@ -176,10 +211,10 @@ int32_t CANSimple::get_encoder_estimates_callback(const Axis& axis) {
can_setSignal<float>(txmsg, axis.encoder_.pos_estimate_.any().value_or(0.0f), 0, 32, true);
can_setSignal<float>(txmsg, axis.encoder_.vel_estimate_.any().value_or(0.0f), 32, 32, true);
return odCAN->write(txmsg);
return canbus_->send_message(txmsg);
}
int32_t CANSimple::get_sensorless_estimates_callback(const Axis& axis) {
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
@@ -191,10 +226,10 @@ int32_t CANSimple::get_sensorless_estimates_callback(const Axis& axis) {
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 odCAN->write(txmsg);
return canbus_->send_message(txmsg);
}
int32_t CANSimple::get_encoder_count_callback(const Axis& axis) {
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;
@@ -203,7 +238,7 @@ int32_t CANSimple::get_encoder_count_callback(const Axis& axis) {
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 odCAN->write(txmsg);
return canbus_->send_message(txmsg);
}
void CANSimple::set_input_pos_callback(Axis& axis, const can_Message_t& msg) {
@@ -252,7 +287,7 @@ 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));
}
int32_t CANSimple::get_iq_callback(const Axis& axis) {
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;
@@ -269,10 +304,10 @@ int32_t CANSimple::get_iq_callback(const Axis& axis) {
can_setSignal<float>(txmsg, Idq_setpoint->first, 0, 32, true);
can_setSignal<float>(txmsg, Idq_setpoint->second, 32, 32, true);
return odCAN->write(txmsg);
return canbus_->send_message(txmsg);
}
int32_t CANSimple::get_vbus_voltage_callback(const Axis& axis) {
bool CANSimple::get_vbus_voltage_callback(const Axis& axis) {
can_Message_t txmsg;
txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS;
@@ -284,7 +319,7 @@ int32_t CANSimple::get_vbus_voltage_callback(const Axis& axis) {
static_assert(sizeof(vbus_voltage) == sizeof(floatBytes));
can_setSignal<float>(txmsg, vbus_voltage, 0, 32, true);
return odCAN->write(txmsg);
return canbus_->send_message(txmsg);
}
void CANSimple::clear_errors_callback(Axis& axis, const can_Message_t& msg) {
@@ -295,11 +330,20 @@ 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);
}
}
for (auto& a: axes) {
MEASURE_TIME(a.task_times_.can_heartbeat) {
if (a.config_.can.heartbeat_rate_ms > 0) {
if ((now - a.can_.last_heartbeat) >= a.config_.can.heartbeat_rate_ms) {
if (send_heartbeat(a) >= 0)
if (send_heartbeat(a))
a.can_.last_heartbeat = now;
}
@@ -309,7 +353,7 @@ uint32_t CANSimple::service_stack() {
if (a.config_.can.encoder_rate_ms > 0) {
if ((now - a.can_.last_encoder) >= a.config_.can.encoder_rate_ms) {
if (get_encoder_estimates_callback(a) >= 0)
if (get_encoder_estimates_callback(a))
a.can_.last_encoder = now;
}
@@ -322,7 +366,7 @@ uint32_t CANSimple::service_stack() {
return nextServiceTime;
}
int32_t CANSimple::send_heartbeat(const Axis& axis) {
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
@@ -332,23 +376,5 @@ int32_t CANSimple::send_heartbeat(const Axis& axis) {
can_setSignal(txmsg, axis.error_, 0, 32, true);
can_setSignal(txmsg, axis.current_state_, 32, 32, true);
return odCAN->write(txmsg);
}
void CANSimple::send_cyclic(Axis& axis) {
const uint32_t now = HAL_GetTick();
if (axis.config_.can.heartbeat_rate_ms > 0) {
if ((now - axis.can_.last_heartbeat) >= axis.config_.can.heartbeat_rate_ms) {
if(send_heartbeat(axis) >= 0)
axis.can_.last_heartbeat = now;
}
}
if (axis.config_.can.encoder_rate_ms > 0) {
if ((now - axis.can_.last_encoder) >= axis.config_.can.encoder_rate_ms) {
if(get_encoder_estimates_callback(axis) >= 0)
axis.can_.last_encoder = now;
}
}
return canbus_->send_message(txmsg);
}
Firmware/communication/can/can_simple.hpp
+29 -19
View File
@@ -1,9 +1,10 @@
#ifndef __CAN_SIMPLE_HPP_
#define __CAN_SIMPLE_HPP_
#include "odrive_can.hpp"
#include "canbus.hpp"
#include "axis.hpp"
class CANSimple : public ODriveCAN {
class CANSimple {
public:
enum {
MSG_CO_NMT_CTRL = 0x000, // CANOpen NMT Message REC
@@ -34,29 +35,30 @@ class CANSimple : public ODriveCAN {
MSG_CO_HEARTBEAT_CMD = 0x700, // CANOpen NMT Heartbeat SEND
};
using ODriveCAN::ODriveCAN;
CANSimple(CanBusBase* canbus) : canbus_(canbus) {}
// Cyclic Senders
static int32_t send_heartbeat(const Axis& axis);
static void send_cyclic(Axis& axis);
bool init();
uint32_t service_stack();
private:
uint32_t service_stack() final;
void handle_can_message(const can_Message_t& msg) final;
static void doCommand(Axis& axis, const can_Message_t& cmd);
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)
static int32_t get_motor_error_callback(const Axis& axis);
static int32_t get_encoder_error_callback(const Axis& axis);
static int32_t get_controller_error_callback(const Axis& axis);
static int32_t get_sensorless_error_callback(const Axis& axis);
static int32_t get_encoder_estimates_callback(const Axis& axis);
static int32_t get_encoder_count_callback(const Axis& axis);
static int32_t get_iq_callback(const Axis& axis);
static int32_t get_sensorless_estimates_callback(const Axis& axis);
static int32_t get_vbus_voltage_callback(const Axis& axis);
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_vbus_voltage_callback(const Axis& axis);
// Set functions
static void set_axis_nodeid_callback(Axis& axis, const can_Message_t& msg);
@@ -89,6 +91,14 @@ class CANSimple : public ODriveCAN {
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
Firmware/communication/can/odrive_can.cpp
+173 -140
View File
@@ -11,164 +11,195 @@
// #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;
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;
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 nextServiceTime = service_stack();
uint32_t next_service_time = UINT32_MAX;
uint32_t rxNum = available();
for (uint32_t i = 0; i < rxNum; i++) {
can_Message_t rxmsg;
read(rxmsg);
handle_can_message(rxmsg);
}
HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_TX_MAILBOX_EMPTY);
osSemaphoreWait(sem_can, nextServiceTime);
} 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);
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);
osSemaphoreWait(sem_can, next_service_time);
} 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);
}
}
}
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 | CAN_IT_TX_MAILBOX_EMPTY);
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
int32_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);
else
return -1;
return (int32_t)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 = CAN_FREQ / 125000UL;
config_.baud_rate = baudRate;
reinit_can();
break;
case CAN_BAUD_250K:
handle_->Init.Prescaler = CAN_FREQ / 250000UL;
config_.baud_rate = baudRate;
reinit_can();
break;
case CAN_BAUD_500K:
handle_->Init.Prescaler = CAN_FREQ / 500000UL;
config_.baud_rate = baudRate;
reinit_can();
break;
case CAN_BAUD_1000K:
handle_->Init.Prescaler = CAN_FREQ / 1000000UL;
config_.baud_rate = baudRate;
reinit_can();
break;
default:
// baudRate is invalid, so don't accept it.
break;
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::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 | CAN_IT_TX_MAILBOX_EMPTY);
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);
}
}
void ODriveCAN::set_error(Error error) {
error_ |= error;
// 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 = {.FilterActivation = DISABLE};
return HAL_CAN_ConfigFilter(handle_, &hal_filter) == HAL_OK;
}
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) {
@@ -191,12 +222,14 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
osSemaphoreRelease(sem_can);
}
void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef *hcan) {
// osSemaphoreRelease(sem_can);
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);
//HAL_CAN_ResetError(hcan);
}
Firmware/communication/can/odrive_can.hpp
+36 -26
View File
@@ -3,9 +3,10 @@
#include <cmsis_os.h>
#include "can_helpers.hpp"
#include "canbus.hpp"
#include "fibre/protocol.hpp"
#include "odrive_main.h"
#include "can_simple.hpp"
#include <autogen/interfaces.hpp>
#define CAN_CLK_HZ (42000000)
#define CAN_CLK_MHZ (42)
@@ -19,41 +20,50 @@ enum {
CAN_BAUD_1M = 1000000
};
class ODriveCAN : public ODriveIntf::CanIntf {
public:
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(baud_rate); }
};
ODriveCAN(ODriveCAN::Config_t &config, CAN_HandleTypeDef *handle);
ODriveCAN() {}
bool apply_config();
bool start_server(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();
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();
void reinit_can();
void set_error(Error error);
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;
// I/O Functions
uint32_t available();
int32_t write(can_Message_t &txmsg);
bool read(can_Message_t &rxmsg);
ODriveCAN::Config_t &config_;
protected:
virtual uint32_t service_stack() = 0;
virtual void handle_can_message(const can_Message_t &msg) = 0;
private:
// 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;
void set_baud_rate(uint32_t baudRate);
};
#endif // __ODRIVE_CAN_HPP
Firmware/communication/communication.cpp
+2 -2
View File
@@ -55,8 +55,8 @@ void init_communication(void) {
start_i2c_server();
}
if (odCAN && odrv.config_.enable_can_a) {
odCAN->start_can_server();
if (odrv.config_.enable_can_a) {
odrv.can_.start_server(&hcan1);
}
}
Firmware/odrive-interface.yaml
+4 -5
View File
@@ -179,7 +179,7 @@ interfaces:
Example: `step_gpio_pin` of both axes were set to the same GPIO.
oscilloscope: {type: Oscilloscope}
can: {type: Can, c_name: get_can()}
can: {type: Can}
test_property: uint32
functions:
@@ -370,10 +370,8 @@ interfaces:
config:
c_is_class: False
attributes:
baud_rate: readonly uint32
baud_rate: {type: uint32, c_setter: 'set_baud_rate'}
protocol: Protocol
functions:
set_baud_rate: {in: {baudRate: uint32}}
ODrive.Endpoint:
c_is_class: False
@@ -1163,7 +1161,8 @@ valuetypes:
Status: {doc: The pin is used for status output (see `config.error_gpio_pin`)}
ODrive.Can.Protocol:
values: {Simple: }
flags:
Simple:
ODrive.Axis.AxisState: # TODO: remove redundant "Axis" in name
values:
tools/odrive/enums.py
+1 -1
View File
@@ -22,7 +22,7 @@ GPIO_MODE_MECH_BRAKE = 14
GPIO_MODE_STATUS = 15
# ODrive.Can.Protocol
PROTOCOL_SIMPLE = 0
PROTOCOL_SIMPLE = 0x00000001
# ODrive.Axis.AxisState
AXIS_STATE_UNDEFINED = 0
tools/odrive/tests/can_test.py
+1
View File
@@ -136,6 +136,7 @@ class TestSimpleCAN():
test_assert_eq(my_req('get_vbus_voltage')['vbus_voltage'], odrive.handle.vbus_voltage, accuracy=0.01)
my_cmd('set_node_id', node_id=node_id+20)
time.sleep(0.1) # TODO: remove this hack (see note in firmware)
asyncio.run(request(canbus.handle, node_id+20, extended_id, 'get_vbus_voltage'))
test_assert_eq(axis.config.can.node_id, node_id+20)