check lateral acc signal from the car

dzid26 · Nov 8, 2024 · c313e72 · c313e72
1 parent 84b6e0d
commit c313e72
Showing 1 changed file with 10 additions and 0 deletions.
diff --git a/board/safety/safety_bmw.h b/board/safety/safety_bmw.h
@@ -52,6 +52,7 @@ const CanMsg BMW_TX_MSGS[] = {
 
 #define CAN_BMW_SPEED_FAC 0.1
 #define CAN_BMW_ANGLE_FAC 0.04395
+#define CAN_BMW_ACC_FAC 0.025
 #define CAN_ACTUATOR_POS_FAC 0.125
 #define CAN_ACTUATOR_TQ_FAC 0.125
 #define CAN_ACTUATOR_CONTROL_STATUS_SOFTOFF_BIT 2
@@ -63,6 +64,8 @@ bool bmw_fmax_limit_check(float val, const float MAX_VAL, const float MIN_VAL) {
 // rounding error margin
 float BMW_MARGIN = 0.1;
 
+#define BMW_LAT_ACC_MAX 3.0 // EU guideline
+
 const struct lookup_t BMW_LOOKUP_MAX_ANGLE = {
     {5., 15., 30.},     // m/s
     {200., 20., 10.}};  // deg
@@ -137,6 +140,13 @@ static void bmw_rx_hook(const CANPacket_t *to_push) {
 
     // check moving forward and reverse
     vehicle_moving = (GET_BYTE(to_push, 1) & 0x30U) != 0U;
+
+    // check lateral acceleration limits
+    float bmw_lat_acc = to_signed((GET_BYTE(to_push, 4) << 4) | (GET_BYTE(to_push, 3) >> 4), 12) * CAN_BMW_ACC_FAC;
+    if (ABS(bmw_lat_acc) > BMW_LAT_ACC_MAX) {
+      print("Too big lateral acc \n");
+      controls_allowed = false;
+    }
   }
 
   // STEPPER_SERVO_CAN: get STEERING_STATUS