mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-08-23 00:53:43 +08:00
Code Issues Packages Projects Releases Wiki Activity

remove CS.steeringRateLimited (#25251)

Browse Source 
* remove CS.steeringRateLimited

* bump cereal

* update refs
This commit is contained in:
Adeeb Shihadeh
2022-07-24 14:56:55 -07:00
committed by GitHub
parent 10b212dc7e
commit d462a08056
26 changed files with 39 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal
+1 -1
Submodule cereal updated: bc999518e7...9ae66d0712
selfdrive/car/chrysler/carcontroller.py
-2
View File
@@ -10,7 +10,6 @@ class CarController:
self.CP = CP
self.apply_steer_last = 0
self.frame = 0
self.steer_rate_limited = False
self.hud_count = 0
self.last_lkas_falling_edge = 0
@@ -67,7 +66,6 @@ class CarController:
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorqueEps, self.params)
if not lkas_active:
apply_steer = 0
self.steer_rate_limited = new_steer != apply_steer
self.apply_steer_last = apply_steer
can_sends.append(create_lkas_command(self.packer, self.CP, int(apply_steer), lkas_control_bit))
selfdrive/car/chrysler/interface.py
-2
View File
@@ -75,8 +75,6 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# events
events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low])
selfdrive/car/ford/carcontroller.py
-2
View File
@@ -25,7 +25,6 @@ class CarController:
self.frame = 0
self.apply_steer_last = 0
self.steer_rate_limited = False
self.main_on_last = False
self.lkas_enabled_last = False
self.steer_alert_last = False
@@ -46,7 +45,6 @@ class CarController:
# apply rate limits
new_steer = actuators.steeringAngleDeg
apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo)
self.steer_rate_limited = new_steer != apply_steer
# send steering commands at 20Hz
if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0:
selfdrive/car/ford/interface.py
-2
View File
@@ -70,8 +70,6 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
events = self.create_common_events(ret)
ret.events = events.to_msg()
selfdrive/car/gm/carcontroller.py
-2
View File
@@ -22,7 +22,6 @@ class CarController:
self.lka_steering_cmd_counter_last = -1
self.lka_icon_status_last = (False, False)
self.steer_rate_limited = False
self.params = CarControllerParams()
@@ -51,7 +50,6 @@ class CarController:
if lkas_enabled:
new_steer = int(round(actuators.steer * self.params.STEER_MAX))
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
else:
apply_steer = 0
selfdrive/car/gm/interface.py
-2
View File
@@ -156,8 +156,6 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_loopback)
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT:
be = create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS)
selfdrive/car/hyundai/carcontroller.py
-2
View File
@@ -43,7 +43,6 @@ class CarController:
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.steer_rate_limited = False
self.last_button_frame = 0
self.accel = 0
@@ -59,7 +58,6 @@ class CarController:
steer = clip(steer, -0.7, 0.7)
new_steer = int(round(steer * self.params.STEER_MAX))
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 CC.latActive:
apply_steer = 0
selfdrive/car/hyundai/interface.py
-1
View File
@@ -314,7 +314,6 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# On some newer model years, the CANCEL button acts as a pause/resume button based on the PCM state
# To avoid re-engaging when openpilot cancels, check user engagement intention via buttons
selfdrive/car/mazda/carcontroller.py
-3
View File
@@ -12,7 +12,6 @@ class CarController:
self.CP = CP
self.apply_steer_last = 0
self.packer = CANPacker(dbc_name)
self.steer_rate_limited = False
self.brake_counter = 0
self.frame = 0
@@ -20,14 +19,12 @@ class CarController:
can_sends = []
apply_steer = 0
self.steer_rate_limited = False
if CC.latActive:
# calculate steer and also set limits due to driver torque
new_steer = int(round(CC.actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,
CS.out.steeringTorque, CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
if CC.cruiseControl.cancel:
# If brake is pressed, let us wait >70ms before trying to disable crz to avoid
selfdrive/car/subaru/carcontroller.py
-2
View File
@@ -11,7 +11,6 @@ class CarController:
self.es_distance_cnt = -1
self.es_lkas_cnt = -1
self.cruise_button_prev = 0
self.steer_rate_limited = False
self.frame = 0
self.p = CarControllerParams(CP)
@@ -33,7 +32,6 @@ class CarController:
new_steer = int(round(apply_steer))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
self.steer_rate_limited = new_steer != apply_steer
if not CC.latActive:
apply_steer = 0
selfdrive/car/subaru/interface.py
-2
View File
@@ -111,8 +111,6 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp, self.cp_cam)
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
ret.events = self.create_common_events(ret).to_msg()
return ret
selfdrive/car/toyota/carcontroller.py
-2
View File
@@ -20,7 +20,6 @@ class CarController:
self.alert_active = False
self.last_standstill = False
self.standstill_req = False
self.steer_rate_limited = False
self.packer = CANPacker(dbc_name)
self.gas = 0
@@ -52,7 +51,6 @@ class CarController:
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.torque_rate_limits)
self.steer_rate_limited = new_steer != apply_steer
if not CC.latActive:
apply_steer = 0
selfdrive/car/toyota/interface.py
-2
View File
@@ -243,8 +243,6 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# events
events = self.create_common_events(ret)
selfdrive/car/volkswagen/carcontroller.py
-3
View File
@@ -22,8 +22,6 @@ class CarController:
self.graMsgStartFramePrev = 0
self.graMsgBusCounterPrev = 0
self.steer_rate_limited = False
def update(self, CC, CS, ext_bus):
actuators = CC.actuators
hud_control = CC.hudControl
@@ -46,7 +44,6 @@ class CarController:
if CC.latActive:
new_steer = int(round(actuators.steer * P.STEER_MAX))
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
if apply_steer == 0:
hcaEnabled = False
self.hcaEnabledFrameCount = 0
selfdrive/car/volkswagen/interface.py
-1
View File
@@ -163,7 +163,6 @@ class CarInterface(CarInterfaceBase):
buttonEvents = []
ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType)
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# Check for and process state-change events (button press or release) from
# the turn stalk switch or ACC steering wheel/control stalk buttons.
selfdrive/controls/controlsd.py
+3 -1
View File
@@ -176,6 +176,7 @@ class Controls:
self.logged_comm_issue = None
self.button_timers = {ButtonEvent.Type.decelCruise: 0, ButtonEvent.Type.accelCruise: 0}
self.last_actuators = car.CarControl.Actuators.new_message()
self.steer_limited = False
self.desired_curvature = 0.0
self.desired_curvature_rate = 0.0
@@ -594,7 +595,7 @@ class Controls:
lat_plan.curvatures,
lat_plan.curvatureRates)
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, params,
self.last_actuators, self.desired_curvature,
self.last_actuators, self.steer_limited, self.desired_curvature,
self.desired_curvature_rate, self.sm['liveLocationKalman'])
else:
lac_log = log.ControlsState.LateralDebugState.new_message()
@@ -721,6 +722,7 @@ class Controls:
self.last_actuators, can_sends = self.CI.apply(CC)
self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
CC.actuatorsOutput = self.last_actuators
self.steer_limited = abs(CC.actuators.steer - CC.actuatorsOutput.steer) > 1e-2
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)
selfdrive/controls/lib/latcontrol.py
+3 -3
View File
@@ -16,14 +16,14 @@ class LatControl(ABC):
self.steer_max = 1.0
@abstractmethod
def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk):
pass
def reset(self):
self.sat_count = 0.
def _check_saturation(self, saturated, CS):
if saturated and CS.vEgo > 10. and not CS.steeringRateLimited and not CS.steeringPressed:
def _check_saturation(self, saturated, CS, steer_limited):
if saturated and CS.vEgo > 10. and not steer_limited and not CS.steeringPressed:
self.sat_count += self.sat_count_rate
else:
self.sat_count -= self.sat_count_rate
selfdrive/controls/lib/latcontrol_angle.py
+2 -2
View File
@@ -7,7 +7,7 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
class LatControlAngle(LatControl):
def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk):
angle_log = log.ControlsState.LateralAngleState.new_message()
if CS.vEgo < MIN_STEER_SPEED or not active:
@@ -19,7 +19,7 @@ class LatControlAngle(LatControl):
angle_steers_des += params.angleOffsetDeg
angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
angle_log.saturated = self._check_saturation(angle_control_saturated, CS)
angle_log.saturated = self._check_saturation(angle_control_saturated, CS, steer_limited)
angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
angle_log.steeringAngleDesiredDeg = angle_steers_des
return 0, float(angle_steers_des), angle_log
selfdrive/controls/lib/latcontrol_indi.py
+2 -2
View File
@@ -63,7 +63,7 @@ class LatControlINDI(LatControl):
self.steer_filter.x = 0.
self.speed = 0.
def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk):
self.speed = CS.vEgo
# Update Kalman filter
y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
@@ -115,6 +115,6 @@ class LatControlINDI(LatControl):
indi_log.delayedOutput = float(self.steer_filter.x)
indi_log.delta = float(delta_u)
indi_log.output = float(output_steer)
indi_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS)
indi_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited)
return float(output_steer), float(steers_des), indi_log
selfdrive/controls/lib/latcontrol_pid.py
+2 -2
View File
@@ -17,7 +17,7 @@ class LatControlPID(LatControl):
super().reset()
self.pid.reset()
def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
@@ -43,6 +43,6 @@ class LatControlPID(LatControl):
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS)
pid_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited)
return output_steer, angle_steers_des, pid_log
selfdrive/controls/lib/latcontrol_torque.py
+3 -3
View File
@@ -33,7 +33,7 @@ class LatControlTorque(LatControl):
self.kf = CP.lateralTuning.torque.kf
self.steering_angle_deadzone_deg = CP.lateralTuning.torque.steeringAngleDeadzoneDeg
def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk):
pid_log = log.ControlsState.LateralTorqueState.new_message()
if CS.vEgo < MIN_STEER_SPEED or not active:
@@ -66,7 +66,7 @@ class LatControlTorque(LatControl):
# convert friction into lateral accel units for feedforward
friction_compensation = interp(apply_deadzone(error, lateral_accel_deadzone), [-FRICTION_THRESHOLD, FRICTION_THRESHOLD], [-self.friction, self.friction])
ff += friction_compensation / self.kf
freeze_integrator = CS.steeringRateLimited or CS.steeringPressed or CS.vEgo < 5
freeze_integrator = steer_limited or CS.steeringPressed or CS.vEgo < 5
output_torque = self.pid.update(error,
feedforward=ff,
speed=CS.vEgo,
@@ -78,7 +78,7 @@ class LatControlTorque(LatControl):
pid_log.d = self.pid.d
pid_log.f = self.pid.f
pid_log.output = -output_torque
pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS)
pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited)
pid_log.actualLateralAccel = actual_lateral_accel
pid_log.desiredLateralAccel = desired_lateral_accel
selfdrive/controls/lib/tests/test_latcontrol.py
+1 -2
View File
@@ -26,7 +26,6 @@ class TestLatControl(unittest.TestCase):
controller = controller(CP, CI)
CS = car.CarState.new_message()
CS.vEgo = 30
@@ -35,7 +34,7 @@ class TestLatControl(unittest.TestCase):
params = log.LiveParametersData.new_message()
for _ in range(1000):
_, _, lac_log = controller.update(True, CS, CP, VM, params, last_actuators, 1, 0)
_, _, lac_log = controller.update(True, CS, CP, VM, params, last_actuators, True, 1, 0)
self.assertTrue(lac_log.saturated)
selfdrive/test/process_replay/ref_commit
+1 -1
View File
@@ -1 +1 @@
2bfa96aad8b951988cf9d0dd95d3c9d9019a1c0e
8f918a54cfefcaada91a7eca0c14ca4d4b6f11c8
selfdrive/test/process_replay/test_processes.py
+20 -6
View File
@@ -5,7 +5,7 @@ import os
import sys
from collections import defaultdict
from tqdm import tqdm
from typing import Any, Dict
from typing import Any, DefaultDict, Dict
from selfdrive.car.car_helpers import interface_names
from selfdrive.test.openpilotci import get_url, upload_file
@@ -105,7 +105,7 @@ def test_process(cfg, lr, ref_log_path, new_log_path, ignore_fields=None, ignore
return str(e), log_msgs
def format_diff(results, ref_commit):
def format_diff(results, log_paths, ref_commit):
diff1, diff2 = "", ""
diff2 += f"***** tested against commit {ref_commit} *****\n"
@@ -115,13 +115,22 @@ def format_diff(results, ref_commit):
diff2 += f"***** differences for segment {segment} *****\n"
for proc, diff in list(result.items()):
diff1 += f"\t{proc}\n"
# long diff
diff2 += f"*** process: {proc} ***\n"
diff2 += f"\tref: {log_paths[segment]['ref']}\n\n"
diff2 += f"\tnew: {log_paths[segment]['new']}\n\n"
# short diff
diff1 += f" {proc}\n"
if isinstance(diff, str):
diff1 += f"\t\t{diff}\n"
diff1 += f" ref: {log_paths[segment]['ref']}\n"
diff1 += f" new: {log_paths[segment]['new']}\n\n"
diff1 += f" {diff}\n"
failed = True
elif len(diff):
diff1 += f" ref: {log_paths[segment]['ref']}\n"
diff1 += f" new: {log_paths[segment]['new']}\n\n"
cnt: Dict[str, int] = {}
for d in diff:
diff2 += f"\t{str(d)}\n"
@@ -130,7 +139,7 @@ def format_diff(results, ref_commit):
cnt[k] = 1 if k not in cnt else cnt[k] + 1
for k, v in sorted(cnt.items()):
diff1 += f"\t\t{k}: {v}\n"
diff1 += f" {k}: {v}\n"
failed = True
return diff1, diff2, failed
@@ -187,6 +196,8 @@ if __name__ == "__main__":
untested = (set(interface_names) - set(excluded_interfaces)) - {c.lower() for c in tested_cars}
assert len(untested) == 0, f"Cars missing routes: {str(untested)}"
log_paths: DefaultDict[str, Dict[str, str]] = defaultdict(dict)
with concurrent.futures.ProcessPoolExecutor(max_workers=args.jobs) as pool:
if not args.upload_only:
download_segments = [seg for car, seg in segments if car in tested_cars]
@@ -214,13 +225,16 @@ if __name__ == "__main__":
dat = None if args.upload_only else log_data[segment]
pool_args.append((segment, cfg, args, cur_log_fn, ref_log_path, dat))
log_paths[segment]['ref'] = ref_log_path
log_paths[segment]['new'] = cur_log_fn
results: Any = defaultdict(dict)
p2 = pool.map(run_test_process, pool_args)
for (segment, proc, subtest_name, result) in tqdm(p2, desc="Running Tests", total=len(pool_args)):
if not args.upload_only:
results[segment][proc + subtest_name] = result
diff1, diff2, failed = format_diff(results, ref_commit)
diff1, diff2, failed = format_diff(results, log_paths, ref_commit)
if not upload:
with open(os.path.join(PROC_REPLAY_DIR, "diff.txt"), "w") as f:
f.write(diff2)
tools/tuning/measure_steering_accuracy.py
+1 -1
View File
@@ -71,7 +71,7 @@ if __name__ == "__main__":
active = sm['controlsState'].active
steer = sm['carControl'].actuatorsOutput.steer
standstill = sm['carState'].standstill
steer_limited = sm['carState'].steeringRateLimited
steer_limited = abs(sm['carControl'].actuators.steer - sm['carControl'].actuatorsOutput.steer) > 1e-2
overriding = sm['carState'].steeringPressed
changing_lanes = sm['lateralPlan'].laneChangeState != 0
d_path_points = sm['lateralPlan'].dPathPoints