Honda Nidec: fwd stock AEB (#257)

* 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

board/safety/safety_honda.h

@@ -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) {
@@ -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)
@@ -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
@@ -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;
     }
   }
@@ -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;
     }

tests/safety/libpandasafety_py.py

@@ -57,8 +57,9 @@ void set_toyota_rt_torque_last(int t);
 
 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);

tests/safety/test.c

@@ -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){
@@ -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"));
@@ -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){

tests/safety/test_honda.py

@@ -112,9 +112,9 @@ class TestHondaSafety(unittest.TestCase):
     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)
@@ -204,17 +204,22 @@ class TestHondaSafety(unittest.TestCase):
     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]:
@@ -252,27 +257,33 @@ class TestHondaSafety(unittest.TestCase):
     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
+    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)))
+            # 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)