diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 0438dd76a0..60f3a94e48 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/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,14 +167,19 @@ class CarController(): if (frame % 2) == 0: idx = frame // 2 ts = frame * DT_CTRL - 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)) - self.apply_brake_last = apply_brake - - if CS.CP.enableGasInterceptor: - # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. - # This prevents unexpected pedal range rescaling - can_sends.append(create_gas_command(self.packer, apply_gas, idx)) + 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)) + self.apply_brake_last = apply_brake + + if CS.CP.enableGasInterceptor: + # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. + # This prevents unexpected pedal range rescaling + can_sends.append(create_gas_command(self.packer, apply_gas, idx)) return can_sends diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 4aaa98aa66..77f74681b6 100644 --- a/selfdrive/car/honda/carstate.py +++ b/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) diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 178a452ec6..45aea3fc1c 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/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,27 +104,40 @@ 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) - - if car_fingerprint not in HONDA_BOSCH: - acc_hud_values = { - 'PCM_SPEED': pcm_speed * CV.MS_TO_KPH, - 'PCM_GAS': hud.pcm_accel, - 'CRUISE_SPEED': hud.v_cruise, - 'ENABLE_MINI_CAR': 1, - 'HUD_LEAD': hud.car, - 'HUD_DISTANCE': 3, # max distance setting on display - 'IMPERIAL_UNIT': int(not is_metric), - 'SET_ME_X01_2': 1, - 'SET_ME_X01': 1, - "FCM_OFF": stock_hud["FCM_OFF"], - "FCM_OFF_2": stock_hud["FCM_OFF_2"], - "FCM_PROBLEM": stock_hud["FCM_PROBLEM"], - "ICONS": stock_hud["ICONS"], - } + radar_disabled = car_fingerprint in HONDA_BOSCH and openpilot_longitudinal_control + bus_lkas = get_lkas_cmd_bus(car_fingerprint, has_relay, radar_disabled) + + 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, + 'CRUISE_SPEED': hud.v_cruise, + 'ENABLE_MINI_CAR': 1, + 'HUD_LEAD': hud.car, + 'HUD_DISTANCE': 3, # max distance setting on display + 'IMPERIAL_UNIT': int(not is_metric), + 'SET_ME_X01_2': 1, + 'SET_ME_X01': 1, + "FCM_OFF": stock_hud["FCM_OFF"], + "FCM_OFF_2": stock_hud["FCM_OFF_2"], + "FCM_PROBLEM": stock_hud["FCM_PROBLEM"], + "ICONS": stock_hud["ICONS"], + } commands.append(packer.make_can_msg("ACC_HUD", bus_pt, acc_hud_values, idx)) lkas_hud_values = { @@ -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 diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index ac8bfe3b9f..d74db1b1fb 100755 --- a/selfdrive/car/honda/interface.py +++ b/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) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 347a4695bb..fe576192e7 100755 --- a/selfdrive/controls/controlsd.py +++ b/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): diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 808b04c340..c2643d76e5 100755 --- a/selfdrive/controls/radard.py +++ b/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)