mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-06 12:33:51 +08:00
firestar5683
79c8945a41
Integrate torque feedback, lane-change smoothing and low-speed Turn Assist through dedicated lateral owners. Co-authored-by: Dom <168790843+firestar5683@users.noreply.github.com> Co-authored-by: pharmacomaniac <blittle65@gmail.com>
54 lines
1.8 KiB
Python
54 lines
1.8 KiB
Python
"""Vehicle-owned default torque controller for the five Volt identities."""
|
|
|
|
import math
|
|
|
|
import numpy as np
|
|
|
|
from opendbc.car import structs
|
|
from opendbc.car.gm.values import CAR
|
|
from openpilot.starpilot.lateral.gm_ordinary_policy import GMOrdinaryTorquePolicy
|
|
|
|
VOLT_IDENTITIES = {
|
|
CAR.CHEVROLET_VOLT,
|
|
CAR.CHEVROLET_VOLT_ASCM,
|
|
CAR.CHEVROLET_VOLT_CAMERA,
|
|
CAR.CHEVROLET_VOLT_CC,
|
|
CAR.CHEVROLET_VOLT_2019,
|
|
}
|
|
|
|
|
|
def supported_cp(cp):
|
|
return (
|
|
cp.brand == 'gm'
|
|
and cp.carFingerprint in VOLT_IDENTITIES
|
|
and not cp.notCar
|
|
and not cp.passive
|
|
and not cp.dashcamOnly
|
|
and cp.steerControlType == structs.CarParams.SteerControlType.torque
|
|
and cp.lateralTuning.which() == 'torque'
|
|
and math.isfinite(cp.lateralTuning.torque.latAccelFactor)
|
|
and cp.lateralTuning.torque.latAccelFactor > 0
|
|
and math.isfinite(cp.lateralTuning.torque.latAccelOffset)
|
|
and math.isfinite(cp.lateralTuning.torque.friction)
|
|
and cp.lateralTuning.torque.friction >= 0
|
|
)
|
|
|
|
|
|
class VoltTorquePolicy(GMOrdinaryTorquePolicy):
|
|
def __init__(self, parent, cp):
|
|
self.parent = parent
|
|
if not supported_cp(cp):
|
|
raise ValueError('Unsupported Volt torque profile')
|
|
self.dt = parent.dt
|
|
coefficients = ((1.525, 1.05, 0.155, 0.0), (1.525, 0.95, 0.150, 0.0))
|
|
lateral_values = np.arange(-5.0, 5.0, 0.01)
|
|
torque_values = []
|
|
for value in lateral_values:
|
|
a, b, c, d = coefficients[0 if value >= 0 else 1]
|
|
sig_input = a * value
|
|
sigmoid = np.sign(sig_input) * (1 / (1 + math.exp(-abs(sig_input))) - 0.5)
|
|
torque_values.append(float(sigmoid * b + value * c + d))
|
|
parent.torque_from_lateral_accel = lambda value, tune: np.interp(value, lateral_values, torque_values)
|
|
parent.lateral_accel_from_torque = lambda value, tune: np.interp(value, torque_values, lateral_values)
|
|
super().__init__(parent, cp)
|