mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-22 08:43:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

base LatControl class (#21967)

Browse Source 
* base LatControl class, move sat check out of pid.py

clean up

clean up

* fix

* global variable for min control speed

* nicer name

* unify latcontrol class init arguments

* add to release files

* saturated if close to limit

* move angle mode saturation checks into class

* check_saturation function takes in current saturated status

undo

* apply latcontrol_angle's active checking to all controllers

* clean up

* move those back

* make abstract baseclass

* add test for saturation

* keep clip

* update ref

* fix static analysis

Co-authored-by: Willem Melching <willem.melching@gmail.com>
old-commit-hash: 9de8f8cd8c384596868edf3307da51819b28bb4b
This commit is contained in:
Shane Smiskol
2022-01-26 08:10:41 -08:00
committed by GitHub
parent d947f4ff5b
commit beeb0442ce
14 changed files with 116 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+4 -18
View File
@@ -31,8 +31,6 @@ from selfdrive.manager.process_config import managed_processes
SOFT_DISABLE_TIME = 3 # seconds
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
LANE_DEPARTURE_THRESHOLD = 0.1
STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL
STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
REPLAY = "REPLAY" in os.environ
SIMULATION = "SIMULATION" in os.environ
@@ -128,13 +126,13 @@ class Controls:
self.VM = VehicleModel(self.CP)
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP)
self.LaC = LatControlAngle(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
self.LaC = LatControlINDI(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.LaC = LatControlLQR(self.CP, self.CI)
self.initialized = False
self.state = State.disabled
@@ -148,7 +146,6 @@ class Controls:
self.cruise_mismatch_counter = 0
self.can_rcv_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
@@ -518,19 +515,8 @@ class Controls:
lac_log.output = steer
lac_log.saturated = abs(steer) >= 0.9
# Check for difference between desired angle and angle for angle based control
angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
if angle_control_saturated and not CS.steeringPressed and self.active:
self.saturated_count += 1
else:
self.saturated_count = 0
# Send a "steering required alert" if saturation count has reached the limit
if (lac_log.saturated and not CS.steeringPressed) or \
(self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
if lac_log.active and lac_log.saturated and not CS.steeringPressed:
dpath_points = lat_plan.dPathPoints
if len(dpath_points):
# Check if we deviated from the path