diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 1151c5c82..76fb47376 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -1,3 +1,4 @@ +import math from collections import namedtuple from cereal import car @@ -5,10 +6,12 @@ from openpilot.common.numpy_fast import clip, interp from openpilot.common.realtime import DT_CTRL from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import create_gas_interceptor_command +from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.selfdrive.car.honda import hondacan from openpilot.selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.controls.lib.drive_helpers import rate_limit +from openpilot.selfdrive.controls.lib.pid import PIDController VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState @@ -125,6 +128,10 @@ class CarController(CarControllerBase): self.gas = 0.0 self.brake = 0.0 self.last_steer = 0.0 + self.pitch = 0.0 + self.gasonly_pid = PIDController(k_p=([0,], [0,]), + k_i=([0., 5., 35.], [1.2, 0.8, 0.5]), + rate=1 / DT_CTRL / 2) def update(self, CC, CS, now_nanos, frogpilot_toggles): actuators = CC.actuators @@ -133,6 +140,9 @@ class CarController(CarControllerBase): hud_v_cruise = hud_control.setSpeed / conversion if hud_control.speedVisible else 255 pcm_cancel_cmd = CC.cruiseControl.cancel + if len(CC.orientationNED) == 3: + self.pitch = CC.orientationNED[1] + if CC.longActive: accel = actuators.accel gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, self.CP.carFingerprint) @@ -173,8 +183,11 @@ class CarController(CarControllerBase): CS.CP.openpilotLongitudinalControl)) # wind brake from air resistance decel at high speed - wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15]) + wind_brake_ms2 = interp(CS.out.vEgo, [0.0, 13.4, 22.4, 31.3, 40.2], [0.000, 0.049, 0.136, 0.267, 0.441]) # in m/s2 units + hill_brake = math.sin(self.pitch) * ACCELERATION_DUE_TO_GRAVITY + # all of this is only relevant for HONDA NIDEC + wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15]) # not in m/s2 units max_accel = interp(CS.out.vEgo, self.params.NIDEC_MAX_ACCEL_BP, self.params.NIDEC_MAX_ACCEL_V) # TODO this 1.44 is just to maintain previous behavior pcm_speed_BP = [-wind_brake, @@ -217,12 +230,25 @@ class CarController(CarControllerBase): if self.CP.carFingerprint in HONDA_BOSCH: self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX) - self.gas = interp(accel, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V) + + if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS: + gas_pedal_force = self.accel # radarless does not need a pid + elif (actuators.longControlState == LongCtrlState.pid) and not CS.out.gasPressed: # perform a gas-only pid + gas_error = self.accel - CS.out.aEgo + self.gasonly_pid.neg_limit = self.params.BOSCH_ACCEL_MIN + self.gasonly_pid.pos_limit = self.params.BOSCH_ACCEL_MAX + gas_pedal_force = self.gasonly_pid.update(gas_error, speed=CS.out.vEgo, feedforward=self.accel) + gas_pedal_force += wind_brake_ms2 + hill_brake + else: + gas_pedal_force = self.accel + self.gasonly_pid.reset() + gas_pedal_force += wind_brake_ms2 + hill_brake + self.gas = interp(gas_pedal_force, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V) stopping = actuators.longControlState == LongCtrlState.stopping self.stopping_counter = self.stopping_counter + 1 if stopping else 0 can_sends.extend(hondacan.create_acc_commands(self.packer, self.CAN, CC.enabled, CC.longActive, self.accel, self.gas, - self.stopping_counter, self.CP.carFingerprint)) + self.stopping_counter, self.CP.carFingerprint, accel + wind_brake_ms2 + hill_brake)) else: apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) apply_brake = int(clip(apply_brake * self.params.NIDEC_BRAKE_MAX, 0, self.params.NIDEC_BRAKE_MAX - 1)) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index c075a759b..7ad80c847 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -85,10 +85,10 @@ class CarInterface(CarInterfaceBase): ret.longitudinalActuatorDelay = 0.5 # s if candidate in HONDA_BOSCH_RADARLESS: ret.stopAccel = CarControllerParams.BOSCH_ACCEL_MIN # stock uses -4.0 m/s^2 once stopped but limited by safety model - else: - # default longitudinal tuning for all hondas - ret.longitudinalTuning.kiBP = [0., 5., 35.] - ret.longitudinalTuning.kiV = [1.2, 0.8, 0.5] + + # default longitudinal tuning for all hondas + ret.longitudinalTuning.kiBP = [0., 5., 35.] + ret.longitudinalTuning.kiV = [1.2, 0.8, 0.5] eps_modified = False for fw in car_fw: @@ -117,6 +117,9 @@ class CarInterface(CarInterfaceBase): ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] + if candidate == CAR.HONDA_CIVIC_BOSCH: + CarControllerParams.BOSCH_GAS_LOOKUP_V = [0, 750] + elif candidate == CAR.HONDA_ACCORD: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end