diff --git a/board/safety/safety_bmw.h b/board/safety/safety_bmw.h index 1f1d15be12..8a6720b6bf 100644 --- 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; }