jyoung/panda/board/safety/safety_mazda.h

// CAN msgs we care about
#define MAZDA_LKAS 0x243
#define MAZDA_LANEINFO 0x440
#define MAZDA_CRZ_CTRL 0x21c
#define MAZDA_WHEEL_SPEED 0x215
#define MAZDA_STEER_TORQUE 0x240

// CAN bus numbers
#define MAZDA_MAIN 0
#define MAZDA_AUX 1
#define MAZDA_CAM 2

#define MAZDA_MAX_STEER 2048

// max delta torque allowed for real time checks
#define MAZDA_MAX_RT_DELTA 940
// 250ms between real time checks
#define MAZDA_RT_INTERVAL 250000
#define MAZDA_MAX_RATE_UP 10
#define MAZDA_MAX_RATE_DOWN 25
#define MAZDA_DRIVER_TORQUE_ALLOWANCE 15
#define MAZDA_DRIVER_TORQUE_FACTOR 1


int mazda_cruise_engaged_last = 0;
int mazda_rt_torque_last = 0;
int mazda_desired_torque_last = 0;
uint32_t mazda_ts_last = 0;
struct sample_t mazda_torque_driver;         // last few driver torques measured

// track msgs coming from OP so that we know what CAM msgs to drop and what to forward
int mazda_op_lkas_detected = 0;
int mazda_op_laneinfo_detected = 0;

int mazda_forward_cam = 0;
int mazda_giraffe_switch_2_on = 0;

void mazda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
  int bus = GET_BUS(to_push);
  int addr = GET_ADDR(to_push);

  if ((addr == MAZDA_STEER_TORQUE) && (bus == MAZDA_MAIN)) {
    int torque_driver_new = GET_BYTE(to_push, 0) - 127;
    // update array of samples
    update_sample(&mazda_torque_driver, torque_driver_new);
  }

  // enter controls on rising edge of ACC, exit controls on ACC off
  if ((addr == MAZDA_CRZ_CTRL) && (bus == MAZDA_MAIN)) {
    int cruise_engaged = GET_BYTE(to_push, 0) & 8;
    if (cruise_engaged != 0) {
      if (!mazda_cruise_engaged_last) {
	controls_allowed = 1;
      }
    }
    else {
      controls_allowed = 0;
    }
    mazda_cruise_engaged_last = cruise_engaged;
  }

  // we have msgs on bus MAZDA_CAM
  if (bus == MAZDA_CAM) {
    // the stock CAM is connected
    if (addr == MAZDA_LKAS) {
      mazda_forward_cam = 1;
    }
    // if we see wheel speed msgs on MAZDA_CAM bus then giraffe switch 2 is high
    // (hardware passthru)
    if (addr == MAZDA_WHEEL_SPEED) {
      mazda_giraffe_switch_2_on = 1;
    }
  }
}

static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
  int tx = 1;
  int addr = GET_ADDR(to_send);
  int bus = GET_BUS(to_send);

  // Check if msg is sent on the main BUS
  if (bus == MAZDA_MAIN) {
    if ((addr == MAZDA_LKAS) && !mazda_op_lkas_detected){
      mazda_op_lkas_detected = 1;
    }
    if ((addr == MAZDA_LANEINFO) && !mazda_op_laneinfo_detected){
      mazda_op_laneinfo_detected = 1;
    }

    // steer cmd checks
    if (addr == MAZDA_LKAS) {
      int desired_torque = (((GET_BYTE(to_send, 0) & 0x0f) << 8) | GET_BYTE(to_send, 1)) - MAZDA_MAX_STEER;
      bool violation = 0;
      uint32_t ts = TIM2->CNT;

      if (controls_allowed) {

	// *** global torque limit check ***
	violation |= max_limit_check(desired_torque, MAZDA_MAX_STEER, -MAZDA_MAX_STEER);

	// *** torque rate limit check ***
	int desired_torque_last = mazda_desired_torque_last;
	violation |= driver_limit_check(desired_torque, desired_torque_last, &mazda_torque_driver,
					MAZDA_MAX_STEER, MAZDA_MAX_RATE_UP, MAZDA_MAX_RATE_DOWN,
					MAZDA_DRIVER_TORQUE_ALLOWANCE, MAZDA_DRIVER_TORQUE_FACTOR);
	// used next time
	mazda_desired_torque_last = desired_torque;

	// *** torque real time rate limit check ***
	violation |= rt_rate_limit_check(desired_torque, mazda_rt_torque_last, MAZDA_MAX_RT_DELTA);

	// every RT_INTERVAL set the new limits
	uint32_t ts_elapsed = get_ts_elapsed(ts, mazda_ts_last);
	if (ts_elapsed > ((uint32_t) MAZDA_RT_INTERVAL)) {
	  mazda_rt_torque_last = desired_torque;
	  mazda_ts_last = ts;
	}
      }

      // no torque if controls is not allowed
      if (!controls_allowed && (desired_torque != 0)) {
	violation = 1;
      }

      // reset to 0 if either controls is not allowed or there's a violation
      if (violation || !controls_allowed) {
	mazda_desired_torque_last = 0;
	mazda_rt_torque_last = 0;
	mazda_ts_last = ts;
      }

      if (violation) {
	tx = 0;
      }
    }
  }
  return tx;
}

static int mazda_fwd_hook(int bus, CAN_FIFOMailBox_TypeDef *to_fwd) {
  int bus_fwd = -1;
  if (mazda_forward_cam && !mazda_giraffe_switch_2_on) {
    int addr = GET_ADDR(to_fwd);
    if (bus == MAZDA_MAIN) {
      bus_fwd = MAZDA_CAM;
    }
    else if (bus == MAZDA_CAM) {
      // drop stock CAM_LKAS and CAM_LANEINFI if OP is sending them
      if (!((addr == MAZDA_LKAS) && mazda_op_lkas_detected) &&
          !((addr == MAZDA_LANEINFO) && mazda_op_laneinfo_detected)) {
	bus_fwd = MAZDA_MAIN;
      }
    }
    else {
      bus_fwd = -1;
    }
  }
  return bus_fwd;
}

const safety_hooks mazda_hooks = {
  .init = nooutput_init,
  .rx = mazda_rx_hook,
  .tx = mazda_tx_hook,
  .tx_lin = nooutput_tx_lin_hook,
  .ignition = default_ign_hook,
  .fwd = mazda_fwd_hook,
};