stash

board/safety.h

@@ -72,7 +72,12 @@ int safety_rx_hook(CANPacket_t *to_push) {
 int safety_tx_hook(CANPacket_t *to_send) {
   bool longitudinal_allowed = get_longitudinal_allowed();
   bool gas_allowed = get_gas_allowed(longitudinal_allowed);
-  return (relay_malfunction ? -1 : current_hooks->tx(to_send, longitudinal_allowed, gas_allowed));
+  bool lateral_allowed = get_lateral_allowed();
+
+  // current_control_allowed excludes pre-enabled state (brake at standstill)
+  // steering and (positive) acceleration are disallowed while current_controls_allowed is false
+//  bool current_controls_allowed = get_current_controls_allowed();
+  return (relay_malfunction ? -1 : current_hooks->tx(to_send, longitudinal_allowed, lateral_allowed, gas_allowed));
 }
 
 int safety_tx_lin_hook(int lin_num, uint8_t *data, int len) {
@@ -88,6 +93,11 @@ bool get_longitudinal_allowed(void) {
   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(bool longitudinal_allowed) {
   // No +acceleration/gas command while pre-enabled at a stop with brake
   return longitudinal_allowed && !brake_pressed_prev;
@@ -494,6 +504,7 @@ float interpolate(struct lookup_t xy, float x) {
 // Safety checks for longitudinal actuation
 bool longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limits, const bool longitudinal_allowed) {
   bool violation = false;
+  bool longitudinal_allowed =
   if (!longitudinal_allowed) {
     violation |= desired_accel != limits.inactive_accel;
   } else {
@@ -544,8 +555,9 @@ bool long_accel_check(int desired_accel, const LongitudinalLimits limits, const
 bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLimits limits) {
   bool violation = false;
   uint32_t ts = microsecond_timer_get();
+  bool lateral_allowed = get_lateral_allowed();
 
-  if (controls_allowed) {
+  if (lateral_allowed) {
     // *** global torque limit check ***
     violation |= max_limit_check(desired_torque, limits.max_steer, -limits.max_steer);
 
@@ -572,7 +584,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 (!lateral_allowed && (desired_torque != 0)) {
     violation = true;
   }
 
@@ -614,7 +626,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 || !lateral_allowed) {
     valid_steer_req_count = 0;
     invalid_steer_req_count = 0;
     desired_torque_last = 0;

board/safety/safety_ford.h

@@ -121,7 +121,7 @@ static int ford_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed, bool ga
     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

@@ -114,13 +114,14 @@ static int nissan_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed, bool
 
   // steer cmd checks
   if (addr == 0x169) {
+    bool lateral_allowed = get_lateral_allowed();
     int desired_angle = ((GET_BYTE(to_send, 0) << 10) | (GET_BYTE(to_send, 1) << 2) | ((GET_BYTE(to_send, 2) >> 6) & 0x3U));
     bool lka_active = (GET_BYTE(to_send, 6) >> 4) & 1U;
 
     // 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.;
@@ -136,14 +137,14 @@ static int nissan_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed, bool
     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, bool longitudinal_allowed, bool g
                                  (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, bool longitudinal_allowed, bool g
     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/safety_toyota.h

@@ -132,7 +132,7 @@ static int toyota_rx_hook(CANPacket_t *to_push) {
   return valid;
 }
 
-static int toyota_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed, bool gas_allowed) {
+static int toyota_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed, bool lateral_allowed, bool gas_allowed) {
 
   int tx = 1;
   int addr = GET_ADDR(to_send);

board/safety_declarations.h

@@ -109,7 +109,7 @@ 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_gas_allowed(bool longitudinal_allowed);
+bool get_current_controls_allowed();
 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[]);