Skip to content

Commit

Permalink
Cadillac: added max steer safety
Browse files Browse the repository at this point in the history
  • Loading branch information
rbiasini committed May 26, 2018
1 parent dbc11a1 commit bea5187
Show file tree
Hide file tree
Showing 3 changed files with 41 additions and 10 deletions.
9 changes: 9 additions & 0 deletions board/safety.h
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@ int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
int safety_tx_lin_hook(int lin_num, uint8_t *data, int len);
int safety_ignition_hook();
uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last);
int to_signed(int d, int bits);

typedef void (*safety_hook_init)(int16_t param);
typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
Expand Down Expand Up @@ -109,3 +110,11 @@ int safety_set_mode(uint16_t mode, int16_t param) {
uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last) {
return ts > ts_last ? ts - ts_last : (0xFFFFFFFF - ts_last) + 1 + ts;
}

// convert a trimmed integer to signed 32 bit int
int to_signed(int d, int bits) {
if (d >= (1 << (bits - 1))) {
d -= (1 << bits);
}
return d;
}
35 changes: 32 additions & 3 deletions board/safety/safety_cadillac.h
Original file line number Diff line number Diff line change
@@ -1,20 +1,49 @@
const int STEER_MAX = 150; // 1s
const int CADILLAC_IGNITION_TIMEOUT = 1000000; // 1s

int cadillac_can_seen = 0;
int cadillac_cruise_engaged_last = 0;
uint32_t cadillac_ts_last = 0;

static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus_number = (to_push->RDTR >> 4) & 0xFF;
uint32_t addr = to_push->RIR >> 21;

if (addr == 0x135 && bus_number == 0) {
if ((addr == 0x135) && (bus_number == 0)) {
cadillac_can_seen = 1;
cadillac_ts_last = TIM2->CNT; // reset timer when gear msg is received
}

// enter controls on rising edge of ACC, exit controls on ACC off
if ((addr == 0x370) && (bus_number == 0)) {
int cruise_engaged = to_push->RDLR & 0x800000; // bit 23
if (cruise_engaged && !cadillac_cruise_engaged_last) {
controls_allowed = 1;
} else if (!cruise_engaged) {
controls_allowed = 0;
}
cadillac_cruise_engaged_last = cruise_engaged;
}
}

static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
uint32_t addr = to_send->RIR >> 21;

// block steering cmd above 150
if (addr == 0x151 || addr == 0x152 || addr == 0x153 || addr == 0x154) {
int lkas_cmd = ((to_send->RDLR & 0x3f) << 8) + ((to_send->RDLR & 0xff00) >> 8);
lkas_cmd = to_signed(lkas_cmd, 14);
// block message is controls are allowed and lkas command exceeds max, or
// if controls aren't allowed and lkas cmd isn't 0
if (controls_allowed && ((lkas_cmd > STEER_MAX) || (lkas_cmd < -STEER_MAX))) {
return 0;
} else if (!controls_allowed && lkas_cmd) return 0;
}
return true;
}

static void cadillac_init(int16_t param) {
cadillac_can_seen = 0;
controls_allowed = 1;
}

static int cadillac_ign_hook() {
Expand All @@ -31,7 +60,7 @@ static int cadillac_ign_hook() {
const safety_hooks cadillac_hooks = {
.init = cadillac_init,
.rx = cadillac_rx_hook,
.tx = alloutput_tx_hook,
.tx = cadillac_tx_hook,
.tx_lin = alloutput_tx_lin_hook,
.ignition = cadillac_ign_hook,
.fwd = alloutput_fwd_hook,
Expand Down
7 changes: 0 additions & 7 deletions board/safety/safety_toyota_ipas.h
Original file line number Diff line number Diff line change
Expand Up @@ -35,13 +35,6 @@ uint32_t ts_angle_last = 0;

int controls_allowed_last = 0;

int to_signed(int d, int bits) {
if (d >= (1 << (bits - 1))) {
d -= (1 << bits);
}
return d;
}

// interp function that holds extreme values
float interpolate(struct lookup_t xy, float x) {
int size = sizeof(xy.x) / sizeof(xy.x[0]);
Expand Down

0 comments on commit bea5187

Please sign in to comment.