diff --git a/conf/modules/can.xml b/conf/modules/can.xml index a0075dd494..b7b4f3bd79 100644 --- a/conf/modules/can.xml +++ b/conf/modules/can.xml @@ -11,12 +11,11 @@ mcu - uavcan
- + diff --git a/conf/modules/uavcan.xml b/conf/modules/uavcan.xml index 511f05c14c..9398dbad77 100644 --- a/conf/modules/uavcan.xml +++ b/conf/modules/uavcan.xml @@ -10,6 +10,9 @@ + + can +
@@ -37,4 +40,3 @@ - diff --git a/sw/airborne/arch/chibios/mcu_periph/can_arch.c b/sw/airborne/arch/chibios/mcu_periph/can_arch.c index 8d922f7003..0af6052333 100644 --- a/sw/airborne/arch/chibios/mcu_periph/can_arch.c +++ b/sw/airborne/arch/chibios/mcu_periph/can_arch.c @@ -85,11 +85,23 @@ static void can_thd_rx(void* arg) { snprintf(thd_name, 10, "can%d_rx", cas->if_index); chRegSetThreadName(thd_name); + event_listener_t rxe; + chEvtRegister(&cas->cand->error_event, &rxe, EVENT_MASK(1)); + + struct pprzaddr_can addr = { .can_ifindex = cas->if_index }; while(!chThdShouldTerminateX()) { + + eventmask_t evts = chEvtWaitAnyTimeout(ALL_EVENTS, TIME_IMMEDIATE); + // receive error + if (evts & EVENT_MASK(1)) { + chEvtGetAndClearFlags(&rxe); + canp->nb_errors++; + } + CANRxFrame rx_frame; msg_t status = canReceiveTimeout(cas->cand, CAN_ANY_MAILBOX, &rx_frame, chTimeMS2I(50)); if(status == MSG_OK) { @@ -110,7 +122,7 @@ static void can_thd_rx(void* arg) { .can_id = id, .len = can_dlc_to_len(rx_frame.DLC), .flags = 0, - .timestamp = get_sys_time_msec(), + .timestamp = TIME_I2US(chVTGetSystemTimeX()) }; if(rx_frame.FDF) { @@ -142,9 +154,9 @@ int can_transmit_frame(struct pprzcan_frame* txframe, struct pprzaddr_can* addr) } if(txframe->can_id & CAN_FRAME_EFF) { frame.common.XTD = 1; - frame.ext.EID = txframe->can_id & CAN_EID_MASK + frame.ext.EID = txframe->can_id & CAN_EID_MASK; } else { - frame.std.SID = txframe->can_id & CAN_SID_MASK + frame.std.SID = txframe->can_id & CAN_SID_MASK; } memcpy(frame.data8, txframe->data, txframe->len); @@ -308,4 +320,4 @@ static bool canConfigureIface(struct can_arch_periph* cas) cas->cfg.btr = CAN_BTR_SJW(0) | CAN_BTR_TS1(bs1 - 1) | CAN_BTR_TS2(bs2 - 1) | CAN_BTR_BRP(prescaler - 1); #endif return true; -} \ No newline at end of file +} diff --git a/sw/airborne/arch/chibios/modules/uavcan/uavcan.c b/sw/airborne/arch/chibios/modules/uavcan/uavcan.c index 559125df8b..73624b5314 100644 --- a/sw/airborne/arch/chibios/modules/uavcan/uavcan.c +++ b/sw/airborne/arch/chibios/modules/uavcan/uavcan.c @@ -25,6 +25,8 @@ */ #include "uavcan.h" +#include "mcu_periph/can.h" +#include "modules/core/threads.h" #ifndef UAVCAN_NODE_ID #define UAVCAN_NODE_ID 100 @@ -45,17 +47,10 @@ static uavcan_event *uavcan_event_hd = NULL; #define UAVCAN_CAN1_BAUDRATE UAVCAN_BAUDRATE #endif -static THD_WORKING_AREA(uavcan1_rx_wa, 1024 * 2); -static THD_WORKING_AREA(uavcan1_tx_wa, 1024 * 2); struct uavcan_iface_t uavcan1 = { - .can_driver = &CAND1, + .can_net = {.can_ifindex = 1}, .can_baudrate = UAVCAN_CAN1_BAUDRATE, - .can_cfg = {0}, - .thread_rx_wa = uavcan1_rx_wa, - .thread_rx_wa_size = sizeof(uavcan1_rx_wa), - .thread_tx_wa = uavcan1_tx_wa, - .thread_tx_wa_size = sizeof(uavcan1_tx_wa), .node_id = UAVCAN_CAN1_NODE_ID, .transfer_id = 0, .initialized = false @@ -71,117 +66,56 @@ struct uavcan_iface_t uavcan1 = { #define UAVCAN_CAN2_BAUDRATE UAVCAN_BAUDRATE #endif -static THD_WORKING_AREA(uavcan2_rx_wa, 1024 * 2); -static THD_WORKING_AREA(uavcan2_tx_wa, 1024 * 2); struct uavcan_iface_t uavcan2 = { - .can_driver = &CAND2, + .can_net = {.can_ifindex = 2}, .can_baudrate = UAVCAN_CAN2_BAUDRATE, - .can_cfg = {0}, - .thread_rx_wa = uavcan2_rx_wa, - .thread_rx_wa_size = sizeof(uavcan2_rx_wa), - .thread_tx_wa = uavcan2_tx_wa, - .thread_tx_wa_size = sizeof(uavcan2_tx_wa), .node_id = UAVCAN_CAN2_NODE_ID, .transfer_id = 0, .initialized = false }; #endif -/* - * REMOVE ME - This is only here because the can driver is not yet used - */ -void can_init(void) { } -/* - * Receiver thread. - */ -static THD_FUNCTION(uavcan_rx, p) -{ - event_listener_t el; - CANRxFrame rx_msg; +static void can_frame_cb(struct pprzcan_frame* rx_msg, UNUSED struct pprzaddr_can* src_addr, void* user_data) { + struct uavcan_iface_t *iface = (struct uavcan_iface_t *)user_data; + + pprz_mtx_lock(&iface->mutex); + CanardCANFrame rx_frame; - struct uavcan_iface_t *iface = (struct uavcan_iface_t *)p; - - chRegSetThreadName("uavcan_rx"); - chEvtRegister(&iface->can_driver->rxfull_event, &el, EVENT_MASK(0)); - while (true) { - if (chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100)) == 0) { - continue; - } - chMtxLock(&iface->mutex); - - // Wait until a CAN message is received - while (canReceive(iface->can_driver, CAN_ANY_MAILBOX, &rx_msg, TIME_IMMEDIATE) == MSG_OK) { - // Process message. - const uint32_t timestamp = TIME_I2US(chVTGetSystemTimeX()); - memcpy(rx_frame.data, rx_msg.data8, 8); - rx_frame.data_len = rx_msg.DLC; -#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2) - if (rx_msg.common.XTD) { - rx_frame.id = CANARD_CAN_FRAME_EFF | rx_msg.ext.EID; - } else { - rx_frame.id = rx_msg.std.SID; - } -#else - if (rx_msg.IDE) { - rx_frame.id = CANARD_CAN_FRAME_EFF | rx_msg.EID; - } else { - rx_frame.id = rx_msg.SID; - } -#endif - - // Let canard handle the frame - canardHandleRxFrame(&iface->canard, &rx_frame, timestamp); - } - chMtxUnlock(&iface->mutex); + memcpy(rx_frame.data, rx_msg->data, 8); + rx_frame.data_len = rx_msg->len; + if (rx_msg->can_id & CAN_FRAME_EFF) { + rx_frame.id = CANARD_CAN_FRAME_EFF | (rx_msg->can_id & CAN_EID_MASK); + } else { + rx_frame.id = rx_msg->can_id & CAN_SID_MASK; } - chEvtUnregister(&iface->can_driver->rxfull_event, &el); + + // Let canard handle the frame + canardHandleRxFrame(&iface->canard, &rx_frame, rx_msg->timestamp); + + pprz_mtx_unlock(&iface->mutex); } /* * Transmitter thread. */ -static THD_FUNCTION(uavcan_tx, p) +static void uavcan_tx(void* p) { - event_listener_t txc, txe, txr; struct uavcan_iface_t *iface = (struct uavcan_iface_t *)p; uint8_t err_cnt = 0; - chRegSetThreadName("uavcan_tx"); - chEvtRegister(&iface->can_driver->txempty_event, &txc, EVENT_MASK(0)); - chEvtRegister(&iface->can_driver->error_event, &txe, EVENT_MASK(1)); - chEvtRegister(&iface->tx_request, &txr, EVENT_MASK(2)); - while (true) { - eventmask_t evts = chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100)); - // Succesfull transmit - if (evts == 0) { - continue; - } + pprz_bsem_wait(&iface->bsem); - // Transmit error - if (evts & EVENT_MASK(1)) { - chEvtGetAndClearFlags(&txe); - continue; - } - - chMtxLock(&iface->mutex); + pprz_mtx_lock(&iface->mutex); for (const CanardCANFrame *txf = NULL; (txf = canardPeekTxQueue(&iface->canard)) != NULL;) { - CANTxFrame tx_msg; - memcpy(tx_msg.data8, txf->data, 8); -#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2) - tx_msg.DLC = txf->data_len; // TODO fixme for FDCAN (>8 bytes) - tx_msg.ext.EID = txf->id & CANARD_CAN_EXT_ID_MASK; - tx_msg.common.XTD = 1; - tx_msg.common.RTR = 0; -#else - tx_msg.DLC = txf->data_len; - tx_msg.EID = txf->id & CANARD_CAN_EXT_ID_MASK; - tx_msg.IDE = CAN_IDE_EXT; - tx_msg.RTR = CAN_RTR_DATA; -#endif - if (!canTryTransmitI(iface->can_driver, CAN_ANY_MAILBOX, &tx_msg)) { + struct pprzcan_frame tx_msg; + memcpy(tx_msg.data, txf->data, 8); + tx_msg.len = txf->data_len; + tx_msg.can_id = (txf->id & CANARD_CAN_EXT_ID_MASK) | CAN_FRAME_EFF; + + if (!can_transmit_frame(&tx_msg, &iface->can_net)) { err_cnt = 0; canardPopTxQueue(&iface->canard); } else { @@ -193,13 +127,13 @@ static THD_FUNCTION(uavcan_tx, p) } // Timeout - just exit and try again later - chMtxUnlock(&iface->mutex); + pprz_mtx_unlock(&iface->mutex); chThdSleepMilliseconds(++err_cnt); - chMtxLock(&iface->mutex); + pprz_mtx_lock(&iface->mutex); continue; } } - chMtxUnlock(&iface->mutex); + pprz_mtx_unlock(&iface->mutex); } } @@ -239,133 +173,14 @@ static bool shouldAcceptTransfer(const CanardInstance *ins __attribute__((unused return false; } -/** - * Try to compute the timing registers for the can interface and set the configuration - */ -static bool uavcanConfigureIface(struct uavcan_iface_t *iface) -{ - if (iface->can_baudrate < 1) { - return false; - } - - // Hardware configurationn -#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2) - const uint32_t pclk = STM32_FDCANCLK; -#else - const uint32_t pclk = STM32_PCLK1; -#endif - static const int MaxBS1 = 16; - static const int MaxBS2 = 8; - - /* - * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG - * CAN in Automation, 2003 - * - * According to the source, optimal quanta per bit are: - * Bitrate Optimal Maximum - * 1000 kbps 8 10 - * 500 kbps 16 17 - * 250 kbps 16 17 - * 125 kbps 16 17 - */ - const int max_quanta_per_bit = (iface->can_baudrate >= 1000000) ? 10 : 17; - static const int MaxSamplePointLocation = 900; - - /* - * Computing (prescaler * BS): - * BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2)) -- See the Reference Manual - * BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2)) -- Simplified - * let: - * BS = 1 + BS1 + BS2 -- Number of time quanta per bit - * PRESCALER_BS = PRESCALER * BS - * ==> - * PRESCALER_BS = PCLK / BITRATE - */ - const uint32_t prescaler_bs = pclk / iface->can_baudrate; - -// Searching for such prescaler value so that the number of quanta per bit is highest. - uint8_t bs1_bs2_sum = max_quanta_per_bit - 1; - while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0) { - if (bs1_bs2_sum <= 2) { - return false; // No solution - } - bs1_bs2_sum--; - } - - const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum); - if ((prescaler < 1U) || (prescaler > 1024U)) { - return false; // No solution - } - - /* - * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum. - * We need to find the values so that the sample point is as close as possible to the optimal value. - * - * Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2] (* Where 7/8 is 0.875, the recommended sample point location *) - * {{bs2 -> (1 + bs1)/7}} - * - * Hence: - * bs2 = (1 + bs1) / 7 - * bs1 = (7 * bs1_bs2_sum - 1) / 8 - * - * Sample point location can be computed as follows: - * Sample point location = (1 + bs1) / (1 + bs1 + bs2) - * - * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one: - * - With rounding to nearest - * - With rounding to zero - */ -// First attempt with rounding to nearest - uint8_t bs1 = ((7 * bs1_bs2_sum - 1) + 4) / 8; - uint8_t bs2 = bs1_bs2_sum - bs1; - uint16_t sample_point_permill = 1000 * (1 + bs1) / (1 + bs1 + bs2); - -// Second attempt with rounding to zero - if (sample_point_permill > MaxSamplePointLocation) { - bs1 = (7 * bs1_bs2_sum - 1) / 8; - bs2 = bs1_bs2_sum - bs1; - sample_point_permill = 1000 * (1 + bs1) / (1 + bs1 + bs2); - } - - /* - * Final validation - * Helpful Python: - * def sample_point_from_btr(x): - * assert 0b0011110010000000111111000000000 & x == 0 - * ts2,ts1,brp = (x>>20)&7, (x>>16)&15, x&511 - * return (1+ts1+1)/(1+ts1+1+ts2+1) - * - */ - if ((iface->can_baudrate != (pclk / (prescaler * (1 + bs1 + bs2)))) || (bs1 < 1) || (bs1 > MaxBS1) || (bs2 < 1) - || (bs2 > MaxBS2)) { - return false; - } - - // Configure the interface -#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2) - iface->can_cfg.NBTP = (0 << FDCAN_NBTP_NSJW_Pos) | ((bs1 - 1) << FDCAN_NBTP_NTSEG1_Pos) | (( - bs2 - 1) << FDCAN_NBTP_NTSEG2_Pos) | ((prescaler - 1) << FDCAN_NBTP_NBRP_Pos); - iface->can_cfg.CCCR = FDCAN_CCCR_FDOE | FDCAN_CCCR_BRSE; -#else - iface->can_cfg.mcr = CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP; - iface->can_cfg.btr = CAN_BTR_SJW(0) | CAN_BTR_TS1(bs1 - 1) | CAN_BTR_TS2(bs2 - 1) | CAN_BTR_BRP(prescaler - 1); -#endif - return true; -} /** * Initialize uavcan interface */ static void uavcanInitIface(struct uavcan_iface_t *iface) { - // First try to configure abort if failed - if (!uavcanConfigureIface(iface)) { - return; - } - - // Initialize mutexes/events for multithread locking - chMtxObjectInit(&iface->mutex); - chEvtObjectInit(&iface->tx_request); + pprz_mtx_init(&iface->mutex); + pprz_bsem_init(&iface->bsem, true); // Initialize canard canardInit(&iface->canard, iface->canard_memory_pool, sizeof(iface->canard_memory_pool), @@ -373,13 +188,12 @@ static void uavcanInitIface(struct uavcan_iface_t *iface) // Update the uavcan node ID canardSetLocalNodeID(&iface->canard, iface->node_id); - // Start the can interface - canStart(iface->can_driver, &iface->can_cfg); - // Start the receiver and transmitter thread - chThdCreateStatic(iface->thread_rx_wa, iface->thread_rx_wa_size, NORMALPRIO + 8, uavcan_rx, (void *)iface); - chThdCreateStatic(iface->thread_tx_wa, iface->thread_tx_wa_size, NORMALPRIO + 7, uavcan_tx, (void *)iface); - iface->initialized = true; + can_register_callback(can_frame_cb, &iface->can_net, (void *)iface); + + if(!pprz_thread_create(&iface->thread_tx, 2048, "uavcan_tx", NORMALPRIO+7, uavcan_tx, (void *)iface)) { + iface->initialized = true; + } } /** @@ -388,25 +202,9 @@ static void uavcanInitIface(struct uavcan_iface_t *iface) void uavcan_init(void) { #if UAVCAN_USE_CAN1 -#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2) - // Configure the RAM - uavcan1.can_cfg.RXF0C = (32 << FDCAN_RXF0C_F0S_Pos) | (0 << FDCAN_RXF0C_F0SA_Pos); - uavcan1.can_cfg.RXF1C = (32 << FDCAN_RXF1C_F1S_Pos) | (128 << FDCAN_RXF1C_F1SA_Pos); - uavcan1.can_cfg.TXBC = (32 << FDCAN_TXBC_TFQS_Pos) | (256 << FDCAN_TXBC_TBSA_Pos); - uavcan1.can_cfg.TXESC = 0x000; // 8 Byte mode only (4 words per message) - uavcan1.can_cfg.RXESC = 0x000; // 8 Byte mode only (4 words per message) -#endif uavcanInitIface(&uavcan1); #endif #if UAVCAN_USE_CAN2 -#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2) - // Configure the RAM - uavcan2.can_cfg.RXF0C = (32 << FDCAN_RXF0C_F0S_Pos) | (384 << FDCAN_RXF0C_F0SA_Pos); - uavcan2.can_cfg.RXF1C = (32 << FDCAN_RXF1C_F1S_Pos) | (512 << FDCAN_RXF1C_F1SA_Pos); - uavcan2.can_cfg.TXBC = (32 << FDCAN_TXBC_TFQS_Pos) | (640 << FDCAN_TXBC_TBSA_Pos); - uavcan2.can_cfg.TXESC = 0x000; // 8 Byte mode only (4 words per message) - uavcan2.can_cfg.RXESC = 0x000; // 8 Byte mode only (4 words per message) -#endif uavcanInitIface(&uavcan2); #endif } @@ -434,12 +232,11 @@ void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature uint16_t payload_len) { if (!iface->initialized) { return; } - - chMtxLock(&iface->mutex); + pprz_mtx_lock(&iface->mutex); canardBroadcast(&iface->canard, data_type_signature, data_type_id, &iface->transfer_id, priority, payload, payload_len); - chMtxUnlock(&iface->mutex); - chEvtBroadcast(&iface->tx_request); + pprz_mtx_unlock(&iface->mutex); + pprz_bsem_signal(&iface->bsem); } diff --git a/sw/airborne/arch/chibios/modules/uavcan/uavcan.h b/sw/airborne/arch/chibios/modules/uavcan/uavcan.h index 2920324034..1b392873c4 100644 --- a/sw/airborne/arch/chibios/modules/uavcan/uavcan.h +++ b/sw/airborne/arch/chibios/modules/uavcan/uavcan.h @@ -26,24 +26,25 @@ #ifndef MODULES_UAVCAN_ARCH_H #define MODULES_UAVCAN_ARCH_H -#include #include #include +#include "mcu_periph/can.h" +#include "modules/core/threads.h" /** uavcan interface structure */ struct uavcan_iface_t { - CANDriver *can_driver; + struct pprzaddr_can can_net; uint32_t can_baudrate; - CANConfig can_cfg; event_source_t tx_request; - mutex_t mutex; - void *thread_rx_wa; - void *thread_tx_wa; - void *thread_uavcan_wa; - size_t thread_rx_wa_size; - size_t thread_tx_wa_size; - size_t thread_uavcan_wa_size; + + pprz_mutex_t mutex; + pprz_thread_t thread_tx; + // void *thread_tx_wa; + // void *thread_uavcan_wa; + // size_t thread_tx_wa_size; + // size_t thread_uavcan_wa_size; + pprz_bsem_t bsem; uint8_t node_id; CanardInstance canard; diff --git a/sw/airborne/mcu_periph/can.c b/sw/airborne/mcu_periph/can.c index e0041c1dda..bb761c2e7b 100644 --- a/sw/airborne/mcu_periph/can.c +++ b/sw/airborne/mcu_periph/can.c @@ -29,6 +29,16 @@ struct can_periph can1 = { .fd = 1, + .nb_errors = 0, + .callbacks = {0}, + .callback_user_data = {0} +}; +#endif + +#if USE_CAN2 +struct can_periph can2 = { + .fd = 2, + .nb_errors = 0, .callbacks = {0}, .callback_user_data = {0} }; diff --git a/sw/airborne/mcu_periph/can.h b/sw/airborne/mcu_periph/can.h index 72c3b46c63..b079b36964 100644 --- a/sw/airborne/mcu_periph/can.h +++ b/sw/airborne/mcu_periph/can.h @@ -73,7 +73,7 @@ struct pprzcan_frame { socketcan_id_t can_id; uint8_t len; uint8_t flags; // CAN FD specific flags - uint32_t timestamp; // timestamp in ms. + uint32_t timestamp; // timestamp in us. uint8_t data[SOCKETCAN_MAX_DLEN]; }; @@ -89,6 +89,7 @@ typedef void(* can_rx_frame_callback_t)(struct pprzcan_frame* rxframe, struct pp struct can_periph { void* arch_struct; int fd; + uint32_t nb_errors; can_rx_frame_callback_t callbacks[CAN_NB_CALLBACKS_MAX]; void* callback_user_data[CAN_NB_CALLBACKS_MAX]; }; diff --git a/sw/airborne/modules/core/slcan.c b/sw/airborne/modules/core/slcan.c index 651af13cbb..def2371e03 100644 --- a/sw/airborne/modules/core/slcan.c +++ b/sw/airborne/modules/core/slcan.c @@ -258,10 +258,11 @@ void slcan_can_rx_cb(struct pprzcan_frame* rxframe, struct pprzaddr_can* src_add } if(slcan->timestamp) { - slcan->tx_buf[idx++] = nibble2hex((rxframe->timestamp >> 12)); - slcan->tx_buf[idx++] = nibble2hex((rxframe->timestamp >> 8)); - slcan->tx_buf[idx++] = nibble2hex((rxframe->timestamp >> 4)); - slcan->tx_buf[idx++] = nibble2hex((rxframe->timestamp >> 0)); + uint16_t timestamp_ms = rxframe->timestamp / 1000; + slcan->tx_buf[idx++] = nibble2hex((timestamp_ms >> 12)); + slcan->tx_buf[idx++] = nibble2hex((timestamp_ms >> 8)); + slcan->tx_buf[idx++] = nibble2hex((timestamp_ms >> 4)); + slcan->tx_buf[idx++] = nibble2hex((timestamp_ms >> 0)); } slcan->tx_buf[idx++] = '\r';