carrot/panda/board/safety/safety_nissan.h
FrogAi 9b1ff06bba Controls - Always On Lateral
Maintain openpilot lateral control when the brake or gas pedals are used. Deactivation occurs only through the 'Cruise Control' button.

Lots of credit goes to "pfeiferj"! Couldn't of done it without him!

https: //github.com/pfeiferj/openpilot
Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>
2024-06-01 02:34:36 -07:00

167 lines
4.9 KiB
C

const SteeringLimits NISSAN_STEERING_LIMITS = {
.angle_deg_to_can = 100,
.angle_rate_up_lookup = {
{0., 5., 15.},
{5., .8, .15}
},
.angle_rate_down_lookup = {
{0., 5., 15.},
{5., 3.5, .4}
},
};
const CanMsg NISSAN_TX_MSGS[] = {
{0x169, 0, 8}, // LKAS
{0x2b1, 0, 8}, // PROPILOT_HUD
{0x4cc, 0, 8}, // PROPILOT_HUD_INFO_MSG
{0x20b, 2, 6}, // CRUISE_THROTTLE (X-Trail)
{0x20b, 1, 6}, // CRUISE_THROTTLE (Altima)
{0x280, 2, 8} // CANCEL_MSG (Leaf)
};
// Signals duplicated below due to the fact that these messages can come in on either CAN bus, depending on car model.
RxCheck nissan_rx_checks[] = {
{.msg = {{0x1b6, 0, 8, .frequency = 100U},
{0x1b6, 1, 8, .frequency = 100U}, { 0 }}}, // PRO_PILOT (100HZ)
{.msg = {{0x2, 0, 5, .frequency = 100U},
{0x2, 1, 5, .frequency = 100U}, { 0 }}}, // STEER_ANGLE_SENSOR
{.msg = {{0x285, 0, 8, .frequency = 50U},
{0x285, 1, 8, .frequency = 50U}, { 0 }}}, // WHEEL_SPEEDS_REAR
{.msg = {{0x30f, 2, 3, .frequency = 10U},
{0x30f, 1, 3, .frequency = 10U}, { 0 }}}, // CRUISE_STATE
{.msg = {{0x15c, 0, 8, .frequency = 50U},
{0x15c, 1, 8, .frequency = 50U},
{0x239, 0, 8, .frequency = 50U}}}, // GAS_PEDAL
{.msg = {{0x454, 0, 8, .frequency = 10U},
{0x454, 1, 8, .frequency = 10U},
{0x1cc, 0, 4, .frequency = 100U}}}, // DOORS_LIGHTS / BRAKE
};
// EPS Location. false = V-CAN, true = C-CAN
const int NISSAN_PARAM_ALT_EPS_BUS = 1;
bool nissan_alt_eps = false;
static void nissan_rx_hook(const CANPacket_t *to_push) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
if (bus == (nissan_alt_eps ? 1 : 0)) {
if (addr == 0x2) {
// Current steering angle
// Factor -0.1, little endian
int angle_meas_new = (GET_BYTES(to_push, 0, 4) & 0xFFFFU);
// Multiply by -10 to match scale of LKAS angle
angle_meas_new = to_signed(angle_meas_new, 16) * -10;
// update array of samples
update_sample(&angle_meas, angle_meas_new);
}
if (addr == 0x285) {
// Get current speed and standstill
uint16_t right_rear = (GET_BYTE(to_push, 0) << 8) | (GET_BYTE(to_push, 1));
uint16_t left_rear = (GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3));
vehicle_moving = (right_rear | left_rear) != 0U;
UPDATE_VEHICLE_SPEED((right_rear + left_rear) / 2.0 * 0.005 / 3.6);
}
if (addr == 0x1b6) {
acc_main_on = GET_BIT(to_push, 36U);
}
// X-Trail 0x15c, Leaf 0x239
if ((addr == 0x15c) || (addr == 0x239)) {
if (addr == 0x15c){
gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3U)) > 3U;
} else {
acc_main_on = GET_BIT(to_push, 17U);
gas_pressed = GET_BYTE(to_push, 0) > 3U;
}
}
// X-trail 0x454, Leaf 0x239
if ((addr == 0x454) || (addr == 0x239)) {
if (addr == 0x454){
brake_pressed = (GET_BYTE(to_push, 2) & 0x80U) != 0U;
} else {
brake_pressed = ((GET_BYTE(to_push, 4) >> 5) & 1U) != 0U;
}
}
}
// Handle cruise enabled
if ((addr == 0x30f) && (bus == (nissan_alt_eps ? 1 : 2))) {
bool cruise_engaged = (GET_BYTE(to_push, 0) >> 3) & 1U;
pcm_cruise_check(cruise_engaged);
}
generic_rx_checks((addr == 0x169) && (bus == 0));
}
static bool nissan_tx_hook(const CANPacket_t *to_send) {
bool tx = true;
int addr = GET_ADDR(to_send);
bool violation = false;
// steer cmd checks
if (addr == 0x169) {
int desired_angle = ((GET_BYTE(to_send, 0) << 10) | (GET_BYTE(to_send, 1) << 2) | ((GET_BYTE(to_send, 2) >> 6) & 0x3U));
bool lka_active = (GET_BYTE(to_send, 6) >> 4) & 1U;
// Factor is -0.01, offset is 1310. Flip to correct sign, but keep units in CAN scale
desired_angle = -desired_angle + (1310.0f * NISSAN_STEERING_LIMITS.angle_deg_to_can);
if (steer_angle_cmd_checks(desired_angle, lka_active, NISSAN_STEERING_LIMITS)) {
violation = true;
}
}
// acc button check, only allow cancel button to be sent
if (addr == 0x20b) {
// Violation of any button other than cancel is pressed
violation |= ((GET_BYTE(to_send, 1) & 0x3dU) > 0U);
}
if (violation) {
tx = false;
}
return tx;
}
static int nissan_fwd_hook(int bus_num, int addr) {
int bus_fwd = -1;
if (bus_num == 0) {
bool block_msg = (addr == 0x280); // CANCEL_MSG
if (!block_msg) {
bus_fwd = 2; // ADAS
}
}
if (bus_num == 2) {
// 0x169 is LKAS, 0x2b1 LKAS_HUD, 0x4cc LKAS_HUD_INFO_MSG
bool block_msg = ((addr == 0x169) || (addr == 0x2b1) || (addr == 0x4cc));
if (!block_msg) {
bus_fwd = 0; // V-CAN
}
}
return bus_fwd;
}
static safety_config nissan_init(uint16_t param) {
nissan_alt_eps = GET_FLAG(param, NISSAN_PARAM_ALT_EPS_BUS);
return BUILD_SAFETY_CFG(nissan_rx_checks, NISSAN_TX_MSGS);
}
const safety_hooks nissan_hooks = {
.init = nissan_init,
.rx = nissan_rx_hook,
.tx = nissan_tx_hook,
.fwd = nissan_fwd_hook,
};