diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 18b88433..3b91e075 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -7,8 +7,10 @@ #include #include #include +#include BoardConfig_t board_config; +CANConfig_t can_config; Encoder::Config_t encoder_configs[AXIS_COUNT]; SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT]; ControllerConfig_t controller_configs[AXIS_COUNT]; @@ -19,9 +21,13 @@ bool user_config_loaded_; SystemStats_t system_stats_ = { 0 }; Axis *axes[AXIS_COUNT]; +ODriveCAN *odCAN; + + typedef Config< BoardConfig_t, + CANConfig_t, Encoder::Config_t[AXIS_COUNT], SensorlessEstimator::Config_t[AXIS_COUNT], ControllerConfig_t[AXIS_COUNT], @@ -31,6 +37,7 @@ typedef Config< void save_configuration(void) { if (ConfigFormat::safe_store_config( &board_config, + &can_config, &encoder_configs, &sensorless_configs, &controller_configs, @@ -47,6 +54,7 @@ void load_configuration(void) { if (NVM_init() || ConfigFormat::safe_load_config( &board_config, + &can_config, &encoder_configs, &sensorless_configs, &controller_configs, @@ -54,6 +62,7 @@ void load_configuration(void) { &axis_configs)) { //If loading failed, restore defaults board_config = BoardConfig_t(); + can_config = CANConfig_t(); for (size_t i = 0; i < AXIS_COUNT; ++i) { encoder_configs[i] = Encoder::Config_t(); sensorless_configs[i] = SensorlessEstimator::Config_t(); @@ -104,6 +113,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); } } } @@ -154,6 +164,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]); diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 4e4db160..0df04433 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -47,6 +47,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_; @@ -79,11 +80,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 diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index 622bdefb..843f46b3 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -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", @@ -167,7 +164,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"), @@ -202,8 +199,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 (;;) { diff --git a/Firmware/communication/interface_can.cpp b/Firmware/communication/interface_can.cpp index 3bf822bd..b020f4c1 100644 --- a/Firmware/communication/interface_can.cpp +++ b/Firmware/communication/interface_can.cpp @@ -36,262 +36,48 @@ #include "utils.h" #include -#include #include +#include -#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] - -// defined in can.c -extern CAN_HandleTypeDef hcan1; -extern CAN_HandleTypeDef hcan2; -extern CAN_HandleTypeDef hcan3; - -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, CANConfig_t &config) + : handle_{handle}, + config_{config} { } - -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)); +static void can_server_thread_wrapper(void* ctx){ + reinterpret_cast(ctx)->can_server_thread(); + reinterpret_cast(ctx)->thread_id_valid_ = false; } -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(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 - - 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 - } - - 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); + // Do nothing } } -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 - +bool ODriveCAN::start_can_server() { HAL_StatusTypeDef status; - ctx.node_id = calc_crc(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 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); + status = HAL_CAN_Start(handle_); if (status != HAL_OK) return false; - status = HAL_CAN_Start(ctx.handle); + status = HAL_CAN_ActivateNotification(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; - 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; + 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; - server_thread(&ctx); return true; -} - -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; - } -} - -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++; -} - -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); - } else { - ctx->unhandled_messages++; - } -} - -void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef *hcan) { if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo0FullCallbackCnt++; } - -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++; } - -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++; -} - +} \ No newline at end of file diff --git a/Firmware/communication/interface_can.hpp b/Firmware/communication/interface_can.hpp index 2cd487b0..548a444f 100644 --- a/Firmware/communication/interface_can.hpp +++ b/Firmware/communication/interface_can.hpp @@ -1,55 +1,50 @@ #ifndef __INTERFACE_CAN_HPP #define __INTERFACE_CAN_HPP -#include "fibre/protocol.hpp" -#include #include +#include +#include "fibre/protocol.hpp" -struct CAN_context { - CAN_HandleTypeDef *handle = nullptr; - uint8_t node_id = 0; - uint64_t serial_number = 0; +struct { + uint32_t id; + bool isExt; + uint8_t len; + uint8_t buf[8]; +} CAN_message_t; - 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) +// 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 +}; +struct CANConfig_t { + uint8_t node_id; + uint32_t baud; +}; - 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; +class ODriveCAN { + public: + ODriveCAN(CAN_HandleTypeDef *handle, CANConfig_t &config); - osSemaphoreId sem_send_heartbeat; + bool start_can_server(); + void can_server_thread(); - // 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; + osThreadId thread_id_; + volatile bool thread_id_valid_ = false; 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) - ); + make_protocol_object("config", + make_protocol_property("node_id", &config_.node_id), + make_protocol_property("baud_rate", &config_.baud))); } + + private: + CAN_HandleTypeDef *handle_ = nullptr; + CANConfig_t &config_; }; -bool start_can_server(CAN_context& ctx, CAN_TypeDef *hcan, uint64_t serial_number); - -#endif // __INTERFACE_CAN_HPP +#endif // __INTERFACE_CAN_HPP