Added .max_counter rx check

dzid26 · Nov 7, 2024 · 2786bd0 · 2786bd0
1 parent e57c3b5
commit 2786bd0
Showing 1 changed file with 22 additions and 8 deletions.
diff --git a/board/safety/safety_bmw.h b/board/safety/safety_bmw.h
@@ -2,7 +2,7 @@
 #define BMW_EngineAndBrake 0xA8
 #define BMW_AccPedal 0xAA
 #define BMW_Speed 0x1A0
-#define BMW_SteeringWheelAngl_slow 0xC8
+#define BMW_SteeringWheelAngle_slow 0xC8
 #define BMW_CruiseControlStatus 0x200
 #define BMW_DynamicCruiseControlStatus 0x193
 #define BMW_CruiseControlStalk 0x194
@@ -14,12 +14,12 @@
 
 static float interpolate(struct lookup_t xy, float x);
 
-RxCheck bmw_rx_checks[] = {  // todo add .check_checksum and .max_counter
-  {.msg = {{BMW_EngineAndBrake,       BMW_PT_CAN, 8, .frequency = 100U}, { 0 }, { 0 }}},
-  {.msg = {{BMW_AccPedal,             BMW_PT_CAN, 8, .frequency = 100U}, { 0 }, { 0 }}},
-  {.msg = {{BMW_Speed,                BMW_PT_CAN, 8, .frequency = 50U}, { 0 }, { 0 }}},
-  {.msg = {{BMW_SteeringWheelAngl_slow,   BMW_PT_CAN, 6, .frequency = 5U}, { 0 }, { 0 }}},
-  {.msg = {{BMW_TransmissionDataDisplay,   BMW_PT_CAN, 6, .frequency = 5U}, { 0 }, { 0 }}},
+RxCheck bmw_rx_checks[] = {  // todo add .check_checksum
+  {.msg = {{BMW_EngineAndBrake,       BMW_PT_CAN, 8, .max_counter = 14U, .frequency = 100U}, { 0 }, { 0 }}},
+  {.msg = {{BMW_AccPedal,             BMW_PT_CAN, 8, .max_counter = 14U, .frequency = 100U}, { 0 }, { 0 }}},
+  {.msg = {{BMW_Speed,                BMW_PT_CAN, 8, .max_counter = 14U, .frequency = 50U}, { 0 }, { 0 }}},
+  {.msg = {{BMW_SteeringWheelAngle_slow,   BMW_PT_CAN, 6, .max_counter = 0U, .frequency = 5U}, { 0 }, { 0 }}},
+  {.msg = {{BMW_TransmissionDataDisplay,   BMW_PT_CAN, 6, .max_counter = 14U, .frequency = 5U}, { 0 }, { 0 }}},
   // todo cruise control type dependant, use param:
   // {.msg = {{BMW_CruiseControlStatus,  BMW_PT_CAN, 8, .frequency = 5U}, { 0 }, { 0 }}},
   // {.msg = {{BMW_DynamicCruiseControlStatus,  BMW_F_CAN, 7, .frequency = 5U}, { 0 }, { 0 }}},
@@ -28,6 +28,19 @@ RxCheck bmw_rx_checks[] = {  // todo add .check_checksum and .max_counter
 };
 
 
+static uint8_t bmw_get_counter(const CANPacket_t *to_push) {
+  uint8_t cnt = 0;
+  int addr = GET_ADDR(to_push);
+  if (addr == BMW_TransmissionDataDisplay) {
+    cnt = (GET_BYTE(to_push, 3) >> 4) & 0xFU;
+  } else if (addr == BMW_Speed) {
+    cnt = (GET_BYTE(to_push, 6) >> 4) & 0xFU;
+  } else {
+    cnt = GET_BYTE(to_push, 1) & 0xFU;
+  }
+  return cnt;
+}
+
 const CanMsg BMW_TX_MSGS[] = {
   {BMW_CruiseControlStalk, BMW_PT_CAN, 4},   // Normal cruise control send status on PT-CAN
   {BMW_CruiseControlStalk, BMW_F_CAN, 4},    // Dynamic cruise control send status on F-CAN
@@ -137,7 +150,7 @@ static void bmw_rx_hook(const CANPacket_t *to_push) {
   }
 
   //get latest steering wheel angle rate
-  if ((addr == BMW_SteeringWheelAngl_slow) && (bus == BMW_PT_CAN)) {
+  if ((addr == BMW_SteeringWheelAngle_slow) && (bus == BMW_PT_CAN)) {
     float meas_angle = to_signed((GET_BYTE(to_push, 1) << 8) | GET_BYTE(to_push, 0), 16) * CAN_BMW_ANGLE_FAC; // deg
     float angle_rate = to_signed((GET_BYTE(to_push, 4) << 8) | GET_BYTE(to_push, 3), 16) * CAN_BMW_ANGLE_FAC; // deg/s
     // todo use common steer_angle_cmd_checks()
@@ -241,4 +254,5 @@ const safety_hooks bmw_hooks = {
   .rx = bmw_rx_hook,
   .tx = bmw_tx_hook,
   .fwd = default_fwd_hook,
+  .get_counter = bmw_get_counter,
 };