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

Merge branch 'feature/CAN' into develop

Browse Source
This commit is contained in:
Paul Guenette
2019-05-25 13:52:04 +02:00
parent 4ecaba769f 9b86d0a638
commit 41c23b0376
20 changed files with 1148 additions and 364 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+2
View File
@@ -41,3 +41,5 @@ tup.config
/_site
/.bundle
*.exe
Firmware/.vscode/c_cpp_properties.json
Vendored
+3 -16
View File
@@ -3,21 +3,8 @@
{
"name": "Win32",
"includePath": [
"${workspaceRoot}",
"${workspaceRoot}/fibre/cpp/include/**",
"${workspaceRoot}/MotorControl",
"${workspaceRoot}/communication",
"${workspaceRoot}/Drivers/DRV8301",
"${workspaceRoot}/Board/v3/Inc",
"${workspaceRoot}/Board/v3/Drivers/CMSIS/Include",
"${workspaceRoot}/Board/v3/Drivers/CMSIS/Device/ST/STM32F4xx/Include",
"${workspaceRoot}/Board/v3/Drivers/STM32F4xx_HAL_Driver/Inc",
"${workspaceRoot}/Board/v3/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy",
"${workspaceRoot}/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Core/Inc",
"${workspaceRoot}/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc",
"${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS",
"${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/include",
"${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F"
"${workspaceRoot}/**",
"C:/Tools/doctest/doctest"
],
"defines": [
"STM32F405xx",
@@ -30,7 +17,7 @@
"__packed=\"__attribute__((__packed__))\"",
"__GNUC__"
],
"intelliSenseMode": "clang-x64",
"intelliSenseMode": "gcc-x64",
"compilerPath": "\"${ARM_GCC_ROOT}/bin/arm-none-eabi-gcc.exe\" -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float",
"cStandard": "c11",
"cppStandard": "c++14"
Firmware/.vscode/settings.json
Vendored
+1 -1
View File
@@ -1,5 +1,5 @@
{
"C_Cpp.clang_format_style": "{ BasedOnStyle: Google, IndentWidth: 4, ColumnLimit: 0 }",
"C_Cpp.clang_format_style": "{ BasedOnStyle: Google, IndentWidth: 4, ColumnLimit: 0, AlignConsecutiveAssignments: true }",
"C_Cpp.intelliSenseEngine": "Default",
"C_Cpp.intelliSenseEngineFallback": "Disabled",
"files.exclude": {
Firmware/Board/v3/Inc/freertos_vars.h
+1
View File
@@ -7,6 +7,7 @@ extern osSemaphoreId sem_usb_irq;
extern osSemaphoreId sem_uart_dma;
extern osSemaphoreId sem_usb_rx;
extern osSemaphoreId sem_usb_tx;
extern osSemaphoreId sem_can;
extern osThreadId defaultTaskHandle;
extern osThreadId usb_irq_thread;
Firmware/Board/v3/Src/can.c
+6 -6
View File
@@ -63,15 +63,15 @@ void MX_CAN1_Init(void)
{
hcan1.Instance = CAN1;
hcan1.Init.Prescaler = 7;
hcan1.Init.Prescaler = 8;
hcan1.Init.Mode = CAN_MODE_NORMAL;
hcan1.Init.SyncJumpWidth = CAN_SJW_1TQ;
hcan1.Init.TimeSeg1 = CAN_BS1_6TQ;
hcan1.Init.TimeSeg2 = CAN_BS2_5TQ;
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 = DISABLE;
hcan1.Init.AutoBusOff = ENABLE;
hcan1.Init.AutoWakeUp = ENABLE;
hcan1.Init.AutoRetransmission = DISABLE;
hcan1.Init.AutoRetransmission = ENABLE;
hcan1.Init.ReceiveFifoLocked = DISABLE;
hcan1.Init.TransmitFifoPriority = DISABLE;
if (HAL_CAN_Init(&hcan1) != HAL_OK)
Firmware/Board/v3/Src/freertos.c
+5
View File
@@ -85,6 +85,7 @@ osSemaphoreId sem_usb_irq;
osSemaphoreId sem_uart_dma;
osSemaphoreId sem_usb_rx;
osSemaphoreId sem_usb_tx;
osSemaphoreId sem_can;
osThreadId usb_irq_thread;
@@ -187,6 +188,10 @@ void MX_FREERTOS_Init(void) {
osSemaphoreDef(sem_usb_tx);
sem_usb_tx = osSemaphoreCreate(osSemaphore(sem_usb_tx), 1);
osSemaphoreDef(sem_can);
sem_can = osSemaphoreCreate(osSemaphore(sem_can), 1);
osSemaphoreWait(sem_can, 0);
init_deferred_interrupts();
// Load persistent configuration (or defaults)
Firmware/MotorControl/axis.cpp
+17 -12
View File
@@ -3,8 +3,9 @@
#include <functional>
#include "gpio.h"
#include "utils.h"
#include "odrive_main.h"
#include "utils.h"
#include "communication/interface_can.hpp"
Axis::Axis(const AxisHardwareConfig_t& hw_config,
Config_t& config,
@@ -12,15 +13,14 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config,
SensorlessEstimator& sensorless_estimator,
Controller& controller,
Motor& motor,
TrapezoidalTrajectory& trap)
: hw_config_(hw_config),
TrapezoidalTrajectory& trap) :
hw_config_(hw_config),
config_(config),
encoder_(encoder),
sensorless_estimator_(sensorless_estimator),
controller_(controller),
motor_(motor),
trap_(trap)
{
trap_(trap) {
encoder_.axis_ = this;
sensorless_estimator_.axis_ = this;
controller_.axis_ = this;
@@ -49,7 +49,7 @@ static void run_state_machine_loop_wrapper(void* ctx) {
// @brief Starts run_state_machine_loop in a new thread
void Axis::start_thread() {
osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, 4*512);
osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, 4 * 512);
thread_id_ = osThreadCreate(osThread(thread_def), this);
thread_id_valid_ = true;
}
@@ -82,6 +82,10 @@ void Axis::load_default_step_dir_pin_config(
config->dir_gpio_pin = hw_config.dir_gpio_pin;
}
void Axis::load_default_can_id(const int& id, Config_t& config){
config.can_node_id = id;
}
void Axis::decode_step_dir_pins() {
step_port_ = get_gpio_port_by_pin(config_.step_gpio_pin);
step_pin_ = get_gpio_pin_by_pin(config_.step_gpio_pin);
@@ -154,7 +158,9 @@ bool Axis::do_updates() {
// Sub-components should use set_error which will propegate to this error_
encoder_.update();
sensorless_estimator_.update();
return check_for_errors();
bool ret = check_for_errors();
odCAN->send_heartbeat(this);
return ret;
}
// @brief Feed the watchdog to prevent watchdog timeouts.
@@ -279,7 +285,7 @@ bool Axis::run_idle_loop() {
// run_control_loop ignores missed modulation timing updates
// if and only if we're in AXIS_STATE_IDLE
safety_critical_disarm_motor_pwm(motor_);
run_control_loop([this](){
run_control_loop([this]() {
return true;
});
return check_for_errors();
@@ -287,7 +293,6 @@ bool Axis::run_idle_loop() {
// Infinite loop that does calibration and enters main control loop as appropriate
void Axis::run_state_machine_loop() {
// Allocate the map for anti-cogging algorithm and initialize all values to 0.0f
// TODO: Move this somewhere else
// TODO: respect changes of CPR
@@ -301,7 +306,7 @@ void Axis::run_state_machine_loop() {
// arm!
motor_.arm();
for (;;) {
// Load the task chain if a specific request is pending
if (requested_state_ != AXIS_STATE_UNDEFINED) {
@@ -328,7 +333,7 @@ void Axis::run_state_machine_loop() {
task_chain_[pos++] = requested_state_;
task_chain_[pos++] = AXIS_STATE_IDLE;
}
task_chain_[pos++] = AXIS_STATE_UNDEFINED; // TODO: bounds checking
task_chain_[pos++] = AXIS_STATE_UNDEFINED; // TODO: bounds checking
requested_state_ = AXIS_STATE_UNDEFINED;
// Auto-clear any invalid state error
error_ &= ~ERROR_INVALID_STATE;
@@ -400,7 +405,7 @@ void Axis::run_state_machine_loop() {
default:
invalid_state_label:
error_ |= ERROR_INVALID_STATE;
status = false; // this will set the state to idle
status = false; // this will set the state to idle
break;
}
Firmware/MotorControl/axis.hpp
+8 -1
View File
@@ -21,6 +21,7 @@ public:
ERROR_CONTROLLER_FAILED = 0x200,
ERROR_POS_CTRL_DURING_SENSORLESS = 0x400,
ERROR_WATCHDOG_TIMER_EXPIRED = 0x800,
ERROR_ESTOP_REQUESTED = 0x1000
};
enum State_t {
@@ -67,6 +68,8 @@ public:
uint16_t dir_gpio_pin = 0;
LockinConfig_t lockin;
uint8_t can_node_id = 0; // Both axes will have the same id to start
uint32_t can_heartbeat_rate_ms = 100;
};
enum thread_signals {
@@ -100,6 +103,7 @@ public:
static void load_default_step_dir_pin_config(
const AxisHardwareConfig_t& hw_config, Config_t* config);
static void load_default_can_id(const int& id, Config_t& config);
bool check_DRV_fault();
bool check_PSU_brownout();
@@ -210,6 +214,7 @@ public:
State_t& current_state_ = task_chain_[0];
uint32_t loop_counter_ = 0;
LockinState_t lockin_state_ = LOCKIN_STATE_INACTIVE;
uint32_t last_heartbeat_ = 0;
// watchdog
uint32_t watchdog_reset_value_ = 0; //computed from config_.watchdog_timeout in update_watchdog_settings()
@@ -248,7 +253,9 @@ public:
make_protocol_property("finish_on_vel", &config_.lockin.finish_on_vel),
make_protocol_property("finish_on_distance", &config_.lockin.finish_on_distance),
make_protocol_property("finish_on_enc_idx", &config_.lockin.finish_on_enc_idx)
)
),
make_protocol_property("can_node_id", &config_.can_node_id),
make_protocol_property("can_heartbeat_rate_ms", &config_.can_heartbeat_rate_ms)
),
make_protocol_object("motor", motor_.make_protocol_definitions()),
make_protocol_object("controller", controller_.make_protocol_definitions()),
Firmware/MotorControl/main.cpp
+11
View File
@@ -7,8 +7,10 @@
#include <communication/interface_usb.h>
#include <communication/interface_uart.h>
#include <communication/interface_i2c.h>
#include <communication/interface_can.hpp>
BoardConfig_t board_config;
ODriveCAN::Config_t can_config;
Encoder::Config_t encoder_configs[AXIS_COUNT];
SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT];
Controller::Config_t controller_configs[AXIS_COUNT];
@@ -20,9 +22,11 @@ bool user_config_loaded_;
SystemStats_t system_stats_ = { 0 };
Axis *axes[AXIS_COUNT];
ODriveCAN *odCAN;
typedef Config<
BoardConfig_t,
ODriveCAN::Config_t,
Encoder::Config_t[AXIS_COUNT],
SensorlessEstimator::Config_t[AXIS_COUNT],
Controller::Config_t[AXIS_COUNT],
@@ -33,6 +37,7 @@ typedef Config<
void save_configuration(void) {
if (ConfigFormat::safe_store_config(
&board_config,
&can_config,
&encoder_configs,
&sensorless_configs,
&controller_configs,
@@ -50,6 +55,7 @@ extern "C" int load_configuration(void) {
if (NVM_init() ||
ConfigFormat::safe_load_config(
&board_config,
&can_config,
&encoder_configs,
&sensorless_configs,
&controller_configs,
@@ -58,6 +64,7 @@ extern "C" int load_configuration(void) {
&axis_configs)) {
//If loading failed, restore defaults
board_config = BoardConfig_t();
can_config = ODriveCAN::Config_t();
for (size_t i = 0; i < AXIS_COUNT; ++i) {
encoder_configs[i] = Encoder::Config_t();
sensorless_configs[i] = SensorlessEstimator::Config_t();
@@ -67,6 +74,7 @@ extern "C" int load_configuration(void) {
axis_configs[i] = Axis::Config_t();
// Default step/dir pins are different, so we need to explicitly load them
Axis::load_default_step_dir_pin_config(hw_configs[i].axis_config, &axis_configs[i]);
Axis::load_default_can_id(i, axis_configs[i]);
}
} else {
user_config_loaded_ = true;
@@ -98,6 +106,7 @@ void enter_dfu_mode() {
extern "C" {
int odrive_main(void);
void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName) {
for (;;); // TODO: safe action
}
@@ -112,6 +121,7 @@ void vApplicationIdleHook(void) {
system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t);
system_stats_.min_stack_space_usb_irq = uxTaskGetStackHighWaterMark(usb_irq_thread) * sizeof(StackType_t);
system_stats_.min_stack_space_startup = uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t);
system_stats_.min_stack_space_can = uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t);
}
}
}
@@ -160,6 +170,7 @@ int odrive_main(void) {
#endif
// Construct all objects.
odCAN = new ODriveCAN(&hcan1, can_config);
for (size_t i = 0; i < AXIS_COUNT; ++i) {
Encoder *encoder = new Encoder(hw_configs[i].encoder_config,
encoder_configs[i]);
Firmware/MotorControl/odrive_main.h
+4
View File
@@ -54,6 +54,7 @@ typedef struct {
uint32_t min_stack_space_uart;
uint32_t min_stack_space_usb_irq;
uint32_t min_stack_space_startup;
uint32_t min_stack_space_can;
} SystemStats_t;
extern SystemStats_t system_stats_;
@@ -87,11 +88,14 @@ struct BoardConfig_t {
extern BoardConfig_t board_config;
extern bool user_config_loaded_;
// Forward Declarations
class Axis;
class Motor;
class ODriveCAN;
constexpr size_t AXIS_COUNT = 2;
extern Axis *axes[AXIS_COUNT];
extern ODriveCAN *odCAN;
// if you use the oscilloscope feature you can bump up this value
#define OSCILLOSCOPE_SIZE 128
Firmware/Tests/test_runner.cpp
+153
View File
@@ -0,0 +1,153 @@
#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN
#define DOCTEST_CONFIG_TREAT_CHAR_STAR_AS_STRING
#define DOCTEST_CONFIG_USE_STD_HEADERS
#define DOCTEST_CONFIG_NO_TRY_CATCH_IN_ASSERTS
#define DOCTEST_CONFIG_NO_EXCEPTIONS
#define DOCTEST_CONFIG_NO_WINDOWS_SEH
#define DOCTEST_CONFIG_NO_POSIX_SIGNALS
// #define DOCTEST_CONFIG_VOID_CAST_EXPRESSIONS
#include <doctest.h>
using std::cout;
using std::endl;
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;
};
// Fetch a specific signal from the message
template <typename T>
T can_getSignal(can_Message_t msg, const uint8_t startBit, const uint8_t length, const bool isIntel, const float factor, const float offset) {
uint64_t tempVal = 0;
uint64_t mask = (1ULL << length) - 1;
if (isIntel) {
std::memcpy(&tempVal, msg.buf, sizeof(tempVal));
tempVal = (tempVal >> startBit) & mask;
} else {
std::reverse(std::begin(msg.buf), std::end(msg.buf));
std::memcpy(&tempVal, msg.buf, sizeof(tempVal));
tempVal = (tempVal >> (64 - startBit - length)) & mask;
}
T retVal;
std::memcpy(&retVal, &tempVal, sizeof(T));
return static_cast<T>((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>
T 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);
}
TEST_CASE("fake") {
cout << endl;
}
TEST_SUITE("CAN Functions") {
TEST_CASE("reverse") {
can_Message_t rxmsg;
rxmsg.id = 0x000;
rxmsg.isExt = false;
rxmsg.len = 8;
rxmsg.buf[0] = 0x12;
rxmsg.buf[1] = 0x34;
std::reverse(std::begin(rxmsg.buf), std::end(rxmsg.buf));
CHECK(rxmsg.buf[0] == 0x00);
CHECK(rxmsg.buf[6] == 0x34);
CHECK(rxmsg.buf[7] == 0x12);
}
TEST_CASE("getSignal") {
can_Message_t rxmsg;
auto val = 0x1234;
std::memcpy(rxmsg.buf, &val, sizeof(val));
val = can_getSignal<uint16_t>(rxmsg, 0, 16, true, 1, 0);
CHECK(val == 0x1234);
val = can_getSignal<uint16_t>(rxmsg, 0, 16, false, 1, 0);
CHECK(val == 0x3412);
float myFloat = 1234.6789f;
std::memcpy(rxmsg.buf, &myFloat, sizeof(myFloat));
auto floatVal = can_getSignal<float>(rxmsg, 0, 32, true, 1, 0);
CHECK(floatVal == 1234.6789f);
}
TEST_CASE("setSignal") {
can_Message_t txmsg;
can_setSignal<uint16_t>(txmsg, 0x1234, 0, 16, true, 1.0f, 0.0f);
CHECK(can_getSignal<uint16_t>(txmsg, 0, 16, true, 1.0f, 0.0f) == 0x1234);
can_setSignal<uint16_t>(txmsg, 0xABCD, 16, 16, true, 1.0f, 0.0f);
CHECK(can_getSignal<uint16_t>(txmsg, 0, 16, true, 1.0f, 0.0f) == 0x1234);
CHECK(can_getSignal<uint16_t>(txmsg, 16, 16, true, 1.0f, 0.0f) == 0xABCD);
can_setSignal<float>(txmsg, 1234.5678f, 32, 32, true, 1.0f, 0.0f);
CHECK(can_getSignal<uint16_t>(txmsg, 0, 16, true, 1.0f, 0.0f) == 0x1234);
CHECK(can_getSignal<uint16_t>(txmsg, 16, 16, true, 1.0f, 0.0f) == 0xABCD);
CHECK(can_getSignal<float>(txmsg, 32, 32, true, 1.0f, 0.0f));
can_setSignal<uint16_t>(txmsg, 0x1234, 0, 16, false, 1.0f, 0.0f);
CHECK(can_getSignal<uint16_t>(txmsg, 0, 16, false, 1.0f, 0.0f) == 0x1234);
CHECK(can_getSignal<uint16_t>(txmsg, 16, 16, true, 1.0f, 0.0f) == 0xABCD);
CHECK(can_getSignal<float>(txmsg, 32, 32, true, 1.0f, 0.0f));
can_setSignal<float>(txmsg, 234981.0f, 12, 32, false, 2.0f, 1.1f);
CHECK(can_getSignal<float>(txmsg, 12, 32, false, 2.0f, 1.1f) == 234981.0f);
}
}
Firmware/Tupfile.lua
+4 -2
View File
@@ -169,6 +169,7 @@ build{
'MotorControl/sensorless_estimator.cpp',
'MotorControl/trapTraj.cpp',
'MotorControl/main.cpp',
'communication/can_simple.cpp',
'communication/communication.cpp',
'communication/ascii_protocol.cpp',
'communication/interface_uart.cpp',
@@ -182,6 +183,7 @@ build{
'Drivers/DRV8301',
'MotorControl',
'fibre/cpp/include',
'.'
'.',
"C:/Tools/doctest/doctest"
}
}
}
Firmware/build.lua
+1 -1
View File
@@ -85,7 +85,7 @@ function GCCToolchain(prefix, builddir, compiler_flags, linker_flags)
end
return {
compile_c = function(src, flags, includes, outputs) gcc_generic_compiler(prefix..'gcc -std=c99', compiler_flags, calculate_stack_usage, src, flags, includes, outputs) end,
compile_cpp = function(src, flags, includes, outputs) gcc_generic_compiler(prefix..'g++ -std=c++14', compiler_flags, calculate_stack_usage, src, flags, includes, outputs) end,
compile_cpp = function(src, flags, includes, outputs) gcc_generic_compiler(prefix..'g++ -std=c++17 -Wno-register', compiler_flags, calculate_stack_usage, src, flags, includes, outputs) end,
compile_asm = function(src, flags, includes, outputs) gcc_generic_compiler(prefix..'gcc -x assembler-with-cpp', compiler_flags, false, src, flags, includes, outputs) end,
link = function(objects, output_name)
output_name = builddir..'/'..output_name
Firmware/communication/can_simple.cpp
+400
View File
@@ -0,0 +1,400 @@
#include "can_simple.hpp"
#include <odrive_main.h>
#include <cstring>
static const uint8_t NUM_NODE_ID_BITS = 6;
static constexpr uint8_t NUM_CMD_ID_BITS = 11 - NUM_NODE_ID_BITS;
void CANSimple::handle_can_message(can_Message_t& msg) {
// 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) {
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_MOVE_TO_POS:
move_to_pos_callback(axis, msg);
break;
case MSG_SET_POS_SETPOINT:
set_pos_setpoint_callback(axis, msg);
break;
case MSG_SET_VEL_SETPOINT:
set_vel_setpoint_callback(axis, msg);
break;
case MSG_SET_CUR_SETPOINT:
set_current_setpoint_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_A_PER_CSS:
set_traj_A_per_css_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;
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 = false;
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 = false;
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 = false;
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 = msg.buf[0] & 0x3F; // Node ID bitmask
}
void CANSimple::set_axis_requested_state_callback(Axis* axis, can_Message_t& msg) {
axis->requested_state_ = static_cast<Axis::State_t>(can_getSignal<int16_t>(msg, 0, 16, true, 1, 0));
}
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 = false;
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 = false;
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 = false;
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::move_to_pos_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.move_to_pos(can_getSignal<int32_t>(msg, 0, 32, true, 1, 0));
}
void CANSimple::set_pos_setpoint_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.set_pos_setpoint(can_getSignal<int32_t>(msg, 0, 32, true, 1, 0), can_getSignal<int16_t>(msg, 32, 16, true, 0.1f, 0), can_getSignal<int16_t>(msg, 48, 16, true, 0.01f, 0));
}
void CANSimple::set_vel_setpoint_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.set_vel_setpoint(can_getSignal<int32_t>(msg, 0, 32, true, 0.01f, 0.0f), can_getSignal<int32_t>(msg, 4, 32, true, 0.01f, 0.0f));
}
void CANSimple::set_current_setpoint_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.set_current_setpoint(can_getSignal<int32_t>(msg, 0, 32, true, 0.01f, 0));
}
void CANSimple::set_vel_limit_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.config_.vel_limit = can_getSignal<float>(msg, 0, 32, true, 1, 0);
}
void CANSimple::start_anticogging_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.start_anticogging_calibration();
}
void CANSimple::set_traj_vel_limit_callback(Axis* axis, can_Message_t& msg) {
axis->trap_.config_.vel_limit = can_getSignal<float>(msg, 0, 32, true, 1, 0);
}
void CANSimple::set_traj_accel_limits_callback(Axis* axis, can_Message_t& msg) {
axis->trap_.config_.accel_limit = can_getSignal<float>(msg, 0, 32, true, 1, 0);
axis->trap_.config_.decel_limit = can_getSignal<float>(msg, 32, 32, true, 1, 0);
}
void CANSimple::set_traj_A_per_css_callback(Axis* axis, can_Message_t& msg) {
axis->trap_.config_.A_per_css = can_getSignal<float>(msg, 0, 32, true, 1, 0);
}
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 = false;
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 = false;
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::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 = false;
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);
}
uint8_t CANSimple::get_node_id(uint32_t msgID) {
return ((msgID >> NUM_CMD_ID_BITS) & 0x03F); // Upper 6 bits
}
uint8_t CANSimple::get_cmd_id(uint32_t msgID) {
return (msgID & 0x01F); // Bottom 5 bits
}
Firmware/communication/can_simple.hpp
+82
View File
@@ -0,0 +1,82 @@
#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_CO_HEARTBEAT_CMD = 0x700, // CANOpen NMT Heartbeat SEND
MSG_ODRIVE_HEARTBEAT = 0x001,
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_MOVE_TO_POS,
MSG_SET_POS_SETPOINT,
MSG_SET_VEL_SETPOINT,
MSG_SET_CUR_SETPOINT,
MSG_SET_VEL_LIMIT,
MSG_START_ANTICOGGING,
MSG_SET_TRAJ_VEL_LIMIT,
MSG_SET_TRAJ_ACCEL_LIMITS,
MSG_SET_TRAJ_A_PER_CSS,
MSG_GET_IQ,
MSG_GET_SENSORLESS_ESTIMATES,
MSG_RESET_ODRIVE,
MSG_GET_VBUS_VOLTAGE,
};
static void handle_can_message(can_Message_t& msg);
static void send_heartbeat(Axis* axis);
private:
static void nmt_callback(Axis* axis, can_Message_t& msg);
static void estop_callback(Axis* axis, can_Message_t& msg);
static void get_motor_error_callback(Axis* axis, can_Message_t& msg);
static void get_encoder_error_callback(Axis* axis, can_Message_t& msg);
static void get_controller_error_callback(Axis* axis, can_Message_t& msg);
static void get_sensorless_error_callback(Axis* axis, can_Message_t& msg);
static void set_axis_nodeid_callback(Axis* axis, can_Message_t& msg);
static void set_axis_requested_state_callback(Axis* axis, can_Message_t& msg);
static void set_axis_startup_config_callback(Axis* axis, can_Message_t& msg);
static void get_encoder_estimates_callback(Axis* axis, can_Message_t& msg);
static void get_encoder_count_callback(Axis* axis, can_Message_t& msg);
static void move_to_pos_callback(Axis* axis, can_Message_t& msg);
static void set_pos_setpoint_callback(Axis* axis, can_Message_t& msg);
static void set_vel_setpoint_callback(Axis* axis, can_Message_t& msg);
static void set_current_setpoint_callback(Axis* axis, can_Message_t& msg);
static void set_vel_limit_callback(Axis* axis, can_Message_t& msg);
static void start_anticogging_callback(Axis* axis, can_Message_t& msg);
static void set_traj_vel_limit_callback(Axis* axis, can_Message_t& msg);
static void set_traj_accel_limits_callback(Axis* axis, can_Message_t& msg);
static void set_traj_A_per_css_callback(Axis* axis, can_Message_t& msg);
static void get_iq_callback(Axis* axis, can_Message_t& msg);
static void get_sensorless_estimates_callback(Axis* axis, can_Message_t& msg);
static void get_vbus_voltage_callback(Axis* axis, can_Message_t& msg);
// Utility functions
static uint8_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
Firmware/communication/communication.cpp
+3 -7
View File
@@ -91,13 +91,9 @@ void init_communication(void) {
osDelay(1);
}
float oscilloscope[OSCILLOSCOPE_SIZE] = {0};
size_t oscilloscope_pos = 0;
static CAN_context can1_ctx;
// Helper class because the protocol library doesn't yet
// support non-member functions
// TODO: make this go away
@@ -136,6 +132,7 @@ static inline auto make_obj_tree() {
make_protocol_ro_property("min_stack_space_comms", &system_stats_.min_stack_space_comms),
make_protocol_ro_property("min_stack_space_usb", &system_stats_.min_stack_space_usb),
make_protocol_ro_property("min_stack_space_uart", &system_stats_.min_stack_space_uart),
make_protocol_ro_property("min_stack_space_can", &system_stats_.min_stack_space_can),
make_protocol_ro_property("min_stack_space_usb_irq", &system_stats_.min_stack_space_usb_irq),
make_protocol_ro_property("min_stack_space_startup", &system_stats_.min_stack_space_startup),
make_protocol_object("usb",
@@ -170,7 +167,7 @@ static inline auto make_obj_tree() {
),
make_protocol_object("axis0", axes[0]->make_protocol_definitions()),
make_protocol_object("axis1", axes[1]->make_protocol_definitions()),
make_protocol_object("can", can1_ctx.make_protocol_definitions()),
make_protocol_object("can", odCAN->make_protocol_definitions()),
make_protocol_property("test_property", &test_property),
make_protocol_function("test_function", static_functions, &StaticFunctions::test_function, "delta"),
make_protocol_function("get_oscilloscope_val", static_functions, &StaticFunctions::get_oscilloscope_val, "index"),
@@ -205,8 +202,7 @@ void communication_task(void * ctx) {
if (board_config.enable_i2c_instead_of_can) {
start_i2c_server();
} else {
// TODO: finish implementing CAN
// start_can_server(can1_ctx, CAN1, serial_number);
odCAN->start_can_server();
}
for (;;) {
Firmware/communication/interface_can.cpp
+179 -262
View File
@@ -1,297 +1,214 @@
/*
*
* Zero-config node ID negotiation
* -------------------------------
*
* A heartbeat message is a message with a 8 byte unique serial number as payload.
* A regular message is any message that is not a heartbeat message.
*
* All nodes MUST obey these four rules:
*
* a) At a given point in time, a node MUST consider a node ID taken (by others)
* if any of the following is true:
* - the node received a (not self-emitted) heartbeat message with that node ID
* within the last second
* - the node attempted and failed at sending a heartbeat message with that
* node ID within the last second (failed in the sense of not ACK'd)
*
* b) At a given point in time, a node MUST NOT consider a node ID self-assigned
* if, within the last second, it did not succeed in sending a heartbeat
* message with that node ID.
*
* c) At a given point in time, a node MUST NOT send any heartbeat message with
* a node ID that is taken.
*
* d) At a given point in time, a node MUST NOT send any regular message with
* a node ID that is not self-assigned.
*
* Hardware allocation
* -------------------
* RX FIFO0:
* - filter bank 0: heartbeat messages
*/
#include "interface_can.hpp"
#include "fibre/crc.hpp"
#include "freertos_vars.h"
#include "utils.h"
#include <can.h>
#include <stm32f4xx_hal.h>
#include <cmsis_os.h>
#include <stm32f4xx_hal.h>
#define CAN_HEARTBEAT_INTERVAL 1000 // [ms]
#define CAN_HEARTBEAT_MARGIN 10 // maximum time that a heartbeat message can be delayed until we stop sending other messages [ms]
// Specific CAN Protocols
#include "can_simple.hpp"
// defined in can.c
extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;
extern CAN_HandleTypeDef hcan3;
// Safer context handling via maps instead of arrays
// #include <unordered_map>
// std::unordered_map<CAN_HandleTypeDef *, ODriveCAN *> ctxMap;
static CAN_context* ctxs[3] = { nullptr, nullptr, nullptr };
struct CAN_context* get_can_ctx(CAN_HandleTypeDef *hcan) {
#if defined(CAN1)
if (hcan->Instance == CAN1) return ctxs[0];
#endif
#if defined(CAN2)
if (hcan->Instance == CAN2) return ctxs[1];
#endif
#if defined(CAN3)
if (hcan->Instance == CAN3) return ctxs[2];
#endif
return nullptr;
// Constructor is called by communication.cpp and the handle is assigned appropriately
ODriveCAN::ODriveCAN(CAN_HandleTypeDef *handle, ODriveCAN::Config_t &config)
: handle_{handle},
config_{config} {
// ctxMap[handle_] = this;
}
void consider_node_id_in_use(CAN_context* ctx, uint8_t node_id) {
ctx->node_ids_in_use_0[node_id >> 5] |= (1 << (node_id & 0x1f));
}
bool is_node_id_in_use(CAN_context* ctx, uint32_t node_id) {
if (node_id == 0) // node ID 0 is reserved (is it though?)
return true;
return (ctx->node_ids_in_use_0[node_id >> 5] & (1 << (node_id & 0x1f)))
|| (ctx->node_ids_in_use_1[node_id >> 5] & (1 << (node_id & 0x1f)));
}
bool select_another_node_id(CAN_context* ctx) {
ctx->node_id_expiry = osKernelSysTick() - 1;
// Find a new node ID that is not in use
for (uint8_t i = 0; i < 32; i++) {
// Each time we select a new node ID, we use the next byte from the serial
// number to get advance the node ID.
uint8_t poor_mans_random_byte = ((uint8_t*)ctx->serial_number)[ctx->node_id_rng_state];
if (++(ctx->node_id_rng_state) >= sizeof(ctx->serial_number))
ctx->node_id_rng_state = 0;
ctx->node_id = calc_crc<uint8_t, 1>(ctx->node_id, poor_mans_random_byte);
if (!is_node_id_in_use(ctx, ctx->node_id))
return true;
}
return false;
}
void server_thread(CAN_context* ctx) {
uint32_t next_1s_tick = osKernelSysTick() + 1000;
void ODriveCAN::can_server_thread() {
for (;;) {
if (deadline_to_timeout(next_1s_tick) == 0)
// wait until either the next heartbeat is due or a hearbeat was requested
// by releasing the semaphore
osSemaphoreWait(ctx->sem_send_heartbeat, deadline_to_timeout(next_1s_tick));
if (!is_in_the_future(next_1s_tick))
memcpy(ctx->node_ids_in_use_1, ctx->node_ids_in_use_0, sizeof(ctx->node_ids_in_use_1));
next_1s_tick += 1000;
if (!is_in_the_future(next_1s_tick))
next_1s_tick = osKernelSysTick(); // fast-forward if we missed several 1 second ticks
uint32_t status = HAL_CAN_GetError(handle_);
if (status == HAL_CAN_ERROR_NONE) {
can_Message_t rxmsg;
if (is_node_id_in_use(ctx, ctx->node_id)) {
if (!select_another_node_id(ctx))
continue;
else
next_1s_tick += ctx->node_id; // shift the 1s tick by a bit
osSemaphoreWait(sem_can, 10); // Poll every 10ms regardless of sempahore status
while (available()) {
read(rxmsg);
switch (config_.protocol) {
case CAN_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);
}
}
uint8_t data[8];
//uint8_t data[] = { ctx->node_id }; // this would be the correct data for CANopen - TODO: make it compatible
*(uint64_t*)data = ctx->serial_number;
CAN_TxHeaderTypeDef header = {
.StdId = 0x700u + ctx->node_id,
.ExtId = 0,
.IDE = CAN_ID_STD,
.RTR = CAN_RTR_DATA,
.DLC = sizeof(data),
.TransmitGlobalTime = DISABLE
};
HAL_CAN_AddTxMessage(ctx->handle, &header, data, &ctx->last_heartbeat_mailbox);
}
}
bool start_can_server(CAN_context& ctx, CAN_TypeDef *port, uint64_t serial_number) {
//MX_CAN1_Init(); // TODO: flatten
#if defined(CAN1)
if (port == CAN1) ctx.handle = &hcan1, ctxs[0] = &ctx; else
#endif
#if defined(CAN2)
// TODO: move CubeMX stuff into this file so all symbols are defined
//if (port == CAN2) ctx.handle = &hcan2, ctxs[1] = &ctx; else
#endif
#if defined(CAN3)
if (port == CAN3) ctx.handle = &hcan3, ctxs[2] = &ctx; else
#endif
return false; // fail if none of the above checks matched
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;
ctx.node_id = calc_crc<uint8_t, 1>(0, (const uint8_t*)UID_BASE, 12);
ctx.serial_number = serial_number;
osSemaphoreDef(sem_send_heartbeat);
ctx.sem_send_heartbeat = osSemaphoreCreate(osSemaphore(sem_send_heartbeat), 1);
osSemaphoreWait(ctx.sem_send_heartbeat, 0);
set_baud_rate(config_.baud);
//// Set up heartbeat filter
CAN_FilterTypeDef sFilterConfig = {
.FilterIdHigh = ((0x700u + ctx.node_id) << 5) | (0x0 << 2), // own heartbeat (standard ID, no RTR)
.FilterIdLow = (0x700u << 5) | (0x0 << 2), // any heartbeat (standard ID, no RTR)
.FilterMaskIdHigh = (0x7ffu << 5) | (0x3 << 2),
.FilterMaskIdLow = (0x780u << 5) | (0x3 << 2),
.FilterFIFOAssignment = CAN_RX_FIFO0,
.FilterBank = 0,
.FilterMode = CAN_FILTERMODE_IDMASK,
.FilterScale = CAN_FILTERSCALE_16BIT, // two 16-bit filters
.FilterActivation = ENABLE,
.SlaveStartFilterBank = 0
};
status = HAL_CAN_ConfigFilter(ctx.handle, &sFilterConfig);
if (status != HAL_OK)
return false;
status = HAL_CAN_Init(handle_);
status = HAL_CAN_Start(ctx.handle);
if (status != HAL_OK)
return false;
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_ActivateNotification(ctx.handle,
CAN_IT_TX_MAILBOX_EMPTY |
CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_RX_FIFO1_MSG_PENDING | /* we probably only want this */
CAN_IT_RX_FIFO0_FULL | CAN_IT_RX_FIFO1_FULL |
CAN_IT_RX_FIFO0_OVERRUN | CAN_IT_RX_FIFO1_OVERRUN |
CAN_IT_WAKEUP | CAN_IT_SLEEP_ACK |
CAN_IT_ERROR_WARNING | CAN_IT_ERROR_PASSIVE |
CAN_IT_BUSOFF | CAN_IT_LAST_ERROR_CODE |
CAN_IT_ERROR);
if (status != HAL_OK)
return false;
server_thread(&ctx);
return true;
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, 512);
thread_id_ = osThreadCreate(osThread(can_server_thread_def), this);
thread_id_valid_ = true;
return status;
}
void tx_complete_callback(CAN_HandleTypeDef *hcan, uint8_t mailbox_idx) {
CAN_context *ctx = get_can_ctx(hcan);
if (!ctx) return;
ctx->tx_msg_cnt++;
if (mailbox_idx == ctx->last_heartbeat_mailbox) {
// we succeeded in sending a heartbeat
// now we're allowed to send messages for the next second plus a small margin
ctx->node_id_expiry = osKernelSysTick() + CAN_HEARTBEAT_INTERVAL + CAN_HEARTBEAT_MARGIN;
}
}
// 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;
void tx_aborted_callback(CAN_HandleTypeDef *hcan, uint8_t mailbox_idx) {
//__asm volatile ("bkpt");
if (!get_can_ctx(hcan))
return;
get_can_ctx(hcan)->TxMailboxAbortCallbackCnt++;
}
uint32_t retTxMailbox = 0;
if (HAL_CAN_GetTxMailboxesFreeLevel(handle_) > 0)
HAL_CAN_AddTxMessage(handle_, &header, txmsg.buf, &retTxMailbox);
void tx_error(CAN_context *ctx, uint8_t mailbox_idx) {
if (mailbox_idx == ctx->last_heartbeat_mailbox) {
// Consider the node ID in use
consider_node_id_in_use(ctx, ctx->node_id);
// Try to find a new node ID that is not in use and immediately
// resend heartbeat if we find one
if (select_another_node_id(ctx))
osSemaphoreRelease(ctx->sem_send_heartbeat);
}
}
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { tx_complete_callback(hcan, 0); }
void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { tx_complete_callback(hcan, 1); }
void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { tx_complete_callback(hcan, 2); }
void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef *hcan) { tx_aborted_callback(hcan, 0); }
void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef *hcan) { tx_aborted_callback(hcan, 1); }
void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef *hcan) { tx_aborted_callback(hcan, 2); }
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
CAN_context *ctx = get_can_ctx(hcan);
if (!ctx) return;
ctx->received_msg_cnt++;
CAN_RxHeaderTypeDef header;
uint8_t data[8];
HAL_StatusTypeDef status = HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &header, data);
if (status != HAL_OK) {
ctx->unexpected_errors++;
return;
}
uint8_t node_id = header.StdId & 0x07fu;
if ((header.StdId & 0x780u) == 0x700u) {
ctx->received_ack++;
consider_node_id_in_use(ctx, node_id);
return retTxMailbox;
} else {
ctx->unhandled_messages++;
return -1;
}
}
void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef *hcan) { if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo0FullCallbackCnt++; }
uint32_t ODriveCAN::available() {
return (HAL_CAN_GetRxFifoFillLevel(handle_, CAN_RX_FIFO0) + HAL_CAN_GetRxFifoFillLevel(handle_, CAN_RX_FIFO1));
}
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) { if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1MsgPendingCallbackCnt++; }
void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef *hcan) { if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1FullCallbackCnt++; }
void HAL_CAN_SleepCallback(CAN_HandleTypeDef *hcan) { if (get_can_ctx(hcan)) get_can_ctx(hcan)->SleepCallbackCnt++; }
void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef *hcan) { if (get_can_ctx(hcan)) get_can_ctx(hcan)->WakeUpFromRxMsgCallbackCnt++; }
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 = baudRate;
reinit_can();
break;
case CAN_BAUD_250K:
handle_->Init.Prescaler = 8; // 21 TQ's
config_.baud = baudRate;
reinit_can();
break;
case CAN_BAUD_500K:
handle_->Init.Prescaler = 4; // 21 TQ's
config_.baud = baudRate;
reinit_can();
break;
case CAN_BAUD_1000K:
handle_->Init.Prescaler = 2; // 21 TQ's
config_.baud = 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_t 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 CAN_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) {
//__asm volatile ("bkpt");
CAN_context *ctx = get_can_ctx(hcan);
if (!ctx) return;
volatile uint32_t original_error = hcan->ErrorCode;
(void) original_error;
// handle transmit errors in all three mailboxes
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST0) {
SET_BIT(hcan->Instance->sTxMailBox[0].TIR, CAN_TI0R_TXRQ);
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST0;
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR0) {
tx_error(ctx, 0);
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR0;
}
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST1) {
SET_BIT(hcan->Instance->sTxMailBox[1].TIR, CAN_TI1R_TXRQ);
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST1;
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR1) {
tx_error(ctx, 1);
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR1;
}
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST2) {
SET_BIT(hcan->Instance->sTxMailBox[2].TIR, CAN_TI2R_TXRQ);
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST2;
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR2) {
tx_error(ctx, 2);
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR2;
}
if (hcan->ErrorCode)
ctx->unexpected_errors++;
HAL_CAN_ResetError(hcan);
}
Firmware/communication/interface_can.hpp
+140 -45
View File
@@ -1,55 +1,150 @@
#ifndef __INTERFACE_CAN_HPP
#define __INTERFACE_CAN_HPP
#include "fibre/protocol.hpp"
#include <stm32f4xx_hal.h>
#include <cmsis_os.h>
#include <stm32f4xx_hal.h>
#include "fibre/protocol.hpp"
#include "odrive_main.h"
struct CAN_context {
CAN_HandleTypeDef *handle = nullptr;
uint8_t node_id = 0;
uint64_t serial_number = 0;
#define CAN_CLK_HZ (42000000)
#define CAN_CLK_MHZ (42)
uint32_t node_ids_in_use_0[4]; // 128 bits (indicate if a node ID was in use up to 1 second ago)
uint32_t node_ids_in_use_1[4]; // 128 bits (indicats if a node ID was in use 1-2 seconds ago)
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};
} ;
uint32_t last_heartbeat_mailbox = 0;
uint32_t tx_msg_cnt = 0;
uint32_t node_id_expiry = 0;
uint8_t node_id_rng_state = 0;
osSemaphoreId sem_send_heartbeat;
// count occurrence various callbacks
uint32_t TxMailboxCompleteCallbackCnt = 0;
uint32_t TxMailboxAbortCallbackCnt = 0;
int RxFifo0MsgPendingCallbackCnt = 0;
int RxFifo0FullCallbackCnt = 0;
int RxFifo1MsgPendingCallbackCnt = 0;
int RxFifo1FullCallbackCnt = 0;
int SleepCallbackCnt = 0;
int WakeUpFromRxMsgCallbackCnt = 0;
int ErrorCallbackCnt = 0;
uint32_t received_msg_cnt = 0;
uint32_t received_ack = 0;
uint32_t unexpected_errors = 0;
uint32_t unhandled_messages = 0;
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_ro_property("node_id", &node_id),
make_protocol_ro_property("TxMailboxCompleteCallbackCnt", &TxMailboxCompleteCallbackCnt),
make_protocol_ro_property("TxMailboxAbortCallbackCnt", &TxMailboxAbortCallbackCnt),
make_protocol_ro_property("received_msg_cnt", &received_msg_cnt),
make_protocol_ro_property("received_ack", &received_ack),
make_protocol_ro_property("unexpected_errors", &unexpected_errors),
make_protocol_ro_property("unhandled_messages", &unhandled_messages)
);
}
struct can_Signal_t {
const uint8_t startBit;
const uint8_t length;
const bool isIntel;
const float factor;
const float offset;
};
bool start_can_server(CAN_context& ctx, CAN_TypeDef *hcan, uint64_t serial_number);
// 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
};
#endif // __INTERFACE_CAN_HPP
enum CAN_Protocol_t {
CAN_PROTOCOL_SIMPLE
};
class ODriveCAN {
public:
struct Config_t {
uint32_t baud = CAN_BAUD_250K;
CAN_Protocol_t protocol = CAN_PROTOCOL_SIMPLE;
};
enum Error_t {
ERROR_NONE = 0x00,
ERROR_DUPLICATE_CAN_IDS = 0x01
};
ODriveCAN(CAN_HandleTypeDef *handle, ODriveCAN::Config_t &config);
// Thread Relevant Data
osThreadId thread_id_;
Error_t 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_t error);
// I/O Functions
uint32_t available();
uint32_t write(can_Message_t &txmsg);
bool read(can_Message_t &rxmsg);
// Communication Protocol Handling
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_property("error", &error_),
make_protocol_object("config",
make_protocol_ro_property("baud_rate", &config_.baud)),
make_protocol_property("can_protocol", &config_.protocol),
make_protocol_function("set_baud_rate", *this, &ODriveCAN::set_baud_rate, "baudRate"));
}
private:
CAN_HandleTypeDef *handle_ = nullptr;
ODriveCAN::Config_t &config_;
void set_baud_rate(uint32_t baudRate);
};
#include <iterator>
template <typename T>
T can_getSignal(can_Message_t msg, const uint8_t startBit, const uint8_t length, const bool isIntel, const float factor, const float offset) {
uint64_t tempVal = 0;
uint64_t mask = (1ULL << length) - 1;
if (isIntel) {
std::memcpy(&tempVal, msg.buf, sizeof(tempVal));
tempVal = (tempVal >> startBit) & mask;
} else {
std::reverse(std::begin(msg.buf), std::end(msg.buf));
std::memcpy(&tempVal, msg.buf, sizeof(tempVal));
tempVal = (tempVal >> (64 - startBit - length)) & mask;
}
T retVal;
std::memcpy(&retVal, &tempVal, sizeof(T));
return static_cast<T>((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>
T 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);
}
DEFINE_ENUM_FLAG_OPERATORS(ODriveCAN::Error_t)
#endif // __INTERFACE_CAN_HPP
ODrive_Workspace.code-workspace
+13 -11
View File
@@ -11,16 +11,7 @@
}
],
"settings": {
"c-cpp-flylint.cppcheck.includePaths": [
"${workspaceRoot}",
"${workspaceRoot}/fibre/cpp/include/fibre",
"${workspaceRoot}/communication",
"${workspaceRoot}/MotorControl",
],
"c-cpp-flylint.cppcheck.platform": "avr8",
"c-cpp-flylint.cppcheck.standard": ["c99","c++14"],
"c-cpp-flylint.cppcheck.standard": ["c99","c++17"],
"files.associations": {
"memory": "cpp",
"utility": "cpp",
@@ -55,7 +46,18 @@
"chrono": "cpp",
"condition_variable": "cpp",
"future": "cpp",
"arm_math.h": "c"
"arm_math.h": "c",
"iostream": "cpp",
"cmath": "cpp",
"csignal": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"ctime": "cpp",
"unordered_map": "cpp",
"fstream": "cpp",
"iomanip": "cpp",
"optional": "cpp",
"sstream": "cpp"
}
}
}
docs/can-protocol.md
+115
View File
@@ -0,0 +1,115 @@
# CAN Protocol
## Hardware Setup
ODrive assumes the CAN PHY is a standard differential twisted pair in a linear bus configuration with 120 ohm termination resistance at each end. ODrive uses 3.3v as the high output, but conforms to the CAN PHY requirement of achieving a differential voltage > 1.5V to represent a "0". As such, it is compatible with standard 5V bus architectures.
ODrive currently supports the following CAN baud rates:
* 125 kbps
* 250 kbps (default)
* 500 kbps
* 1000 kbps
---
## Transport Protocol
We've implemented a very basic CAN protocol that we call "CAN Simple" to get users going with ODrive. This protocol is sufficiently abstracted that it is straightforward to add other protocols such as CANOpen, J1939, or Fibre over ISO-TP in the future. Unfortunately, implementing those protocols is a lot of work, and we wanted to give users a way to control ODrive's basic functions via CAN sooner rather than later.
### CAN Frame
At its most basic, the CAN Simple frame looks like this:
* Upper 6 bits - Node ID - max 0x3F
* Lower 5 bits - Command ID - max 0x1F
To understand how the Node ID and Command ID interact, let's look at an example
`odrv0.axis0.can_node_id = 0x010` - Reserves messages 0x200 through 0x21F
`odrv0.axis1.can_node_id = 0x018` - Reserves messages 0x300 through 0x31F
It may not be obvious, but this allows for some compatibility with CANOpen. Although the address space 0x200 and 0x300 correspond to receive PDO base addresses, we can guarantee they will not conflict if all CANopen node IDs are >= 32. E.g.:
CANopen nodeID = 35 = 0x23
Receive PDO 0x200 + nodeID = 0x223, which does not conflict with the range [0x200 : 0x21F]
Be careful that you don't assign too many nodeIDs per PDO group. Four CAN Simple nodes (32*4) is all of the available address space of a single PDO. If the bus is strictly ODrive CAN Simple nodes, a simple sequential Node ID assignment will work fine.
### Messages
CMD ID | Name | Sender | Signals | Start byte
--: | :-- | :-- | :-- | :--
0x000 | CANOpen NMT Message\*\* | Master | - | - | -
0x700 | CANOpen Heartbeat Message\*\* | Slave | - | - | -
0x001 | ODrive Heartbeat Message | Axis | Axis Error<br>Axis Current State | 0<br>4
0x002 | ODrive Estop Message | Master | - | - | -
0x003 | Get Motor Error\* | Axis | Motor Error | 0
0x004 | Get Encoder Error\* | Axis | Encoder Error | 0
0x005 | Get Sensorless Error\* | Axis | Sensorless Error | 0
0x006 | Set Axis Node ID | Master | Axis CAN Node ID | 0
0x007 | Set Axis Requested State | Master | Axis Requested State | 0
0x008 | Set Axis Startup Config | Master | - Not yet implemented - | -
0x009 | Get Encoder Estimates\* | Master | Encoder Pos Estimate<br>Encoder Vel Estimate | 0<br>4
0x00A | Get Encoder Count\* | Master | Encoder Shadow Count<br>Encoder Count in CPR | 0<br>4
0x00B | Move To Pos | Master | Goal Position | 0
0x00C | Set Pos Setpoint | Master | Pos Setpoint<br>Vel FF<br>Current FF | 0<br>4<br>6
0x00D | Set Vel Setpoint | Master | Vel Setpoint<br>Current FF | 0<br>4
0x00E | Set Current Setpoint | Master | Current Setpoint | 0
0x00F | Set Velocity Limit | Master | Velocity Limit | 0
0x010 | Start Anticogging | Master | - | -
0x011 | Set Traj Vel Limit | Master | Traj Vel Limit | 0
0x012 | Set Traj Accel Limits | Master | Traj Accel Limit<br>Traj Decel Limit | 0<br>4
0x013 | Set Traj A per Count / s^2 | Master | Traj A per CSS | 0
0x014 | Get IQ\* | Axis | Iq Setpoint<br>Iq Measured | 0<br>4
0x015 | Get Sensorless Estimates\* | Master | Sensorless Pos Estimate<br>Sensorless Vel Estimate | 0<br>4
0x016 | Reboot ODrive | Master\*\*\* | |
0x017 | Get Vbus Voltage | Master\*\*\* | Vbus Voltage | 0
\* Note: These messages are call & response. The Master node sends a message with the RTR bit set, and the axis responds with the same ID and specified payload.
\*\* Note: These CANOpen messages are reserved to avoid bus collisions with CANOpen devices. They are not used by CAN Simple.
\*\*\* Note: These messages can be sent to either address on a given ODrive board.
---
### Signals
Name | Type | Bits | Factor | Offset | Byte Order
:-- | :-- | :--: | --: | :--: | :--:
Axis Error | Unsigned Int | 32 | 1 | 0 | Intel
Axis Current State | Unsigned Int | 32 | 1 | 0 | Intel
Motor Error | Unsigned Int | 32 | 1 | 0 | Intel
Encoder Error | Unsigned Int | 32 | 1 | 0 | Intel
Sensorless Error | Unsigned Int | 32 | 1 | 0 | Intel
Axis CAN Node ID | Unsigned Int | 16 | 1 | 0 | Intel
Axis Requested State | Unsigned Int | 32 | 1 | 0 | Intel
Encoder Pos Estimate | IEEE 754 Float | 32 | 1 | 0 | Intel
Encoder Vel Estimate | IEEE 754 Float | 32 | 1 | 0 | Intel
Encoder Shadow Count | Signed Int | 32 | 1 | 0 | Intel
Encoder Count In CPR | Signed Int | 32 | 1 | 0 | Intel
Goal Position | Signed Int | 32 | 1 | 0 | Intel
Pos Setpoint | Signed Int | 32 | 1 | 0 | Intel
Vel FF | Signed Int | 16 | 0.1 | 0 | Intel
Current FF | Signed Int | 16 | 0.01 | 0 | Intel
Vel Setpoint | Signed Int | 32 | 0.01 | 0 | Intel
Current Setpoint | Signed Int | 32 | 0.01 | 0 | Intel
Velocity Limit | IEEE 754 Float | 32 | 1 | 0 | Intel
Traj Vel Limit | IEEE 754 Float | 32 | 1 | 0 | Intel
Traj Accel Limit | IEEE 754 Float | 32 | 1 | 0 | Intel
Traj Decel Limit | IEEE 754 Float | 32 | 1 | 0 | Intel
Traj A per CSS | IEEE 754 Float | 32 | 1 | 0 | Intel
Iq Setpoint | IEEE 754 Float | 32 | 1 | 0 | Intel
Iq Measured | IEEE 754 Float | 32 | 1 | 0 | Intel
Sensorless Pos Estimate | IEEE 754 Float | 32 | 1 | 0 | Intel
Sensorless Vel Estimate | IEEE 754 Float | 32 | 1 | 0 | Intel
Vbus Voltage | IEEE 754 Float | 32 | 1 | 0 | Intel
---
## Configuring ODrive for CAN
Configuration of the CAN parameters should be done via USB before putting the device on the bus.
To set the desired baud rate, use `<odrv>.can.set_baud_rate(<value>)`. The baud rate can be done without rebooting the device. If you'd like to keep the baud rate, simply call `<odrv>.save_configuration()` before rebooting.
Each axis looks like a separate node on the bus. Thus, they've inherited a new configuration property: `can_node_id`. This ID can be from 0 to 63 (0x3F) inclusive.
### Example Configuration
```
odrv0.axis0.config.can_node_id = 3
odrv0.axis1.config.can_node_id = 1
odrv0.can.set_baud_rate(500000)
odrv0.save_configuration()
odrv0.reboot()
```