commented out tx safety for development

dzid26 · Aug 17, 2024 · dba0993 · dba0993
1 parent b1b82fe
commit dba0993
Showing 1 changed file with 37 additions and 41 deletions.
diff --git a/board/safety/safety_bmw.h b/board/safety/safety_bmw.h
@@ -66,7 +66,6 @@ const struct lookup_t BMW_MAX_TQ_RATE = {
     {16., 8., 1.}};   // Nm/10ms
 
 // state of angle limits
-float bmw_desired_angle_last = 0; // last desired steer angle
 float bmw_rt_angle_last = 0.; // last actual angle
 
 float angle_rate_up = 0;
@@ -78,7 +77,7 @@ int bmw_controls_allowed_last = 0;
 
 int lever_position = -1; //0 is when no ignition, so -1 unset
 float bmw_speed = 0;
-float actuator_torque;
+float actuator_torque = 0;
 
 
 static void bmw_rx_hook(const CANPacket_t *to_push) {
@@ -176,45 +175,42 @@ static void bmw_rx_hook(const CANPacket_t *to_push) {
 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
-      float steer_torque = ((float)(int8_t)(GET_BYTE(to_send, 4))) * CAN_ACTUATOR_TQ_FAC; //Nm
-      if (bmw_fmax_limit_check(steer_torque - actuator_torque, max_tq_rate, -max_tq_rate)){
-        print("Violation torque rate");
-        // printf("Tq: %f, ActTq: %f, Max: %f\n", steer_torque, actuator_torque, max_tq_rate);
-        tx = false;
-      }
-    }
-    float desired_angle = 0;
-    if (((GET_BYTE(to_send, 1) >> 4) & 0b11u) == 0x2){ //position control enabled
-      float angle_delta_req = ((float)(int16_t)((GET_BYTE(to_send, 2)) | (GET_BYTE(to_send, 3) << 8))) * CAN_ACTUATOR_POS_FAC; //deg/10ms
-      desired_angle = bmw_rt_angle_last + angle_delta_req; //measured + requested delta
-
-      if (controls_allowed == true) {
-        bool violation = false;
-        //check for max angles
-        violation |= bmw_fmax_limit_check(desired_angle, bmw_max_angle, -bmw_max_angle);
-        print("Violation desired angle");
-        //angle is rate limited in carcontrols so it shouldn't exceed max delta
-        float angle_delta_req_side = (bmw_desired_angle_last >= 0.) ? angle_delta_req : -angle_delta_req;
-        violation |= bmw_fmax_limit_check(angle_delta_req_side, angle_rate_up, -angle_rate_down);
-        print("Violation  delta");
-
-        if (violation) {
-          tx = false;
-          desired_angle = bmw_desired_angle_last; //nothing was sent - hold to previous
-        }
-      }
-    }
-    bmw_desired_angle_last = desired_angle;
-  }
-  if(controls_allowed == false){
-    tx = false;
-  }
+  // int addr = GET_ADDR(to_send);
+  // static float bmw_desired_angle_last = 0; // last desired steer angle
+  // // 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
+  //     float steer_torque = ((float)(int8_t)(GET_BYTE(to_send, 4))) * CAN_ACTUATOR_TQ_FAC; //Nm
+  //     if (bmw_fmax_limit_check(steer_torque - actuator_torque, max_tq_rate, -max_tq_rate)){
+  //       print("Violation torque rate");
+  //       // printf("Tq: %f, ActTq: %f, Max: %f\n", steer_torque, actuator_torque, max_tq_rate);
+  //       tx = false;
+  //     }
+  //   }
+  //   float desired_angle = 0;
+  //   if (((GET_BYTE(to_send, 1) >> 4) & 0b11u) == 0x2){ //position control enabled
+  //     float angle_delta_req = ((float)(int16_t)((GET_BYTE(to_send, 2)) | (GET_BYTE(to_send, 3) << 8))) * CAN_ACTUATOR_POS_FAC; //deg/10ms
+  //     desired_angle = bmw_rt_angle_last + angle_delta_req; //measured + requested delta
+
+  //     if (controls_allowed == true) {
+  //       bool violation = false;
+  //       //check for max angles
+  //       violation |= bmw_fmax_limit_check(desired_angle, bmw_max_angle, -bmw_max_angle);
+  //       print("Violation desired angle");
+  //       //angle is rate limited in carcontrols so it shouldn't exceed max delta
+  //       float angle_delta_req_side = (bmw_desired_angle_last >= 0.) ? angle_delta_req : -angle_delta_req;
+  //       violation |= bmw_fmax_limit_check(angle_delta_req_side, angle_rate_up, -angle_rate_down);
+  //       print("Violation  delta");
+
+  //       if (violation) {
+  //         tx = false;
+  //         desired_angle = bmw_desired_angle_last; //nothing was sent - hold to previous
+  //       }
+  //     }
+  //   }
+  //   bmw_desired_angle_last = desired_angle;
+  // }
 
   return tx;
 }