zhudongjer/carrot
1
1
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Vehicles - GM - Long Pitch Compensation

Browse Source 
Co-Authored-By: Tim Wilson <7284371+twilsonco@users.noreply.github.com>
This commit is contained in:
FrogAi 2024-05-10 23:50:11 -07:00
parent 9e1968be87
commit d88d16f6cb
1 changed files with 30 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
selfdrive/car/gm/carcontroller.py
View File

@ -1,5 +1,6 @@
from cereal import car from cereal import car
from openpilot.common.conversions import Conversions as CV from openpilot.common.conversions import Conversions as CV
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.numpy_fast import interp, clip from openpilot.common.numpy_fast import interp, clip
from openpilot.common.params import Params from openpilot.common.params import Params
from openpilot.common.realtime import DT_CTRL from openpilot.common.realtime import DT_CTRL
@ -8,6 +9,8 @@ from openpilot.selfdrive.car import apply_driver_steer_torque_limits, create_gas
from openpilot.selfdrive.car.gm import gmcan from openpilot.selfdrive.car.gm import gmcan
from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_CAR from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_CAR
from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.interfaces import CarControllerBase
from openpilot.selfdrive.controls.lib.drive_helpers import apply_deadzone
from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
VisualAlert = car.CarControl.HUDControl.VisualAlert VisualAlert = car.CarControl.HUDControl.VisualAlert
NetworkLocation = car.CarParams.NetworkLocation NetworkLocation = car.CarParams.NetworkLocation
@ -20,6 +23,10 @@ CAMERA_CANCEL_DELAY_FRAMES = 10
# Enforce a minimum interval between steering messages to avoid a fault # Enforce a minimum interval between steering messages to avoid a fault
MIN_STEER_MSG_INTERVAL_MS = 15 MIN_STEER_MSG_INTERVAL_MS = 15
# Constants for pitch compensation
PITCH_DEADZONE = 0.01 # [radians] 0.01 ≈ 1% grade
BRAKE_PITCH_FACTOR_BP = [5., 10.] # [m/s] smoothly revert to planned accel at low speeds
BRAKE_PITCH_FACTOR_V = [0., 1.] # [unitless in [0,1]]; don't touch
class CarController(CarControllerBase): class CarController(CarControllerBase):
def __init__(self, dbc_name, CP, VM): def __init__(self, dbc_name, CP, VM):
@ -47,6 +54,9 @@ class CarController(CarControllerBase):
# FrogPilot variables # FrogPilot variables
self.params_ = Params() self.params_ = Params()
self.pitch = FirstOrderFilter(0., 0.09 * 4, DT_CTRL * 4) # runs at 25 Hz
self.accel_g = 0.0
@staticmethod @staticmethod
def calc_pedal_command(accel: float, long_active: bool) -> float: def calc_pedal_command(accel: float, long_active: bool) -> float:
if not long_active: return 0. if not long_active: return 0.
@ -64,6 +74,7 @@ class CarController(CarControllerBase):
def update(self, CC, CS, now_nanos, frogpilot_variables): def update(self, CC, CS, now_nanos, frogpilot_variables):
actuators = CC.actuators actuators = CC.actuators
accel = actuators.accel
hud_control = CC.hudControl hud_control = CC.hudControl
hud_alert = hud_control.visualAlert hud_alert = hud_control.visualAlert
hud_v_cruise = hud_control.setSpeed hud_v_cruise = hud_control.setSpeed
@ -110,6 +121,14 @@ class CarController(CarControllerBase):
# Gas/regen, brakes, and UI commands - all at 25Hz # Gas/regen, brakes, and UI commands - all at 25Hz
if self.frame % 4 == 0: if self.frame % 4 == 0:
stopping = actuators.longControlState == LongCtrlState.stopping stopping = actuators.longControlState == LongCtrlState.stopping
# Pitch compensated acceleration;
# TODO: include future pitch (sm['modelDataV2'].orientation.y) to account for long actuator delay
if frogpilot_variables.long_pitch and len(CC.orientationNED) > 1:
self.pitch.update(CC.orientationNED[1])
self.accel_g = ACCELERATION_DUE_TO_GRAVITY * apply_deadzone(self.pitch.x, PITCH_DEADZONE) # driving uphill is positive pitch
accel += self.accel_g
at_full_stop = CC.longActive and CS.out.standstill at_full_stop = CC.longActive and CS.out.standstill
near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE) near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE)
interceptor_gas_cmd = 0 interceptor_gas_cmd = 0
@ -122,19 +141,20 @@ class CarController(CarControllerBase):
self.apply_brake = int(min(-100 * self.CP.stopAccel, self.params.MAX_BRAKE)) self.apply_brake = int(min(-100 * self.CP.stopAccel, self.params.MAX_BRAKE))
else: else:
# Normal operation # Normal operation
brake_accel = actuators.accel + self.accel_g * interp(CS.out.vEgo, BRAKE_PITCH_FACTOR_BP, BRAKE_PITCH_FACTOR_V)
if self.CP.carFingerprint in EV_CAR: if self.CP.carFingerprint in EV_CAR:
self.params.update_ev_gas_brake_threshold(CS.out.vEgo) self.params.update_ev_gas_brake_threshold(CS.out.vEgo)
if frogpilot_variables.sport_plus: if frogpilot_variables.sport_plus:
self.apply_gas = int(round(interp(actuators.accel, self.params.EV_GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS))) self.apply_gas = int(round(interp(accel if frogpilot_variables.long_pitch else actuators.accel, self.params.EV_GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS)))
else: else:
self.apply_gas = int(round(interp(actuators.accel, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) self.apply_gas = int(round(interp(accel if frogpilot_variables.long_pitch else actuators.accel, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
self.apply_brake = int(round(interp(actuators.accel, self.params.EV_BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) self.apply_brake = int(round(interp(brake_accel if frogpilot_variables.long_pitch else actuators.accel, self.params.EV_BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
else: else:
if frogpilot_variables.sport_plus: if frogpilot_variables.sport_plus:
self.apply_gas = int(round(interp(actuators.accel, self.params.GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS))) self.apply_gas = int(round(interp(accel if frogpilot_variables.long_pitch else actuators.accel, self.params.GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS)))
else: else:
self.apply_gas = int(round(interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) self.apply_gas = int(round(interp(accel if frogpilot_variables.long_pitch else actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
self.apply_brake = int(round(interp(actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) self.apply_brake = int(round(interp(brake_accel if frogpilot_variables.long_pitch else actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
# Don't allow any gas above inactive regen while stopping # Don't allow any gas above inactive regen while stopping
# FIXME: brakes aren't applied immediately when enabling at a stop # FIXME: brakes aren't applied immediately when enabling at a stop
if stopping: if stopping:
@ -178,6 +198,9 @@ class CarController(CarControllerBase):
send_fcw = hud_alert == VisualAlert.fcw send_fcw = hud_alert == VisualAlert.fcw
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled, can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled,
hud_v_cruise * CV.MS_TO_KPH, hud_control, send_fcw)) hud_v_cruise * CV.MS_TO_KPH, hud_control, send_fcw))
else:
# to keep accel steady for logs when not sending gas
accel += self.accel_g
# Radar needs to know current speed and yaw rate (50hz), # Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz) # and that ADAS is alive (10hz)
@ -227,6 +250,7 @@ class CarController(CarControllerBase):
can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status)) can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status))
new_actuators = actuators.copy() new_actuators = actuators.copy()
new_actuators.accel = accel
new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX
new_actuators.steerOutputCan = self.apply_steer_last new_actuators.steerOutputCan = self.apply_steer_last
new_actuators.gas = self.apply_gas new_actuators.gas = self.apply_gas