From 864f74307af947e7dcff93a15d7a290c952f0b38 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 2 Apr 2024 14:20:41 -0700 Subject: [PATCH] GM long pitch compensation Co-Authored-By: Tim Wilson <7284371+twilsonco@users.noreply.github.com> --- common/params.cc | 1 + selfdrive/car/gm/carcontroller.py | 29 +++++++++++++++++-- selfdrive/controls/controlsd.py | 2 ++ .../ui/qt/offroad/vehicle_settings.cc | 4 ++- .../ui/qt/offroad/vehicle_settings.h | 2 +- 5 files changed, 33 insertions(+), 5 deletions(-) diff --git a/common/params.cc b/common/params.cc index d0ea870cc0..082015b2d6 100644 --- a/common/params.cc +++ b/common/params.cc @@ -285,6 +285,7 @@ std::unordered_map keys = { {"LateralTune", PERSISTENT}, {"LeadInfo", PERSISTENT}, {"LongitudinalTune", PERSISTENT}, + {"LongPitch", PERSISTENT}, {"LowVoltageShutdown", PERSISTENT}, {"ManualUpdateInitiated", PERSISTENT}, {"ModelUI", PERSISTENT}, diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 565384e0d5..a1ba626edd 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -1,5 +1,6 @@ from cereal import car from openpilot.common.conversions import Conversions as CV +from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.numpy_fast import interp from openpilot.common.realtime import DT_CTRL from opendbc.can.packer import CANPacker @@ -7,6 +8,8 @@ from openpilot.selfdrive.car import apply_driver_steer_torque_limits from openpilot.selfdrive.car.gm import gmcan from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR 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 NetworkLocation = car.CarParams.NetworkLocation @@ -19,6 +22,10 @@ CAMERA_CANCEL_DELAY_FRAMES = 10 # Enforce a minimum interval between steering messages to avoid a fault 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): def __init__(self, dbc_name, CP, VM): @@ -44,9 +51,12 @@ class CarController(CarControllerBase): self.packer_ch = CANPacker(DBC[self.CP.carFingerprint]['chassis']) # FrogPilot variables + self.pitch = FirstOrderFilter(0., 0.09 * 4, DT_CTRL * 4) # runs at 25 Hz + self.accel_g = 0.0 def update(self, CC, CS, now_nanos, frogpilot_variables): actuators = CC.actuators + accel = actuators.accel hud_control = CC.hudControl hud_alert = hud_control.visualAlert hud_v_cruise = hud_control.setSpeed @@ -93,16 +103,25 @@ class CarController(CarControllerBase): # Gas/regen, brakes, and UI commands - all at 25Hz if self.frame % 4 == 0: 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 + if not CC.longActive: # ASCM sends max regen when not enabled self.apply_gas = self.params.INACTIVE_REGEN self.apply_brake = 0 else: 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: - self.apply_gas = int(round(interp(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_gas = int(round(interp(accel if frogpilot_variables.long_pitch else actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) + brake_accel = actuators.accel + self.accel_g * interp(CS.out.vEgo, BRAKE_PITCH_FACTOR_BP, BRAKE_PITCH_FACTOR_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 # FIXME: brakes aren't applied immediately when enabling at a stop if stopping: @@ -133,6 +152,9 @@ class CarController(CarControllerBase): send_fcw = hud_alert == VisualAlert.fcw 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)) + 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), # and that ADAS is alive (10hz) @@ -173,6 +195,7 @@ class CarController(CarControllerBase): can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status)) new_actuators = actuators.copy() + new_actuators.accel = accel new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last new_actuators.gas = self.apply_gas diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 9aeebd3c64..8ce14dfc49 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -942,6 +942,8 @@ class Controls: lateral_tune = self.params.get_bool("LateralTune") self.force_auto_tune = lateral_tune and self.params.get_float("ForceAutoTune") + self.frogpilot_variables.long_pitch = self.params.get_bool("LongPitch") + longitudinal_tune = self.CP.openpilotLongitudinalControl and self.params.get_bool("LongitudinalTune") self.frogpilot_variables.sport_plus = longitudinal_tune and self.params.get_int("AccelerationProfile") == 3 diff --git a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc index a1518f800d..8539e4e77f 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc @@ -112,6 +112,8 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil addItem(disableOpenpilotLong); std::vector> vehicleToggles { + {"LongPitch", tr("Long Pitch Compensation"), tr("Smoothen out the gas and pedal controls."), ""}, + {"LongitudinalTune", tr("Longitudinal Tune"), tr("Use a custom Toyota longitudinal tune.\n\nCydia = More focused on TSS-P vehicles but works for all Toyotas\n\nDragonPilot = Focused on TSS2 vehicles\n\nFrogPilot = Takes the best of both worlds with some personal tweaks focused around FrogsGoMoo's 2019 Lexus ES 350"), ""}, }; @@ -248,7 +250,7 @@ void FrogPilotVehiclesPanel::hideToggles() { std::set evCarKeys = {}; std::set gmTruckKeys = {}; std::set imprezaKeys = {}; - std::set longitudinalKeys = {"LongitudinalTune"}; + std::set longitudinalKeys = {"LongitudinalTune", "LongPitch"}; std::set sngKeys = {}; for (auto &[key, toggle] : toggles) { diff --git a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h index 2f4b5eac62..78ef149337 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h @@ -29,7 +29,7 @@ private: QString carMake; QStringList models; - std::set gmKeys = {}; + std::set gmKeys = {"LongPitch"}; std::set subaruKeys = {}; std::set toyotaKeys = {"LongitudinalTune"};