use DI_vehicleSpeed

commaai · Oct 2, 2024 · f5fa518 · f5fa518
1 parent 6de7337
commit f5fa518
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 6 deletions.
diff --git a/board/safety/safety_tesla.h b/board/safety/safety_tesla.h
@@ -18,9 +18,12 @@ static void tesla_rx_hook(const CANPacket_t *to_push) {
 
   if(bus == 0) {
     if(addr == 0x155){
-      // Vehicle speed: (val * 0.5) * KPH_TO_MPS
-      float speed = ((((GET_BYTE(to_push, 6) & 0x0FU) << 6) | (GET_BYTE(to_push, 5) >> 2)) * 0.5) * 0.277778;
       vehicle_moving = !GET_BIT(to_push, 41U);
+    }
+
+    if(addr == 0x257){
+      // Vehicle speed: ((val * 0.08) - 40) * KPH_TO_MPS
+      float speed = (((((GET_BYTE(to_push, 2)) << 4) | (GET_BYTE(to_push, 1) >> 4)) * 0.08) - 40) * 0.277778;
       UPDATE_VEHICLE_SPEED(speed);
     }
 
@@ -177,7 +180,8 @@ static safety_config tesla_init(uint16_t param) {
 
   static RxCheck tesla_model3_y_rx_checks[] = {
     {.msg = {{0x2b9, 2, 8, .frequency = 25U}, { 0 }, { 0 }}},   // DAS_control
-    {.msg = {{0x155, 0, 8, .frequency = 50U}, { 0 }, { 0 }}},   // ESP_wheelRotation (speed in kph)
+    {.msg = {{0x155, 0, 8, .frequency = 50U}, { 0 }, { 0 }}},   // ESP_wheelRotation (standstill)
+    {.msg = {{0x257, 0, 8, .frequency = 50U}, { 0 }, { 0 }}},   // DI_speed (speed in kph)
     {.msg = {{0x370, 0, 8, .frequency = 100U}, { 0 }, { 0 }}},  // EPAS3S_internalSAS (steering angle)
     {.msg = {{0x118, 0, 8, .frequency = 100U}, { 0 }, { 0 }}},  // DI_systemStatus (gas pedal)
     {.msg = {{0x39d, 0, 5, .frequency = 25U}, { 0 }, { 0 }}},   // IBST_status (brakes)
@@ -194,4 +198,4 @@ const safety_hooks tesla_hooks = {
   .rx = tesla_rx_hook,
   .tx = tesla_tx_hook,
   .fwd = tesla_fwd_hook,
-};
+};
diff --git a/tests/safety/test_tesla.py b/tests/safety/test_tesla.py
@@ -29,8 +29,8 @@ def _user_brake_msg(self, brake):
     return self.packer.make_can_msg_panda("IBST_status", 0, values)
 
   def _speed_msg(self, speed):
-    values = {"ESP_vehicleSpeed": speed / 0.277778}
-    return self.packer.make_can_msg_panda("ESP_B", 0, values)
+    values = {"DI_vehicleSpeed": speed / 0.277778}
+    return self.packer.make_can_msg_panda("DI_speed", 0, values)
 
   def _vehicle_moving_msg(self, speed: float):
     values = {"ESP_vehicleStandstillSts": 1 if speed <= self.STANDSTILL_THRESHOLD else 0}