safety: add longitudinal pre-enable state for brake at a standstill

board/safety.h

@@ -82,9 +82,20 @@ int safety_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
 }
 
 bool get_longitudinal_allowed(void) {
+  // No longitudinal control when overriding with gas. Brake is allowed when pre-enabling at a standstill
   return controls_allowed && !gas_pressed_prev;
 }
 
+bool get_lateral_allowed(void) {
+  // No steering allowed while pre-enabling at a standstill with brake
+  return controls_allowed && !brake_pressed_prev;
+}
+
+bool get_gas_allowed(void) {
+  // No +acceleration/gas command while pre-enabled at a stop with brake
+  return get_longitudinal_allowed() && !brake_pressed_prev;
+}
+
 // Given a CRC-8 poly, generate a static lookup table to use with a fast CRC-8
 // algorithm. Called at init time for safety modes using CRC-8.
 void gen_crc_lookup_table_8(uint8_t poly, uint8_t crc_lut[]) {
@@ -486,7 +497,7 @@ float interpolate(struct lookup_t xy, float x) {
 // Safety checks for longitudinal actuation
 bool longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limits) {
   bool violation = false;
-  if (!get_longitudinal_allowed()) {
+  if (!get_gas_allowed()) {
     violation |= desired_accel != limits.inactive_accel;
   } else {
     violation |= max_limit_check(desired_accel, limits.max_accel, limits.min_accel);
@@ -500,7 +511,7 @@ bool longitudinal_speed_checks(int desired_speed, const LongitudinalLimits limit
 
 bool longitudinal_gas_checks(int desired_gas, const LongitudinalLimits limits) {
   bool violation = false;
-  if (!get_longitudinal_allowed()) {
+  if (!get_gas_allowed()) {
     violation |= desired_gas != limits.inactive_gas;
   } else {
     violation |= max_limit_check(desired_gas, limits.max_gas, limits.min_gas);
@@ -516,15 +527,15 @@ bool longitudinal_brake_checks(int desired_brake, const LongitudinalLimits limit
 }
 
 bool longitudinal_interceptor_checks(CANPacket_t *to_send) {
-  return !get_longitudinal_allowed() && (GET_BYTE(to_send, 0) || GET_BYTE(to_send, 1));
+  return !get_gas_allowed() && (GET_BYTE(to_send, 0) || GET_BYTE(to_send, 1));
 }
 
 // Safety checks for torque-based steering commands
 bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLimits limits) {
   bool violation = false;
   uint32_t ts = microsecond_timer_get();
 
-  if (controls_allowed) {
+  if (get_lateral_allowed()) {
     // *** global torque limit check ***
     violation |= max_limit_check(desired_torque, limits.max_steer, -limits.max_steer);
 
@@ -551,7 +562,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
   }
 
   // no torque if controls is not allowed
-  if (!controls_allowed && (desired_torque != 0)) {
+  if (!get_lateral_allowed() && (desired_torque != 0)) {
     violation = true;
   }
 
@@ -593,7 +604,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
   }
 
   // reset to 0 if either controls is not allowed or there's a violation
-  if (violation || !controls_allowed) {
+  if (violation || !get_lateral_allowed()) {
     valid_steer_req_count = 0;
     invalid_steer_req_count = 0;
     desired_torque_last = 0;

board/safety/safety_ford.h

@@ -119,7 +119,7 @@ static int ford_tx_hook(CANPacket_t *to_send) {
     bool steer_control_enabled = steer_control_type != 0U;
 
     // No steer control allowed when controls are not allowed
-    if (!controls_allowed && steer_control_enabled) {
+    if (!get_lateral_allowed() && steer_control_enabled) {
       tx = 0;
     }
   }

board/safety/safety_nissan.h

@@ -118,7 +118,7 @@ static int nissan_tx_hook(CANPacket_t *to_send) {
     // offeset 1310 * NISSAN_DEG_TO_CAN
     desired_angle =  desired_angle - 131000;
 
-    if (controls_allowed && lka_active) {
+    if (get_lateral_allowed() && lka_active) {
       // add 1 to not false trigger the violation
       float delta_angle_float;
       delta_angle_float = (interpolate(NISSAN_LOOKUP_ANGLE_RATE_UP, vehicle_speed) * NISSAN_DEG_TO_CAN) + 1.;
@@ -134,14 +134,14 @@ static int nissan_tx_hook(CANPacket_t *to_send) {
     desired_angle_last = desired_angle;
 
     // desired steer angle should be the same as steer angle measured when controls are off
-    if ((!controls_allowed) &&
+    if ((!get_lateral_allowed()) &&
           ((desired_angle < (angle_meas.min - 1)) ||
           (desired_angle > (angle_meas.max + 1)))) {
       violation = 1;
     }
 
     // no lka_enabled bit if controls not allowed
-    if (!controls_allowed && lka_active) {
+    if (!get_lateral_allowed() && lka_active) {
       violation = 1;
     }
   }

board/safety/safety_tesla.h

@@ -137,7 +137,7 @@ static int tesla_tx_hook(CANPacket_t *to_send) {
                                  (steer_control_type != 3);    // DISABLED
 
     // Rate limit while steering
-    if(controls_allowed && steer_control_enabled) {
+    if(get_lateral_allowed() && steer_control_enabled) {
       // Add 1 to not false trigger the violation
       float delta_angle_float;
       delta_angle_float = (interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, vehicle_speed) * TESLA_DEG_TO_CAN);
@@ -153,12 +153,12 @@ static int tesla_tx_hook(CANPacket_t *to_send) {
     desired_angle_last = desired_angle;
 
     // Angle should be the same as current angle while not steering
-    if(!controls_allowed && ((desired_angle < (angle_meas.min - 1)) || (desired_angle > (angle_meas.max + 1)))) {
+    if(!get_lateral_allowed() && ((desired_angle < (angle_meas.min - 1)) || (desired_angle > (angle_meas.max + 1)))) {
       violation = true;
     }
 
     // No angle control allowed when controls are not allowed
-    if(!controls_allowed && steer_control_enabled) {
+    if(!get_lateral_allowed() && steer_control_enabled) {
       violation = true;
     }
   }

board/safety_declarations.h

@@ -112,6 +112,8 @@ bool driver_limit_check(int val, int val_last, struct sample_t *val_driver,
   const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN,
   const int MAX_ALLOWANCE, const int DRIVER_FACTOR);
 bool get_longitudinal_allowed(void);
+bool get_lateral_allowed(void);
+bool get_gas_allowed(void);
 bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
 float interpolate(struct lookup_t xy, float x);
 void gen_crc_lookup_table_8(uint8_t poly, uint8_t crc_lut[]);