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

* Toyota: add missing offset from speed signal * Let's also define ABS value in macro
arne182 · Mar 12, 2020 · 0696730 · 0696730
1 parent 5b1a8dc
commit 0696730
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 6 deletions.
diff --git a/board/config.h b/board/config.h
@@ -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)

diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h
@@ -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

diff --git a/tests/safety/test.c b/tests/safety/test.c
@@ -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)

diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py
@@ -80,11 +80,12 @@ def _accel_msg(self, accel):
     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):