From c313e7202eae59d3dbb09d3b376cecb632903f37 Mon Sep 17 00:00:00 2001
From: dzid26 <dzidmail@gmail.com>
Date: Fri, 8 Nov 2024 01:11:22 +0000
Subject: [PATCH] check lateral acc signal from the car

---
 board/safety/safety_bmw.h | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/board/safety/safety_bmw.h b/board/safety/safety_bmw.h
index 148c90c86c..96673154c3 100644
--- 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