Skip to content

Commit

Permalink
Added Nissan safety (#244)
Browse files Browse the repository at this point in the history
* Added Nissan safety
  • Loading branch information
avolmensky authored Feb 26, 2020
1 parent d7f1195 commit db94a5b
Show file tree
Hide file tree
Showing 6 changed files with 440 additions and 0 deletions.
3 changes: 3 additions & 0 deletions board/safety.h
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
#include "safety/safety_chrysler.h"
#include "safety/safety_subaru.h"
#include "safety/safety_mazda.h"
#include "safety/safety_nissan.h"
#include "safety/safety_volkswagen.h"
#include "safety/safety_elm327.h"

Expand All @@ -31,6 +32,7 @@
#define SAFETY_TESLA 10U
#define SAFETY_SUBARU 11U
#define SAFETY_MAZDA 13U
#define SAFETY_NISSAN 14U
#define SAFETY_VOLKSWAGEN_MQB 15U
#define SAFETY_TOYOTA_IPAS 16U
#define SAFETY_ALLOUTPUT 17U
Expand Down Expand Up @@ -206,6 +208,7 @@ const safety_hook_config safety_hook_registry[] = {
{SAFETY_CADILLAC, &cadillac_hooks},
{SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},
{SAFETY_TESLA, &tesla_hooks},
{SAFETY_NISSAN, &nissan_hooks},
{SAFETY_ALLOUTPUT, &alloutput_hooks},
{SAFETY_GM_ASCM, &gm_ascm_hooks},
{SAFETY_FORD, &ford_hooks},
Expand Down
215 changes: 215 additions & 0 deletions board/safety/safety_nissan.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,215 @@

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

const struct lookup_t NISSAN_LOOKUP_ANGLE_RATE_UP = {
{2., 7., 17.},
{5., .8, .15}};

const struct lookup_t NISSAN_LOOKUP_ANGLE_RATE_DOWN = {
{2., 7., 17.},
{5., 3.5, .5}};

const struct lookup_t NISSAN_LOOKUP_MAX_ANGLE = {
{3.3, 12, 32},
{540., 120., 23.}};

const int NISSAN_DEG_TO_CAN = 100;

const AddrBus NISSAN_TX_MSGS[] = {{0x169, 0}, {0x20b, 2}};

AddrCheckStruct nissan_rx_checks[] = {
{.addr = {0x2}, .bus = 0, .expected_timestep = 100000U},
{.addr = {0x29a}, .bus = 0, .expected_timestep = 50000U},
{.addr = {0x1b6}, .bus = 1, .expected_timestep = 100000U},
};
const int NISSAN_RX_CHECK_LEN = sizeof(nissan_rx_checks) / sizeof(nissan_rx_checks[0]);

float nissan_speed = 0;
//int nissan_controls_allowed_last = 0;
uint32_t nissan_ts_angle_last = 0;
int nissan_cruise_engaged_last = 0;
int nissan_desired_angle_last = 0;
int nissan_gas_prev = 0;
int nissan_brake_prev = 0;

struct sample_t nissan_angle_meas; // last 3 steer angles


static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

bool valid = addr_safety_check(to_push, nissan_rx_checks, NISSAN_RX_CHECK_LEN,
NULL, NULL, NULL);

if (valid) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);

if (bus == 0) {
if (addr == 0x2) {
// Current steering angle
// Factor -0.1, little endian
int angle_meas_new = (GET_BYTES_04(to_push) & 0xFFFF);
// Need to multiply by 10 here as LKAS and Steering wheel are different base unit
angle_meas_new = to_signed(angle_meas_new, 16) * 10;

// update array of samples
update_sample(&nissan_angle_meas, angle_meas_new);
}

if (addr == 0x29a) {
// Get current speed
// Factor 0.00555
nissan_speed = ((GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3))) * 0.00555 / 3.6;
}

// exit controls on rising edge of gas press
if (addr == 0x15c) {
int gas = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3));
if ((gas > 0) && (nissan_gas_prev == 0)) {
controls_allowed = 0;
}
nissan_gas_prev = gas;
}

// 0x169 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x169)) {
relay_malfunction = true;
}
}

if (bus == 1) {
if (addr == 0x1b6) {
int cruise_engaged = (GET_BYTE(to_push, 4) >> 6) & 1;
if (cruise_engaged && !nissan_cruise_engaged_last) {
controls_allowed = 1;
}
if (!cruise_engaged) {
controls_allowed = 0;
}
nissan_cruise_engaged_last = cruise_engaged;
}

// exit controls on rising edge of brake press, or if speed > 0 and brake
if (addr == 0x454) {
int brake = (GET_BYTE(to_push, 2) & 0x80);
if ((brake > 0) && ((nissan_brake_prev == 0) || (nissan_speed > 0.))) {
controls_allowed = 0;
}
nissan_brake_prev = brake;
}
}
}
return valid;
}


static int nissan_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);
int bus = GET_BUS(to_send);
bool violation = 0;

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

if (relay_malfunction) {
tx = 0;
}

// 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) & 0x3));
bool lka_active = (GET_BYTE(to_send, 6) >> 4) & 1;

// offeset 1310 * NISSAN_DEG_TO_CAN
desired_angle = desired_angle - 131000;

if (controls_allowed && lka_active) {
// add 1 to not false trigger the violation
float delta_angle_float;
delta_angle_float = (interpolate(NISSAN_LOOKUP_ANGLE_RATE_UP, nissan_speed) * NISSAN_DEG_TO_CAN) + 1.;
int delta_angle_up = (int)(delta_angle_float);
delta_angle_float = (interpolate(NISSAN_LOOKUP_ANGLE_RATE_DOWN, nissan_speed) * NISSAN_DEG_TO_CAN) + 1.;
int delta_angle_down = (int)(delta_angle_float);
int highest_desired_angle = nissan_desired_angle_last + ((nissan_desired_angle_last > 0) ? delta_angle_up : delta_angle_down);
int lowest_desired_angle = nissan_desired_angle_last - ((nissan_desired_angle_last >= 0) ? delta_angle_down : delta_angle_up);

// Limit maximum steering angle at current speed
int maximum_angle = ((int)interpolate(NISSAN_LOOKUP_MAX_ANGLE, nissan_speed));

if (highest_desired_angle > (maximum_angle * NISSAN_DEG_TO_CAN)) {
highest_desired_angle = (maximum_angle * NISSAN_DEG_TO_CAN);
}
if (lowest_desired_angle < (-maximum_angle * NISSAN_DEG_TO_CAN)) {
lowest_desired_angle = (-maximum_angle * NISSAN_DEG_TO_CAN);
}

// check for violation;
violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);

//nissan_controls_allowed_last = controls_allowed;
}
nissan_desired_angle_last = desired_angle;

// desired steer angle should be the same as steer angle measured when controls are off
if ((!controls_allowed) &&
((desired_angle < (nissan_angle_meas.min - 1)) ||
(desired_angle > (nissan_angle_meas.max + 1)))) {
violation = 1;
}

// no lka_enabled bit if controls not allowed
if (!controls_allowed && lka_active) {
violation = 1;
}
}

// 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) & 0x3d) > 0);
}

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

return tx;
}


static int nissan_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
int bus_fwd = -1;
int addr = GET_ADDR(to_fwd);

if (bus_num == 0) {
bus_fwd = 2; // ADAS
}

if (bus_num == 2) {
// 0x169 is LKAS
int block_msg = (addr == 0x169);
if (!block_msg) {
bus_fwd = 0; // V-CAN
}
}

if (relay_malfunction) {
bus_fwd = -1;
}

// fallback to do not forward
return bus_fwd;
}

const safety_hooks nissan_hooks = {
.init = nooutput_init,
.rx = nissan_rx_hook,
.tx = nissan_tx_hook,
.tx_lin = nooutput_tx_lin_hook,
.fwd = nissan_fwd_hook,
.addr_check = nissan_rx_checks,
.addr_check_len = sizeof(nissan_rx_checks) / sizeof(nissan_rx_checks[0]),
};
1 change: 1 addition & 0 deletions python/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -123,6 +123,7 @@ class Panda(object):
SAFETY_TESLA = 10
SAFETY_SUBARU = 11
SAFETY_MAZDA = 13
SAFETY_NISSAN = 14
SAFETY_VOLKSWAGEN_MQB = 15
SAFETY_TOYOTA_IPAS = 16
SAFETY_ALLOUTPUT = 17
Expand Down
4 changes: 4 additions & 0 deletions tests/safety/libpandasafety_py.py
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,10 @@
void set_volkswagen_rt_torque_last(int t);
void set_volkswagen_torque_driver(int min, int max);
void init_tests_nissan(void);
void set_nissan_desired_angle_last(int t);
void set_nissan_brake_prev(int t);
""")

libpandasafety = ffi.dlopen(libpandasafety_fn)
18 changes: 18 additions & 0 deletions tests/safety/test.c
Original file line number Diff line number Diff line change
Expand Up @@ -272,6 +272,14 @@ void set_honda_fwd_brake(bool c){
honda_fwd_brake = c;
}

void set_nissan_brake_prev(bool c){
nissan_brake_prev = c;
}

void set_nissan_desired_angle_last(int t){
nissan_desired_angle_last = t;
}

void init_tests(void){
// get HW_TYPE from env variable set in test.sh
hw_type = atoi(getenv("HW_TYPE"));
Expand Down Expand Up @@ -361,6 +369,16 @@ void init_tests_honda(void){
honda_fwd_brake = false;
}

void init_tests_nissan(void){
init_tests();
nissan_angle_meas.min = 0;
nissan_angle_meas.max = 0;
nissan_desired_angle_last = 0;
nissan_gas_prev = 0;
nissan_brake_prev = 0;
set_timer(0);
}

void set_gmlan_digital_output(int to_set){
}

Expand Down
Loading

0 comments on commit db94a5b

Please sign in to comment.