sunnypilot-byd/opendbc_repo/opendbc/safety/lateral.h

#include "opendbc/safety/sunnypilot/mads.h"
#include "opendbc/safety/declarations.h"

// ISO 11270
static const float ISO_LATERAL_ACCEL = 3.0;  // m/s^2

static const float EARTH_G = 9.81;
static const float AVERAGE_ROAD_ROLL = 0.06;  // ~3.4 degrees, 6% superelevation

// check that commanded torque value isn't too far from measured
static bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
                        const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR) {

  // *** val rate limit check ***
  int highest_allowed_rl = SAFETY_MAX(val_last, 0) + MAX_RATE_UP;
  int lowest_allowed_rl = SAFETY_MIN(val_last, 0) - MAX_RATE_UP;

  // if we've exceeded the meas val, we must start moving toward 0
  int highest_allowed = SAFETY_MIN(highest_allowed_rl, SAFETY_MAX(val_last - MAX_RATE_DOWN, SAFETY_MAX(val_meas->max, 0) + MAX_ERROR));
  int lowest_allowed = SAFETY_MAX(lowest_allowed_rl, SAFETY_MIN(val_last + MAX_RATE_DOWN, SAFETY_MIN(val_meas->min, 0) - MAX_ERROR));

  // check for violation
  return safety_max_limit_check(val, highest_allowed, lowest_allowed);
}

// check that commanded value isn't fighting against driver
static bool driver_limit_check(int val, int val_last, const struct sample_t *val_driver,
                        const int MAX_VAL, const int MAX_RATE_UP, const int MAX_RATE_DOWN,
                        const int MAX_ALLOWANCE, const int DRIVER_FACTOR) {

  // torque delta/rate limits
  int highest_allowed_rl = SAFETY_MAX(val_last, 0) + MAX_RATE_UP;
  int lowest_allowed_rl = SAFETY_MIN(val_last, 0) - MAX_RATE_UP;

  // driver
  int driver_max_limit = MAX_VAL + (MAX_ALLOWANCE + val_driver->max) * DRIVER_FACTOR;
  int driver_min_limit = -MAX_VAL + (-MAX_ALLOWANCE + val_driver->min) * DRIVER_FACTOR;

  // if we've exceeded the applied torque, we must start moving toward 0
  int highest_allowed = SAFETY_MIN(highest_allowed_rl, SAFETY_MAX(val_last - MAX_RATE_DOWN,
                                             SAFETY_MAX(driver_max_limit, 0)));
  int lowest_allowed = SAFETY_MAX(lowest_allowed_rl, SAFETY_MIN(val_last + MAX_RATE_DOWN,
                                           SAFETY_MIN(driver_min_limit, 0)));

  // check for violation
  return safety_max_limit_check(val, highest_allowed, lowest_allowed);
}

// real time check, mainly used for steer torque rate limiter
static bool rt_torque_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) {

  // *** torque real time rate limit check ***
  int highest_val = SAFETY_MAX(val_last, 0) + MAX_RT_DELTA;
  int lowest_val = SAFETY_MIN(val_last, 0) - MAX_RT_DELTA;

  // check for violation
  return safety_max_limit_check(val, highest_val, lowest_val);
}

// Safety checks for torque-based steering commands
bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueSteeringLimits limits) {
  bool violation = false;
  uint32_t ts = microsecond_timer_get();

  if (controls_allowed || controls_allowed_lateral) {
    // Some safety models support variable torque limit based on vehicle speed
    int max_torque = limits.max_torque;
    if (limits.dynamic_max_torque) {
      const float fudged_speed = (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.;
      max_torque = safety_interpolate(limits.max_torque_lookup, fudged_speed) + 1;
      max_torque = SAFETY_CLAMP(max_torque, -limits.max_torque, limits.max_torque);
    }

    // *** global torque limit check ***
    violation |= safety_max_limit_check(desired_torque, max_torque, -max_torque);

    // *** torque rate limit check ***
    if (limits.type == TorqueDriverLimited) {
      violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver,
                                      max_torque, limits.max_rate_up, limits.max_rate_down,
                                      limits.driver_torque_allowance, limits.driver_torque_multiplier);
    } else {
      violation |= dist_to_meas_check(desired_torque, desired_torque_last, &torque_meas,
                                      limits.max_rate_up, limits.max_rate_down, limits.max_torque_error);
    }
    desired_torque_last = desired_torque;

    // *** torque real time rate limit check ***
    violation |= rt_torque_rate_limit_check(desired_torque, rt_torque_last, limits.max_rt_delta);

    // every RT_INTERVAL set the new limits
    uint32_t ts_elapsed = safety_get_ts_elapsed(ts, ts_torque_check_last);
    if (ts_elapsed > MAX_RT_INTERVAL) {
      rt_torque_last = desired_torque;
      ts_torque_check_last = ts;
    }
  }

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

  // certain safety modes set their steer request bit low for one or more frame at a
  // predefined max frequency to avoid steering faults in certain situations
  bool steer_req_mismatch = (steer_req == 0) && (desired_torque != 0);
  if (!limits.has_steer_req_tolerance) {
    if (steer_req_mismatch) {
      violation = true;
    }

  } else {
    if (steer_req_mismatch) {
      if (invalid_steer_req_count == 0) {
        // disallow torque cut if not enough recent matching steer_req messages
        if (valid_steer_req_count < limits.min_valid_request_frames) {
          violation = true;
        }

        // or we've cut torque too recently in time
        uint32_t ts_elapsed = safety_get_ts_elapsed(ts, ts_steer_req_mismatch_last);
        if (ts_elapsed < limits.min_valid_request_rt_interval) {
          violation = true;
        }
      } else {
        // or we're cutting more frames consecutively than allowed
        if (invalid_steer_req_count >= limits.max_invalid_request_frames) {
          violation = true;
        }
      }

      valid_steer_req_count = 0;
      ts_steer_req_mismatch_last = ts;
      invalid_steer_req_count = SAFETY_MIN(invalid_steer_req_count + 1, limits.max_invalid_request_frames);
    } else {
      valid_steer_req_count = SAFETY_MIN(valid_steer_req_count + 1, limits.min_valid_request_frames);
      invalid_steer_req_count = 0;
    }
  }

  // reset to 0 if either controls is not allowed or there's a violation
  if (violation || !(controls_allowed || controls_allowed_lateral)) {
    valid_steer_req_count = 0;
    invalid_steer_req_count = 0;
    desired_torque_last = 0;
    rt_torque_last = 0;
    ts_torque_check_last = ts;
    ts_steer_req_mismatch_last = ts;
  }

  return violation;
}

static bool rt_angle_rate_limit_check(AngleSteeringLimits limits) {
  bool violation = false;
  uint32_t ts = microsecond_timer_get();

  // *** angle real time rate limit check ***
  int max_rt_msgs = ((float)limits.frequency * MAX_RT_INTERVAL / 1e6 * 1.2) + 1;  // 1.2x buffer
  if ((int)rt_angle_msgs > max_rt_msgs) {
    violation = true;
  }

  rt_angle_msgs += 1U;

  // every RT_INTERVAL reset message counter
  uint32_t ts_elapsed = safety_get_ts_elapsed(ts, ts_angle_check_last);
  if (ts_elapsed >= MAX_RT_INTERVAL) {
    rt_angle_msgs = 0;
    ts_angle_check_last = ts;
  }

  return violation;
}

static bool rt_curvature_rate_limit_check(CurvatureSteeringLimits limits) {
  bool violation = false;
  uint32_t ts = microsecond_timer_get();

  // *** curvature real time rate limit check ***
  int max_rt_msgs = ((float)limits.frequency * MAX_RT_INTERVAL / 1e6 * 1.2) + 1;  // 1.2x buffer
  uint32_t rt_msgs = curvature_state.rt_msgs + curvature_state.rt_msgs_prev;
  if ((int)rt_msgs > max_rt_msgs) {
    violation = true;
  }
  curvature_state.rt_msgs += 1U;

  //roll the window every half interval
  if (safety_get_ts_elapsed(ts, curvature_state.ts_check_last) >= (MAX_RT_INTERVAL / 2U)) {
    curvature_state.rt_msgs_prev = curvature_state.rt_msgs;
    curvature_state.rt_msgs = 0U;
    curvature_state.ts_check_last = ts;
  }

  return violation;
}

// Safety checks for angle-based steering commands
bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const AngleSteeringLimits limits) {
  bool violation = false;

  if ((controls_allowed || controls_allowed_lateral) && steer_control_enabled) {
    // convert floating point angle rate limits to integers in the scale of the desired angle on CAN,
    // add 1 to not false trigger the violation. also fudge the speed by 1 m/s so rate limits are
    // always slightly above openpilot's in case we read an updated speed in between angle commands
    // TODO: this speed fudge can be much lower, look at data to determine the lowest reasonable offset
    const float fudged_speed = (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.;
    int delta_angle_up = (safety_interpolate(limits.angle_rate_up_lookup, fudged_speed) * limits.angle_deg_to_can) + 1.;
    int delta_angle_down = (safety_interpolate(limits.angle_rate_down_lookup, fudged_speed) * limits.angle_deg_to_can) + 1.;

    // allow down limits at zero since small floats from openpilot will be rounded to 0
    // TODO: openpilot should be cognizant of this and not send small floats
    int highest_desired_angle = desired_angle_last + ((desired_angle_last > 0) ? delta_angle_up : delta_angle_down);
    int lowest_desired_angle = desired_angle_last - ((desired_angle_last >= 0) ? delta_angle_down : delta_angle_up);

    // check for violation;
    violation |= safety_max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
  }
  desired_angle_last = desired_angle;

  // Angle should be close to current angle while not steering
  if (!steer_control_enabled) {
    const int max_inactive_angle = SAFETY_CLAMP(angle_meas.max, -limits.max_angle, limits.max_angle) + 1;
    const int min_inactive_angle = SAFETY_CLAMP(angle_meas.min, -limits.max_angle, limits.max_angle) - 1;
    violation |= safety_max_limit_check(desired_angle, max_inactive_angle, min_inactive_angle);
  }

  // No angle control allowed when controls are not allowed
  if (!(controls_allowed || controls_allowed_lateral)) {
    violation |= steer_control_enabled;
  }

  // reset to current angle if either controls is not allowed or there's a violation
  if (violation || !(controls_allowed || controls_allowed_lateral)) {
    desired_angle_last = SAFETY_CLAMP(angle_meas.values[0], -limits.max_angle, limits.max_angle);
  }

  return violation;
}

// Safety checks for curvature-based steering commands
bool steer_curvature_cmd_checks(int desired_curvature, int steer_power, bool steer_control_enabled, const CurvatureSteeringLimits limits) {
  // Highway curves are rolled in the direction of the turn, add tolerance to compensate
  static const float MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL + (EARTH_G * AVERAGE_ROAD_ROLL);  // ~3.6 m/s^2
  // Lower than ISO 11270 lateral jerk limit, which is 5.0 m/s^3
  static const float MAX_LATERAL_JERK = 3.0 + (EARTH_G * AVERAGE_ROAD_ROLL);  // ~3.6 m/s^3

  const float fudged_speed = SAFETY_MAX((vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.0, 1.0);
  bool violation = false;

  speed_mismatch_check((float)vehicle_speed_2.values[0] / VEHICLE_SPEED_FACTOR);

  if ((controls_allowed || controls_allowed_lateral) && steer_control_enabled) {
    // *** absolute curvature cap ***
    violation |= safety_max_limit_check(desired_curvature, limits.max_curvature, -limits.max_curvature);

    // *** ISO lateral jerk limit ***
    const float max_curvature_rate_sec = MAX_LATERAL_JERK / (fudged_speed * fudged_speed);
    const float max_curvature_delta = max_curvature_rate_sec / (float)limits.frequency;
    const int max_curvature_delta_can = (max_curvature_delta * limits.curvature_to_can) + 1.;

    const int highest_desired_curvature = curvature_state.desired_last + max_curvature_delta_can;
    const int lowest_desired_curvature = curvature_state.desired_last - max_curvature_delta_can;
    violation |= safety_max_limit_check(desired_curvature, highest_desired_curvature, lowest_desired_curvature);

    // *** ISO lateral accel limit ***
    const float max_curvature = MAX_LATERAL_ACCEL / (fudged_speed * fudged_speed);
    const int max_curvature_can = (max_curvature * limits.curvature_to_can) + 1.;
    violation |= safety_max_limit_check(desired_curvature, max_curvature_can, -max_curvature_can);

    // *** curvature error from measured ***
    if (limits.max_curvature_error && ((vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR) > limits.curvature_error_min_speed)) {
      const int lowest_desired_curvature_error = curvature_state.meas.min - limits.max_curvature_error - 1;
      const int highest_desired_curvature_error = curvature_state.meas.max + limits.max_curvature_error + 1;
      violation |= safety_max_limit_check(desired_curvature, highest_desired_curvature_error, lowest_desired_curvature_error);
    }

    // *** real time rate limit check ***
    violation |= rt_curvature_rate_limit_check(limits);
  }
  curvature_state.desired_last = desired_curvature;

  // Curvature must be 0 while not steering
  if (!steer_control_enabled) {
    violation |= desired_curvature != 0;
  }

  // *** steer power check ***
  // allow power winddown after disengage to prevent EPS fault
  if (limits.max_steer_power != 0) {
    violation |= safety_max_limit_check(steer_power, limits.max_steer_power, 0);
    violation |= (steer_power != 0) && !steer_control_enabled;
    violation |= !(controls_allowed || controls_allowed_lateral) && (steer_power != 0) && (steer_power >= curvature_state.steer_power_last);
    curvature_state.steer_power_last = steer_power;
  } else {
    // No curvature control allowed when controls are not allowed
    violation |= !(controls_allowed || controls_allowed_lateral) && steer_control_enabled;
  }

  // reset to zero or measured curvature depending on EPS expectation
  if (violation || !(controls_allowed || controls_allowed_lateral)) {
    curvature_state.desired_last = limits.inactive_curvature_is_zero ? 0 : curvature_state.meas.values[0];
  }

  return violation;
}

static float get_curvature_factor(const float speed, const AngleSteeringParams params) {
  // Matches VehicleModel.curvature_factor()
  return 1. / (1. - (params.slip_factor * (speed * speed))) / params.wheelbase;
}

static float get_angle_from_curvature(const float curvature, const float curvature_factor, const AngleSteeringParams params) {
  // Matches VehicleModel.get_steer_from_curvature()
  static const float RAD_TO_DEG = 57.29577951308232;
  return curvature * params.steer_ratio / curvature_factor * RAD_TO_DEG;
}

bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, const AngleSteeringLimits limits,
                               const AngleSteeringParams params) {
  // This check uses a simple vehicle model to allow for constant lateral acceleration and jerk limits across all speeds.
  // TODO: remove the inaccurate breakpoint angle limiting function above and always use this one

  // Highway curves are rolled in the direction of the turn, add tolerance to compensate
  static const float MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL + (EARTH_G * AVERAGE_ROAD_ROLL);  // ~3.6 m/s^2
  // Lower than ISO 11270 lateral jerk limit, which is 5.0 m/s^3
  static const float MAX_LATERAL_JERK = 3.0 + (EARTH_G * AVERAGE_ROAD_ROLL);  // ~3.6 m/s^3

  const float fudged_speed = SAFETY_MAX((vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.0, 1.0);
  const float curvature_factor = get_curvature_factor(fudged_speed, params);

  bool violation = false;

  if ((controls_allowed || controls_allowed_lateral) && steer_control_enabled) {
    // *** ISO lateral jerk limit ***
    // calculate maximum angle rate per second
    const float max_curvature_rate_sec = MAX_LATERAL_JERK / (fudged_speed * fudged_speed);
    const float max_angle_rate_sec = get_angle_from_curvature(max_curvature_rate_sec, curvature_factor, params);

    // finally get max angle delta per frame
    const float max_angle_delta = max_angle_rate_sec / (float)limits.frequency;
    const int max_angle_delta_can = (max_angle_delta * limits.angle_deg_to_can) + 1.;

    // NOTE: symmetric up and down limits
    const int highest_desired_angle = desired_angle_last + max_angle_delta_can;
    const int lowest_desired_angle = desired_angle_last - max_angle_delta_can;

    violation |= safety_max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);

    // *** ISO lateral accel limit ***
    const float max_curvature = MAX_LATERAL_ACCEL / (fudged_speed * fudged_speed);
    const float max_angle = get_angle_from_curvature(max_curvature, curvature_factor, params);
    const int max_angle_can = (max_angle * limits.angle_deg_to_can) + 1.;

    violation |= safety_max_limit_check(desired_angle, max_angle_can, -max_angle_can);

    // *** angle real time rate limit check ***
    violation |= rt_angle_rate_limit_check(limits);
  }
  desired_angle_last = desired_angle;

  // Angle should either be 0 or same as current angle while not steering
  if (!steer_control_enabled) {
    const int max_inactive_angle = SAFETY_CLAMP(angle_meas.max, -limits.max_angle, limits.max_angle) + 1;
    const int min_inactive_angle = SAFETY_CLAMP(angle_meas.min, -limits.max_angle, limits.max_angle) - 1;
    violation |= safety_max_limit_check(desired_angle, max_inactive_angle, min_inactive_angle);
  }

  // No angle control allowed when controls are not allowed
  if (!(controls_allowed || controls_allowed_lateral)) {
    violation |= steer_control_enabled;
  }

  // reset to current angle if either controls is not allowed or there's a violation
  if (violation || !(controls_allowed || controls_allowed_lateral)) {
    desired_angle_last = SAFETY_CLAMP(angle_meas.values[0], -limits.max_angle, limits.max_angle);
  }

  return violation;
}