mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-10 10:13:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Honda2

Browse Source
This commit is contained in:
firestar5683
2025-10-08 22:49:50 -05:00
parent 93713aaacf
commit 9bcfeb1cf3
3 changed files with 36 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/honda/carcontroller.py
+29 -3
View File
@@ -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]),
k_f=1, 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))
selfdrive/car/honda/hondacan.py
+3 -3
View File
@@ -76,14 +76,14 @@ def create_brake_command(packer, CAN, apply_brake, pump_on, pcm_override, pcm_ca
return packer.make_can_msg("BRAKE_COMMAND", CAN.pt, values)
def create_acc_commands(packer, CAN, enabled, active, accel, gas, stopping_counter, car_fingerprint):
def create_acc_commands(packer, CAN, enabled, active, accel, gas, stopping_counter, car_fingerprint, gas_force):
commands = []
min_gas_accel = CarControllerParams.BOSCH_GAS_LOOKUP_BP[0]
control_on = 5 if enabled else 0
gas_command = gas if active and accel > min_gas_accel else -30000
gas_command = gas if active and gas_force > min_gas_accel else -30000
accel_command = accel if active else 0
braking = 1 if active and accel < min_gas_accel else 0
braking = 1 if active and gas_force < min_gas_accel else 0
standstill = 1 if active and stopping_counter > 0 else 0
standstill_release = 1 if active and stopping_counter == 0 else 0
selfdrive/car/honda/interface.py
+4 -4
View File
@@ -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: