Line data Source code
1 : /*
2 : * sent.cpp
3 : *
4 : * SENT protocol decoder
5 : *
6 : * TODO support MAF sensors like 04E906051 see https://github.com/rusefi/rusefi-hardware/issues/146
7 : *
8 : * @date Oct 01, 2022
9 : * @author Andrey Gusakov <dron0gus@gmail.com>, (c) 2022
10 : */
11 :
12 : #if EFI_PROD_CODE || EFI_UNIT_TEST
13 :
14 : #include "pch.h"
15 :
16 : #if EFI_SENT_SUPPORT
17 :
18 : #include "sent.h"
19 : #include "init.h"
20 : #include "sent_logic.h"
21 : #include "sent_constants.h"
22 :
23 : /*==========================================================================*/
24 : /* Protocol definitions. */
25 : /*==========================================================================*/
26 :
27 : #define SENT_OFFSET_INTERVAL 12
28 : #define SENT_SYNC_INTERVAL (56 - SENT_OFFSET_INTERVAL) // 56 ticks - 12
29 :
30 : #define SENT_MAX_INTERVAL 15
31 :
32 : #define SENT_CRC_SEED 0x05
33 :
34 : /* Sync + Status + Signals + CRC: 9 pulses */
35 : #define SENT_MSG_TOTAL (1 + SENT_MSG_PAYLOAD_SIZE)
36 :
37 : /* use 3 full frames + one additional pulse for unit time calibration */
38 : #define SENT_CALIBRATION_PULSES (1 + 3 * SENT_MSG_PAYLOAD_SIZE)
39 :
40 : /*==========================================================================*/
41 : /* Decoder configuration */
42 : /*==========================================================================*/
43 :
44 : /*==========================================================================*/
45 : /* Decoder */
46 : /*==========================================================================*/
47 :
48 : /* Helpers for Msg manipulations */
49 : /* nibbles order: status, sig0_MSN, sig0_MidN, sig0_LSN, sig1_MSN, sig1_MidN, sig1_LSN, CRC */
50 : /* we shift rxReg left for 4 bits on each nibble received and put newest nibble
51 : * in [3:0] bits of rxReg, so when full message is received:
52 : * CRC is [3:0] - nibble 7
53 : * status is [31:28] - nibble 0
54 : * sig0 is [27:16], sig1 is [15:4] */
55 : #define MsgGetNibble(msg, n) (((msg) >> (4 * (7 - (n)))) & 0xf)
56 : #define MsgGetStat(msg) MsgGetNibble(msg, 0)
57 : #define MsgGetSig0(msg) (((msg) >> (4 * 4)) & 0xfff)
58 : #define MsgGetSig1(msg) (((msg) >> (1 * 4)) & 0xfff)
59 : #define MsgGetCrc(msg) MsgGetNibble(msg, 7)
60 :
61 : /* convert CPU ticks to float Us */
62 : #define TicksToUs(ticks) ((float)(ticks) * 1000.0 * 1000.0 / CORE_CLOCK)
63 :
64 0 : void sent_channel::restart() {
65 0 : state = SENT_STATE_CALIB;
66 0 : pulseCounter = 0;
67 0 : currentStatePulseCounter = 0;
68 0 : pausePulseReceived = false;
69 0 : tickPerUnit = 0;
70 :
71 : /* reset slow channels */
72 0 : SlowChannelDecoderReset();
73 0 : scMsgFlags = 0;
74 :
75 : #if SENT_STATISTIC_COUNTERS
76 0 : statistic.ShortIntervalErr = 0;
77 0 : statistic.LongIntervalErr = 0;
78 0 : statistic.SyncErr = 0;
79 0 : statistic.CrcErrCnt = 0;
80 0 : statistic.FrameCnt = 0;
81 0 : statistic.PauseCnt = 0;
82 0 : statistic.sc = 0;
83 0 : statistic.scCrcErr = 0;
84 0 : statistic.RestartCnt++;
85 : #endif
86 0 : }
87 :
88 13878 : void sent_channel::calcTickPerUnit(uint32_t clocks) {
89 : /* int division with rounding */
90 13878 : tickPerUnit = (clocks + (SENT_SYNC_INTERVAL + SENT_OFFSET_INTERVAL) / 2) /
91 : (SENT_SYNC_INTERVAL + SENT_OFFSET_INTERVAL);
92 13878 : }
93 :
94 126874 : float sent_channel::getTickTime() {
95 126874 : return tickPerUnit;
96 : }
97 :
98 28852 : bool sent_channel::isSyncPulse(uint32_t clocks)
99 : {
100 : /* check if pulse looks like sync with allowed +/-20% deviation */
101 28852 : uint32_t syncClocks = (SENT_SYNC_INTERVAL + SENT_OFFSET_INTERVAL) * tickPerUnit;
102 :
103 28852 : if (((100 * clocks) >= (syncClocks * 80)) &&
104 17371 : ((100 * clocks) <= (syncClocks * 120)))
105 13830 : return 1;
106 :
107 15022 : return 0;
108 : }
109 :
110 126874 : int sent_channel::FastChannelDecoder(uint32_t clocks) {
111 126874 : pulseCounter++;
112 :
113 : /* special case - tick time calculation */
114 126874 : if (state == SENT_STATE_CALIB) {
115 101 : if ((tickPerUnit == 0) || (currentStatePulseCounter == 0)) {
116 : /* invalid or not yet calculated tickPerUnit */
117 7 : calcTickPerUnit(clocks);
118 : /* lets assume this is sync pulse... */
119 7 : currentStatePulseCounter = 1;
120 : } else {
121 : /* some tickPerUnit calculated...
122 : * Check next 1 + 6 + 1 pulses if they are valid with current tickPerUnit */
123 94 : criticalAssert(tickPerUnit != 0, "zero tickPerUnit", 0);
124 94 : int checkInterval = (clocks + tickPerUnit / 2) / tickPerUnit - SENT_OFFSET_INTERVAL;
125 94 : if ((checkInterval >= 0) && (checkInterval <= SENT_MAX_INTERVAL)) {
126 53 : currentStatePulseCounter++;
127 : /* Should end up with CRC pulse */
128 53 : if (currentStatePulseCounter == (1 + SENT_MSG_PAYLOAD_SIZE)) {
129 6 : pulseCounter = 0;
130 6 : currentStatePulseCounter = 0;
131 6 : state = SENT_STATE_INIT;
132 : }
133 : } else {
134 41 : currentStatePulseCounter = 1;
135 41 : calcTickPerUnit(clocks);
136 : }
137 : }
138 101 : if (pulseCounter >= SENT_CALIBRATION_PULSES) {
139 : /* failed to calculate valid tickPerUnit, restart */
140 0 : restart();
141 : }
142 101 : return 0;
143 : }
144 :
145 : /* special case for out-of-sync state */
146 126773 : if (state == SENT_STATE_INIT) {
147 1703 : if (isSyncPulse(clocks)) {
148 : /* adjust unit time */
149 309 : calcTickPerUnit(clocks);
150 : /* we get here from calibration phase. calibration phase end with CRC nibble
151 : * if we had to skip ONE pulse before we get sync - that means device may send pause
152 : * pulse in between of messages */
153 309 : pausePulseReceived = false;
154 309 : if (currentStatePulseCounter == 1) {
155 26 : pausePulseReceived = true;
156 : }
157 : /* next state */
158 309 : currentStatePulseCounter = 0;
159 309 : state = SENT_STATE_STATUS;
160 : } else {
161 1394 : currentStatePulseCounter++;
162 : /* 3 frames skipped, no SYNC detected - recalibrate */
163 1394 : if (currentStatePulseCounter >= (SENT_MSG_TOTAL * 3)) {
164 0 : restart();
165 : }
166 : }
167 : /* done for this pulse */
168 1703 : return 0;
169 : }
170 :
171 125070 : int interval = (clocks + tickPerUnit / 2) / tickPerUnit - SENT_OFFSET_INTERVAL;
172 :
173 125070 : if (interval < 0) {
174 : #if SENT_STATISTIC_COUNTERS
175 279 : statistic.ShortIntervalErr++;
176 : #endif //SENT_STATISTIC_COUNTERS
177 279 : state = SENT_STATE_INIT;
178 279 : return -1;
179 : }
180 :
181 124791 : switch(state)
182 : {
183 0 : case SENT_STATE_CALIB:
184 : case SENT_STATE_INIT:
185 : /* handled above, should not get in here */
186 0 : return -1;
187 :
188 16617 : case SENT_STATE_SYNC:
189 16617 : if (isSyncPulse(clocks))
190 : {
191 : /* measured tick interval will be used until next sync pulse */
192 13331 : calcTickPerUnit(clocks);
193 13331 : rxReg = 0;
194 13331 : state = SENT_STATE_STATUS;
195 : }
196 : else
197 : {
198 3286 : if (pausePulseReceived) {
199 : #if SENT_STATISTIC_COUNTERS
200 : // Increment sync interval err count
201 0 : statistic.SyncErr++;
202 0 : if (interval > SENT_SYNC_INTERVAL)
203 : {
204 0 : statistic.LongIntervalErr++;
205 : }
206 : else
207 : {
208 0 : statistic.ShortIntervalErr++;
209 : }
210 : #endif // SENT_STATISTIC_COUNTERS
211 : /* wait for next sync and recalibrate tickPerUnit */
212 0 : state = SENT_STATE_INIT;
213 0 : return -1;
214 : } else {
215 : /* This is possibly pause pulse */
216 : /* TODO: check:
217 : * Minimum Length 12 ticks (equivalent to a nibble with 0 value) - this is already checked
218 : * Maximum Length 768 ticks (3 * 256) */
219 : #if SENT_STATISTIC_COUNTERS
220 3286 : statistic.PauseCnt++;
221 : #endif // SENT_STATISTIC_COUNTERS
222 3286 : pausePulseReceived = true;
223 : }
224 : }
225 16617 : return 0;
226 :
227 13798 : case SENT_STATE_STATUS:
228 : /* it is possible that pause pulse was threaded as sync and we are here with sync pulse */
229 13798 : if ((pausePulseReceived == false) && isSyncPulse(clocks)) {
230 : #if SENT_STATISTIC_COUNTERS
231 190 : statistic.PauseCnt++;
232 : #endif // SENT_STATISTIC_COUNTERS
233 : /* measured tick interval will be used until next sync pulse */
234 190 : calcTickPerUnit(clocks);
235 190 : return 0;
236 : }
237 : // fallthrough
238 : case SENT_STATE_SIG1_DATA1:
239 : case SENT_STATE_SIG1_DATA2:
240 : case SENT_STATE_SIG1_DATA3:
241 : case SENT_STATE_SIG2_DATA1:
242 : case SENT_STATE_SIG2_DATA2:
243 : case SENT_STATE_SIG2_DATA3:
244 : case SENT_STATE_CRC:
245 107984 : if (interval > SENT_MAX_INTERVAL)
246 : {
247 : #if SENT_STATISTIC_COUNTERS
248 24 : statistic.LongIntervalErr++;
249 : #endif
250 :
251 24 : state = SENT_STATE_INIT;
252 24 : return -1;
253 : }
254 :
255 107960 : rxReg = (rxReg << 4) | (uint32_t)interval;
256 :
257 107960 : if (state != SENT_STATE_CRC)
258 : {
259 : /* TODO: refactor */
260 94571 : state = (SENT_STATE_enum)((int)state + 1);
261 94571 : return 0;
262 : }
263 :
264 : #if SENT_STATISTIC_COUNTERS
265 13389 : statistic.FrameCnt++;
266 : #endif // SENT_STATISTIC_COUNTERS
267 13389 : pausePulseReceived = false;
268 13389 : state = SENT_STATE_SYNC;
269 : /* CRC check */
270 : /* TODO: find correct way to calculate CRC */
271 13389 : if ((MsgGetCrc(rxReg) == crc4(rxReg)) ||
272 16866 : (MsgGetCrc(rxReg) == crc4_gm(rxReg)) ||
273 3477 : (MsgGetCrc(rxReg) == crc4_gm_v2(rxReg)))
274 : {
275 : /* Full packet with correct CRC has been received */
276 13389 : rxLast = rxReg;
277 13389 : hasValidFast = true;
278 : /* TODO: add timestamp? */
279 13389 : return 1;
280 : }
281 : else
282 : {
283 : #if SENT_STATISTIC_COUNTERS
284 0 : statistic.CrcErrCnt++;
285 : #endif // SENT_STATISTIC_COUNTERS
286 0 : return -1;
287 : }
288 : return 0;
289 : }
290 :
291 0 : return 0;
292 : }
293 :
294 126874 : int sent_channel::Decoder(uint32_t clocks, uint8_t flags) {
295 : int ret;
296 :
297 : #if SENT_STATISTIC_COUNTERS
298 126874 : if (flags & SENT_FLAG_HW_OVERFLOW) {
299 0 : statistic.hwOverflowCnt++;
300 : }
301 : #endif
302 :
303 : /* TODO: handle flags */
304 : (void)flags;
305 :
306 126874 : ret = FastChannelDecoder(clocks);
307 126874 : if (ret > 0) {
308 : /* valid packet received, can process slow channels */
309 13389 : SlowChannelDecoder();
310 113485 : } else if (ret < 0) {
311 : /* packet is incorrect, reset slow channel state machine */
312 303 : SlowChannelDecoderReset();
313 : }
314 :
315 126874 : return ret;
316 : }
317 :
318 6 : int sent_channel::GetMsg(uint32_t* rx) {
319 6 : if (rx) {
320 0 : *rx = rxLast;
321 : }
322 :
323 6 : if (!hasValidFast) {
324 1 : return -1;
325 : }
326 : /* TODO: add check for time since last message received */
327 5 : return 0;
328 : }
329 :
330 0 : int sent_channel::GetSignals(uint8_t *pStat, uint16_t *pSig0, uint16_t *pSig1) {
331 0 : uint32_t rx;
332 0 : int ret = GetMsg(&rx);
333 :
334 0 : if (ret < 0) {
335 0 : return ret;
336 : }
337 :
338 : /* NOTE different MSB packing for sig0 and sig1
339 : * is it protocol-defined or device-specific?
340 : * Also looks like some devices send 16 + 8 bit, not 12 + 12 */
341 0 : if (pStat) {
342 0 : *pStat = MsgGetStat(rx);
343 : }
344 :
345 0 : if (pSig0) {
346 0 : uint16_t tmp = MsgGetSig0(rx);
347 0 : *pSig0 = tmp;
348 : }
349 :
350 0 : if (pSig1) {
351 0 : uint16_t tmp = MsgGetSig1(rx);
352 : /* swap */
353 0 : tmp = ((tmp >> 8) & 0x00f) |
354 0 : ((tmp << 8) & 0xf00) |
355 0 : (tmp & 0x0f0);
356 0 : *pSig1 = tmp;
357 : }
358 :
359 0 : return 0;
360 : }
361 :
362 216 : int sent_channel::StoreSlowChannelValue(uint8_t id, uint16_t data)
363 : {
364 : size_t i;
365 :
366 : /* Update already allocated messagebox? */
367 1369 : for (i = 0; i < SENT_SLOW_CHANNELS_MAX; i++) {
368 1317 : if ((scMsgFlags & BIT(i)) && (scMsg[i].id == id)) {
369 164 : scMsg[i].data = data;
370 164 : return 0;
371 : }
372 : }
373 :
374 : /* New message? Allocate messagebox */
375 620 : for (i = 0; i < SENT_SLOW_CHANNELS_MAX; i++) {
376 593 : if (!(scMsgFlags & BIT(i)))
377 : {
378 25 : scMsg[i].data = data;
379 25 : scMsg[i].id = id;
380 25 : scMsgFlags |= (1 << i);
381 25 : return 0;
382 : }
383 : }
384 :
385 : /* No free mailboxes for new ID */
386 27 : return -1;
387 : }
388 :
389 0 : int sent_channel::GetSlowChannelValue(uint8_t id)
390 : {
391 : size_t i;
392 :
393 0 : for (i = 0; i < SENT_SLOW_CHANNELS_MAX; i++) {
394 0 : if ((scMsgFlags & BIT(i)) && (scMsg[i].id == id)) {
395 0 : return scMsg[i].data;
396 : }
397 : }
398 :
399 : /* not found */
400 0 : return -1;
401 : }
402 :
403 13389 : int sent_channel::SlowChannelDecoder()
404 : {
405 : /* bit 2 and bit 3 from status nibble are used to transfer short messages */
406 13389 : bool b2 = !!(MsgGetStat(rxLast) & BIT(2));
407 13389 : bool b3 = !!(MsgGetStat(rxLast) & BIT(3));
408 :
409 : /* shift in new data */
410 13389 : scShift2 = (scShift2 << 1) | b2;
411 13389 : scShift3 = (scShift3 << 1) | b3;
412 13389 : scCrcShift = (scCrcShift << 2) | ((uint32_t)b2 << 1) | b3;
413 :
414 : if (1) {
415 : /* Short Serial Message format */
416 :
417 : /* 0b1000.0000.0000.0000? */
418 13389 : if ((scShift3 & 0xffff) == 0x8000) {
419 : /* Done receiving */
420 :
421 : /* TODO: add crc check */
422 :
423 0 : uint8_t id = (scShift2 >> 12) & 0x0f;
424 0 : uint16_t data = (scShift2 >> 4) & 0xff;
425 :
426 0 : return StoreSlowChannelValue(id, data);
427 : }
428 : }
429 : if (1) {
430 : /* Enhanced Serial Message format */
431 :
432 : /* 0b11.1111.0xxx.xx0x.xxx0 ? */
433 13389 : if ((scShift3 & 0x3f821) == 0x3f000) {
434 : uint8_t id;
435 :
436 216 : uint8_t crc = (scShift2 >> 12) & 0x3f;
437 : #if SENT_STATISTIC_COUNTERS
438 216 : statistic.sc++;
439 : #endif
440 216 : if (crc == crc6(scCrcShift)) {
441 : /* C-flag: configuration bit is used to indicate 16 bit format */
442 216 : bool sc16Bit = !!(scShift3 & (1 << 10));
443 216 : if (!sc16Bit) {
444 : /* 12 bit message, 8 bit ID */
445 216 : id = ((scShift3 >> 1) & 0x0f) |
446 216 : ((scShift3 >> 2) & 0xf0);
447 216 : uint16_t data = scShift2 & 0x0fff; /* 12 bit */
448 :
449 : /* TODO: add crc check */
450 216 : return StoreSlowChannelValue(id, data);
451 : } else {
452 : /* 16 bit message, 4 bit ID */
453 0 : id = (scShift3 >> 6) & 0x0f;
454 0 : uint16_t data = (scShift2 & 0x0fff) |
455 0 : (((scShift3 >> 1) & 0x0f) << 12);
456 :
457 0 : return StoreSlowChannelValue(id, data);
458 : }
459 : } else {
460 : #if SENT_STATISTIC_COUNTERS
461 0 : statistic.scCrcErr++;
462 : #endif
463 : }
464 : }
465 : }
466 :
467 13173 : return 0;
468 : }
469 :
470 303 : void sent_channel::SlowChannelDecoderReset()
471 : {
472 : /* packet is incorrect, reset slow channel state machine */
473 303 : scShift2 = 0;
474 303 : scShift3 = 0;
475 303 : }
476 :
477 : /* This is correct for Si7215 */
478 : /* This CRC is calculated for WHOLE message expect last nibble (CRC) */
479 13389 : uint8_t sent_channel::crc4(uint32_t data)
480 : {
481 : size_t i;
482 13389 : uint8_t crc = SENT_CRC_SEED; // initialize checksum with seed "0101"
483 13389 : const uint8_t CrcLookup[16] = {0, 13, 7, 10, 14, 3, 9, 4, 1, 12, 6, 11, 15, 2, 8, 5};
484 :
485 107112 : for (i = 0; i < 7; i++) {
486 93723 : crc = crc ^ MsgGetNibble(data, i);
487 93723 : crc = CrcLookup[crc];
488 : }
489 :
490 13389 : return crc;
491 : }
492 :
493 : /* TODO: double check two following and use same CRC routine? */
494 :
495 : /* This is correct for GM throttle body */
496 : /* This CRC is calculated for message expect status nibble and minus CRC nibble */
497 11154 : uint8_t sent_channel::crc4_gm(uint32_t data)
498 : {
499 : size_t i;
500 11154 : uint8_t crc = SENT_CRC_SEED; // initialize checksum with seed "0101"
501 11154 : const uint8_t CrcLookup[16] = {0, 13, 7, 10, 14, 3, 9, 4, 1, 12, 6, 11, 15, 2, 8, 5};
502 :
503 78078 : for (i = 1; i < 7; i++) {
504 66924 : crc = CrcLookup[crc];
505 66924 : crc = (crc ^ MsgGetNibble(data, i)) & 0xf;
506 : }
507 :
508 11154 : return crc;
509 : }
510 :
511 : /* This is correct for GDI fuel pressure sensor */
512 : /* This CRC is calculated for message expect status nibble and minus CRC nibble */
513 3477 : uint8_t sent_channel::crc4_gm_v2(uint32_t data)
514 : {
515 : size_t i;
516 3477 : uint8_t crc = SENT_CRC_SEED; // initialize checksum with seed "0101"
517 3477 : const uint8_t CrcLookup[16] = {0, 13, 7, 10, 14, 3, 9, 4, 1, 12, 6, 11, 15, 2, 8, 5};
518 :
519 24339 : for (i = 1; i < 7; i++) {
520 20862 : crc = CrcLookup[crc];
521 20862 : crc = (crc ^ MsgGetNibble(data, i)) & 0xf;
522 : }
523 : // One more round with 0 as input
524 3477 : crc = CrcLookup[crc];
525 :
526 3477 : return crc;
527 : }
528 :
529 216 : uint8_t sent_channel::crc6(uint32_t data)
530 : {
531 : size_t i;
532 : /* Seed 0x15 (21) */
533 216 : uint8_t crc = 0x15;
534 : /* CRC table for poly = 0x59 (x^6 + x^4 + x^3 + 1) */
535 216 : const uint8_t crc6_table[64] = {
536 : 0, 25, 50, 43, 61, 36, 15, 22, 35, 58, 17, 8, 30, 7, 44, 53,
537 : 31, 6, 45, 52, 34, 59, 16, 9, 60, 37, 14, 23, 1, 24, 51, 42,
538 : 62, 39, 12, 21, 3, 26, 49, 40, 29, 4, 47, 54, 32, 57, 18, 11,
539 : 33, 56, 19, 10, 28, 5, 46, 55, 2, 27, 48, 41, 63, 38, 13, 20 };
540 :
541 1080 : for (i = 0; i < 4; i++) {
542 864 : uint8_t tmp = (data >> (24 - 6 * (i + 1))) & 0x3f;
543 864 : crc = tmp ^ crc6_table[crc];
544 : }
545 : // Extra round with 0 input
546 216 : crc = 0 ^ crc6_table[crc];
547 :
548 216 : return crc;
549 : }
550 :
551 : #endif /* EFI_SENT_SUPPORT */
552 : #endif // EFI_PROD_CODE || EFI_UNIT_TEST
553 :
554 : #if EFI_PROD_CODE
555 : #if EFI_SENT_SUPPORT
556 :
557 : static sent_channel channels[SENT_CHANNELS_NUM];
558 :
559 : void sent_channel::Info() {
560 : uint8_t stat;
561 : uint16_t sig0, sig1;
562 :
563 : efiPrintf("Unit time %lu CPU ticks %f uS", tickPerUnit, TicksToUs(getTickTime()));
564 : efiPrintf("Pause pulse detected %s", pausePulseReceived ? "Yes" : "No");
565 : efiPrintf("Total pulses %lu", pulseCounter);
566 :
567 : if (GetSignals(&stat, &sig0, &sig1) == 0) {
568 : efiPrintf("Last valid fast msg Status 0x%01x Sig0 0x%03x Sig1 0x%03x", stat, sig0, sig1);
569 : }
570 :
571 : if (scMsgFlags) {
572 : efiPrintf("Slow channels:");
573 : for (int i = 0; i < SENT_SLOW_CHANNELS_MAX; i++) {
574 : if (scMsgFlags & BIT(i)) {
575 : efiPrintf(" ID %d: %d", scMsg[i].id, scMsg[i].data);
576 : }
577 : }
578 : }
579 :
580 : #if SENT_STATISTIC_COUNTERS
581 : efiPrintf("HW overflows %lu\n", statistic.hwOverflowCnt);
582 :
583 : efiPrintf("Pause pulses %lu\n", statistic.PauseCnt);
584 : efiPrintf("Restarts %lu", statistic.RestartCnt);
585 : efiPrintf("Interval errors %lu short, %lu long", statistic.ShortIntervalErr, statistic.LongIntervalErr);
586 : efiPrintf("Total frames %lu with CRC error %lu (%f%%)", statistic.FrameCnt, statistic.CrcErrCnt, statistic.CrcErrCnt * 100.0 / statistic.FrameCnt);
587 : efiPrintf("Total slow channel messages %lu with crc6 errors %lu (%f%%)", statistic.sc, statistic.scCrcErr, statistic.scCrcErr * 100.0 / statistic.sc);
588 : efiPrintf("Sync errors %lu", statistic.SyncErr);
589 : #endif
590 : }
591 :
592 : /*==========================================================================*/
593 : /* Decoder thread settings. */
594 : /*==========================================================================*/
595 :
596 : /* 4 per channel should be enough */
597 : #define SENT_MB_SIZE (4 * SENT_CHANNELS_NUM)
598 :
599 : static msg_t sent_mb_buffer[SENT_MB_SIZE];
600 : static MAILBOX_DECL(sent_mb, sent_mb_buffer, SENT_MB_SIZE);
601 :
602 : static THD_WORKING_AREA(waSentDecoderThread, 256);
603 :
604 : void SENT_ISR_Handler(uint8_t channel, uint16_t clocks, uint8_t flags) {
605 : /* encode to fit msg_t */
606 : msg_t msg = (flags << 24) | (channel << 16) | clocks;
607 :
608 : /* called from ISR */
609 : chSysLockFromISR();
610 : chMBPostI(&sent_mb, msg);
611 : chSysUnlockFromISR();
612 : }
613 :
614 : static void SentDecoderThread(void*) {
615 : while (true) {
616 : msg_t ret;
617 : msg_t msg;
618 :
619 : ret = chMBFetchTimeout(&sent_mb, &msg, TIME_INFINITE);
620 :
621 : if (ret == MSG_OK) {
622 : uint16_t tick = msg & 0xffff;
623 : uint8_t n = (msg >> 16) & 0xff;
624 : uint8_t flags = (msg >> 24) & 0xff;
625 :
626 : if (n < SENT_CHANNELS_NUM) {
627 : sent_channel &channel = channels[n];
628 :
629 : if (channel.Decoder(tick, flags) > 0) {
630 : /* report only for first channel */
631 : if (n == 0) {
632 : uint16_t sig0, sig1;
633 : channel.GetSignals(NULL, &sig0, &sig1);
634 : engine->sent_state.value0 = sig0;
635 : engine->sent_state.value1 = sig1;
636 :
637 : #if SENT_STATISTIC_COUNTERS
638 : engine->sent_state.errorRate = 100.0 * channel.statistic.getErrorRate();
639 : #endif // SENT_STATISTIC_COUNTERS
640 : }
641 :
642 : SentInput input = static_cast<SentInput>((size_t)SentInput::INPUT1 + n);
643 : /* Call high level decoder from here */
644 : /* TODO: implemnet subscribers, like it is done for ADC */
645 : sentTpsDecode(input);
646 : sentPressureDecode(input);
647 : }
648 : }
649 : }
650 : }
651 : }
652 :
653 : static void printSentInfo() {
654 : for (int i = 0; i < SENT_CHANNELS_NUM; i++) {
655 : sent_channel &channel = channels[i];
656 :
657 : const char * pinName = getBoardSpecificPinName(engineConfiguration->sentInputPins[i]);
658 : efiPrintf("---- SENT input %d ---- on %s", i + 1, pinName);
659 : channel.Info();
660 : efiPrintf("--------------------");
661 : }
662 : }
663 :
664 : /* Don't be confused: this actually returns throttle body position */
665 : /* TODO: remove, replace with getSentValues() */
666 : float getSentValue(SentInput input) {
667 : size_t index = static_cast<size_t>(input) - static_cast<size_t>(SentInput::INPUT1);
668 :
669 : if (index < SENT_CHANNELS_NUM) {
670 : uint16_t sig0, sig1;
671 : sent_channel &channel = channels[index];
672 :
673 : if (channel.GetSignals(NULL, &sig0, &sig1) == 0) {
674 :
675 : // GM sig0 + sig1 == 0xfff but Ford does not
676 : /* scale to 0.0 .. 1.0 */
677 : return sig0;
678 : }
679 : }
680 :
681 : return NAN;
682 : }
683 :
684 : int getSentValues(SentInput input, uint16_t *sig0, uint16_t *sig1) {
685 : size_t index = static_cast<size_t>(input) - static_cast<size_t>(SentInput::INPUT1);
686 :
687 : if (index < SENT_CHANNELS_NUM) {
688 : sent_channel &channel = channels[index];
689 :
690 : return channel.GetSignals(NULL, sig0, sig1);
691 : }
692 :
693 : /* invalid channel */
694 : return -1;
695 : }
696 :
697 : /* Should be called once */
698 : void initSent(void) {
699 : /* init interval mailbox */
700 : chMBObjectInit(&sent_mb, sent_mb_buffer, SENT_MB_SIZE);
701 :
702 : chThdCreateStatic(waSentDecoderThread, sizeof(waSentDecoderThread), NORMALPRIO, SentDecoderThread, nullptr);
703 :
704 : /* Start HW layer */
705 : startSent();
706 :
707 : addConsoleAction("sentinfo", &printSentInfo);
708 : }
709 :
710 : #endif /* EFI_SENT_SUPPORT */
711 : #endif /* EFI_PROD_CODE */
|