From fda93554d6b28078f31894b2b0121ead60ba5759 Mon Sep 17 00:00:00 2001 From: Fabien-B Date: Fri, 14 Feb 2025 09:09:11 +0100 Subject: [PATCH] CAN abstraction (#3430) --------- Co-authored-by: Fabien-B --- conf/modules/can.xml | 28 ++ .../arch/chibios/mcu_periph/can_arch.c | 311 ++++++++++++++++++ .../arch/chibios/mcu_periph/can_arch.h | 18 + sw/airborne/arch/stm32/mcu_periph/can_arch.c | 123 ++++--- sw/airborne/arch/stm32/mcu_periph/can_arch.h | 4 +- sw/airborne/mcu.c | 6 + sw/airborne/mcu_periph/can.c | 66 +++- sw/airborne/mcu_periph/can.h | 94 +++++- tests/modules/test_arch/mcu_periph/can_arch.h | 4 + 9 files changed, 586 insertions(+), 68 deletions(-) create mode 100644 conf/modules/can.xml create mode 100644 sw/airborne/arch/chibios/mcu_periph/can_arch.c create mode 100644 sw/airborne/arch/chibios/mcu_periph/can_arch.h create mode 100644 tests/modules/test_arch/mcu_periph/can_arch.h diff --git a/conf/modules/can.xml b/conf/modules/can.xml new file mode 100644 index 0000000000..c9137a01c1 --- /dev/null +++ b/conf/modules/can.xml @@ -0,0 +1,28 @@ + + + + + + General CAN driver + To activate a specific CAN peripheral, define flag USE_CANx where x is your CAN peripheral number + + + + + mcu + uavcan + +
+ +
+ + + + + + + + + + + diff --git a/sw/airborne/arch/chibios/mcu_periph/can_arch.c b/sw/airborne/arch/chibios/mcu_periph/can_arch.c new file mode 100644 index 0000000000..8d922f7003 --- /dev/null +++ b/sw/airborne/arch/chibios/mcu_periph/can_arch.c @@ -0,0 +1,311 @@ +/* + * Copyright (C) 2025 The Paparazzi Team + * + * This file is part of paparazzi. + * + * Paparazzi is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * See LICENSE file for the full license version, or see http://www.gnu.org/licenses/ + */ + +#include "mcu_periph/can_arch.h" +#include "mcu_periph/can.h" +#include "mcu_periph/sys_time_arch.h" +#include "stdio.h" +#include "string.h" + +#include +#include + + +struct can_arch_periph { + int if_index; + CANDriver* cand; + CANConfig cfg; + uint32_t can_baudrate; + + void *thread_rx_wa; + size_t thread_rx_wa_size; +}; + +static void can_thd_rx(void* arg); +static void can_start(struct can_periph* canp); +static bool canConfigureIface(struct can_arch_periph* cas); + +#if USE_CAN1 + +static THD_WORKING_AREA(can1_rx_wa, 1024 * 2); + +struct can_arch_periph can1_arch_s = { + .if_index = 1, + .cand = &CAND1, + .cfg = {0}, + .can_baudrate = 1000000U, + .thread_rx_wa = can1_rx_wa, + .thread_rx_wa_size = sizeof(can1_rx_wa), +}; + +#endif + +#if USE_CAN2 + +static THD_WORKING_AREA(can2_rx_wa, 1024 * 2); + +struct can_arch_periph can2_arch_s = { + .if_index = 2, + .cand = &CAND2, + .cfg = {0}, + .can_baudrate = 1000000U, + .thread_rx_wa = can2_rx_wa, + .thread_rx_wa_size = sizeof(can2_rx_wa), +}; + +#endif + +void can_hw_init() { + #if USE_CAN1 + can1.arch_struct = &can1_arch_s; + can_start(&can1); + #endif + #if USE_CAN2 + can2.arch_struct = &can2_arch_s; + can_start(&can2); + #endif +} + + + +static void can_thd_rx(void* arg) { + struct can_periph* canp = (struct can_periph*)arg; + struct can_arch_periph* cas = (struct can_arch_periph*)canp->arch_struct; + char thd_name[10]; + snprintf(thd_name, 10, "can%d_rx", cas->if_index); + chRegSetThreadName(thd_name); + + struct pprzaddr_can addr = { + .can_ifindex = cas->if_index + }; + + while(!chThdShouldTerminateX()) { + CANRxFrame rx_frame; + msg_t status = canReceiveTimeout(cas->cand, CAN_ANY_MAILBOX, &rx_frame, chTimeMS2I(50)); + if(status == MSG_OK) { + uint32_t id = 0; + if(rx_frame.common.XTD) { + id = rx_frame.ext.EID | CAN_FRAME_EFF; + } else { + id = rx_frame.std.SID; + } + if(rx_frame.common.RTR) { + id |= CAN_FRAME_RTR; + } + if(rx_frame.common.ESI) { + id |= CAN_FRAME_ERR; + } + + struct pprzcan_frame pprz_frame = { + .can_id = id, + .len = can_dlc_to_len(rx_frame.DLC), + .flags = 0, + .timestamp = get_sys_time_msec(), + }; + + if(rx_frame.FDF) { + pprz_frame.flags |= CANFD_FDF; + } + if(rx_frame.common.ESI) { + pprz_frame.flags |= CANFD_ESI; + } + + + + memcpy(pprz_frame.data, rx_frame.data8, pprz_frame.len); + + for(int i=0; icallbacks[i] != NULL) { + canp->callbacks[i](&pprz_frame, &addr, canp->callback_user_data[i]); + } + } + } + } + +} + +int can_transmit_frame(struct pprzcan_frame* txframe, struct pprzaddr_can* addr) { + CANTxFrame frame; + frame.DLC = can_len_to_dlc(txframe->len); + if(txframe->can_id & CAN_FRAME_RTR) { + frame.common.RTR = 1; + } + if(txframe->can_id & CAN_FRAME_EFF) { + frame.common.XTD = 1; + frame.ext.EID = txframe->can_id & CAN_EID_MASK + } else { + frame.std.SID = txframe->can_id & CAN_SID_MASK + } + memcpy(frame.data8, txframe->data, txframe->len); + + #if USE_CAN1 + if(addr->can_ifindex == 1 || addr->can_ifindex == 0) { + msg_t ret = canTransmitTimeout(&CAND1, CAN_ANY_MAILBOX, &frame, TIME_IMMEDIATE); + if(ret != MSG_OK) { + return ret; + } + } + #endif + + #if USE_CAN2 + if(addr->can_ifindex == 2 || addr->can_ifindex == 0) { + msg_t ret = canTransmitTimeout(&CAND2, CAN_ANY_MAILBOX, &frame, TIME_IMMEDIATE); + if(ret != MSG_OK) { + return ret; + } + } + #endif + + return 0; +} + +static void can_start(struct can_periph* canp) { + struct can_arch_periph* cas = (struct can_arch_periph*)canp->arch_struct; + + #if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2) + // Configure the RAM + can1_arch_s.cfg.RXF0C = (32 << FDCAN_RXF0C_F0S_Pos) | (0 << FDCAN_RXF0C_F0SA_Pos); + can1_arch_s.cfg.RXF1C = (32 << FDCAN_RXF1C_F1S_Pos) | (128 << FDCAN_RXF1C_F1SA_Pos); + can1_arch_s.cfg.TXBC = (32 << FDCAN_TXBC_TFQS_Pos) | (256 << FDCAN_TXBC_TBSA_Pos); + can1_arch_s.cfg.TXESC = 0x000; // 8 Byte mode only (4 words per message) + can1_arch_s.cfg.RXESC = 0x000; // 8 Byte mode only (4 words per message) + #endif + if (!canConfigureIface(cas)) { + return; + } + + + canStart(cas->cand, &can1_arch_s.cfg); + chThdCreateStatic(cas->thread_rx_wa, cas->thread_rx_wa_size, + NORMALPRIO + 8, can_thd_rx, canp); +} + + +/** + * Try to compute the timing registers for the can interface and set the configuration + */ +static bool canConfigureIface(struct can_arch_periph* cas) +{ + if (cas->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 = (cas->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 / cas->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 ((cas->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) + cas->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); + #if USE_CANFD + cas->cfg.CCCR = FDCAN_CCCR_FDOE | FDCAN_CCCR_BRSE; + #else + cas->cfg.CCCR = 0; + #endif + +#else + cas->cfg.mcr = CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP; + 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/mcu_periph/can_arch.h b/sw/airborne/arch/chibios/mcu_periph/can_arch.h new file mode 100644 index 0000000000..131dad4b26 --- /dev/null +++ b/sw/airborne/arch/chibios/mcu_periph/can_arch.h @@ -0,0 +1,18 @@ +/* + * Copyright (C) 2025 The Paparazzi Team + * + * This file is part of paparazzi. + * + * Paparazzi is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * See LICENSE file for the full license version, or see http://www.gnu.org/licenses/ + */ + +#pragma once + +#include + +void can_hw_init(void); diff --git a/sw/airborne/arch/stm32/mcu_periph/can_arch.c b/sw/airborne/arch/stm32/mcu_periph/can_arch.c index 2318967d7f..9c370b2cb2 100644 --- a/sw/airborne/arch/stm32/mcu_periph/can_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/can_arch.c @@ -50,11 +50,26 @@ void _can_run_rx_callback(uint32_t id, uint8_t *buf, uint8_t len); -bool can_initialized = false; +struct can_arch_periph { + uint32_t canport; + bool can_initialized; + struct pprzcan_frame rxframe; + bool new_rxframe; + struct pprzaddr_can addr; +}; + +struct can_arch_periph can1_arch_s = { + .canport = CAN1, + .can_initialized = false, + .addr = {.can_ifindex = 1}, + .rxframe = {0}, + .new_rxframe = false; +}; + void can_hw_init(void) { - + can1.arch_struct = &can1_arch_s; #ifdef STM32F1 /* Enable peripheral clocks. */ @@ -94,7 +109,7 @@ void can_hw_init(void) #endif /* Reset CAN. */ - can_reset(CAN1); + can_reset(can1_arch_s.canport); /* CAN cell init. * For time quanta calculation see STM32 reference manual @@ -122,7 +137,7 @@ void can_hw_init(void) * NOTE: Although it is out of spec I managed to have CAN run at 2MBit * Just decrease the prescaler to 1. It worked for me(tm) (esden) */ - if (can_init(CAN1, + if (can_init(can1_arch_s.canport, false, /* TTCM: Time triggered comm mode? */ true, /* ABOM: Automatic bus-off management? */ false, /* AWUM: Automatic wakeup mode? */ @@ -146,7 +161,7 @@ void can_hw_init(void) * driver should... */ - can_reset(CAN1); + can_reset(can1_arch_s.canport); return; } @@ -159,84 +174,86 @@ void can_hw_init(void) true); /* Enable the filter. */ /* Enable CAN RX interrupt. */ - can_enable_irq(CAN1, CAN_IER_FMPIE0); + can_enable_irq(can1_arch_s.canport, CAN_IER_FMPIE0); /* Remember that we succeeded to initialize. */ - can_initialized = true; + can1_arch_s.can_initialized = true; } -int can_hw_transmit(uint32_t id, const uint8_t *buf, uint8_t len) -{ - if (!can_initialized) { +int can_transmit_frame(struct pprzcan_frame* txframe, struct pprzaddr_can* addr) { + if (!can1_arch_s.can_initialized) { return -2; } - if (len > 8) { - return -1; + if(txframe->len > 8) { + return -1; //does not currently support CANFD } - - /* FIXME: we are discarding the const qualifier for buf here. - * We should probably fix libopencm3 to actually have the - * const qualifier too... - */ - return can_transmit(CAN1, - id, /* (EX/ST)ID: CAN ID */ + return can_transmit(can1_arch_s.canport, #ifdef USE_CAN_EXT_ID + txframe->can_id & CAN_EID_MASK, true, /* IDE: CAN ID extended */ #else - false, /* IDE: CAN ID not extended */ + txframe->can_id & CAN_SID_MASK, + false, /* IDE: CAN ID standard */ #endif - false, /* RTR: Request transmit? */ - len, /* DLC: Data length */ - (uint8_t *)buf); + txframe->can_id & CAN_FRAME_RTR, /* RTR: Request transmit? */ + can_len_to_dlc(txframe->len), /* DLC: Data length */ + (uint8_t *)txframe->data); + } + #ifdef STM32F1 void usb_lp_can_rx0_isr(void) +#elif STM32F4 +void can1_rx0_isr(void) +#else +#error "CAN unsuported on this MCU!" +void __unsupported_isr(void) +#endif { uint32_t id; uint8_t fmi; bool ext, rtr; - uint8_t length, data[8]; - uint16_t timestamp; + uint8_t dlc; + struct pprzcan_frame* rxframe = &can1_arch_s.rxframe; - can_receive(CAN1, + + can_receive(can1_arch_s.canport, 0, /* FIFO: 0 */ false, /* Release */ - &id, + &rxframe->can_id, &ext, &rtr, &fmi, - &length, - data, - ×tamp); + &dlc, + rxframe->data, + &rxframe->timestamp); + + rxframe->len = can_dlc_to_len(dlc); - _can_run_rx_callback(id, data, length); + if(ext) { + rxframe->can_id |= CAN_FRAME_EFF; + } + + if(rtr) { + rxframe->can_id |= CAN_FRAME_RTR; + } - can_fifo_release(CAN1, 0); + can1_arch_s.new_rxframe = true; + + can_fifo_release(can1_arch_s.canport, 0); } -#elif STM32F4 -void can1_rx0_isr(void){ - uint32_t id; - uint8_t fmi; - bool ext, rtr; - uint8_t length, data[8]; - uint16_t timestamp; - can_receive(CAN1, - 0, /* FIFO: 0 */ - false, /* Release */ - &id, - &ext, - &rtr, - &fmi, - &length, - data, - ×tamp); - _can_run_rx_callback(id, data, length); - - can_fifo_release(CAN1, 0); +void can_event() { + if(can1_arch_s.new_rxframe) { + for(int i=0; i= len) { + return i; + } + } + return 15; +} + + +void can_init() { - can_rx_callback = callback; can_hw_init(); } -int ppz_can_transmit(uint32_t id, const uint8_t *buf, uint8_t len) -{ - return can_hw_transmit(id, buf, len); + +static int add_can_callback(struct can_periph* canp, can_rx_frame_callback_t callback, void* user_data) { + for(int i =0; icallbacks[i] == NULL) { + canp->callbacks[i] = callback; + canp->callback_user_data[i] = user_data; + return 0; + } + } + // no available slot + return -1; } -void _can_run_rx_callback(uint32_t id, uint8_t *buf, uint8_t len) -{ - if (can_rx_callback) { - can_rx_callback(id, buf, len); +int can_register_callback(can_rx_frame_callback_t callback, struct pprzaddr_can* src_addr, void* user_data) { + #if USE_CAN1 + if(src_addr->can_ifindex == 1 || src_addr->can_ifindex == 0) { + int ret = add_can_callback(&can1, callback, user_data); + if(ret) { return ret; } } + #endif + #if USE_CAN2 + if(src_addr->can_ifindex == 2 || src_addr->can_ifindex == 0) { + int ret = add_can_callback(&can2, callback, user_data); + if(ret) { return ret; } + } + #endif + return 0; } diff --git a/sw/airborne/mcu_periph/can.h b/sw/airborne/mcu_periph/can.h index 666ffd574f..72c3b46c63 100644 --- a/sw/airborne/mcu_periph/can.h +++ b/sw/airborne/mcu_periph/can.h @@ -23,9 +23,97 @@ #ifndef CAN_H #define CAN_H -typedef void(* can_rx_callback_t)(uint32_t id, uint8_t *buf, uint8_t len); +#include "std.h" +#include "mcu_periph/can_arch.h" -void ppz_can_init(can_rx_callback_t callback); -int ppz_can_transmit(uint32_t id, const uint8_t *buf, uint8_t len); +#ifndef CAN_FD_MODE +#define CAN_FD_MODE TRUE +#endif +#ifdef CAN_FD_MODE +#define SOCKETCAN_MAX_DLEN 64U +#else +#define SOCKETCAN_MAX_DLEN 8U +#endif + +#ifndef CAN_NB_CALLBACKS_MAX +#define CAN_NB_CALLBACKS_MAX 10 +#endif + +// CAN identifier +// +------+--------------------------------------------------------------+ +// | Bits | Description | +// +======+==============================================================+ +// | 0-28 | CAN identifier (11/29 bit) | +// +------+--------------------------------------------------------------+ +// | 29 | Error message frame flag (0 = data frame, 1 = error message) | +// +------+--------------------------------------------------------------+ +// | 30 | Remote transmission request flag (1 = RTR frame) | +// +------+--------------------------------------------------------------+ +// | 31 | Frame format flag (0 = standard 11 bit, 1 = extended 29 bit) | +// +------+--------------------------------------------------------------+ +typedef uint32_t socketcan_id_t; + +// error flag +#define CAN_FRAME_ERR (1<<29) +// remote transmition request +#define CAN_FRAME_RTR (1<<30) +// extended identifier +#define CAN_FRAME_EFF (1<<31) + +#define CAN_EID_MASK 0x1FFFFFFF +#define CAN_SID_MASK 0x7FF + +// /* CAN FD specific flags from Linux Kernel (include/uapi/linux/can.h) */ +#define CANFD_BRS 0x01 +#define CANFD_ESI 0x02 +#define CANFD_FDF 0x04 + +struct pprzcan_frame { + socketcan_id_t can_id; + uint8_t len; + uint8_t flags; // CAN FD specific flags + uint32_t timestamp; // timestamp in ms. + uint8_t data[SOCKETCAN_MAX_DLEN]; +}; + +// socketaddr_can paparazzi abstraction +struct pprzaddr_can { + //sa_family_t can_family; + int can_ifindex; // network interface index +}; + +typedef void(* can_rx_frame_callback_t)(struct pprzcan_frame* rxframe, struct pprzaddr_can* src_addr, void* user_data); + + +struct can_periph { + void* arch_struct; + int fd; + can_rx_frame_callback_t callbacks[CAN_NB_CALLBACKS_MAX]; + void* callback_user_data[CAN_NB_CALLBACKS_MAX]; +}; + +#if USE_CAN1 +extern struct can_periph can1; +#endif +#if USE_CAN2 +extern struct can_periph can2; +#endif + +void can_init(void); + +/** + * Add a callback on received frames from an interface + * @param callback The callback called on received frames + * @param src_addr Interface from which frames are received. 0 means all interfaces. + * @param user_data Pointer that will be passed in callback parameters + * @return 0 if the callback was successfully added. + */ +int can_register_callback(can_rx_frame_callback_t callback, struct pprzaddr_can* src_addr, void* user_data); + +int can_transmit_frame(struct pprzcan_frame* txframe, struct pprzaddr_can* dst_addr); + + +uint8_t can_dlc_to_len(uint8_t dlc); +uint8_t can_len_to_dlc(uint8_t len); #endif /* CAN_H */ diff --git a/tests/modules/test_arch/mcu_periph/can_arch.h b/tests/modules/test_arch/mcu_periph/can_arch.h new file mode 100644 index 0000000000..75b5213dcd --- /dev/null +++ b/tests/modules/test_arch/mcu_periph/can_arch.h @@ -0,0 +1,4 @@ +#pragma once + +void can_hw_init(void); +