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

FrogPilot 0.9.7

Browse Source
This commit is contained in:
FrogAi
2024-06-27 10:22:08 -07:00
parent da00915ac8
commit b705b02e70
682 changed files with 181798 additions and 1348 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/gm/interface.py
+80 -22
View File
@@ -1,6 +1,6 @@
#!/usr/bin/env python3
import os
from cereal import car
from cereal import car, custom
from math import fabs, exp
from panda import Panda
@@ -12,7 +12,10 @@ from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerP
from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LatControlInputs, NanoFFModel
from openpilot.selfdrive.controls.lib.drive_helpers import get_friction
from openpilot.selfdrive.frogpilot.frogpilot_variables import params
ButtonType = car.CarState.ButtonEvent.Type
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
GearShifter = car.CarState.GearShifter
TransmissionType = car.CarParams.TransmissionType
@@ -93,7 +96,9 @@ class CarInterface(CarInterfaceBase):
return self.torque_from_lateral_accel_linear
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs):
use_new_api = params.get_bool("NewLongAPIGM")
ret.carName = "gm"
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)]
ret.autoResumeSng = False
@@ -107,7 +112,14 @@ class CarInterface(CarInterfaceBase):
else:
ret.transmissionType = TransmissionType.automatic
ret.longitudinalTuning.kiBP = [5., 35.]
if use_new_api:
ret.longitudinalTuning.kiBP = [5., 35.]
else:
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.15]
ret.longitudinalTuning.kpBP = [5., 35.]
ret.longitudinalTuning.kiBP = [0.]
if candidate in CAMERA_ACC_CAR:
ret.experimentalLongitudinalAvailable = candidate not in CC_ONLY_CAR
@@ -119,7 +131,14 @@ class CarInterface(CarInterfaceBase):
ret.minSteerSpeed = 10 * CV.KPH_TO_MS
# Tuning for experimental long
ret.longitudinalTuning.kiV = [2.0, 1.5]
if use_new_api:
ret.longitudinalTuning.kiV = [2.0, 1.5]
ret.vEgoStopping = 0.1
ret.vEgoStarting = 0.1
else:
ret.longitudinalTuning.kpV = [2.0, 1.5]
ret.longitudinalTuning.kiV = [0.72]
ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
@@ -130,6 +149,8 @@ class CarInterface(CarInterfaceBase):
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG
elif candidate in SDGM_CAR:
if use_new_api:
ret.longitudinalTuning.kiV = [0., 0.] # TODO: tuning
ret.experimentalLongitudinalAvailable = False
ret.networkLocation = NetworkLocation.fwdCamera
ret.pcmCruise = True
@@ -139,7 +160,7 @@ class CarInterface(CarInterfaceBase):
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_SDGM
else: # ASCM, OBD-II harness
ret.openpilotLongitudinalControl = True
ret.openpilotLongitudinalControl = not disable_openpilot_long
ret.networkLocation = NetworkLocation.gateway
ret.radarUnavailable = RADAR_HEADER_MSG not in fingerprint[CanBus.OBSTACLE] and not docs
ret.pcmCruise = False # stock non-adaptive cruise control is kept off
@@ -148,7 +169,11 @@ class CarInterface(CarInterfaceBase):
ret.minSteerSpeed = 7 * CV.MPH_TO_MS
# Tuning
ret.longitudinalTuning.kiV = [2.4, 1.5]
if use_new_api:
ret.longitudinalTuning.kiV = [2.4, 1.5]
else:
ret.longitudinalTuning.kpV = [2.4, 1.5]
ret.longitudinalTuning.kiV = [0.36]
if ret.enableGasInterceptor:
# Need to set ASCM long limits when using pedal interceptor, instead of camera ACC long limits
@@ -248,12 +273,19 @@ class CarInterface(CarInterfaceBase):
elif candidate == CAR.CADILLAC_CT6_CC:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CHEVROLET_MALIBU_CC:
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CHEVROLET_TRAX:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if ret.enableGasInterceptor:
ret.networkLocation = NetworkLocation.fwdCamera
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM
ret.minEnableSpeed = -1
ret.pcmCruise = False
ret.openpilotLongitudinalControl = True
ret.openpilotLongitudinalControl = not disable_openpilot_long
ret.stoppingControl = True
ret.autoResumeSng = True
@@ -261,8 +293,14 @@ class CarInterface(CarInterfaceBase):
ret.flags |= GMFlags.PEDAL_LONG.value
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_PEDAL_LONG
# Note: Low speed, stop and go not tested. Should be fairly smooth on highway
ret.longitudinalTuning.kiBP = [0.0, 5., 35.]
ret.longitudinalTuning.kiV = [0.0, 0.35, 0.5]
if use_new_api:
ret.longitudinalTuning.kiBP = [0.0, 5., 35.]
ret.longitudinalTuning.kiV = [0.0, 0.35, 0.5]
else:
ret.longitudinalTuning.kpBP = [5., 35.]
ret.longitudinalTuning.kpV = [0.35, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.0]
ret.longitudinalTuning.kiV = [0.1, 0.1]
ret.longitudinalTuning.kf = 0.15
ret.stoppingDecelRate = 0.8
else: # Pedal used for SNG, ACC for longitudinal control otherwise
@@ -277,17 +315,24 @@ class CarInterface(CarInterfaceBase):
ret.radarUnavailable = True
ret.experimentalLongitudinalAvailable = False
ret.minEnableSpeed = 24 * CV.MPH_TO_MS
ret.openpilotLongitudinalControl = True
ret.openpilotLongitudinalControl = not disable_openpilot_long
ret.pcmCruise = False
if use_new_api:
ret.longitudinalTuning.kiBP = [10.7, 10.8, 28.]
ret.longitudinalTuning.kiV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed
else:
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.56] # == 2 km/h/s, 1.25 mph/s
ret.longitudinalActuatorDelay = 1. # TODO: measure this
ret.longitudinalTuning.kpBP = [10.7, 10.8, 28.] # 10.7 m/s == 24 mph
ret.longitudinalTuning.kpV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.1]
ret.stoppingDecelRate = 11.18 # == 25 mph/s (.04 rate)
ret.longitudinalActuatorDelayLowerBound = 1. # TODO: measure this
ret.longitudinalActuatorDelayUpperBound = 2.
ret.longitudinalTuning.kiBP = [10.7, 10.8, 28.]
ret.longitudinalTuning.kiV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed
if candidate in CC_ONLY_CAR:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_NO_ACC
@@ -302,8 +347,8 @@ class CarInterface(CarInterfaceBase):
return ret
# returns a car.CarState
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback)
def _update(self, c, frogpilot_toggles):
ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback, frogpilot_toggles)
# Don't add event if transitioning from INIT, unless it's to an actual button
if self.CS.cruise_buttons != CruiseButtons.UNPRESS or self.CS.prev_cruise_buttons != CruiseButtons.INIT:
@@ -311,7 +356,9 @@ class CarInterface(CarInterfaceBase):
*create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT,
unpressed_btn=CruiseButtons.UNPRESS),
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button,
{1: ButtonType.gapAdjustCruise})
{1: ButtonType.gapAdjustCruise}),
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled,
{1: FrogPilotButtonType.lkas}),
]
# The ECM allows enabling on falling edge of set, but only rising edge of resume
@@ -328,10 +375,21 @@ class CarInterface(CarInterfaceBase):
if below_min_enable_speed and not (ret.standstill and ret.brake >= 20 and
(self.CP.networkLocation == NetworkLocation.fwdCamera and not self.CP.carFingerprint in SDGM_CAR)):
events.add(EventName.belowEngageSpeed)
if ret.cruiseState.standstill and not self.CP.autoResumeSng:
if ret.cruiseState.standstill and not (self.CP.autoResumeSng or self.disable_resumeRequired):
events.add(EventName.resumeRequired)
if ret.vEgo < self.CP.minSteerSpeed:
self.resumeRequired_shown = True
# Disable the "resumeRequired" event after it's been shown once to not annoy the driver
if self.resumeRequired_shown and not ret.cruiseState.standstill:
self.disable_resumeRequired = True
if ret.vEgo < self.CP.minSteerSpeed and not self.disable_belowSteerSpeed:
events.add(EventName.belowSteerSpeed)
self.belowSteerSpeed_shown = True
# Disable the "belowSteerSpeed" event after it's been shown once to not annoy the driver
if self.belowSteerSpeed_shown and ret.vEgo >= self.CP.minSteerSpeed:
self.disable_belowSteerSpeed = True
if (self.CP.flags & GMFlags.CC_LONG.value) and ret.vEgo < self.CP.minEnableSpeed and ret.cruiseState.enabled:
events.add(EventName.speedTooLow)
@@ -344,4 +402,4 @@ class CarInterface(CarInterfaceBase):
ret.events = events.to_msg()
return ret
return ret, fp_ret