Skip to content

Commit

Permalink
Honda Nidec: fwd stock AEB (commaai#257)
Browse files Browse the repository at this point in the history 
* Honda Nidec: fwd stock AEB

* bug

* correct aeb bit

* typo

* Revert "typo"

This reverts commit d424198b1fbc5a783c39b5bdd7d034865ed61a83.

* typo

* bug fix

* don't have tx hook blocking aeb messages from stock system

* typo

* fwd stock brake even if they are the same

* only stop forwarding aeb when aeb flag clears

* Fix safety tests

* Added fwd safety tests

* tiny bit more readable
  • Loading branch information
@rbiasini
rbiasini authored Aug 31, 2019
1 parent 6f532c6 commit d68356b
Show file tree
Hide file tree
Showing 4 changed files with 84 additions and 44 deletions.
47 changes: 35 additions & 12 deletions board/safety/safety_honda.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,16 +8,19 @@
// brake > 0mph

const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 328; // ratio between offset and gain from dbc file
int honda_brake_prev = 0;
int honda_brake = 0;
int honda_gas_prev = 0;
bool honda_brake_pressed_prev = false;
bool honda_moving = false;
bool honda_bosch_hardware = false;
bool honda_alt_brake_msg = false;
bool honda_fwd_brake = false;

static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

int addr = GET_ADDR(to_push);
int len = GET_LEN(to_push);
int bus = GET_BUS(to_push);

// sample speed
if (addr == 0x158) {
Expand Down Expand Up @@ -51,11 +54,11 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on rising edge of brake press or on brake press when speed > 0
bool is_user_brake_msg = honda_alt_brake_msg ? ((addr) == 0x1BE) : ((addr) == 0x17C);
if (is_user_brake_msg) {
int brake = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20);
if (brake && (!(honda_brake_prev) || honda_moving)) {
bool brake_pressed = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20);
if (brake_pressed && (!(honda_brake_pressed_prev) || honda_moving)) {
controls_allowed = 0;
}
honda_brake_prev = brake;
honda_brake_pressed_prev = brake_pressed;
}

// exit controls on rising edge of gas press if interceptor (0x201 w/ len = 6)
Expand All @@ -81,6 +84,20 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
honda_gas_prev = gas;
}
}
if ((bus == 2) && (addr == 0x1FA)) {
bool honda_stock_aeb = GET_BYTE(to_push, 3) & 0x20;
int honda_stock_brake = (GET_BYTE(to_push, 0) << 2) + ((GET_BYTE(to_push, 1) >> 6) & 0x3);

// Forward AEB when stock braking is higher than openpilot braking
// only stop forwarding when AEB event is over
if (!honda_stock_aeb) {
honda_fwd_brake = false;
} else if (honda_stock_brake >= honda_brake) {
honda_fwd_brake = true;
} else {
// Leave honda forward brake as is
}
}
}

// all commands: gas, brake and steering
Expand All @@ -98,18 +115,21 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
// and the the latching controls_allowed flag is True
int pedal_pressed = honda_gas_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) ||
(honda_brake_prev && honda_moving);
(honda_brake_pressed_prev && honda_moving);
bool current_controls_allowed = controls_allowed && !(pedal_pressed);

// BRAKE: safety check
if (addr == 0x1FA) {
int brake = (GET_BYTE(to_send, 0) << 2) + ((GET_BYTE(to_send, 1) >> 6) & 0x3);
if ((addr == 0x1FA) && (bus == 0)) {
honda_brake = (GET_BYTE(to_send, 0) << 2) + ((GET_BYTE(to_send, 1) >> 6) & 0x3);
if (!current_controls_allowed || !long_controls_allowed) {
if (brake != 0) {
if (honda_brake != 0) {
tx = 0;
}
}
if (brake > 255) {
if (honda_brake > 255) {
tx = 0;
}
if (honda_fwd_brake) {
tx = 0;
}
}
Expand Down Expand Up @@ -175,9 +195,12 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
if (bus_num == 2) {
// block stock lkas messages and stock acc messages (if OP is doing ACC)
int addr = GET_ADDR(to_fwd);
int is_lkas_msg = (addr == 0xE4) || (addr == 0x194) || (addr == 0x33D);
int is_acc_msg = (addr == 0x1FA) || (addr == 0x30C) || (addr == 0x39F);
int block_fwd = is_lkas_msg || (is_acc_msg && long_controls_allowed);
bool is_lkas_msg = (addr == 0xE4) || (addr == 0x194) || (addr == 0x33D);
bool is_acc_hud_msg = (addr == 0x30C) || (addr == 0x39F);
bool is_brake_msg = addr == 0x1FA;
bool block_fwd = is_lkas_msg ||
(is_acc_hud_msg && long_controls_allowed) ||
(is_brake_msg && long_controls_allowed && !honda_fwd_brake);
if (!block_fwd) {
bus_fwd = 0;
}
Expand Down
3 changes: 2 additions & 1 deletion tests/safety/libpandasafety_py.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,8 +57,9 @@
void init_tests_honda(void);
bool get_honda_moving(void);
int get_honda_brake_prev(void);
bool get_honda_brake_pressed_prev(void);
int get_honda_gas_prev(void);
void set_honda_fwd_brake(bool);
void set_honda_alt_brake_msg(bool);
void set_honda_bosch_hardware(bool);
int get_honda_bosch_hardware(void);
Expand Down
11 changes: 8 additions & 3 deletions tests/safety/test.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -227,8 +227,8 @@ bool get_honda_moving(void){
return honda_moving;
}

int get_honda_brake_prev(void){
return honda_brake_prev;
bool get_honda_brake_pressed_prev(void){
return honda_brake_pressed_prev;
}

int get_honda_gas_prev(void){
Expand All @@ -247,6 +247,10 @@ int get_honda_bosch_hardware(void) {
return honda_bosch_hardware;
}

void set_honda_fwd_brake(bool c){
honda_fwd_brake = c;
}

void init_tests(void){
// get HW_TYPE from env variable set in test.sh
hw_type = atoi(getenv("HW_TYPE"));
Expand Down Expand Up @@ -315,8 +319,9 @@ void init_tests_subaru(void){
void init_tests_honda(void){
init_tests();
honda_moving = false;
honda_brake_prev = 0;
honda_brake_pressed_prev = false;
honda_gas_prev = 0;
honda_fwd_brake = false;
}

void set_gmlan_digital_output(int to_set){
Expand Down
67 changes: 39 additions & 28 deletions tests/safety/test_honda.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,9 +112,9 @@ def test_sample_speed(self):
self.assertEqual(1, self.safety.get_honda_moving())

def test_prev_brake(self):
self.assertFalse(self.safety.get_honda_brake_prev())
self.assertFalse(self.safety.get_honda_brake_pressed_prev())
self.safety.safety_rx_hook(self._brake_msg(True))
self.assertTrue(self.safety.get_honda_brake_prev())
self.assertTrue(self.safety.get_honda_brake_pressed_prev())

def test_disengage_on_brake(self):
self.safety.set_controls_allowed(1)
Expand Down Expand Up @@ -204,17 +204,22 @@ def test_allow_engage_with_gas_interceptor_pressed(self):
self.safety.set_gas_interceptor_detected(False)

def test_brake_safety_check(self):
for long_controls_allowed in [0, 1]:
self.safety.set_long_controls_allowed(long_controls_allowed)
for brake in np.arange(0, MAX_BRAKE + 10, 1):
for controls_allowed in [True, False]:
self.safety.set_controls_allowed(controls_allowed)
if controls_allowed and long_controls_allowed:
send = MAX_BRAKE >= brake >= 0
else:
send = brake == 0
self.assertEqual(send, self.safety.safety_tx_hook(self._send_brake_msg(brake)))
for fwd_brake in [False, True]:
self.safety.set_honda_fwd_brake(fwd_brake)
for long_controls_allowed in [0, 1]:
self.safety.set_long_controls_allowed(long_controls_allowed)
for brake in np.arange(0, MAX_BRAKE + 10, 1):
for controls_allowed in [True, False]:
self.safety.set_controls_allowed(controls_allowed)
if fwd_brake:
send = False # block openpilot brake msg when fwd'ing stock msg
elif controls_allowed and long_controls_allowed:
send = MAX_BRAKE >= brake >= 0
else:
send = brake == 0
self.assertEqual(send, self.safety.safety_tx_hook(self._send_brake_msg(brake)))
self.safety.set_long_controls_allowed(True)
self.safety.set_honda_fwd_brake(False)

def test_gas_interceptor_safety_check(self):
for long_controls_allowed in [0, 1]:
Expand Down Expand Up @@ -252,27 +257,33 @@ def test_fwd_hook(self):
buss = range(0x0, 0x3)
msgs = range(0x1, 0x800)
long_controls_allowed = [0, 1]
fwd_brake = [False, True]

self.safety.set_honda_bosch_hardware(0)

for l in long_controls_allowed:
self.safety.set_long_controls_allowed(l)
blocked_msgs = [0xE4, 0x194, 0x33D]
if l:
blocked_msgs += [0x1FA ,0x30C, 0x39F]
for b in buss:
for m in msgs:
if b == 0:
fwd_bus = 2
elif b == 1:
fwd_bus = -1
elif b == 2:
fwd_bus = -1 if m in blocked_msgs else 0

# assume len 8
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
for f in fwd_brake:
self.safety.set_honda_fwd_brake(f)
for l in long_controls_allowed:
self.safety.set_long_controls_allowed(l)
blocked_msgs = [0xE4, 0x194, 0x33D]
if l:
blocked_msgs += [0x30C, 0x39F]
if not f:
blocked_msgs += [0x1FA]
for b in buss:
for m in msgs:
if b == 0:
fwd_bus = 2
elif b == 1:
fwd_bus = -1
elif b == 2:
fwd_bus = -1 if m in blocked_msgs else 0

# assume len 8
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))

self.safety.set_long_controls_allowed(True)
self.safety.set_honda_fwd_brake(False)



Expand Down

0 comments on commit d68356b

Please sign in to comment.