mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-08-17 14:13:41 +08:00
dragonpilot
113 lines
4.0 KiB
Python
113 lines
4.0 KiB
Python
#from common.numpy_fast import clip
|
|
from selfdrive.car import apply_std_steer_torque_limits
|
|
from selfdrive.car.subaru import subarucan
|
|
from selfdrive.car.subaru.values import DBC
|
|
from opendbc.can.packer import CANPacker
|
|
from common.params import Params
|
|
params = Params()
|
|
from selfdrive.dragonpilot.dragonconf import dp_get_last_modified
|
|
|
|
class CarControllerParams():
|
|
def __init__(self, car_fingerprint):
|
|
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.lkas_active = False
|
|
self.apply_steer_last = 0
|
|
self.es_distance_cnt = -1
|
|
self.es_lkas_cnt = -1
|
|
self.steer_rate_limited = False
|
|
|
|
# Setup detection helper. Routes commands to
|
|
# an appropriate CAN bus number.
|
|
self.params = CarControllerParams(CP.carFingerprint)
|
|
self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
|
|
|
|
# dragonpilot
|
|
self.turning_signal_timer = 0
|
|
self.dragon_enable_steering_on_signal = False
|
|
self.dragon_lat_ctrl = True
|
|
self.dp_last_modified = None
|
|
self.lane_change_enabled = True
|
|
|
|
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line):
|
|
""" Controls thread """
|
|
|
|
# dragonpilot, don't check for param too often as it's a kernel call
|
|
if frame % 500 == 0:
|
|
modified = dp_get_last_modified()
|
|
if self.dp_last_modified != modified:
|
|
self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True
|
|
if self.dragon_lat_ctrl:
|
|
self.lane_change_enabled = False if params.get("LaneChangeEnabled", encoding='utf8') == "1" else False
|
|
if not self.lane_change_enabled:
|
|
self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False
|
|
else:
|
|
self.dragon_enable_steering_on_signal = False
|
|
else:
|
|
self.dragon_enable_steering_on_signal = False
|
|
self.dp_last_modified = modified
|
|
|
|
P = self.params
|
|
|
|
# Send CAN commands.
|
|
can_sends = []
|
|
|
|
### STEER ###
|
|
|
|
if (frame % P.STEER_STEP) == 0:
|
|
|
|
final_steer = actuators.steer if enabled else 0.
|
|
apply_steer = int(round(final_steer * P.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, P)
|
|
self.steer_rate_limited = new_steer != apply_steer
|
|
|
|
lkas_enabled = enabled
|
|
|
|
if not lkas_enabled:
|
|
apply_steer = 0
|
|
|
|
# dragonpilot
|
|
if enabled:
|
|
if self.dragon_enable_steering_on_signal:
|
|
if not CS.out.leftBlinker and not CS.out.rightBlinker:
|
|
self.turning_signal_timer = 0
|
|
else:
|
|
self.turning_signal_timer = 100
|
|
|
|
if self.turning_signal_timer > 0:
|
|
self.turning_signal_timer -= 1
|
|
apply_steer = 0
|
|
else:
|
|
self.turning_signal_timer = 0
|
|
|
|
if not self.dragon_lat_ctrl:
|
|
apply_steer = 0
|
|
|
|
can_sends.append(subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, apply_steer, frame, P.STEER_STEP))
|
|
|
|
self.apply_steer_last = apply_steer
|
|
|
|
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
|