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

controlsd: split up publishing and logic (#33483)

Browse Source 
* split up the pubs

* move all event adds

* split out alerts

* lil more

* clenaup

* update test

* cleanup
This commit is contained in:
Adeeb Shihadeh
2024-09-05 16:28:57 -07:00
committed by GitHub
parent e0be4a1f85
commit aef650013e
5 changed files with 106 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+95 -97
View File
@@ -117,6 +117,8 @@ class Controls:
self.params.remove("ExperimentalMode")
self.CS_prev = car.CarState.new_message()
self.CC_prev = car.CarControl.new_message()
self.lac_log_prev = None
self.AM = AlertManager()
self.events = Events()
@@ -216,16 +218,10 @@ class Controls:
if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
self.events.add(EventName.overheat)
if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
# under 7% of space free no enable allowed
self.events.add(EventName.outOfSpace)
if self.sm['deviceState'].memoryUsagePercent > 90 and not SIMULATION:
self.events.add(EventName.lowMemory)
# TODO: enable this once loggerd CPU usage is more reasonable
#cpus = list(self.sm['deviceState'].cpuUsagePercent)
#if max(cpus, default=0) > 95 and not SIMULATION:
# self.events.add(EventName.highCpuUsage)
# Alert if fan isn't spinning for 5 seconds
if self.sm['peripheralState'].pandaType != log.PandaState.PandaType.unknown:
if self.sm['peripheralState'].fanSpeedRpm < 500 and self.sm['deviceState'].fanSpeedPercentDesired > 50:
@@ -248,6 +244,13 @@ class Controls:
else:
self.events.add(EventName.calibrationInvalid)
# Lane departure warning
if self.sm.valid['modelV2'] and CS.canValid:
self.ldw.update(self.sm.frame, self.sm['modelV2'], CS, self.CC_prev)
if self.is_ldw_enabled and self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated:
if self.ldw.warning:
self.events.add(EventName.ldw)
# Handle lane change
if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
direction = self.sm['modelV2'].meta.laneChangeDirection
@@ -348,6 +351,37 @@ class Controls:
if self.cruise_mismatch_counter > int(6. / DT_CTRL):
self.events.add(EventName.cruiseMismatch)
# Send a "steering required alert" if saturation count has reached the limit
lac_log = self.lac_log_prev
if CS.steeringPressed:
self.last_steering_pressed_frame = self.sm.frame
recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0
if self.lac_log_prev is not None and self.lac_log_prev.active and not recent_steer_pressed and not self.CP.notCar:
lac_log = self.lac_log_prev
if self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode:
undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2
turning = abs(lac_log.desiredLateralAccel) > 1.0
good_speed = CS.vEgo > 5
max_torque = abs(self.sm['carOutput'].actuatorsOutput.steer) > 0.99
if undershooting and turning and good_speed and max_torque:
self.events.add(EventName.steerSaturated)
elif lac_log.saturated:
# TODO probably should not use dpath_points but curvature
dpath_points = self.sm['modelV2'].position.y
if len(dpath_points):
# Check if we deviated from the path
# TODO use desired vs actual curvature
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
steering_value = self.CC_prev.actuators.steeringAngleDeg
else:
steering_value = self.CC_prev.actuators.steer
left_deviation = steering_value > 0 and dpath_points[0] < -0.20
right_deviation = steering_value < 0 and dpath_points[0] > 0.20
if left_deviation or right_deviation:
self.events.add(EventName.steerSaturated)
# Check for FCW
stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25
model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking
@@ -521,36 +555,6 @@ class Controls:
lac_log.output = actuators.steer
lac_log.saturated = abs(actuators.steer) >= 0.9
if CS.steeringPressed:
self.last_steering_pressed_frame = self.sm.frame
recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0
# Send a "steering required alert" if saturation count has reached the limit
if lac_log.active and not recent_steer_pressed and not self.CP.notCar:
if self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode:
undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2
turning = abs(lac_log.desiredLateralAccel) > 1.0
good_speed = CS.vEgo > 5
max_torque = abs(self.sm['carOutput'].actuatorsOutput.steer) > 0.99
if undershooting and turning and good_speed and max_torque:
lac_log.active and self.events.add(EventName.steerSaturated)
elif lac_log.saturated:
# TODO probably should not use dpath_points but curvature
dpath_points = model_v2.position.y
if len(dpath_points):
# Check if we deviated from the path
# TODO use desired vs actual curvature
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
steering_value = actuators.steeringAngleDeg
else:
steering_value = actuators.steer
left_deviation = steering_value > 0 and dpath_points[0] < -0.20
right_deviation = steering_value < 0 and dpath_points[0] > 0.20
if left_deviation or right_deviation:
self.events.add(EventName.steerSaturated)
# Ensure no NaNs/Infs
for p in ACTUATOR_FIELDS:
attr = getattr(actuators, p)
@@ -563,9 +567,20 @@ class Controls:
return CC, lac_log
def publish_logs(self, CS, CC, lac_log):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
def update_alerts(self, CS):
clear_event_types = set()
if ET.WARNING not in self.state_machine.current_alert_types:
clear_event_types.add(ET.WARNING)
if self.enabled:
clear_event_types.add(ET.NO_ENTRY)
pers = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()}[self.personality]
alerts = self.events.create_alerts(self.state_machine.current_alert_types, [self.CP, CS, self.sm, self.is_metric,
self.state_machine.soft_disable_timer, pers])
self.AM.add_many(self.sm.frame, alerts)
self.AM.process_alerts(self.sm.frame, clear_event_types)
def publish_carControl(self, CS, CC, lac_log):
# Orientation and angle rates can be useful for carcontroller
# Only calibrated (car) frame is relevant for the carcontroller
if self.calibrated_pose is not None:
@@ -591,26 +606,12 @@ class Controls:
hudControl.rightLaneVisible = True
hudControl.leftLaneVisible = True
self.ldw.update(self.sm.frame, self.sm['modelV2'], CS, CC)
if self.is_ldw_enabled and self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated:
if self.sm.valid['modelV2'] and CS.canValid and self.is_ldw_enabled and self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated:
hudControl.leftLaneDepart = self.ldw.left
hudControl.rightLaneDepart = self.ldw.right
if self.ldw.warning:
self.events.add(EventName.ldw)
clear_event_types = set()
if ET.WARNING not in self.state_machine.current_alert_types:
clear_event_types.add(ET.WARNING)
if self.enabled:
clear_event_types.add(ET.NO_ENTRY)
pers = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()}[self.personality]
alerts = self.events.create_alerts(self.state_machine.current_alert_types, [self.CP, CS, self.sm, self.is_metric,
self.state_machine.soft_disable_timer, pers])
self.AM.add_many(self.sm.frame, alerts)
current_alert = self.AM.process_alerts(self.sm.frame, clear_event_types)
if current_alert:
hudControl.visualAlert = current_alert.visual_alert
if self.AM.current_alert:
hudControl.visualAlert = self.AM.current_alert.visual_alert
if not self.CP.passive and self.initialized:
CO = self.sm['carOutput']
@@ -620,53 +621,57 @@ class Controls:
else:
self.steer_limited = abs(CC.actuators.steer - CO.actuatorsOutput.steer) > 1e-2
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state_machine.state == State.softDisabling)
# Curvature & Steering angle
lp = self.sm['liveParameters']
steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll)
# controlsState
dat = messaging.new_message('controlsState')
dat.valid = CS.canValid
controlsState = dat.controlsState
controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
controlsState.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
controlsState.curvature = curvature
controlsState.desiredCurvature = self.desired_curvature
controlsState.longControlState = self.LoC.long_control_state
controlsState.upAccelCmd = float(self.LoC.pid.p)
controlsState.uiAccelCmd = float(self.LoC.pid.i)
controlsState.ufAccelCmd = float(self.LoC.pid.f)
controlsState.cumLagMs = -self.rk.remaining * 1000.
controlsState.forceDecel = bool(force_decel)
cs = dat.controlsState
lp = self.sm['liveParameters']
steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
cs.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll)
cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
cs.desiredCurvature = self.desired_curvature
cs.longControlState = self.LoC.long_control_state
cs.upAccelCmd = float(self.LoC.pid.p)
cs.uiAccelCmd = float(self.LoC.pid.i)
cs.ufAccelCmd = float(self.LoC.pid.f)
cs.cumLagMs = -self.rk.remaining * 1000.
cs.forceDecel = bool((self.sm['driverMonitoringState'].awarenessStatus < 0.) or
(self.state_machine.state == State.softDisabling))
lat_tuning = self.CP.lateralTuning.which()
if self.joystick_mode:
controlsState.lateralControlState.debugState = lac_log
cs.lateralControlState.debugState = lac_log
elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
controlsState.lateralControlState.angleState = lac_log
cs.lateralControlState.angleState = lac_log
elif lat_tuning == 'pid':
controlsState.lateralControlState.pidState = lac_log
cs.lateralControlState.pidState = lac_log
elif lat_tuning == 'torque':
controlsState.lateralControlState.torqueState = lac_log
cs.lateralControlState.torqueState = lac_log
self.pm.send('controlsState', dat)
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
self.pm.send('carControl', cc_send)
def publish_selfdriveState(self, CS):
# selfdriveState
ss_msg = messaging.new_message('selfdriveState')
ss_msg.valid = CS.canValid
ss = ss_msg.selfdriveState
if current_alert:
ss.alertText1 = current_alert.alert_text_1
ss.alertText2 = current_alert.alert_text_2
ss.alertSize = current_alert.alert_size
ss.alertStatus = current_alert.alert_status
ss.alertType = current_alert.alert_type
ss.alertSound = current_alert.audible_alert
if self.AM.current_alert:
ss.alertText1 = self.AM.current_alert.alert_text_1
ss.alertText2 = self.AM.current_alert.alert_text_2
ss.alertSize = self.AM.current_alert.alert_size
ss.alertStatus = self.AM.current_alert.alert_status
ss.alertType = self.AM.current_alert.alert_type
ss.alertSound = self.AM.current_alert.audible_alert
ss.enabled = self.enabled
ss.active = self.active
ss.state = self.state_machine.state
@@ -683,30 +688,23 @@ class Controls:
self.pm.send('onroadEvents', ce_send)
self.events_prev = self.events.names.copy()
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
self.pm.send('carControl', cc_send)
def step(self):
# Sample data from sockets and get a carState
CS = self.data_sample()
cloudlog.timestamp("Data sampled")
self.update_events(CS)
cloudlog.timestamp("Events updated")
if not self.CP.passive and self.initialized:
self.enabled, self.active = self.state_machine.update(self.events)
self.update_alerts(CS)
# Compute actuators (runs PID loops and lateral MPC)
CC, lac_log = self.state_control(CS)
# Publish data
self.publish_logs(CS, CC, lac_log)
self.publish_carControl(CS, CC, lac_log)
self.publish_selfdriveState(CS)
self.CS_prev = CS
self.CC_prev = CC
self.lac_log_prev = lac_log
def read_personality_param(self):
try:
selfdrive/controls/lib/alertmanager.py
+6 -5
View File
@@ -34,6 +34,7 @@ class AlertEntry:
class AlertManager:
def __init__(self):
self.alerts: dict[str, AlertEntry] = defaultdict(AlertEntry)
self.current_alert: Alert | None = None
def add_many(self, frame: int, alerts: list[Alert]) -> None:
for alert in alerts:
@@ -44,8 +45,8 @@ class AlertManager:
min_end_frame = entry.start_frame + alert.duration
entry.end_frame = max(frame + 1, min_end_frame)
def process_alerts(self, frame: int, clear_event_types: set) -> Alert | None:
current_alert = AlertEntry()
def process_alerts(self, frame: int, clear_event_types: set):
ae = AlertEntry()
for v in self.alerts.values():
if not v.alert:
continue
@@ -54,8 +55,8 @@ class AlertManager:
v.end_frame = -1
# sort by priority first and then by start_frame
greater = current_alert.alert is None or (v.alert.priority, v.start_frame) > (current_alert.alert.priority, current_alert.start_frame)
greater = ae.alert is None or (v.alert.priority, v.start_frame) > (ae.alert.priority, ae.start_frame)
if v.active(frame) and greater:
current_alert = v
ae = v
return current_alert.alert
self.current_alert = ae.alert
selfdrive/controls/lib/tests/test_alertmanager.py
+2 -2
View File
@@ -32,8 +32,8 @@ class TestAlertManager:
for frame in range(duration+10):
if frame < add_duration:
AM.add_many(frame, [alert, ])
current_alert = AM.process_alerts(frame, {})
AM.process_alerts(frame, {})
shown = current_alert is not None
shown = AM.current_alert is not None
should_show = frame <= show_duration
assert shown == should_show, f"{frame=} {add_duration=} {duration=}"
selfdrive/debug/count_events.py
+2 -2
View File
@@ -33,8 +33,8 @@ if __name__ == "__main__":
if len(events) == 0 or ae != events[-1][1]:
events.append((t, ae))
elif msg.which() == 'controlsState':
at = msg.controlsState.alertType
elif msg.which() == 'selfdriveState':
at = msg.selfdriveState.alertType
if "/override" not in at or "lanechange" in at.lower():
if len(alerts) == 0 or alerts[-1][1] != at:
alerts.append((t, at))
selfdrive/test/process_replay/ref_commit
+1 -1
View File
@@ -1 +1 @@
c2022c388da6eb2e26bb23ad6009be9d5314c0be
fb0827a00b21b97733f8385cf7ce87a21a526f4c