Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
can_arch.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2025 The Paparazzi Team
3 *
4 * This file is part of paparazzi.
5 *
6 * Paparazzi is free software: you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation, either version 3 of the License, or
9 * (at your option) any later version.
10 *
11 * See LICENSE file for the full license version, or see http://www.gnu.org/licenses/
12 */
13
14#include "mcu_periph/can_arch.h"
15#include "mcu_periph/can.h"
16#include "mcu_periph/sys_time_arch.h"
17#include "stdio.h"
18#include "string.h"
19
20#include <ch.h>
21#include <hal.h>
22
23
34
35static void can_thd_rx(void* arg);
36static void can_start(struct can_periph* canp);
37static bool canConfigureIface(struct can_arch_periph* cas);
38
39#if USE_CAN1
40
41static THD_WORKING_AREA(can1_rx_wa, 1024 * 2);
42
44 .if_index = 1,
45 .cand = &CAND1,
46 .cfg = {0},
47 .can_baudrate = 1000000U,
48 .memory_offset = 0,
49 .thread_rx_wa = can1_rx_wa,
50 .thread_rx_wa_size = sizeof(can1_rx_wa),
51};
52
53#endif
54
55#if USE_CAN2
56
57static THD_WORKING_AREA(can2_rx_wa, 1024 * 2);
58
60 .if_index = 2,
61 .cand = &CAND2,
62 .cfg = {0},
63 .can_baudrate = 1000000U,
64 .memory_offset = (128*3),
67};
68
69#endif
70
72 #if USE_CAN1
73 can1.arch_struct = &can1_arch_s;
75 #endif
76 #if USE_CAN2
77 can2.arch_struct = &can2_arch_s;
79 #endif
80}
81
82
83
84static void can_thd_rx(void* arg) {
85 struct can_periph* canp = (struct can_periph*)arg;
86 struct can_arch_periph* cas = (struct can_arch_periph*)canp->arch_struct;
87 char thd_name[10];
88 snprintf(thd_name, 10, "can%d_rx", cas->if_index);
90
92 chEvtRegister(&cas->cand->error_event, &rxe, EVENT_MASK(1));
93
94
95 struct pprzaddr_can addr = {
96 .can_ifindex = cas->if_index
97 };
98
99 while(!chThdShouldTerminateX()) {
100
102 // receive error
103 if (evts & EVENT_MASK(1)) {
105 canp->nb_errors++;
106 }
107
108 CANRxFrame rx_frame;
110 if(status == MSG_OK) {
111 uint32_t id = 0;
112 if(rx_frame.common.XTD) {
113 id = rx_frame.ext.EID | CAN_FRAME_EFF;
114 } else {
115 id = rx_frame.std.SID;
116 }
117 if(rx_frame.common.RTR) {
118 id |= CAN_FRAME_RTR;
119 }
120 if(rx_frame.common.ESI) {
121 id |= CAN_FRAME_ERR;
122 }
123
124 struct pprzcan_frame pprz_frame = {
125 .can_id = id,
126 .len = can_dlc_to_len(rx_frame.DLC),
127 .flags = 0,
128 .timestamp = TIME_I2US(chVTGetSystemTimeX())
129 };
130
131 if(rx_frame.FDF) {
132 pprz_frame.flags |= CANFD_FDF;
133 }
134 if(rx_frame.common.ESI) {
135 pprz_frame.flags |= CANFD_ESI;
136 }
137
138
139
140 memcpy(pprz_frame.data, rx_frame.data8, pprz_frame.len);
141
142 for(int i=0; i<CAN_NB_CALLBACKS_MAX; i++) {
143 if(canp->callbacks[i] != NULL) {
144 canp->callbacks[i](&pprz_frame, &addr, canp->callback_user_data[i]);
145 }
146 }
147 }
148 }
149
150}
151
153 CANTxFrame frame = {0};
154 frame.DLC = can_len_to_dlc(txframe->len);
155 if(txframe->can_id & CAN_FRAME_RTR) {
156 frame.common.RTR = 1;
157 }
158 if(txframe->can_id & CAN_FRAME_EFF) {
159 frame.common.XTD = 1;
160 frame.ext.EID = txframe->can_id & CAN_EID_MASK;
161 } else {
162 frame.std.SID = txframe->can_id & CAN_SID_MASK;
163 }
164 memcpy(frame.data8, txframe->data, txframe->len);
165
166 #if USE_CAN1
167 if(addr->can_ifindex == 1 || addr->can_ifindex == 0) {
169 if(ret != MSG_OK) {
170 return ret;
171 }
172 }
173 #endif
174
175 #if USE_CAN2
176 if(addr->can_ifindex == 2 || addr->can_ifindex == 0) {
178 if(ret != MSG_OK) {
179 return ret;
180 }
181 }
182 #endif
183
184 return 0;
185}
186
187static void can_start(struct can_periph* canp) {
188 struct can_arch_periph* cas = (struct can_arch_periph*)canp->arch_struct;
189
190 #if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2)
191 // Configure the RAM
192 cas->cfg.RXF0C = (32 << FDCAN_RXF0C_F0S_Pos) | ((cas->memory_offset+0) << FDCAN_RXF0C_F0SA_Pos);
193 cas->cfg.RXF1C = (32 << FDCAN_RXF1C_F1S_Pos) | ((cas->memory_offset+128) << FDCAN_RXF1C_F1SA_Pos);
194 cas->cfg.TXBC = (32 << FDCAN_TXBC_TFQS_Pos) | ((cas->memory_offset+256) << FDCAN_TXBC_TBSA_Pos);
195 cas->cfg.TXESC = 0x000; // 8 Byte mode only (4 words per message)
196 cas->cfg.RXESC = 0x000; // 8 Byte mode only (4 words per message)
197 #endif
198 if (!canConfigureIface(cas)) {
199 return;
200 }
201
202
203 canStart(cas->cand, &cas->cfg);
204 chThdCreateStatic(cas->thread_rx_wa, cas->thread_rx_wa_size,
206}
207
208
213{
214 if (cas->can_baudrate < 1) {
215 return false;
216 }
217
218 // Hardware configurationn
219#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2)
221#else
222 const uint32_t pclk = STM32_PCLK1;
223#endif
224 static const int MaxBS1 = 16;
225 static const int MaxBS2 = 8;
226
227 /*
228 * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG
229 * CAN in Automation, 2003
230 *
231 * According to the source, optimal quanta per bit are:
232 * Bitrate Optimal Maximum
233 * 1000 kbps 8 10
234 * 500 kbps 16 17
235 * 250 kbps 16 17
236 * 125 kbps 16 17
237 */
238 const int max_quanta_per_bit = (cas->can_baudrate >= 1000000) ? 10 : 17;
239 static const int MaxSamplePointLocation = 900;
240
241 /*
242 * Computing (prescaler * BS):
243 * BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2)) -- See the Reference Manual
244 * BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2)) -- Simplified
245 * let:
246 * BS = 1 + BS1 + BS2 -- Number of time quanta per bit
247 * PRESCALER_BS = PRESCALER * BS
248 * ==>
249 * PRESCALER_BS = PCLK / BITRATE
250 */
251 const uint32_t prescaler_bs = pclk / cas->can_baudrate;
252
253// Searching for such prescaler value so that the number of quanta per bit is highest.
255 while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0) {
256 if (bs1_bs2_sum <= 2) {
257 return false; // No solution
258 }
259 bs1_bs2_sum--;
260 }
261
262 const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum);
263 if ((prescaler < 1U) || (prescaler > 1024U)) {
264 return false; // No solution
265 }
266
267 /*
268 * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum.
269 * We need to find the values so that the sample point is as close as possible to the optimal value.
270 *
271 * Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2] (* Where 7/8 is 0.875, the recommended sample point location *)
272 * {{bs2 -> (1 + bs1)/7}}
273 *
274 * Hence:
275 * bs2 = (1 + bs1) / 7
276 * bs1 = (7 * bs1_bs2_sum - 1) / 8
277 *
278 * Sample point location can be computed as follows:
279 * Sample point location = (1 + bs1) / (1 + bs1 + bs2)
280 *
281 * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one:
282 * - With rounding to nearest
283 * - With rounding to zero
284 */
285// First attempt with rounding to nearest
286 uint8_t bs1 = ((7 * bs1_bs2_sum - 1) + 4) / 8;
288 uint16_t sample_point_permill = 1000 * (1 + bs1) / (1 + bs1 + bs2);
289
290// Second attempt with rounding to zero
292 bs1 = (7 * bs1_bs2_sum - 1) / 8;
293 bs2 = bs1_bs2_sum - bs1;
294 sample_point_permill = 1000 * (1 + bs1) / (1 + bs1 + bs2);
295 }
296
297 /*
298 * Final validation
299 * Helpful Python:
300 * def sample_point_from_btr(x):
301 * assert 0b0011110010000000111111000000000 & x == 0
302 * ts2,ts1,brp = (x>>20)&7, (x>>16)&15, x&511
303 * return (1+ts1+1)/(1+ts1+1+ts2+1)
304 *
305 */
306 if ((cas->can_baudrate != (pclk / (prescaler * (1 + bs1 + bs2)))) || (bs1 < 1) || (bs1 > MaxBS1) || (bs2 < 1)
307 || (bs2 > MaxBS2)) {
308 return false;
309 }
310
311 // Configure the interface
312#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2)
313 cas->cfg.NBTP = (0 << FDCAN_NBTP_NSJW_Pos) | ((bs1 - 1) << FDCAN_NBTP_NTSEG1_Pos) | ((
314 bs2 - 1) << FDCAN_NBTP_NTSEG2_Pos) | ((prescaler - 1) << FDCAN_NBTP_NBRP_Pos);
315 #if USE_CANFD
316 cas->cfg.CCCR = FDCAN_CCCR_FDOE | FDCAN_CCCR_BRSE;
317 #else
318 cas->cfg.CCCR = 0;
319 #endif
320
321#else
323 cas->cfg.btr = CAN_BTR_SJW(0) | CAN_BTR_TS1(bs1 - 1) | CAN_BTR_TS2(bs2 - 1) | CAN_BTR_BRP(prescaler - 1);
324#endif
325 return true;
326}
static uint8_t status
uint8_t can_dlc_to_len(uint8_t dlc)
Definition can.c:49
uint8_t can_len_to_dlc(uint8_t len)
Definition can.c:56
int can_ifindex
Definition can.h:83
#define CANFD_FDF
Definition can.h:70
#define CAN_NB_CALLBACKS_MAX
Definition can.h:40
socketcan_id_t can_id
Definition can.h:73
#define CAN_FRAME_ERR
Definition can.h:58
#define CAN_FRAME_EFF
Definition can.h:62
#define CAN_EID_MASK
Definition can.h:64
#define CAN_SID_MASK
Definition can.h:65
#define CANFD_ESI
Definition can.h:69
#define CAN_FRAME_RTR
Definition can.h:60
static uint8_t frame[20]
static bool canConfigureIface(struct can_arch_periph *cas)
Try to compute the timing registers for the can interface and set the configuration.
Definition can_arch.c:212
static void can_thd_rx(void *arg)
Definition can_arch.c:84
static void can_start(struct can_periph *canp)
Definition can_arch.c:187
void can_hw_init()
Definition can_arch.c:71
int can_transmit_frame(struct pprzcan_frame *txframe, struct pprzaddr_can *addr)
Definition can_arch.c:152
static THD_WORKING_AREA(wa_thd_spi1, SPI_THREAD_STACK_SIZE)
uint16_t foo
Definition main_demo5.c:58
uint16_t memory_offset
Definition can_arch.c:29
CANDriver * cand
Definition can_arch.c:26
uint32_t can_baudrate
Definition can_arch.c:28
struct can_arch_periph can1_arch_s
Definition can_arch.c:61
void * thread_rx_wa
Definition can_arch.c:31
CANConfig cfg
Definition can_arch.c:27
size_t thread_rx_wa_size
Definition can_arch.c:32
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.