From 28a898f8743f688cb4818f5df7e60e9c9d48d371 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 27 Feb 2024 16:34:47 -0700 Subject: [PATCH] Use EV gas lookup tables Co-Authored-By: Tim Wilson <7284371+twilsonco@users.noreply.github.com> Co-Authored-By: Eric Brown <13560103+nworb-cire@users.noreply.github.com> --- common/params.cc | 1 + selfdrive/car/gm/carcontroller.py | 18 +++++++++++++----- selfdrive/car/gm/interface.py | 16 +++++++++++++++- selfdrive/car/gm/values.py | 13 +++++++++++++ selfdrive/controls/controlsd.py | 1 + selfdrive/frogpilot/ui/vehicle_settings.cc | 1 + selfdrive/frogpilot/ui/vehicle_settings.h | 2 +- 7 files changed, 45 insertions(+), 7 deletions(-) diff --git a/common/params.cc b/common/params.cc index b099579..6fd6c92 100644 --- a/common/params.cc +++ b/common/params.cc @@ -268,6 +268,7 @@ std::unordered_map keys = { {"DriveStats", PERSISTENT}, {"DynamicPathWidth", PERSISTENT}, {"EngageVolume", PERSISTENT}, + {"EVTable", PERSISTENT}, {"ExperimentalModeActivation", PERSISTENT}, {"ExperimentalModeViaDistance", PERSISTENT}, {"ExperimentalModeViaLKAS", PERSISTENT}, diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 486b1be..9ebb21a 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -6,7 +6,7 @@ from openpilot.common.realtime import DT_CTRL from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits, create_gas_interceptor_command 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.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, EV_CAR, SDGM_CAR from openpilot.selfdrive.controls.lib.drive_helpers import apply_deadzone from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY @@ -138,11 +138,19 @@ class CarController: else: # Normal operation brake_accel = actuators.accel + self.accel_g * interp(CS.out.vEgo, BRAKE_PITCH_FACTOR_BP, BRAKE_PITCH_FACTOR_V) - if frogpilot_variables.sport_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))) + if self.CP.carFingerprint in EV_CAR and frogpilot_variables.use_ev_tables: + self.params.update_ev_gas_brake_threshold(CS.out.vEgo) + if frogpilot_variables.sport_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: + 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(brake_accel if frogpilot_variables.long_pitch else actuators.accel, self.params.EV_BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) else: - 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(brake_accel if frogpilot_variables.long_pitch else actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) + if frogpilot_variables.sport_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(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(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: diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 7f41366..fcce13e 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -102,6 +102,8 @@ class CarInterface(CarInterfaceBase): ret.enableGasInterceptor = True ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_GAS_INTERCEPTOR + useEVTables = params.get_bool("EVTable") + if candidate in EV_CAR: ret.transmissionType = TransmissionType.direct else: @@ -154,7 +156,7 @@ class CarInterface(CarInterfaceBase): ret.pcmCruise = False # stock non-adaptive cruise control is kept off # supports stop and go, but initial engage must (conservatively) be above 18mph ret.minEnableSpeed = 18 * CV.MPH_TO_MS - ret.minSteerSpeed = 7 * CV.MPH_TO_MS + ret.minSteerSpeed = (6.7 if useEVTables else 7) * CV.MPH_TO_MS # Tuning ret.longitudinalTuning.kpV = [2.4, 1.5] @@ -189,6 +191,18 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_volt() ret.steerActuatorDelay = 0.2 + # softer long tune for ev table + if useEVTables: + ret.longitudinalTuning.kpBP = [5., 15., 35.] + ret.longitudinalTuning.kpV = [0.65, .9, 0.8] + ret.longitudinalTuning.kiBP = [5., 15.] + ret.longitudinalTuning.kiV = [0.04, 0.1] + ret.steerActuatorDelay = 0.18 + ret.stoppingDecelRate = 0.02 # brake_travel/s while trying to stop + ret.stopAccel = -0.5 + ret.startAccel = 0.8 + ret.vEgoStopping = 0.1 + elif candidate == CAR.MALIBU: ret.mass = 1496. ret.wheelbase = 2.83 diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index e6d467a..9af424e 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -4,6 +4,7 @@ from enum import Enum, IntFlag, StrEnum from typing import Dict, List, Union from cereal import car +from openpilot.common.numpy_fast import interp from openpilot.common.params import Params from openpilot.selfdrive.car import dbc_dict from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column @@ -81,6 +82,18 @@ class CarControllerParams: self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, max_regen_acceleration] self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.] + # determined by letting Volt regen to a stop in L gear from 89mph, + # and by letting off gas and allowing car to creep, for determining + # the positive threshold values at very low speed + EV_GAS_BRAKE_THRESHOLD_BP = [1.29, 1.52, 1.55, 1.6, 1.7, 1.8, 2.0, 2.2, 2.5, 5.52, 9.6, 20.5, 23.5, 35.0] # [m/s] + EV_GAS_BRAKE_THRESHOLD_V = [0.0, -0.14, -0.16, -0.18, -0.215, -0.255, -0.32, -0.41, -0.5, -0.72, -0.895, -1.125, -1.145, -1.16] # [m/s^s] + + def update_ev_gas_brake_threshold(self, v_ego): + gas_brake_threshold = interp(v_ego, self.EV_GAS_BRAKE_THRESHOLD_BP, self.EV_GAS_BRAKE_THRESHOLD_V) + self.EV_GAS_LOOKUP_BP = [gas_brake_threshold, max(0., gas_brake_threshold), self.ACCEL_MAX] + self.EV_GAS_LOOKUP_BP_PLUS = [gas_brake_threshold, max(0., gas_brake_threshold), self.ACCEL_MAX_PLUS] + self.EV_BRAKE_LOOKUP_BP = [self.ACCEL_MIN, gas_brake_threshold] + class CAR(StrEnum): HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 7b0ddce..5d76062 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1254,6 +1254,7 @@ class Controls: self.frogpilot_variables.set_speed_offset = self.params.get_int("SetSpeedOffset") * (1 if self.is_metric else CV.MPH_TO_KPH) if quality_of_life else 0 self.random_events = self.params.get_bool("RandomEvents") + self.frogpilot_variables.use_ev_tables = self.params.get_bool("EVTable") self.speed_limit_controller = self.params.get_bool("SpeedLimitController") self.frogpilot_variables.force_mph_dashboard = self.speed_limit_controller and self.params.get_bool("ForceMPHDashboard") diff --git a/selfdrive/frogpilot/ui/vehicle_settings.cc b/selfdrive/frogpilot/ui/vehicle_settings.cc index fb1ec07..83e1cd4 100644 --- a/selfdrive/frogpilot/ui/vehicle_settings.cc +++ b/selfdrive/frogpilot/ui/vehicle_settings.cc @@ -110,6 +110,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil }); std::vector> vehicleToggles { + {"EVTable", "EV Lookup Tables", "Smoothen out the gas and brake controls for EV vehicles.", ""}, {"GasRegenCmd", "GM Truck Gas Tune", "Increase acceleration and smoothen brake to stop. For use on Silverado/Sierra only.", ""}, {"LongPitch", "Long Pitch Compensation", "Reduce speed and acceleration error for greater passenger comfort and improved vehicle efficiency.", ""}, {"LowerVolt", "Lower Volt Enable Speed", "Lower the Volt's minimum enable speed to enable openpilot at any speed.", ""}, diff --git a/selfdrive/frogpilot/ui/vehicle_settings.h b/selfdrive/frogpilot/ui/vehicle_settings.h index 980f817..42320e1 100644 --- a/selfdrive/frogpilot/ui/vehicle_settings.h +++ b/selfdrive/frogpilot/ui/vehicle_settings.h @@ -28,7 +28,7 @@ private: std::map toggles; - std::set gmKeys = {"GasRegenCmd", "LongPitch", "LowerVolt"}; + std::set gmKeys = {"EVTable", "GasRegenCmd", "LongPitch", "LowerVolt"}; std::set subaruKeys = {"CrosstrekTorque"}; std::set toyotaKeys = {"LockDoors", "LongitudinalTune", "SNGHack"};