mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
--------- Co-authored-by: Fabien-B <Fabien-B@github.com>
This commit is contained in:
@@ -0,0 +1,28 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="can" dir="mcu_periph" task="mcu">
|
||||
<doc>
|
||||
<description>
|
||||
General CAN driver
|
||||
To activate a specific CAN peripheral, define flag USE_CANx where x is your CAN peripheral number
|
||||
</description>
|
||||
<configure name="USE_CANFD" value="TRUE" description="Use CANFD or just CAN"/>
|
||||
</doc>
|
||||
<dep>
|
||||
<depends>mcu</depends>
|
||||
<conflicts>uavcan</conflicts>
|
||||
</dep>
|
||||
<header>
|
||||
<file name="can.h" dir="mcu_periph"/>
|
||||
</header>
|
||||
<event fun="can_event()" cond="ifeq ($(ARCH), stm32)"/>
|
||||
<makefile>
|
||||
<configure name="USE_CANFD" default="FALSE"/>
|
||||
<define name="USE_CANFD" value="$(USE_CANFD)"/>
|
||||
<file name="can.c" dir="mcu_periph"/>
|
||||
<file_arch name="can_arch.c" dir="mcu_periph"/>
|
||||
<test>
|
||||
<define name="USE_CAN1"/>
|
||||
</test>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -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 <ch.h>
|
||||
#include <hal.h>
|
||||
|
||||
|
||||
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; i<CAN_NB_CALLBACKS_MAX; i++) {
|
||||
if(canp->callbacks[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;
|
||||
}
|
||||
@@ -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 <hal.h>
|
||||
|
||||
void can_hw_init(void);
|
||||
@@ -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<CAN_NB_CALLBACKS_MAX; i++) {
|
||||
if(can1.callbacks[i] != NULL) {
|
||||
can1.callbacks[i](&can1_arch_s.rxframe, &can1_arch_s.addr, can1.callback_user_data[i]);
|
||||
}
|
||||
}
|
||||
can1_arch_s.new_rxframe = false;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
@@ -31,9 +31,11 @@
|
||||
#define MCU_PERIPH_STM32_CAN_ARCH_H
|
||||
|
||||
void can_hw_init(void);
|
||||
void can_event(void);
|
||||
|
||||
#if defined STM32F1
|
||||
void usb_lp_can1_rx0_irq_handler(void);
|
||||
#endif
|
||||
int can_hw_transmit(uint32_t id, const uint8_t *buf, uint8_t len);
|
||||
|
||||
|
||||
#endif /* MCU_PERIPH_STM32_CAN_ARCH_H */
|
||||
|
||||
@@ -51,6 +51,9 @@
|
||||
#define USING_SOFTI2C 1
|
||||
#include "mcu_periph/softi2c.h"
|
||||
#endif
|
||||
#if USE_CAN1 || USE_CAN2
|
||||
#include "mcu_periph/can.h"
|
||||
#endif
|
||||
#if USE_ADC
|
||||
#include "mcu_periph/adc.h"
|
||||
#endif
|
||||
@@ -195,6 +198,9 @@ void mcu_init(void)
|
||||
#ifdef USE_SOFTI2C1
|
||||
softi2c1_init();
|
||||
#endif
|
||||
#if USE_CAN1 || USE_CAN2
|
||||
can_init();
|
||||
#endif
|
||||
#if USE_ADC
|
||||
adc_init();
|
||||
#endif
|
||||
|
||||
@@ -25,24 +25,68 @@
|
||||
#include "mcu_periph/can.h"
|
||||
#include "mcu_periph/can_arch.h"
|
||||
|
||||
can_rx_callback_t can_rx_callback;
|
||||
#if USE_CAN1
|
||||
|
||||
void _can_run_rx_callback(uint32_t id, uint8_t *buf, uint8_t len);
|
||||
struct can_periph can1 = {
|
||||
.fd = 1,
|
||||
.callbacks = {0},
|
||||
.callback_user_data = {0}
|
||||
};
|
||||
#endif
|
||||
|
||||
void ppz_can_init(can_rx_callback_t callback)
|
||||
static const uint8_t dlc_to_len[] = {0,1,2,3,4,5,6,7,8,12,16,20,24,32,48,64};
|
||||
|
||||
uint8_t can_dlc_to_len(uint8_t dlc) {
|
||||
if(dlc < 16) {
|
||||
return dlc_to_len[dlc];
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint8_t can_len_to_dlc(uint8_t len) {
|
||||
if(len <= 8) {
|
||||
return len;
|
||||
}
|
||||
for(int i=9; i<16; i++) {
|
||||
if(dlc_to_len[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; i<CAN_NB_CALLBACKS_MAX; i++) {
|
||||
// use the first available slot
|
||||
if(canp->callbacks[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;
|
||||
}
|
||||
|
||||
@@ -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 */
|
||||
|
||||
@@ -0,0 +1,4 @@
|
||||
#pragma once
|
||||
|
||||
void can_hw_init(void);
|
||||
|
||||
Reference in New Issue
Block a user