pre-reqs for honda bosch longitudinal control (#1458)

* pre-reqs for honda bosch longitudinal control

* openpilot_longitudinal_control -> radar_disabled

* fix chrysler test

* review feedback

* little cleaner

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

selfdrive/car/honda/carcontroller.py

@@ -5,7 +5,7 @@ from selfdrive.controls.lib.drive_helpers import rate_limit
 from common.numpy_fast import clip, interp
 from selfdrive.car import create_gas_command
 from selfdrive.car.honda import hondacan
-from selfdrive.car.honda.values import CruiseButtons, CAR, VISUAL_HUD
+from selfdrive.car.honda.values import CruiseButtons, CAR, VISUAL_HUD, HONDA_BOSCH
 from opendbc.can.packer import CANPacker
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -135,8 +135,6 @@ class CarController():
     # **** process the car messages ****
 
     # steer torque is converted back to CAN reference (positive when steering right)
-    apply_gas = clip(actuators.gas, 0., 1.)
-    apply_brake = int(clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1))
     apply_steer = int(interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V))
 
     lkas_active = enabled and not CS.steer_not_allowed
@@ -147,14 +145,14 @@ class CarController():
     # Send steering command.
     idx = frame % 4
     can_sends.append(hondacan.create_steering_control(self.packer, apply_steer,
-      lkas_active, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack))
+      lkas_active, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack, CS.CP.openpilotLongitudinalControl))
 
     # Send dashboard UI commands.
     if (frame % 10) == 0:
       idx = (frame//10) % 4
-      can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack, CS.stock_hud))
+      can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack, CS.CP.openpilotLongitudinalControl, CS.stock_hud))
 
-    if CS.CP.radarOffCan:
+    if not CS.CP.openpilotLongitudinalControl:
       if (frame % 2) == 0:
         idx = frame // 2
         can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack))
@@ -169,6 +167,11 @@ class CarController():
       if (frame % 2) == 0:
         idx = frame // 2
         ts = frame * DT_CTRL
+        if CS.CP.carFingerprint in HONDA_BOSCH:
+          pass # TODO: implement
+        else:
+          apply_gas = clip(actuators.gas, 0., 1.)
+          apply_brake = int(clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1))
           pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
           can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
             pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack, CS.stock_brake))

selfdrive/car/honda/carstate.py

@@ -79,7 +79,7 @@ def get_can_signals(CP):
       ("GEARBOX", 100),
     ]
 
-  if CP.radarOffCan:
+  if CP.carFingerprint in HONDA_BOSCH:
     # Civic is only bosch to use the same brake message as other hondas.
     if CP.carFingerprint not in (CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT):
       signals += [("BRAKE_PRESSED", "BRAKE_MODULE", 0)]
@@ -90,6 +90,10 @@ def get_can_signals(CP):
                 ("EPB_STATE", "EPB_STATUS", 0),
                 ("CRUISE_SPEED", "ACC_HUD", 0)]
     checks += [("GAS_PEDAL_2", 100)]
+    if CP.openpilotLongitudinalControl:
+      signals += [("BRAKE_ERROR_1", "STANDSTILL", 1),
+                  ("BRAKE_ERROR_2", "STANDSTILL", 1)]
+      checks += [("STANDSTILL", 50)]
   else:
     # Nidec signals.
     signals += [("BRAKE_ERROR_1", "STANDSTILL", 1),
@@ -206,7 +210,7 @@ class CarState(CarStateBase):
     # LOW_SPEED_LOCKOUT is not worth a warning
     ret.steerWarning = steer_status not in ['NORMAL', 'LOW_SPEED_LOCKOUT', 'NO_TORQUE_ALERT_2']
 
-    if self.CP.radarOffCan:
+    if not self.CP.openpilotLongitudinalControl:
       self.brake_error = 0
     else:
       self.brake_error = cp.vl["STANDSTILL"]['BRAKE_ERROR_1'] or cp.vl["STANDSTILL"]['BRAKE_ERROR_2']
@@ -270,7 +274,7 @@ class CarState(CarStateBase):
 
     self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != 0
 
-    if self.CP.radarOffCan:
+    if self.CP.carFingerprint in HONDA_BOSCH:
       self.cruise_mode = cp.vl["ACC_HUD"]['CRUISE_CONTROL_LABEL']
       ret.cruiseState.standstill = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252.
       ret.cruiseState.speedOffset = calc_cruise_offset(0, ret.vEgo)

selfdrive/car/honda/hondacan.py

@@ -1,12 +1,26 @@
 from selfdrive.config import Conversions as CV
 from selfdrive.car.honda.values import HONDA_BOSCH
 
+# CAN bus layout with relay
+# 0 = ACC-CAN - radar side
+# 1 = F-CAN B - powertrain
+# 2 = ACC-CAN - camera side
+# 3 = F-CAN A - OBDII port
+
+# CAN bus layout with giraffe
+# 0 = F-CAN B - powertrain
+# 1 = ACC-CAN - camera side
+# 2 = ACC-CAN - radar side
 
 def get_pt_bus(car_fingerprint, has_relay):
   return 1 if car_fingerprint in HONDA_BOSCH and has_relay else 0
 
 
-def get_lkas_cmd_bus(car_fingerprint, has_relay):
+def get_lkas_cmd_bus(car_fingerprint, has_relay, radar_disabled=False):
+  if radar_disabled:
+    # when radar is disabled, steering commands are sent directly to powertrain bus
+    return get_pt_bus(car_fingerprint, has_relay)
+  # normally steering commands are sent to radar, which forwards them to powertrain bus
   return 2 if car_fingerprint in HONDA_BOSCH and not has_relay else 0
 
 
@@ -35,12 +49,47 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_
   return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx)
 
 
-def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, has_relay):
+def create_acc_commands(packer, enabled, accel, gas, idx, stopping, starting, car_fingerprint, has_relay):
+  commands = []
+  bus = get_pt_bus(car_fingerprint, has_relay)
+
+  control_on = 5 if enabled else 0
+  # no gas = -30000
+  gas_command = gas if enabled and gas > 0 else -30000
+  accel_command = accel if enabled else 0
+  braking = 1 if enabled and accel < 0 else 0
+  standstill = 1 if enabled and stopping else 0
+  standstill_release = 1 if enabled and starting else 0
+
+  acc_control_values = {
+    # setting CONTROL_ON causes car to set POWERTRAIN_DATA->ACC_STATUS = 1
+    "CONTROL_ON": control_on,
+    "GAS_COMMAND": gas_command, # used for gas
+    "ACCEL_COMMAND": accel_command, # used for brakes
+    "BRAKE_LIGHTS": braking,
+    "BRAKE_REQUEST": braking,
+    "STANDSTILL": standstill,
+    "STANDSTILL_RELEASE": standstill_release,
+  }
+  commands.append(packer.make_can_msg("ACC_CONTROL", bus, acc_control_values, idx))
+
+  acc_control_on_values = {
+    "SET_TO_3": 0x03,
+    "CONTROL_ON": enabled,
+    "SET_TO_FF": 0xff,
+    "SET_TO_75": 0x75,
+    "SET_TO_30": 0x30,
+  }
+  commands.append(packer.make_can_msg("ACC_CONTROL_ON", bus, acc_control_on_values, idx))
+
+  return commands
+
+def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, has_relay, radar_disabled):
   values = {
     "STEER_TORQUE": apply_steer if lkas_active else 0,
     "STEER_TORQUE_REQUEST": lkas_active,
   }
-  bus = get_lkas_cmd_bus(car_fingerprint, has_relay)
+  bus = get_lkas_cmd_bus(car_fingerprint, has_relay, radar_disabled)
   return packer.make_can_msg("STEERING_CONTROL", bus, values, idx)
 
 
@@ -55,12 +104,25 @@ def create_bosch_supplemental_1(packer, car_fingerprint, idx, has_relay):
   return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values, idx)
 
 
-def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay, stock_hud):
+def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay, openpilot_longitudinal_control, stock_hud):
   commands = []
   bus_pt = get_pt_bus(car_fingerprint, has_relay)
-  bus_lkas = get_lkas_cmd_bus(car_fingerprint, has_relay)
+  radar_disabled = car_fingerprint in HONDA_BOSCH and openpilot_longitudinal_control
+  bus_lkas = get_lkas_cmd_bus(car_fingerprint, has_relay, radar_disabled)
 
-  if car_fingerprint not in HONDA_BOSCH:
+  if openpilot_longitudinal_control:
+    if car_fingerprint in HONDA_BOSCH:
+      acc_hud_values = {
+        'CRUISE_SPEED': hud.v_cruise,
+        'ENABLE_MINI_CAR': 1,
+        'SET_TO_1': 1,
+        'HUD_LEAD': hud.car,
+        'HUD_DISTANCE': 3,
+        'ACC_ON': hud.car != 0,
+        'SET_TO_X1': 1,
+        'IMPERIAL_UNIT': int(not is_metric),
+      }
+    else:
       acc_hud_values = {
         'PCM_SPEED': pcm_speed * CV.MS_TO_KPH,
         'PCM_GAS': hud.pcm_accel,
@@ -87,6 +149,12 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx,
   }
   commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values, idx))
 
+  if radar_disabled and car_fingerprint in HONDA_BOSCH:
+    radar_hud_values = {
+      'SET_TO_1' : 0x01,
+    }
+    commands.append(packer.make_can_msg('RADAR_HUD', bus_pt, radar_hud_values, idx))
+
   return commands
 
 

selfdrive/car/honda/interface.py

@@ -478,7 +478,7 @@ class CarInterface(CarInterfaceBase):
     events = self.create_common_events(ret, pcm_enable=False)
     if self.CS.brake_error:
       events.add(EventName.brakeUnavailable)
-    if self.CS.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH:
+    if self.CS.brake_hold and self.CS.CP.openpilotLongitudinalControl:
       events.add(EventName.brakeHold)
     if self.CS.park_brake:
       events.add(EventName.parkBrake)

selfdrive/controls/controlsd.py

@@ -234,7 +234,7 @@ class Controls:
 
     # Only allow engagement with brake pressed when stopped behind another stopped car
     if CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \
-      and not self.CP.radarOffCan and CS.vEgo < 0.3:
+      and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
       self.events.add(EventName.noTarget)
 
   def data_sample(self):

selfdrive/controls/radard.py

@@ -99,7 +99,7 @@ class RadarD():
 
     self.ready = False
 
-  def update(self, frame, sm, rr, has_radar):
+  def update(self, frame, sm, rr, enable_lead):
     self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()])
 
     if sm.updated['controlsState']:
@@ -166,7 +166,7 @@ class RadarD():
     dat.radarState.radarErrors = list(rr.errors)
     dat.radarState.controlsStateMonoTime = sm.logMonoTime['controlsState']
 
-    if has_radar:
+    if enable_lead:
       dat.radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True)
       dat.radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False)
     return dat
@@ -200,7 +200,8 @@ def radard_thread(sm=None, pm=None, can_sock=None):
   rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
   RD = RadarD(CP.radarTimeStep, RI.delay)
 
-  has_radar = not CP.radarOffCan
+  # TODO: always log leads once we can hide them conditionally
+  enable_lead = CP.openpilotLongitudinalControl or not CP.radarOffCan
 
   while 1:
     can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
@@ -211,7 +212,7 @@ def radard_thread(sm=None, pm=None, can_sock=None):
 
     sm.update(0)
 
-    dat = RD.update(rk.frame, sm, rr, has_radar)
+    dat = RD.update(rk.frame, sm, rr, enable_lead)
     dat.radarState.cumLagMs = -rk.remaining*1000.
 
     pm.send('radarState', dat)