Volt TorqueTune

selfdrive/car/gm/carcontroller.py

@@ -1,3 +1,5 @@
+import math
+
 from cereal import car
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.filter_simple import FirstOrderFilter
@@ -7,7 +9,7 @@ from openpilot.common.params_pyx import Params
 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, AccState, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_CAR
+from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_CAR, 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
@@ -46,6 +48,13 @@ class CarController(CarControllerBase):
     self.lka_icon_status_last = (False, False)
 
     self.params = CarControllerParams(self.CP)
+    self.is_volt = self.CP.carFingerprint in (CAR.CHEVROLET_VOLT, CAR.CHEVROLET_VOLT_ASCM, CAR.CHEVROLET_VOLT_CAMERA, CAR.CHEVROLET_VOLT_CC)
+    if self.is_volt:
+      self.mass = CP.mass
+      self.tireRadius = 0.075 * CP.wheelbase + 0.1453
+      self.frontalArea = 1.05 * CP.wheelbase + 0.0679
+      self.coeffDrag = 0.30
+      self.airDensity = 1.225
     self.params_ = Params()
 
     self.packer_pt = CANPacker(DBC[self.CP.carFingerprint]['pt'])
@@ -127,7 +136,7 @@ class CarController(CarControllerBase):
 
         # Pitch compensated acceleration;
         # TODO: include future pitch (sm['modelDataV2'].orientation.y) to account for long actuator delay
-        if frogpilot_toggles.long_pitch and len(CC.orientationNED) > 1:
+        if frogpilot_toggles.long_pitch and len(CC.orientationNED) > 1 and not self.is_volt:
           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
@@ -145,6 +154,18 @@ class CarController(CarControllerBase):
           self.apply_brake = int(min(-100 * frogpilot_toggles.stopAccel, self.params.MAX_BRAKE))
         else:
           # Normal operation
+          if self.is_volt:
+            if len(CC.orientationNED) == 3 and CS.out.vEgo > self.CP.vEgoStopping:
+              volt_pitch_accel = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
+            else:
+              volt_pitch_accel = 0.0
+
+            aero_drag_accel = (0.5 * self.coeffDrag * self.frontalArea * self.airDensity * CS.out.vEgo ** 2) / self.mass
+            accel += aero_drag_accel + volt_pitch_accel
+            brake_accel = actuators.accel + aero_drag_accel + volt_pitch_accel * interp(CS.out.vEgo, BRAKE_PITCH_FACTOR_BP, BRAKE_PITCH_FACTOR_V)
+            accel = clip(accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
+            brake_accel = clip(brake_accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
+
           if self.CP.carFingerprint in EV_CAR:
             self.params.update_ev_gas_brake_threshold(CS.out.vEgo)
             self.apply_gas = int(round(interp(accel, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
@@ -152,6 +173,9 @@ class CarController(CarControllerBase):
           else:
             self.apply_gas = int(round(interp(accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
             self.apply_brake = int(round(interp(brake_accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
+
+          if self.is_volt and self.apply_brake > 0 and self.apply_gas > self.params.ZERO_GAS:
+            self.apply_gas = self.params.INACTIVE_REGEN
           # Don't allow any gas above inactive regen while stopping
           # FIXME: brakes aren't applied immediately when enabling at a stop
           if stopping:

selfdrive/car/gm/values.py

@@ -40,6 +40,7 @@ class CarControllerParams:
     # Gas/brake lookups
     self.ZERO_GAS = 6144  # Coasting
     self.MAX_BRAKE = 400  # ~ -4.0 m/s^2 with regen
+    self.BRAKE_SWITCH_MAX = self.ZERO_GAS
 
     if CP.carFingerprint in (CAMERA_ACC_CAR | SDGM_CAR) and CP.carFingerprint not in CC_ONLY_CAR and CP.carFingerprint != CAR.CHEVROLET_BOLT_EUV:
       self.MAX_GAS = 7496
@@ -59,13 +60,21 @@ class CarControllerParams:
       # lower threshold removes some braking deadzone
       self.max_regen_acceleration = -1. if CP.carFingerprint in EV_CAR else -0.1
 
+    if CP.carFingerprint in (CAR.CHEVROLET_VOLT, CAR.CHEVROLET_VOLT_ASCM, CAR.CHEVROLET_VOLT_CAMERA, CAR.CHEVROLET_VOLT_CC):
+      self.ZERO_GAS = 6150
+      self.BRAKE_SWITCH_MAX = self.MAX_ACC_REGEN if CP.carFingerprint in EV_CAR else self.ZERO_GAS
+      self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, 0.]
+    else:
+      self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, self.max_regen_acceleration]
+
     self.GAS_LOOKUP_BP = [self.max_regen_acceleration, 0., self.ACCEL_MAX]
     self.GAS_LOOKUP_BP_PLUS = [self.max_regen_acceleration, 0., self.ACCEL_MAX_PLUS]
     self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, self.ZERO_GAS, self.MAX_GAS]
     self.GAS_LOOKUP_V_PLUS = [self.MAX_ACC_REGEN, self.ZERO_GAS, self.MAX_GAS_PLUS]
 
-    self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, self.max_regen_acceleration]
     self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.]
+    self.BRAKE_SWITCH_LOOKUP_BP = [0.5, 10]
+    self.BRAKE_SWITCH_LOOKUP_V = [self.ZERO_GAS, self.BRAKE_SWITCH_MAX]
 
   # 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