mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-02 12:02:09 +08:00
martinl
0aeae7d81e
* Add support for Subaru Legacy 2015-18 * syntax fix * Add Legacy 2018 FPv1 * Add Subaru Ascent from upstream * Use GLOBAL_CAR and LEGACY_CAR lists * Change LEGACY_2015 to LEGACY_PREGLOBAL * Add LEGACY_CAR to carstate * Change LEGACY_2015 to LEGACY_PREGLOBAL in test_car_models * Add missing SafetyModel to Ascent * Use GLOBAL_CAR and LEGACY_CAR to set safetyModel * Change LEGACY_CAR to PREGLOBAL_CARS, remove GLOBAL_CAR * Fix PREGLOBAL_CARS in carstate and subarucan * Minor cleanups * Add accelCruise button event * Change Preglobal Driver Torque limit to match Global * Match comments to upstream * Use Steer_Warning and Steer_Error_1 only for Global * Change mph units to match upstream values * Increase Preglobal brakePressed threshold to 2 * Add DashcamOnly to LEGACY_PREGLOBAL * Fix typo in variable name * Update README, add create_preglobal_steering_control * cleanup carcontroller * cleanup values * missed that one * Update STEER_STEP * Update STEER_MAX * Add preglobal signal frequency checks * remove PREGLOBAL_CARS from subarucan * Remove whitespace * Use common frequency checks * cleanup carstate * cleanup subarucan Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> old-commit-hash: e37b8e5d5632a7cc48326f7ca162ef87f259e526
88 lines
3.3 KiB
Python
88 lines
3.3 KiB
Python
from selfdrive.car import apply_std_steer_torque_limits
|
|
from selfdrive.car.subaru import subarucan
|
|
from selfdrive.car.subaru.values import DBC, PREGLOBAL_CARS
|
|
from opendbc.can.packer import CANPacker
|
|
|
|
|
|
class CarControllerParams():
|
|
def __init__(self):
|
|
self.STEER_MAX = 2047 # max_steer 4095
|
|
self.STEER_STEP = 2 # how often we update the steer cmd
|
|
self.STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max
|
|
self.STEER_DELTA_DOWN = 70 # torque decrease per refresh
|
|
self.STEER_DRIVER_ALLOWANCE = 60 # allowed driver torque before start limiting
|
|
self.STEER_DRIVER_MULTIPLIER = 10 # weight driver torque heavily
|
|
self.STEER_DRIVER_FACTOR = 1 # from dbc
|
|
|
|
|
|
class CarController():
|
|
def __init__(self, dbc_name, CP, VM):
|
|
self.apply_steer_last = 0
|
|
self.es_distance_cnt = -1
|
|
self.es_accel_cnt = -1
|
|
self.es_lkas_cnt = -1
|
|
self.fake_button_prev = 0
|
|
self.steer_rate_limited = False
|
|
|
|
self.params = CarControllerParams()
|
|
self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
|
|
|
|
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line):
|
|
|
|
can_sends = []
|
|
|
|
# *** steering ***
|
|
if (frame % self.params.STEER_STEP) == 0:
|
|
|
|
apply_steer = int(round(actuators.steer * self.params.STEER_MAX))
|
|
|
|
# limits due to driver torque
|
|
|
|
new_steer = int(round(apply_steer))
|
|
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
|
|
self.steer_rate_limited = new_steer != apply_steer
|
|
|
|
if not enabled:
|
|
apply_steer = 0
|
|
|
|
if CS.CP.carFingerprint in PREGLOBAL_CARS:
|
|
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, frame, self.params.STEER_STEP))
|
|
else:
|
|
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, self.params.STEER_STEP))
|
|
|
|
self.apply_steer_last = apply_steer
|
|
|
|
|
|
# *** alerts and pcm cancel ***
|
|
|
|
if CS.CP.carFingerprint in PREGLOBAL_CARS:
|
|
if self.es_accel_cnt != CS.es_accel_msg["Counter"]:
|
|
# 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep
|
|
# disengage ACC when OP is disengaged
|
|
if pcm_cancel_cmd:
|
|
fake_button = 1
|
|
# turn main on if off and past start-up state
|
|
elif not CS.out.cruiseState.available and CS.ready:
|
|
fake_button = 1
|
|
else:
|
|
fake_button = CS.button
|
|
|
|
# unstick previous mocked button press
|
|
if fake_button == 1 and self.fake_button_prev == 1:
|
|
fake_button = 0
|
|
self.fake_button_prev = fake_button
|
|
|
|
can_sends.append(subarucan.create_es_throttle_control(self.packer, fake_button, CS.es_accel_msg))
|
|
self.es_accel_cnt = CS.es_accel_msg["Counter"]
|
|
|
|
else:
|
|
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
|
|
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, pcm_cancel_cmd))
|
|
self.es_distance_cnt = CS.es_distance_msg["Counter"]
|
|
|
|
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
|
|
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert, left_line, right_line))
|
|
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
|
|
|
|
return can_sends
|