safety: macro to update vehicle speed (commaai#1762)

dragonpilot-community · Apr 13, 2024 · d28be7b · d28be7b
1 parent b7d719b
commit d28be7b
Show file tree

Hide file tree

Showing 6 changed files with 16 additions and 8 deletions.
diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h
@@ -208,7 +208,7 @@ static void ford_rx_hook(const CANPacket_t *to_push) {
     // Update vehicle speed
     if (addr == FORD_BrakeSysFeatures) {
       // Signal: Veh_V_ActlBrk
-      update_sample(&vehicle_speed, ROUND(((GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1)) * 0.01 / 3.6 * VEHICLE_SPEED_FACTOR));
+      UPDATE_VEHICLE_SPEED(((GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1)) * 0.01 / 3.6);
     }
 
     // Check vehicle speed against a second source

diff --git a/board/safety/safety_nissan.h b/board/safety/safety_nissan.h
@@ -61,7 +61,7 @@ static void nissan_rx_hook(const CANPacket_t *to_push) {
       uint16_t right_rear = (GET_BYTE(to_push, 0) << 8) | (GET_BYTE(to_push, 1));
       uint16_t left_rear = (GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3));
       vehicle_moving = (right_rear | left_rear) != 0U;
-      update_sample(&vehicle_speed, ROUND((right_rear + left_rear) / 2.0 * 0.005 / 3.6 * VEHICLE_SPEED_FACTOR));
+      UPDATE_VEHICLE_SPEED((right_rear + left_rear) / 2.0 * 0.005 / 3.6);
     }
 
     // X-Trail 0x15c, Leaf 0x239

diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h
@@ -165,8 +165,7 @@ static void subaru_rx_hook(const CANPacket_t *to_push) {
 
     vehicle_moving = (fr > 0U) || (rr > 0U) || (rl > 0U) || (fl > 0U);
 
-    float speed = (fr + rr + rl + fl) / 4U * 0.057;
-    update_sample(&vehicle_speed, ROUND(speed * VEHICLE_SPEED_FACTOR));
+    UPDATE_VEHICLE_SPEED((fr + rr + rl + fl) / 4U * 0.057);
   }
 
   if ((addr == MSG_SUBARU_Brake_Status) && (bus == alt_main_bus)) {

diff --git a/board/safety/safety_tesla.h b/board/safety/safety_tesla.h
@@ -72,7 +72,7 @@ static void tesla_rx_hook(const CANPacket_t *to_push) {
       // Vehicle speed: ((0.05 * val) - 25) * MPH_TO_MPS
       float speed = (((((GET_BYTE(to_push, 3) & 0x0FU) << 8) | (GET_BYTE(to_push, 2))) * 0.05) - 25) * 0.447;
       vehicle_moving = ABS(speed) > 0.1;
-      update_sample(&vehicle_speed, ROUND(speed * VEHICLE_SPEED_FACTOR));
+      UPDATE_VEHICLE_SPEED(speed);
     }
 
     if(addr == (tesla_powertrain ? 0x106 : 0x108)) {

diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h
@@ -136,10 +136,18 @@ static void toyota_rx_hook(const CANPacket_t *to_push) {
       }
     }
 
+    // sample speed
     if (addr == 0xaa) {
-      // check that all wheel speeds are at zero value with offset
-      bool standstill = (GET_BYTES(to_push, 0, 4) == 0x6F1A6F1AU) && (GET_BYTES(to_push, 4, 4) == 0x6F1A6F1AU);
-      vehicle_moving = !standstill;
+      int speed = 0;
+      // sum 4 wheel speeds. conversion: raw * 0.01 - 67.67
+      for (uint8_t i = 0U; i < 8U; i += 2U) {
+        int wheel_speed = (GET_BYTE(to_push, i) << 8U) | GET_BYTE(to_push, (i + 1U));
+        speed += wheel_speed - 6767;
+      }
+      // check that all wheel speeds are at zero value
+      vehicle_moving = speed != 0;
+
+      UPDATE_VEHICLE_SPEED(speed / 4.0 * 0.01 / 3.6);
     }
 
     // most cars have brake_pressed on 0x226, corolla and rav4 on 0x224

diff --git a/board/safety_declarations.h b/board/safety_declarations.h
@@ -6,6 +6,7 @@
 
 #define BUILD_SAFETY_CFG(rx, tx) ((safety_config){(rx), (sizeof((rx)) / sizeof((rx)[0])), \
                                                   (tx), (sizeof((tx)) / sizeof((tx)[0]))})
+#define UPDATE_VEHICLE_SPEED(val_ms) (update_sample(&vehicle_speed, ROUND((val_ms) * VEHICLE_SPEED_FACTOR)))
 
 uint32_t GET_BYTES(const CANPacket_t *msg, int start, int len) {
   uint32_t ret = 0U;