safety_bmw.h

#include <stdint.h>
#include <stdbool.h>

#define ENABLED_ACTUATOR 0 // 0 -> GMLAN_HIGH 5V -> Actuator Enable
#define DISABLED_ACTUATOR 1 // 1 -> GMLAN_LOW 0V (floating) -> Actuator Disable

const AddrBus BMW_TX_MSGS[] = {{404, 0}, {404, 1}, {558, 2}};
int SAMPLING_FREQ = 100; //Hz

#define KPH_TO_MS 0.277778

#define CAN_BMW_SPEED_FAC 0.1
#define CAN_BMW_ANGLE_FAC 0.04395
#define CAN_ACTUATOR_POS_FAC 0.125
#define CAN_ACTUATOR_TQ_FAC 0.125
#define CAN_ACTUATOR_CONTROL_STATUS_SOFTOFF_BIT 2

bool bmw_fmax_limit_check(float val, const float MAX_VAL, const float MIN_VAL) {
  return (val > MAX_VAL) || (val < MIN_VAL);
}
 
// rounding error margin
float BMW_MARGIN = 0.1;

const struct lookup_t BMW_LOOKUP_MAX_ANGLE = {
    {5., 15., 30.},     // m/s
    {200., 20., 10.}};  // deg
    

const struct lookup_t BMW_ANGLE_RATE_WINDUP = { // deg/s windup rate limit
    {0., 5., 15.},      // m/s
    {500., 80., 15.}};  // deg/s

const struct lookup_t BMW_ANGLE_RATE_UNWIND = { // deg/s unwind rate limit
    {0., 5., 15.},      // m/s
    {500., 350., 40.}}; // deg/s

const struct lookup_t BMW_MAX_TQ_RATE = {
    {0., 5., 15.},      // m/s
    {16., 8., 1.}};   // Nm/10ms

const uint32_t BMW_RT_INTERVAL = 250000; // 250ms between real time checks

// 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;
float angle_rate_down = 0;
float bmw_max_angle = 0;
float max_tq_rate = 0;

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;

void set_gmlan_digital_output(int to_set);
void reset_gmlan_switch_timeout(void);
void gmlan_switch_init(int timeout_enable);

bool cruise_engaged_last = 0;

static int bmw_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
  int addr = GET_ADDR(to_push);
  int bus = GET_BUS(to_push);

  bool cruise_engaged = false;
  if ((addr == 0x193) || (addr == 0x200)) { //handles both vehicle options VO544 and Vo540
    
    if (addr == 0x193) { //dynamic cruise control
      cruise_engaged = (((GET_BYTE(to_push, 5) >> 3) & 0x1U) == 1U);
    } else if (addr == 0x200) { //normal cruise control option
      cruise_engaged = (((GET_BYTE(to_push, 1) >> 5) & 0x1U) == 1U);
    } else {
      cruise_engaged = false;
    }
    if (!cruise_engaged) {
      controls_allowed = false;
    } 
    if (cruise_engaged && !cruise_engaged_last) {
      controls_allowed = true;
    }
    cruise_engaged_last = cruise_engaged;
  }

  if (addr == 404){ //disable on cruise stalk cancel
    if ((GET_BYTE(to_push, 2) & 0x90) != 0x0){
      controls_allowed = false;
    }
  }
  if (addr == 466) { //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 {
      lever_position = -1; //invalid
    }
    // if not in Drive
    if (lever_position != 8 ){
      controls_allowed = false;
    }
  }

  // exit controls on brake press
  if (addr == 168) {
    // any of two bits at position 61 & 62
    if ((GET_BYTE(to_push, 7)  & 0x60) != 0) {
      controls_allowed = false;
    }
  }
  //get vehicle speed
  if (addr == 416) {
    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)) {
    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
      controls_allowed = false;
      puts("BMW soft off\n");
    }
  }

  //get latest steering wheel angle rate
  if ((addr == 196) && ((bus == 0) || (bus == 1))) {
    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

    if(bmw_fmax_limit_check(meas_angle, bmw_max_angle, -bmw_max_angle)){
      // We should not be able to STEER under these conditions
      controls_allowed = false;
      if (cruise_engaged){
        puts("Too big angle \n");
      }
    }
    
    if (bmw_fmax_limit_check((bmw_rt_angle_last >= 0.) ? angle_rate : -angle_rate, angle_rate_up, -angle_rate_down)) { //should be sensitive for jerks to the outside
      controls_allowed = false;         // todo  ^ handle zero crossing a bit smarter
      if (cruise_engaged){
        puts("To fast angle rate \n");
      }
    }
    
    if ((controls_allowed && !bmw_controls_allowed_last)) {
      bmw_rt_angle_last = meas_angle;
    }
  }
  
  if (controls_allowed == true){
    set_gmlan_digital_output(ENABLED_ACTUATOR);
  #ifdef ALLOW_DEBUG
  } else if((bmw_speed == 0.) && ((lever_position == 0) || (lever_position == 1))) {//activate motor with bmw safety when external tool transmitting and shiflt lever is off or in Park
    //close the relay if external tool is commanding the actuator
    if((addr == 558) && (bus == 2)) {
      puts("Debug enable gmlan\n");
      set_gmlan_digital_output(ENABLED_ACTUATOR);
      reset_gmlan_switch_timeout();
    }
  #endif
  } else {
    set_gmlan_digital_output(DISABLED_ACTUATOR);
  }
  
  bmw_controls_allowed_last = controls_allowed;
  return 1;
}

// all commands: gas/regen, friction brake and steering
// if controls_allowed and no pedals pressed
//     allow all commands up to limit
// else
//     block all commands that produce actuation

static int bmw_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {

  int tx = 1;
  int addr = GET_ADDR(to_send);
  int bus = GET_BUS(to_send);

   if (!msg_allowed(addr, bus, BMW_TX_MSGS, sizeof(BMW_TX_MSGS) / sizeof(BMW_TX_MSGS[0]))) {
    tx = 0;
  }

  // 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)){
        puts("Violation torque rate");
        printf("Tq: %f, ActTq: %f, Max: %f\n", steer_torque, actuator_torque, max_tq_rate);
        
        tx = 0;
      }
    }
    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);
        puts("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);
        puts("Violation  delta");
        
        if (violation) {
          tx = 0;
          desired_angle = bmw_desired_angle_last; //nothing was sent - hold to previous
        }
      }
    }
    bmw_desired_angle_last = desired_angle;
  }
  if(controls_allowed == false){
    tx = 0;
  }
  if (tx != 0) {
    reset_gmlan_switch_timeout();
  }

  return tx;
}

static void bmw_init(int16_t param) {
  UNUSED(param);
  controls_allowed = false;
  bmw_speed = 0;
  lever_position = -1;

  #ifdef ALLOW_DEBUG
    puts("BMW safety init\n");
  #endif
  gmlan_switch_init(1); //init the gmlan switch with 1s timeout enabled
  set_gmlan_digital_output(DISABLED_ACTUATOR);
}

const safety_hooks bmw_hooks = {
  .init = bmw_init,
  .rx = bmw_rx_hook,
  .tx = bmw_tx_hook,
  .tx_lin = nooutput_tx_lin_hook,
  .fwd = default_fwd_hook,
};