retropilot
/
panda
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Toyota: add missing offset from speed signal (#469) 

* Toyota: add missing offset from speed signal

* Let's also define ABS value in macro
master
rbiasini 2020-03-11 17:30:30 -07:00 committed by GitHub
parent 5b1a8dc873
commit 0696730c14
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
board/config.h
View File

@ -36,6 +36,10 @@
__typeof__ (b) _b = (b); \
(_a > _b) ? _a : _b; })
#define ABS(a) \
({ __typeof__ (a) _a = (a); \
(_a > 0) ? _a : (-_a); })
#define MAX_RESP_LEN 0x40U
// Around (1Mbps / 8 bits/byte / 12 bytes per message)

4
board/safety/safety_toyota.h
View File

@ -101,9 +101,9 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// sum 4 wheel speeds
for (int i=0; i<8; i+=2) {
int next_byte = i + 1; // hack to deal with misra 10.8
speed += (GET_BYTE(to_push, i) << 8) + GET_BYTE(to_push, next_byte);
speed += (GET_BYTE(to_push, i) << 8) + GET_BYTE(to_push, next_byte) - 0x1a6f;
}
toyota_moving = (speed / 4) > TOYOTA_STANDSTILL_THRSLD;
toyota_moving = ABS(speed / 4) > TOYOTA_STANDSTILL_THRSLD;
}
// exit controls on rising edge of brake pedal

4
tests/safety/test.c
View File

@ -58,6 +58,10 @@ uint8_t hw_type = HW_TYPE_UNKNOWN;
__typeof__ (b) _b = (b); \
_a > _b ? _a : _b; })
#define ABS(a) \
({ __typeof__ (a) _a = (a); \
(_a > 0) ? _a : (-_a); })
// from llcan.h
#define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF)
#define GET_LEN(msg) ((msg)->RDTR & 0xf)

9
tests/safety/test_toyota.py
View File

@ -80,11 +80,12 @@ class TestToyotaSafety(unittest.TestCase):
return to_send
def _speed_msg(self, s):
offset = (0x6f << 8) + 0x1a # there is a 0x1a6f offset in the signal
to_send = make_msg(0, 0xaa)
to_send[0].RDLR = (s & 0xFF) << 8 | (s >> 8)
to_send[0].RDLR += (s & 0xFF) << 24 | ((s >> 8) << 16)
to_send[0].RDHR = (s & 0xFF) << 8 | (s >> 8)
to_send[0].RDHR += (s & 0xFF) << 24 | ((s >> 8) << 16)
to_send[0].RDLR = ((s & 0xFF) << 8 | (s >> 8)) + offset
to_send[0].RDLR += ((s & 0xFF) << 24 | ((s >> 8) << 16)) + (offset << 16)
to_send[0].RDHR = ((s & 0xFF) << 8 | (s >> 8)) + offset
to_send[0].RDHR += ((s & 0xFF) << 24 | ((s >> 8) << 16)) + (offset << 16)
return to_send
def _brake_msg(self, brake):