check correct bus no for stepper. Added bus and msg ids constants. St…

…eeringWheelAngle rx check on PT-CAN only is sufficient.
dzid26 · Aug 15, 2024 · b1b82fe · b1b82fe
1 parent 416a533
commit b1b82fe
Showing 1 changed file with 18 additions and 11 deletions.
diff --git a/board/safety/safety_bmw.h b/board/safety/safety_bmw.h
@@ -3,6 +3,10 @@
 #define BMW_AccPedal 170
 #define BMW_Speed 416
 #define BMW_SteeringWheelAngle 196
+#define BMW_CruiseControlStatus 512
+#define BMW_DynamicCruiseControlStatus 403
+#define BMW_CruiseControlStalk 404
+#define BMW_TransmissionDataDisplay 466
 
 #define BMW_PT_CAN 0
 #define BMW_F_CAN 1
@@ -14,15 +18,17 @@ RxCheck bmw_rx_checks[] = {
   {.msg = {{BMW_AccPedal,             BMW_PT_CAN, 8, .frequency = 100U}, { 0 }, { 0 }}},
   {.msg = {{BMW_Speed,                BMW_PT_CAN, 8, .frequency = 50U}, { 0 }, { 0 }}},
   {.msg = {{BMW_SteeringWheelAngle,   BMW_PT_CAN, 7, .frequency = 100U}, { 0 }, { 0 }}},
-  // also CruiseControlStatus or DynamicCruiseControlStatus
+  // todo cruise control type dependant, use param:
+  // {.msg = {{BMW_CruiseControlStatus,  BMW_PT_CAN, 8, .frequency = 50U}, { 0 }, { 0 }}},
+  // {.msg = {{BMW_DynamicCruiseControlStatus,  BMW_F_CAN, 7, .frequency = 100U}, { 0 }, { 0 }}},
 };
 
 
 const CanMsg BMW_TX_MSGS[] = {
-  {404, BMW_PT_CAN, 4},
-  {404, BMW_F_CAN, 4},
-  {558, BMW_F_CAN, 8}, // STEPPER_SERVO_CAN allowed on F-CAN
-  {558, BMW_AUX_CAN, 8}, // or standalone bus
+  {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
+  {558, BMW_F_CAN, 8},    // STEPPER_SERVO_CAN is allowed on F-CAN network
+  {558, BMW_AUX_CAN, 8},  // or on bus 2
 };
 
 int SAMPLING_FREQ = 100; //Hz
@@ -91,12 +97,12 @@ static void bmw_rx_hook(const CANPacket_t *to_push) {
     pcm_cruise_check(cruise_engaged);
   }
 
-  if (addr == 404){ //disable on cruise stalk cancel
+  if (addr == BMW_CruiseControlStalk){ //disable on cruise stalk cancel
     if ((GET_BYTE(to_push, 2) & 0x90) != 0x0){
       controls_allowed = false;
     }
   }
-  if (addr == 466) { //TransmissionDataDisplay
+  if (addr == BMW_TransmissionDataDisplay) {
     if ((GET_BYTE(to_push, 0) & 0xF) == ((GET_BYTE(to_push, 0) >> 4) ^ 0xF)) { //check agains shift lever compliment signal
       lever_position = GET_BYTE(to_push, 0) & 0xF; //compliment match
     } else {
@@ -109,16 +115,16 @@ static void bmw_rx_hook(const CANPacket_t *to_push) {
   }
 
   //get vehicle speed
-  if (addr == 416) {
+  if (addr == BMW_Speed) {
     bmw_speed = ((((GET_BYTE(to_push, 1) & 0xF) << 8) | GET_BYTE(to_push, 0)) * CAN_BMW_SPEED_FAC * KPH_TO_MS); //raw to km/h to m/s
     angle_rate_up = interpolate(BMW_ANGLE_RATE_WINDUP, bmw_speed) + BMW_MARGIN;   // deg/1s
     angle_rate_down = interpolate(BMW_ANGLE_RATE_UNWIND, bmw_speed) + BMW_MARGIN; // deg/1s
     bmw_max_angle = interpolate(BMW_LOOKUP_MAX_ANGLE, bmw_speed) + BMW_MARGIN;
     max_tq_rate = interpolate(BMW_MAX_TQ_RATE, bmw_speed) + BMW_MARGIN;
   }
 
-  //get STEERING_STATUS
-  if ((addr == 559)  && (bus == 2)) {
+  // STEPPER_SERVO_CAN: get STEERING_STATUS
+  if ((addr == 559) && ((bus == BMW_F_CAN) || (bus == BMW_AUX_CAN))) {
     actuator_torque = ((float)(int8_t)(GET_BYTE(to_push, 2))) * CAN_ACTUATOR_TQ_FAC; //Nm
 
     if((((GET_BYTE(to_push, 1)>>4)>>CAN_ACTUATOR_CONTROL_STATUS_SOFTOFF_BIT) & 0x1) != 0x0){ //Soft off status means motor is shutting down due to error
@@ -128,7 +134,7 @@ static void bmw_rx_hook(const CANPacket_t *to_push) {
   }
 
   //get latest steering wheel angle rate
-  if ((addr == BMW_SteeringWheelAngle) && ((bus == BMW_PT_CAN) || (bus == BMW_F_CAN))) {
+  if ((addr == BMW_SteeringWheelAngle) && (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
 
@@ -172,6 +178,7 @@ static bool bmw_tx_hook(const CANPacket_t *to_send) {
   bool tx = true;
   int addr = GET_ADDR(to_send);
 
+  // STEPPER_SERVO_CAN: get STEERING_COMMAND
   // do not transmit CAN message if steering angle too high
   if (addr == 558) {
     if (((GET_BYTE(to_send, 1) >> 4) & 0b11u) != 0x0){ //control enabled