Abstract sample speed test (commaai#516)

* all cars move

* honda

* update comments

* vw

board/safety/safety_chrysler.h

@@ -104,6 +104,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       int speed_l = (GET_BYTE(to_push, 0) << 4) + (GET_BYTE(to_push, 1) >> 4);
       int speed_r = (GET_BYTE(to_push, 2) << 4) + (GET_BYTE(to_push, 3) >> 4);
       chrysler_speed = (speed_l + speed_r) / 2;
+      vehicle_moving = chrysler_speed > CHRYSLER_STANDSTILL_THRSLD;
     }
 
     // exit controls on rising edge of gas press
@@ -118,7 +119,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     // exit controls on rising edge of brake press
     if (addr == 320) {
       bool brake_pressed = (GET_BYTE(to_push, 0) & 0x7) == 5;
-      if (brake_pressed && (!brake_pressed_prev || (chrysler_speed > CHRYSLER_STANDSTILL_THRSLD))) {
+      if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
         controls_allowed = 0;
       }
       brake_pressed_prev = brake_pressed;

board/safety/safety_ford.h

@@ -7,7 +7,6 @@
 //      brake rising edge
 //      brake > 0mph
 
-bool ford_moving = false;
 
 static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 
@@ -17,9 +16,9 @@ static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 
   if (addr == 0x217) {
     // wheel speeds are 14 bits every 16
-    ford_moving = false;
+    vehicle_moving = false;
     for (int i = 0; i < 8; i += 2) {
-      ford_moving |= GET_BYTE(to_push, i) | (GET_BYTE(to_push, (int)(i + 1)) & 0xFCU);
+      vehicle_moving |= GET_BYTE(to_push, i) | (GET_BYTE(to_push, (int)(i + 1)) & 0xFCU);
     }
   }
 
@@ -39,7 +38,7 @@ static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   // speed > 0
   if (addr == 0x165) {
     int brake_pressed = GET_BYTE(to_push, 0) & 0x20;
-    if (brake_pressed && (!brake_pressed_prev || ford_moving)) {
+    if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
       controls_allowed = 0;
     }
     brake_pressed_prev = brake_pressed;
@@ -73,7 +72,7 @@ static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 
   // disallow actuator commands if gas or brake (with vehicle moving) are pressed
   // and the the latching controls_allowed flag is True
-  int pedal_pressed = brake_pressed_prev && ford_moving;
+  int pedal_pressed = brake_pressed_prev && vehicle_moving;
   bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS;
   if (!unsafe_allow_gas) {
     pedal_pressed = pedal_pressed || gas_pressed_prev;

board/safety/safety_gm.h

@@ -33,7 +33,6 @@ AddrCheckStruct gm_rx_checks[] = {
 };
 const int GM_RX_CHECK_LEN = sizeof(gm_rx_checks) / sizeof(gm_rx_checks[0]);
 
-bool gm_moving = false;
 int gm_rt_torque_last = 0;
 int gm_desired_torque_last = 0;
 uint32_t gm_ts_last = 0;
@@ -59,7 +58,7 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     // sample speed, really only care if car is moving or not
     // rear left wheel speed
     if (addr == 842) {
-      gm_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1);
+      vehicle_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1);
     }
 
     // ACC steering wheel buttons
@@ -84,7 +83,7 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       // Brake pedal's potentiometer returns near-zero reading
       // even when pedal is not pressed
       bool brake_pressed = GET_BYTE(to_push, 1) >= 10;
-      if (brake_pressed && (!brake_pressed_prev || gm_moving)) {
+      if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
          controls_allowed = 0;
       }
       brake_pressed_prev = brake_pressed;
@@ -140,7 +139,7 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 
   // disallow actuator commands if gas or brake (with vehicle moving) are pressed
   // and the the latching controls_allowed flag is True
-  int pedal_pressed = brake_pressed_prev && gm_moving;
+  int pedal_pressed = brake_pressed_prev && vehicle_moving;
   bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS;
   if (!unsafe_allow_gas) {
     pedal_pressed = pedal_pressed || gas_pressed_prev;

board/safety/safety_honda.h

@@ -36,7 +36,6 @@ AddrCheckStruct honda_bh_rx_checks[] = {
 const int HONDA_BH_RX_CHECKS_LEN = sizeof(honda_bh_rx_checks) / sizeof(honda_bh_rx_checks[0]);
 
 int honda_brake = 0;
-bool honda_moving = false;
 bool honda_alt_brake_msg = false;
 bool honda_fwd_brake = false;
 enum {HONDA_N_HW, HONDA_BG_HW, HONDA_BH_HW} honda_hw = HONDA_N_HW;
@@ -90,7 +89,7 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     // sample speed
     if (addr == 0x158) {
       // first 2 bytes
-      honda_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1);
+      vehicle_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1);
     }
 
     // state machine to enter and exit controls
@@ -120,7 +119,7 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     bool is_user_brake_msg = honda_alt_brake_msg ? ((addr) == 0x1BE) : ((addr) == 0x17C);
     if (is_user_brake_msg) {
       bool brake_pressed = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20);
-      if (brake_pressed && (!brake_pressed_prev || honda_moving)) {
+      if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
         controls_allowed = 0;
       }
       brake_pressed_prev = brake_pressed;
@@ -206,7 +205,7 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 
   // disallow actuator commands if gas or brake (with vehicle moving) are pressed
   // and the the latching controls_allowed flag is True
-  int pedal_pressed = brake_pressed_prev && honda_moving;
+  int pedal_pressed = brake_pressed_prev && vehicle_moving;
   bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS;
   if (!unsafe_allow_gas) {
     pedal_pressed = pedal_pressed || gas_pressed_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD);

board/safety/safety_hyundai.h

@@ -21,7 +21,6 @@ const int HYUNDAI_RX_CHECK_LEN = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_c
 int hyundai_rt_torque_last = 0;
 int hyundai_desired_torque_last = 0;
 int hyundai_cruise_engaged_last = 0;
-int hyundai_speed = 0;
 uint32_t hyundai_ts_last = 0;
 struct sample_t hyundai_torque_driver;         // last few driver torques measured
 
@@ -66,15 +65,16 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 
     // sample subaru wheel speed, averaging opposite corners
     if (addr == 902) {
-      hyundai_speed = GET_BYTES_04(to_push) & 0x3FFF;  // FL
+      int hyundai_speed = GET_BYTES_04(to_push) & 0x3FFF;  // FL
       hyundai_speed += (GET_BYTES_48(to_push) >> 16) & 0x3FFF;  // RL
       hyundai_speed /= 2;
+      vehicle_moving = hyundai_speed > HYUNDAI_STANDSTILL_THRSLD;
     }
 
     // exit controls on rising edge of brake press
     if (addr == 916) {
       bool brake_pressed = (GET_BYTE(to_push, 6) >> 7) != 0;
-      if (brake_pressed && (!brake_pressed_prev || (hyundai_speed > HYUNDAI_STANDSTILL_THRSLD))) {
+      if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
         controls_allowed = 0;
       }
       brake_pressed_prev = brake_pressed;

board/safety/safety_nissan.h

@@ -58,6 +58,7 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
         // Get current speed
         // Factor 0.005
         nissan_speed = ((GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3))) * 0.005 / 3.6;
+        vehicle_moving = nissan_speed > 0.;
       }
 
       // exit controls on rising edge of gas press
@@ -92,7 +93,7 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
         brake_pressed = GET_BYTE(to_push, 0) > 3;
       }
 
-      if (brake_pressed && (!brake_pressed_prev || (nissan_speed > 0.))) {
+      if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
         controls_allowed = 0;
       }
       brake_pressed_prev = brake_pressed;

board/safety/safety_subaru.h

@@ -33,7 +33,6 @@ const int SUBARU_L_RX_CHECK_LEN = sizeof(subaru_l_rx_checks) / sizeof(subaru_l_r
 int subaru_cruise_engaged_last = 0;
 int subaru_rt_torque_last = 0;
 int subaru_desired_torque_last = 0;
-int subaru_speed = 0;
 uint32_t subaru_ts_last = 0;
 bool subaru_global = false;
 struct sample_t subaru_torque_driver;         // last few driver torques measured
@@ -99,15 +98,16 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 
     // sample subaru wheel speed, averaging opposite corners
     if ((addr == 0x13a) && subaru_global) {
-      subaru_speed = (GET_BYTES_04(to_push) >> 12) & 0x1FFF;  // FR
+      int subaru_speed = (GET_BYTES_04(to_push) >> 12) & 0x1FFF;  // FR
       subaru_speed += (GET_BYTES_48(to_push) >> 6) & 0x1FFF;  // RL
       subaru_speed /= 2;
+      vehicle_moving = subaru_speed > SUBARU_STANDSTILL_THRSLD;
     }
 
     // exit controls on rising edge of brake press (TODO: missing check for unsupported legacy models)
     if ((addr == 0x139) && subaru_global) {
       bool brake_pressed = (GET_BYTES_48(to_push) & 0xFFF0) > 0;
-      if (brake_pressed && (!brake_pressed_prev || (subaru_speed > SUBARU_STANDSTILL_THRSLD))) {
+      if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
         controls_allowed = 0;
       }
       brake_pressed_prev = brake_pressed;

board/safety/safety_toyota.h

@@ -50,7 +50,6 @@ int toyota_desired_torque_last = 0;       // last desired steer torque
 int toyota_rt_torque_last = 0;            // last desired torque for real time check
 uint32_t toyota_ts_last = 0;
 int toyota_cruise_engaged_last = 0;       // cruise state
-bool toyota_moving = false;
 struct sample_t toyota_torque_meas;       // last 3 motor torques produced by the eps
 
 
@@ -123,15 +122,15 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
         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) - 0x1a6f;
       }
-      toyota_moving = ABS(speed / 4) > TOYOTA_STANDSTILL_THRSLD;
+      vehicle_moving = ABS(speed / 4) > TOYOTA_STANDSTILL_THRSLD;
     }
 
     // exit controls on rising edge of brake pedal
     // most cars have brake_pressed on 0x226, corolla and rav4 on 0x224
     if ((addr == 0x224) || (addr == 0x226)) {
       int byte = (addr == 0x224) ? 0 : 4;
       bool brake_pressed = ((GET_BYTE(to_push, byte) >> 5) & 1) != 0;
-      if (brake_pressed && (!brake_pressed_prev || toyota_moving)) {
+      if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
         controls_allowed = 0;
       }
       brake_pressed_prev = brake_pressed;

board/safety/safety_volkswagen.h

@@ -55,7 +55,6 @@ struct sample_t volkswagen_torque_driver; // Last few driver torques measured
 int volkswagen_rt_torque_last = 0;
 int volkswagen_desired_torque_last = 0;
 uint32_t volkswagen_ts_last = 0;
-bool volkswagen_moving = false;
 int volkswagen_torque_msg = 0;
 int volkswagen_lane_msg = 0;
 uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F, aka 8H2F/AUTOSAR
@@ -157,7 +156,7 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       int wheel_speed_fr = GET_BYTE(to_push, 6) | (GET_BYTE(to_push, 7) << 8);
       // Check for average front speed in excess of 0.3m/s, 1.08km/h
       // DBC speed scale 0.0075: 0.3m/s = 144, sum both wheels to compare
-      volkswagen_moving = (wheel_speed_fl + wheel_speed_fr) > 288;
+      vehicle_moving = (wheel_speed_fl + wheel_speed_fr) > 288;
     }
 
     // Update driver input torque samples
@@ -193,7 +192,7 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     // Signal: ESP_05.ESP_Fahrer_bremst
     if (addr == MSG_ESP_05) {
       bool brake_pressed = (GET_BYTE(to_push, 3) & 0x4) >> 2;
-      if (brake_pressed && (!brake_pressed_prev || volkswagen_moving)) {
+      if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
         controls_allowed = 0;
       }
       brake_pressed_prev = brake_pressed;
@@ -224,7 +223,7 @@ static int volkswagen_pq_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       int wheel_speed_fr = (GET_BYTE(to_push, 2) | (GET_BYTE(to_push, 3) << 8)) >> 1;
       // Check for average front speed in excess of 0.3m/s, 1.08km/h
       // DBC speed scale 0.01: 0.3m/s = 108, sum both wheels to compare
-      volkswagen_moving = (wheel_speed_fl + wheel_speed_fr) > 216;
+      vehicle_moving = (wheel_speed_fl + wheel_speed_fr) > 216;
     }
 
     // Update driver input torque samples
@@ -260,7 +259,7 @@ static int volkswagen_pq_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     // Signal: Motor_2.Bremslichtschalter
     if ((bus == 0) && (addr == MSG_MOTOR_2)) {
       bool brake_pressed = (GET_BYTE(to_push, 2) & 0x1);
-      if (brake_pressed && (!(brake_pressed_prev) || volkswagen_moving)) {
+      if (brake_pressed && (!(brake_pressed_prev) || vehicle_moving)) {
         controls_allowed = 0;
       }
       brake_pressed_prev = brake_pressed;

board/safety_declarations.h

@@ -89,6 +89,7 @@ bool gas_interceptor_detected = false;
 int gas_interceptor_prev = 0;
 bool gas_pressed_prev = false;
 bool brake_pressed_prev = false;
+bool vehicle_moving = false;
 
 // This can be set with a USB command
 // It enables features we consider to be unsafe, but understand others may have different opinions

tests/safety/common.py

@@ -252,3 +252,17 @@ def test_not_allow_brake_when_moving(self):
     self.assertFalse(self.safety.get_controls_allowed())
     self._rx(self._speed_msg(0))
 
+  def test_sample_speed(self):
+    self.assertFalse(self.safety.get_vehicle_moving())
+
+    # not moving
+    self.safety.safety_rx_hook(self._speed_msg(0))
+    self.assertFalse(self.safety.get_vehicle_moving())
+
+    # speed is at threshold
+    self.safety.safety_rx_hook(self._speed_msg(self.STANDSTILL_THRESHOLD))
+    self.assertFalse(self.safety.get_vehicle_moving())
+
+    # past threshold
+    self.safety.safety_rx_hook(self._speed_msg(self.STANDSTILL_THRESHOLD+1))
+    self.assertTrue(self.safety.get_vehicle_moving())

tests/safety/libpandasafety_py.py

@@ -41,6 +41,7 @@
 int get_gas_interceptor_prev(void);
 bool get_gas_pressed_prev(void);
 bool get_brake_pressed_prev(void);
+bool get_vehicle_moving(void);
 int get_hw_type(void);
 void set_timer(uint32_t t);
 
@@ -57,7 +58,6 @@
 void set_toyota_rt_torque_last(int t);
 
 void init_tests_honda(void);
-bool get_honda_moving(void);
 void set_honda_fwd_brake(bool);
 void set_honda_alt_brake_msg(bool);
 int get_honda_hw(void);
@@ -87,7 +87,6 @@
 void init_tests_volkswagen(void);
 int get_volkswagen_torque_driver_min(void);
 int get_volkswagen_torque_driver_max(void);
-bool get_volkswagen_moving(void);
 void set_volkswagen_desired_torque_last(int t);
 void set_volkswagen_rt_torque_last(int t);
 void set_volkswagen_torque_driver(int min, int max);

tests/safety/test.c

@@ -129,6 +129,10 @@ bool get_brake_pressed_prev(void){
   return brake_pressed_prev;
 }
 
+bool get_vehicle_moving(void){
+  return vehicle_moving;
+}
+
 int get_hw_type(void){
   return hw_type;
 }
@@ -238,14 +242,6 @@ void set_volkswagen_desired_torque_last(int t){
   volkswagen_desired_torque_last = t;
 }
 
-int get_volkswagen_moving(void){
-  return volkswagen_moving;
-}
-
-bool get_honda_moving(void){
-  return honda_moving;
-}
-
 void set_honda_alt_brake_msg(bool c){
   honda_alt_brake_msg = c;
 }
@@ -268,6 +264,7 @@ void init_tests(void){
   safety_mode_cnt = 2U;  // avoid ignoring relay_malfunction logic
   gas_pressed_prev = false;
   brake_pressed_prev = false;
+  vehicle_moving = false;
   unsafe_mode = 0;
 }
 
@@ -324,7 +321,6 @@ void init_tests_subaru(void){
 
 void init_tests_volkswagen(void){
   init_tests();
-  volkswagen_moving = false;
   volkswagen_torque_driver.min = 0;
   volkswagen_torque_driver.max = 0;
   volkswagen_desired_torque_last = 0;
@@ -335,7 +331,6 @@ void init_tests_volkswagen(void){
 
 void init_tests_honda(void){
   init_tests();
-  honda_moving = false;
   honda_fwd_brake = false;
 }
 

tests/safety/test_gm.py

@@ -205,7 +205,7 @@ def test_realtime_limits(self):
   def test_tx_hook_on_pedal_pressed(self):
     for pedal in ['brake', 'gas']:
       if pedal == 'brake':
-        # brake_pressed_prev and honda_moving
+        # brake_pressed_prev and vehicle_moving
         self._rx(self._speed_msg(100))
         self._rx(self._brake_msg(MAX_BRAKE))
       elif pedal == 'gas':
@@ -231,7 +231,7 @@ def test_tx_hook_on_pedal_pressed_on_unsafe_gas_mode(self):
     for pedal in ['brake', 'gas']:
       self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS)
       if pedal == 'brake':
-        # brake_pressed_prev and honda_moving
+        # brake_pressed_prev and vehicle_moving
         self._rx(self._speed_msg(100))
         self._rx(self._brake_msg(MAX_BRAKE))
         allow_ctrl = False