mawei
/
openpilot0
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

selfdrived: controlsd only does controls (#33485) 

* selfdrived

* process replay

* lil more

* set the valids

* rename that
This commit is contained in:
Adeeb Shihadeh 2024-09-06 17:16:32 -07:00 committed by GitHub
parent e459cee1e7
commit e04455cbaa
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
33 changed files with 640 additions and 770 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cereal/car.capnp
View File

@ -100,7 +100,7 @@ struct OnroadEvent @0x9b1657f34caf3ad3 {
cameraFrameRate @110;
processNotRunning @95;
dashcamMode @96;
controlsInitializing @98;
selfdriveInitializing @98;
usbError @99;
roadCameraError @100;
driverCameraError @101;
@ -109,7 +109,7 @@ struct OnroadEvent @0x9b1657f34caf3ad3 {
cruiseMismatch @106;
lkasDisabled @107;
canBusMissing @111;
controlsdLagging @112;
selfdrivedLagging @112;
resumeBlocked @113;
steerTimeLimit @115;
vehicleSensorsInvalid @116;

3
cereal/log.capnp
View File

@ -698,6 +698,7 @@ struct SelfdriveState {
alertSize @6 :AlertSize;
alertType @7 :Text;
alertSound @8 :Car.CarControl.HUDControl.AudibleAlert;
alertHudVisual @12 :Car.CarControl.HUDControl.VisualAlert;
# configurable driving settings
experimentalMode @10 :Bool;
@ -726,7 +727,6 @@ struct SelfdriveState {
}
struct ControlsState @0x97ff69c53601abf1 {
cumLagMs @15 :Float32;
longitudinalPlanMonoTime @28 :UInt64;
lateralPlanMonoTime @50 :UInt64;
@ -880,6 +880,7 @@ struct ControlsState @0x97ff69c53601abf1 {
vCruiseDEPRECATED @22 :Float32; # actual set speed
vCruiseClusterDEPRECATED @63 :Float32; # set speed to display in the UI
startMonoTimeDEPRECATED @48 :UInt64;
cumLagMsDEPRECATED @15 :Float32;
}
struct DrivingModelData {

2
selfdrive/car/car_specific.py
View File

@ -5,7 +5,7 @@ from opendbc.car.interfaces import MAX_CTRL_SPEED, CarStateBase, CarControllerBa
from opendbc.car.volkswagen.values import CarControllerParams as VWCarControllerParams
from opendbc.car.hyundai.interface import ENABLE_BUTTONS as HYUNDAI_ENABLE_BUTTONS
from openpilot.selfdrive.controls.lib.events import Events
from openpilot.selfdrive.selfdrived.events import Events
ButtonType = structs.CarState.ButtonEvent.Type
GearShifter = structs.CarState.GearShifter

4
selfdrive/car/card.py
View File

@ -22,7 +22,7 @@ from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
from openpilot.selfdrive.car.cruise import VCruiseHelper
from openpilot.selfdrive.car.car_specific import CarSpecificEvents, MockCarState
from openpilot.selfdrive.car.helpers import convert_carControl, convert_to_capnp
from openpilot.selfdrive.controls.lib.events import Events, ET
from openpilot.selfdrive.selfdrived.events import Events, ET
REPLAY = "REPLAY" in os.environ
@ -266,7 +266,7 @@ class Car:
self.state_publish(CS, RD)
initialized = (not any(e.name == EventName.controlsInitializing for e in self.sm['onroadEvents']) and
initialized = (not any(e.name == EventName.selfdriveInitializing for e in self.sm['onroadEvents']) and
self.sm.seen['onroadEvents'])
if not self.CP.passive and initialized:
self.controls_update(CS, self.sm['carControl'])

624
selfdrive/controls/controlsd.py
View File

@ -1,132 +1,53 @@
#!/usr/bin/env python3
import os
import math
import time
import threading
from typing import SupportsFloat
import cereal.messaging as messaging
from cereal import car, log
from msgq.visionipc import VisionIpcClient, VisionStreamType
import cereal.messaging as messaging
from openpilot.common.conversions import Conversions as CV
from openpilot.common.git import get_short_branch
from openpilot.common.numpy_fast import clip
from openpilot.common.params import Params
from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL
from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper
from openpilot.common.swaglog import cloudlog
from openpilot.common.gps import get_gps_location_service
from opendbc.car.car_helpers import get_car_interface
from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature, get_startup_event
from openpilot.selfdrive.controls.lib.events import Events, ET
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
from openpilot.selfdrive.controls.lib.selfdrive import StateMachine
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
from openpilot.system.hardware import HARDWARE
JOYSTICK_MAX_LAT_ACCEL = 2.5 # m/s^2
REPLAY = "REPLAY" in os.environ
SIMULATION = "SIMULATION" in os.environ
TESTING_CLOSET = "TESTING_CLOSET" in os.environ
IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"}
ThermalStatus = log.DeviceState.ThermalStatus
State = log.SelfdriveState.OpenpilotState
PandaType = log.PandaState.PandaType
LaneChangeState = log.LaneChangeState
LaneChangeDirection = log.LaneChangeDirection
EventName = car.OnroadEvent.EventName
ButtonType = car.CarState.ButtonEvent.Type
SafetyModel = car.CarParams.SafetyModel
IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
CSID_MAP = {"1": EventName.roadCameraError, "2": EventName.wideRoadCameraError, "0": EventName.driverCameraError}
ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
class Controls:
def __init__(self, CI=None):
def __init__(self) -> None:
self.params = Params()
cloudlog.info("controlsd is waiting for CarParams")
self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
cloudlog.info("controlsd got CarParams")
if CI is None:
cloudlog.info("controlsd is waiting for CarParams")
self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
cloudlog.info("controlsd got CarParams")
self.CI = get_car_interface(self.CP)
# Uses car interface helper functions, altering state won't be considered by card for actuation
self.CI = get_car_interface(self.CP)
else:
self.CI, self.CP = CI, CI.CP
self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='carState')
self.pm = messaging.PubMaster(['carControl', 'controlsState'])
# Ensure the current branch is cached, otherwise the first iteration of controlsd lags
self.branch = get_short_branch()
# Setup sockets
self.pm = messaging.PubMaster(['selfdriveState', 'controlsState', 'carControl', 'onroadEvents'])
self.gps_location_service = get_gps_location_service(self.params)
self.gps_packets = [self.gps_location_service]
self.sensor_packets = ["accelerometer", "gyroscope"]
self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"]
self.log_sock = messaging.sub_sock('androidLog')
# TODO: de-couple controlsd with card/conflate on carState without introducing controls mismatches
self.car_state_sock = messaging.sub_sock('carState', timeout=20)
ignore = self.sensor_packets + self.gps_packets + ['testJoystick']
if SIMULATION:
ignore += ['driverCameraState', 'managerState']
if REPLAY:
# no vipc in replay will make them ignored anyways
ignore += ['roadCameraState', 'wideRoadCameraState']
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
'testJoystick', 'driverAssistance'] + self.camera_packets + self.sensor_packets + self.gps_packets,
ignore_alive=ignore, ignore_avg_freq=ignore+['testJoystick'], ignore_valid=['testJoystick', ],
frequency=int(1/DT_CTRL))
self.joystick_mode = self.params.get_bool("JoystickDebugMode")
# read params
self.is_metric = self.params.get_bool("IsMetric")
self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
# cleanup old params
if not self.CP.experimentalLongitudinalAvailable:
self.params.remove("ExperimentalLongitudinalEnabled")
if not self.CP.openpilotLongitudinalControl:
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()
self.steer_limited = False
self.desired_curvature = 0.0
self.pose_calibrator = PoseCalibrator()
self.calibrated_pose: Pose|None = None
self.LoC = LongControl(self.CP)
self.VM = VehicleModel(self.CP)
self.LaC: LatControl
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP, self.CI)
@ -135,341 +56,16 @@ class Controls:
elif self.CP.lateralTuning.which() == 'torque':
self.LaC = LatControlTorque(self.CP, self.CI)
self.initialized = False
self.enabled = False
self.active = False
self.mismatch_counter = 0
self.cruise_mismatch_counter = 0
self.last_steering_pressed_frame = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.logged_comm_issue = None
self.not_running_prev = None
self.steer_limited = False
self.desired_curvature = 0.0
self.experimental_mode = False
self.personality = self.read_personality_param()
self.recalibrating_seen = False
self.state_machine = StateMachine()
self.can_log_mono_time = 0
self.startup_event = get_startup_event(car_recognized, not self.CP.passive, len(self.CP.carFw) > 0)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
if len(self.CP.carFw) > 0:
set_offroad_alert("Offroad_CarUnrecognized", True)
else:
set_offroad_alert("Offroad_NoFirmware", True)
elif self.CP.passive:
self.events.add(EventName.dashcamMode, static=True)
# controlsd is driven by carState, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
def set_initial_state(self):
if REPLAY and any(ps.controlsAllowed for ps in self.sm['pandaStates']):
self.state_machine.state = State.enabled
def update_events(self, CS):
"""Compute onroadEvents from carState"""
self.events.clear()
# Add joystick event, static on cars, dynamic on nonCars
if self.joystick_mode:
self.events.add(EventName.joystickDebug)
self.startup_event = None
# Add startup event
if self.startup_event is not None:
self.events.add(self.startup_event)
self.startup_event = None
# Don't add any more events if not initialized
if not self.initialized:
self.events.add(EventName.controlsInitializing)
return
# no more events while in dashcam mode
if self.CP.passive:
return
# Block resume if cruise never previously enabled
resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents)
if not self.CP.pcmCruise and CS.vCruise > 250 and resume_pressed:
self.events.add(EventName.resumeBlocked)
if not self.CP.notCar:
self.events.add_from_msg(self.sm['driverMonitoringState'].events)
# Add car events, ignore if CAN isn't valid
if CS.canValid:
self.events.add_from_msg(CS.events)
# Create events for temperature, disk space, and memory
if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
self.events.add(EventName.overheat)
if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
self.events.add(EventName.outOfSpace)
if self.sm['deviceState'].memoryUsagePercent > 90 and not SIMULATION:
self.events.add(EventName.lowMemory)
# 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:
# allow enough time for the fan controller in the panda to recover from stalls
if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 15.0:
self.events.add(EventName.fanMalfunction)
else:
self.last_functional_fan_frame = self.sm.frame
# Handle calibration status
cal_status = self.sm['liveCalibration'].calStatus
if cal_status != log.LiveCalibrationData.Status.calibrated:
if cal_status == log.LiveCalibrationData.Status.uncalibrated:
self.events.add(EventName.calibrationIncomplete)
elif cal_status == log.LiveCalibrationData.Status.recalibrating:
if not self.recalibrating_seen:
set_offroad_alert("Offroad_Recalibration", True)
self.recalibrating_seen = True
self.events.add(EventName.calibrationRecalibrating)
else:
self.events.add(EventName.calibrationInvalid)
# Lane departure warning
if self.is_ldw_enabled and self.sm.valid['driverAssistance']:
if self.sm['driverAssistance'].leftLaneDeparture or self.sm['driverAssistance'].rightLaneDeparture:
self.events.add(EventName.ldw)
# Handle lane change
if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
direction = self.sm['modelV2'].meta.laneChangeDirection
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right):
self.events.add(EventName.laneChangeBlocked)
else:
if direction == LaneChangeDirection.left:
self.events.add(EventName.preLaneChangeLeft)
else:
self.events.add(EventName.preLaneChangeRight)
elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting,
LaneChangeState.laneChangeFinishing):
self.events.add(EventName.laneChange)
for i, pandaState in enumerate(self.sm['pandaStates']):
# All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
if i < len(self.CP.safetyConfigs):
safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \
pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \
pandaState.alternativeExperience != self.CP.alternativeExperience
else:
safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES
# safety mismatch allows some time for pandad to set the safety mode and publish it back from panda
if (safety_mismatch and self.sm.frame*DT_CTRL > 10.) or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200:
self.events.add(EventName.controlsMismatch)
if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
self.events.add(EventName.relayMalfunction)
# Handle HW and system malfunctions
# Order is very intentional here. Be careful when modifying this.
# All events here should at least have NO_ENTRY and SOFT_DISABLE.
num_events = len(self.events)
not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning}
if self.sm.recv_frame['managerState'] and (not_running - IGNORE_PROCESSES):
self.events.add(EventName.processNotRunning)
if not_running != self.not_running_prev:
cloudlog.event("process_not_running", not_running=not_running, error=True)
self.not_running_prev = not_running
else:
if not SIMULATION and not self.rk.lagging:
if not self.sm.all_alive(self.camera_packets):
self.events.add(EventName.cameraMalfunction)
elif not self.sm.all_freq_ok(self.camera_packets):
self.events.add(EventName.cameraFrameRate)
if not REPLAY and self.rk.lagging:
self.events.add(EventName.controlsdLagging)
if not self.sm.valid['radarState']:
self.events.add(EventName.radarFault)
if not self.sm.valid['pandaStates']:
self.events.add(EventName.usbError)
if CS.canTimeout:
self.events.add(EventName.canBusMissing)
elif not CS.canValid:
self.events.add(EventName.canError)
# generic catch-all. ideally, a more specific event should be added above instead
has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE))
no_system_errors = (not has_disable_events) or (len(self.events) == num_events)
if not self.sm.all_checks() and no_system_errors:
if not self.sm.all_alive():
self.events.add(EventName.commIssue)
elif not self.sm.all_freq_ok():
self.events.add(EventName.commIssueAvgFreq)
else:
self.events.add(EventName.commIssue)
logs = {
'invalid': [s for s, valid in self.sm.valid.items() if not valid],
'not_alive': [s for s, alive in self.sm.alive.items() if not alive],
'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok],
}
if logs != self.logged_comm_issue:
cloudlog.event("commIssue", error=True, **logs)
self.logged_comm_issue = logs
else:
self.logged_comm_issue = None
if not (self.CP.notCar and self.joystick_mode):
if not self.sm['livePose'].posenetOK:
self.events.add(EventName.posenetInvalid)
if not self.sm['livePose'].inputsOK:
self.events.add(EventName.locationdTemporaryError)
if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY):
self.events.add(EventName.paramsdTemporaryError)
# conservative HW alert. if the data or frequency are off, locationd will throw an error
if any((self.sm.frame - self.sm.recv_frame[s])*DT_CTRL > 10. for s in self.sensor_packets):
self.events.add(EventName.sensorDataInvalid)
if not REPLAY:
# Check for mismatch between openpilot and car's PCM
cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
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
planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
if (planner_fcw or model_fcw) and not (self.CP.notCar and self.joystick_mode):
self.events.add(EventName.fcw)
for m in messaging.drain_sock(self.log_sock, wait_for_one=False):
try:
msg = m.androidLog.message
if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")):
csid = msg.split("CSID:")[-1].split(" ")[0]
evt = CSID_MAP.get(csid, None)
if evt is not None:
self.events.add(evt)
except UnicodeDecodeError:
pass
# TODO: fix simulator
if not SIMULATION or REPLAY:
# Not show in first 1.5 km to allow for driving out of garage. This event shows after 5 minutes
gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_CTRL < 2.0
if not gps_ok and self.sm['livePose'].inputsOK and (self.distance_traveled > 1500):
self.events.add(EventName.noGps)
if gps_ok:
self.distance_traveled = 0
self.distance_traveled += CS.vEgo * DT_CTRL
if self.sm['modelV2'].frameDropPerc > 20:
self.events.add(EventName.modeldLagging)
# decrement personality on distance button press
if self.CP.openpilotLongitudinalControl:
if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents):
self.personality = (self.personality - 1) % 3
self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
self.events.add(EventName.personalityChanged)
def data_sample(self):
"""Receive data from sockets"""
car_state = messaging.recv_one(self.car_state_sock)
CS = car_state.carState if car_state else self.CS_prev
self.sm.update(0)
if not self.initialized:
all_valid = CS.canValid and self.sm.all_checks()
timed_out = self.sm.frame * DT_CTRL > 6.
if all_valid or timed_out or (SIMULATION and not REPLAY):
available_streams = VisionIpcClient.available_streams("camerad", block=False)
if VisionStreamType.VISION_STREAM_ROAD not in available_streams:
self.sm.ignore_alive.append('roadCameraState')
if VisionStreamType.VISION_STREAM_WIDE_ROAD not in available_streams:
self.sm.ignore_alive.append('wideRoadCameraState')
self.initialized = True
self.set_initial_state()
cloudlog.event(
"controlsd.initialized",
dt=self.sm.frame*DT_CTRL,
timeout=timed_out,
canValid=CS.canValid,
invalid=[s for s, valid in self.sm.valid.items() if not valid],
not_alive=[s for s, alive in self.sm.alive.items() if not alive],
not_freq_ok=[s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok],
error=True,
)
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not self.enabled:
self.mismatch_counter = 0
# All pandas not in silent mode must have controlsAllowed when openpilot is enabled
if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates']
if ps.safetyModel not in IGNORED_SAFETY_MODES):
self.mismatch_counter += 1
# calibrate the live pose and save it for later use
def update(self):
self.sm.update(15)
if self.sm.updated["liveCalibration"]:
self.pose_calibrator.feed_live_calib(self.sm['liveCalibration'])
if self.sm.updated["livePose"]:
device_pose = Pose.from_live_pose(self.sm['livePose'])
self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose)
return CS
def state_control(self, CS):
"""Given the state, this function returns a CarControl packet"""
def state_control(self):
CS = self.sm['carState']
# Update VehicleModel
lp = self.sm['liveParameters']
@ -488,13 +84,12 @@ class Controls:
model_v2 = self.sm['modelV2']
CC = car.CarControl.new_message()
CC.enabled = self.enabled
CC.enabled = self.sm['selfdriveState'].enabled
# Check which actuators can be enabled
standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
(not standstill or self.joystick_mode)
CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
CC.latActive = self.sm['selfdriveState'].active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and not standstill
CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl
actuators = CC.actuators
actuators.longControlState = self.LoC.long_control_state
@ -504,51 +99,21 @@ class Controls:
CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left
CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right
if CS.leftBlinker or CS.rightBlinker:
self.last_blinker_frame = self.sm.frame
# State specific actions
if not CC.latActive:
self.LaC.reset()
if not CC.longActive:
self.LoC.reset()
if not self.joystick_mode:
# accel PID loop
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)
# accel PID loop
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)
# Steering PID loop and lateral MPC
self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature)
actuators.curvature = self.desired_curvature
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
self.steer_limited, self.desired_curvature,
self.calibrated_pose) # TODO what if not available
else:
lac_log = log.ControlsState.LateralDebugState.new_message()
if self.sm.recv_frame['testJoystick'] > 0:
# reset joystick if it hasn't been received in a while
should_reset_joystick = (self.sm.frame - self.sm.recv_frame['testJoystick'])*DT_CTRL > 0.2
if not should_reset_joystick:
joystick_axes = self.sm['testJoystick'].axes
else:
joystick_axes = [0.0, 0.0]
if CC.longActive:
actuators.accel = 4.0*clip(joystick_axes[0], -1, 1)
if CC.latActive:
max_curvature = JOYSTICK_MAX_LAT_ACCEL / max(CS.vEgo ** 2, 1)
max_angle = math.degrees(self.VM.get_steer_from_curvature(max_curvature, CS.vEgo, lp.roll))
steer = clip(joystick_axes[1], -1, 1)
actuators.steer, actuators.steeringAngleDeg, actuators.curvature = steer, steer * max_angle, steer * -max_curvature
lac_log.active = self.active
lac_log.steeringAngleDeg = CS.steeringAngleDeg
lac_log.output = actuators.steer
lac_log.saturated = abs(actuators.steer) >= 0.9
# Steering PID loop and lateral MPC
self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature)
actuators.curvature = self.desired_curvature
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
self.steer_limited, self.desired_curvature,
self.calibrated_pose) # TODO what if not available
# Ensure no NaNs/Infs
for p in ACTUATOR_FIELDS:
@ -562,53 +127,37 @@ class Controls:
return CC, lac_log
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)
def publish(self, CC, lac_log):
CS = self.sm['carState']
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:
CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()
CC.cruiseControl.override = self.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl
CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
if self.joystick_mode and self.sm.recv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
CC.cruiseControl.cancel = True
CC.cruiseControl.override = CC.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl
CC.cruiseControl.cancel = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise)
speeds = self.sm['longitudinalPlan'].speeds
if len(speeds):
CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1
CC.cruiseControl.resume = CC.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1
hudControl = CC.hudControl
hudControl.setSpeed = float(CS.vCruiseCluster * CV.KPH_TO_MS)
hudControl.speedVisible = self.enabled
hudControl.lanesVisible = self.enabled
hudControl.speedVisible = CC.enabled
hudControl.lanesVisible = CC.enabled
hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
hudControl.leadDistanceBars = self.personality + 1
hudControl.leadDistanceBars = self.sm['selfdriveState'].personality.raw + 1
hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual
hudControl.rightLaneVisible = True
hudControl.leftLaneVisible = True
if self.is_ldw_enabled and self.sm.valid['driverAssistance']:
if self.sm.valid['driverAssistance']:
hudControl.leftLaneDepart = self.sm['driverAssistance'].leftLaneDeparture
hudControl.rightLaneDepart = self.sm['driverAssistance'].rightLaneDeparture
if self.AM.current_alert:
hudControl.visualAlert = self.AM.current_alert.visual_alert
if not self.CP.passive and self.initialized:
if self.sm['selfdriveState'].active:
CO = self.sm['carOutput']
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.steer_limited = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \
@ -616,6 +165,8 @@ class Controls:
else:
self.steer_limited = abs(CC.actuators.steer - CO.actuatorsOutput.steer) > 1e-2
# TODO: both controlsState and carControl valids should be set by
# sm.all_checks(), but this creates a circular dependency
# controlsState
dat = messaging.new_message('controlsState')
@ -633,14 +184,11 @@ class Controls:
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))
(self.sm['selfdriveState'].state == State.softDisabling))
lat_tuning = self.CP.lateralTuning.which()
if self.joystick_mode:
cs.lateralControlState.debugState = lac_log
elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
cs.lateralControlState.angleState = lac_log
elif lat_tuning == 'pid':
cs.lateralControlState.pidState = lac_log
@ -655,84 +203,18 @@ class Controls:
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 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
ss.engageable = not self.events.contains(ET.NO_ENTRY)
ss.experimentalMode = self.experimental_mode
ss.personality = self.personality
self.pm.send('selfdriveState', ss_msg)
# onroadEvents - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev):
ce_send = messaging.new_message('onroadEvents', len(self.events))
ce_send.valid = True
ce_send.onroadEvents = self.events.to_msg()
self.pm.send('onroadEvents', ce_send)
self.events_prev = self.events.names.copy()
def step(self):
CS = self.data_sample()
self.update_events(CS)
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_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:
return int(self.params.get('LongitudinalPersonality'))
except (ValueError, TypeError):
return log.LongitudinalPersonality.standard
def params_thread(self, evt):
while not evt.is_set():
self.is_metric = self.params.get_bool("IsMetric")
self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
self.personality = self.read_personality_param()
if self.CP.notCar:
self.joystick_mode = self.params.get_bool("JoystickDebugMode")
time.sleep(0.1)
def controlsd_thread(self):
e = threading.Event()
t = threading.Thread(target=self.params_thread, args=(e, ))
try:
t.start()
while True:
self.step()
self.rk.monitor_time()
finally:
e.set()
t.join()
def run(self):
rk = Ratekeeper(100, print_delay_threshold=None)
while True:
self.update()
CC, lac_log = self.state_control()
self.publish(CC, lac_log)
rk.keep_time()
def main():
config_realtime_process(4, Priority.CTRL_HIGH)
controls = Controls()
controls.controlsd_thread()
controls.run()
if __name__ == "__main__":

22
selfdrive/controls/lib/drive_helpers.py
View File

@ -1,9 +1,6 @@
from cereal import car, log
from cereal import log
from openpilot.common.numpy_fast import clip
from openpilot.common.realtime import DT_CTRL
from openpilot.system.version import get_build_metadata
EventName = car.OnroadEvent.EventName
MIN_SPEED = 1.0
CONTROL_N = 17
@ -29,20 +26,3 @@ def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float:
vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
return float(vel_err)
return 0.0
def get_startup_event(car_recognized, controller_available, fw_seen):
build_metadata = get_build_metadata()
if build_metadata.openpilot.comma_remote and build_metadata.tested_channel:
event = EventName.startup
else:
event = EventName.startupMaster
if not car_recognized:
if fw_seen:
event = EventName.startupNoCar
else:
event = EventName.startupNoFw
elif car_recognized and not controller_available:
event = EventName.startupNoControl
return event

121
selfdrive/controls/tests/test_startup.py
View File

@ -1,121 +0,0 @@
import os
from parameterized import parameterized
from cereal import log, car
import cereal.messaging as messaging
from opendbc.car.fingerprints import _FINGERPRINTS
from opendbc.car.toyota.values import CAR as TOYOTA
from opendbc.car.mazda.values import CAR as MAZDA
from openpilot.common.params import Params
from openpilot.selfdrive.pandad.pandad_api_impl import can_list_to_can_capnp
from openpilot.selfdrive.controls.lib.events import EVENT_NAME
from openpilot.system.manager.process_config import managed_processes
EventName = car.OnroadEvent.EventName
Ecu = car.CarParams.Ecu
COROLLA_FW_VERSIONS = [
(Ecu.engine, 0x7e0, None, b'\x0230ZC2000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00'),
(Ecu.abs, 0x7b0, None, b'F152602190\x00\x00\x00\x00\x00\x00'),
(Ecu.eps, 0x7a1, None, b'8965B02181\x00\x00\x00\x00\x00\x00'),
(Ecu.fwdRadar, 0x750, 0xf, b'8821F4702100\x00\x00\x00\x00'),
(Ecu.fwdCamera, 0x750, 0x6d, b'8646F0201101\x00\x00\x00\x00'),
(Ecu.dsu, 0x791, None, b'881510201100\x00\x00\x00\x00'),
]
COROLLA_FW_VERSIONS_FUZZY = COROLLA_FW_VERSIONS[:-1] + [(Ecu.dsu, 0x791, None, b'xxxxxx')]
COROLLA_FW_VERSIONS_NO_DSU = COROLLA_FW_VERSIONS[:-1]
CX5_FW_VERSIONS = [
(Ecu.engine, 0x7e0, None, b'PYNF-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
(Ecu.abs, 0x760, None, b'K123-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
(Ecu.eps, 0x730, None, b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
(Ecu.fwdRadar, 0x764, None, b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
(Ecu.fwdCamera, 0x706, None, b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
(Ecu.transmission, 0x7e1, None, b'PYNC-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
]
@parameterized.expand([
# TODO: test EventName.startup for release branches
# officially supported car
(EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS, "toyota"),
(EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS, "toyota"),
# dashcamOnly car
(EventName.startupNoControl, MAZDA.MAZDA_CX5, CX5_FW_VERSIONS, "mazda"),
(EventName.startupNoControl, MAZDA.MAZDA_CX5, CX5_FW_VERSIONS, "mazda"),
# unrecognized car with no fw
(EventName.startupNoFw, None, None, ""),
(EventName.startupNoFw, None, None, ""),
# unrecognized car
(EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"),
(EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"),
# fuzzy match
(EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"),
(EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"),
])
def test_startup_alert(expected_event, car_model, fw_versions, brand):
controls_sock = messaging.sub_sock("selfdriveState")
pm = messaging.PubMaster(['can', 'pandaStates'])
params = Params()
params.put_bool("OpenpilotEnabledToggle", True)
# Build capnn version of FW array
if fw_versions is not None:
car_fw = []
cp = car.CarParams.new_message()
for ecu, addr, subaddress, version in fw_versions:
f = car.CarParams.CarFw.new_message()
f.ecu = ecu
f.address = addr
f.fwVersion = version
f.brand = brand
if subaddress is not None:
f.subAddress = subaddress
car_fw.append(f)
cp.carVin = "1" * 17
cp.carFw = car_fw
params.put("CarParamsCache", cp.to_bytes())
else:
os.environ['SKIP_FW_QUERY'] = '1'
managed_processes['controlsd'].start()
managed_processes['card'].start()
assert pm.wait_for_readers_to_update('can', 5)
pm.send('can', can_list_to_can_capnp([[0, b"", 0]]))
assert pm.wait_for_readers_to_update('pandaStates', 5)
msg = messaging.new_message('pandaStates', 1)
msg.pandaStates[0].pandaType = log.PandaState.PandaType.uno
pm.send('pandaStates', msg)
# fingerprint
if (car_model is None) or (fw_versions is not None):
finger = {addr: 1 for addr in range(1, 100)}
else:
finger = _FINGERPRINTS[car_model][0]
msgs = [[addr, b'\x00'*length, 0] for addr, length in finger.items()]
for _ in range(1000):
# card waits for pandad to echo back that it has changed the multiplexing mode
if not params.get_bool("ObdMultiplexingChanged"):
params.put_bool("ObdMultiplexingChanged", True)
pm.send('can', can_list_to_can_capnp(msgs))
assert pm.wait_for_readers_to_update('can', 5, dt=0.001), f"step: {_}"
ctrls = messaging.drain_sock(controls_sock)
if len(ctrls):
event_name = ctrls[0].selfdriveState.alertType.split("/")[0]
assert EVENT_NAME[expected_event] == event_name, f"expected {EVENT_NAME[expected_event]} for '{car_model}', got {event_name}"
break
else:
raise Exception(f"failed to fingerprint {car_model}")

4
selfdrive/debug/cycle_alerts.py
View File

@ -6,8 +6,8 @@ from cereal import car, log
import cereal.messaging as messaging
from opendbc.car.honda.interface import CarInterface
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.events import ET, Events
from openpilot.selfdrive.controls.lib.alertmanager import AlertManager
from openpilot.selfdrive.selfdrived.events import ET, Events
from openpilot.selfdrive.selfdrived.alertmanager import AlertManager
from openpilot.system.manager.process_config import managed_processes
EventName = car.OnroadEvent.EventName

2
selfdrive/monitoring/helpers.py
View File

@ -2,7 +2,7 @@ from math import atan2
from cereal import car
import cereal.messaging as messaging
from openpilot.selfdrive.controls.lib.events import Events
from openpilot.selfdrive.selfdrived.events import Events
from openpilot.common.numpy_fast import interp
from openpilot.common.realtime import DT_DMON
from openpilot.common.filter_simple import FirstOrderFilter

4
selfdrive/controls/lib/alertmanager.py → selfdrive/selfdrived/alertmanager.py
View File

@ -6,10 +6,10 @@ from dataclasses import dataclass
from openpilot.common.basedir import BASEDIR
from openpilot.common.params import Params
from openpilot.selfdrive.controls.lib.events import Alert
from openpilot.selfdrive.selfdrived.events import Alert
with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f:
with open(os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json")) as f:
OFFROAD_ALERTS = json.load(f)

0
selfdrive/controls/lib/alerts_offroad.json → selfdrive/selfdrived/alerts_offroad.json
View File

17
selfdrive/controls/lib/events.py → selfdrive/selfdrived/events.py
View File

@ -318,10 +318,11 @@ def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM
def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
axes = sm['testJoystick'].axes
gb, steer = list(axes)[:2] if len(axes) else (0., 0.)
vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%"
return NormalPermanentAlert("Joystick Mode", vals)
# TODO: add some info back
#axes = sm['testJoystick'].axes
#gb, steer = list(axes)[:2] if len(axes) else (0., 0.)
#vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%"
return NormalPermanentAlert("Joystick Mode", "")
def personality_changed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
personality = str(personality).title()
@ -342,7 +343,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
ET.PERMANENT: NormalPermanentAlert("Joystick Mode"),
},
EventName.controlsInitializing: {
EventName.selfdriveInitializing: {
ET.NO_ENTRY: NoEntryAlert("System Initializing"),
},
@ -773,9 +774,9 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
ET.NO_ENTRY: NoEntryAlert("Low Communication Rate Between Processes"),
},
EventName.controlsdLagging: {
ET.SOFT_DISABLE: soft_disable_alert("Controls Lagging"),
ET.NO_ENTRY: NoEntryAlert("Controls Process Lagging: Reboot Your Device"),
EventName.selfdrivedLagging: {
ET.SOFT_DISABLE: soft_disable_alert("System Lagging"),
ET.NO_ENTRY: NoEntryAlert("Selfdrive Process Lagging: Reboot Your Device"),
},
# Thrown when manager detects a service exited unexpectedly while driving

518
selfdrive/selfdrived/selfdrived.py Executable file
View File

@ -0,0 +1,518 @@
#!/usr/bin/env python3
import os
import time
import threading
import cereal.messaging as messaging
from cereal import car, log
from msgq.visionipc import VisionIpcClient, VisionStreamType
from openpilot.common.git import get_short_branch
from openpilot.common.params import Params
from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL
from openpilot.common.swaglog import cloudlog
from openpilot.common.gps import get_gps_location_service
from openpilot.selfdrive.selfdrived.events import Events, ET
from openpilot.selfdrive.selfdrived.state import StateMachine
from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert
from openpilot.system.hardware import HARDWARE
from openpilot.system.version import get_build_metadata
REPLAY = "REPLAY" in os.environ
SIMULATION = "SIMULATION" in os.environ
TESTING_CLOSET = "TESTING_CLOSET" in os.environ
IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"}
ThermalStatus = log.DeviceState.ThermalStatus
State = log.SelfdriveState.OpenpilotState
PandaType = log.PandaState.PandaType
LaneChangeState = log.LaneChangeState
LaneChangeDirection = log.LaneChangeDirection
EventName = car.OnroadEvent.EventName
ButtonType = car.CarState.ButtonEvent.Type
SafetyModel = car.CarParams.SafetyModel
IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
CSID_MAP = {"1": EventName.roadCameraError, "2": EventName.wideRoadCameraError, "0": EventName.driverCameraError}
def get_startup_event(car_recognized, controller_available, fw_seen):
build_metadata = get_build_metadata()
if build_metadata.openpilot.comma_remote and build_metadata.tested_channel:
event = EventName.startup
else:
event = EventName.startupMaster
if not car_recognized:
if fw_seen:
event = EventName.startupNoCar
else:
event = EventName.startupNoFw
elif car_recognized and not controller_available:
event = EventName.startupNoControl
return event
class SelfdriveD:
def __init__(self):
self.params = Params()
# Ensure the current branch is cached, otherwise the first cycle lags
self.branch = get_short_branch()
cloudlog.info("selfdrived is waiting for CarParams")
self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
cloudlog.info("selfdrived got CarParams")
# Setup sockets
self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'])
self.gps_location_service = get_gps_location_service(self.params)
self.gps_packets = [self.gps_location_service]
self.sensor_packets = ["accelerometer", "gyroscope"]
self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"]
self.log_sock = messaging.sub_sock('androidLog')
# TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches
self.car_state_sock = messaging.sub_sock('carState', timeout=20)
ignore = self.sensor_packets + self.gps_packets
if SIMULATION:
ignore += ['driverCameraState', 'managerState']
if REPLAY:
# no vipc in replay will make them ignored anyways
ignore += ['roadCameraState', 'wideRoadCameraState']
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
'controlsState', 'carControl', 'driverAssistance'] + \
self.camera_packets + self.sensor_packets + self.gps_packets,
ignore_alive=ignore, ignore_avg_freq=ignore+['radarState',],
frequency=int(1/DT_CTRL))
# read params
self.is_metric = self.params.get_bool("IsMetric")
self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
# cleanup old params
if not self.CP.experimentalLongitudinalAvailable:
self.params.remove("ExperimentalLongitudinalEnabled")
if not self.CP.openpilotLongitudinalControl:
self.params.remove("ExperimentalMode")
self.CS_prev = car.CarState.new_message()
self.AM = AlertManager()
self.events = Events()
self.initialized = False
self.enabled = False
self.active = False
self.mismatch_counter = 0
self.cruise_mismatch_counter = 0
self.last_steering_pressed_frame = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.logged_comm_issue = None
self.not_running_prev = None
self.experimental_mode = False
self.personality = self.read_personality_param()
self.recalibrating_seen = False
self.state_machine = StateMachine()
self.startup_event = get_startup_event(car_recognized, not self.CP.passive, len(self.CP.carFw) > 0)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
if len(self.CP.carFw) > 0:
set_offroad_alert("Offroad_CarUnrecognized", True)
else:
set_offroad_alert("Offroad_NoFirmware", True)
elif self.CP.passive:
self.events.add(EventName.dashcamMode, static=True)
# selfdrived is driven by carState, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
def set_initial_state(self):
if REPLAY and any(ps.controlsAllowed for ps in self.sm['pandaStates']):
self.state_machine.state = State.enabled
def update_events(self, CS):
"""Compute onroadEvents from carState"""
self.events.clear()
if self.sm['controlsState'].lateralControlState.which() == 'debugState':
self.events.add(EventName.joystickDebug)
self.startup_event = None
# Add startup event
if self.startup_event is not None:
self.events.add(self.startup_event)
self.startup_event = None
# Don't add any more events if not initialized
if not self.initialized:
self.events.add(EventName.selfdriveInitializing)
return
# no more events while in dashcam mode
if self.CP.passive:
return
# Block resume if cruise never previously enabled
resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents)
if not self.CP.pcmCruise and CS.vCruise > 250 and resume_pressed:
self.events.add(EventName.resumeBlocked)
if not self.CP.notCar:
self.events.add_from_msg(self.sm['driverMonitoringState'].events)
# Add car events, ignore if CAN isn't valid
if CS.canValid:
self.events.add_from_msg(CS.events)
# Create events for temperature, disk space, and memory
if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
self.events.add(EventName.overheat)
if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
self.events.add(EventName.outOfSpace)
if self.sm['deviceState'].memoryUsagePercent > 90 and not SIMULATION:
self.events.add(EventName.lowMemory)
# 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:
# allow enough time for the fan controller in the panda to recover from stalls
if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 15.0:
self.events.add(EventName.fanMalfunction)
else:
self.last_functional_fan_frame = self.sm.frame
# Handle calibration status
cal_status = self.sm['liveCalibration'].calStatus
if cal_status != log.LiveCalibrationData.Status.calibrated:
if cal_status == log.LiveCalibrationData.Status.uncalibrated:
self.events.add(EventName.calibrationIncomplete)
elif cal_status == log.LiveCalibrationData.Status.recalibrating:
if not self.recalibrating_seen:
set_offroad_alert("Offroad_Recalibration", True)
self.recalibrating_seen = True
self.events.add(EventName.calibrationRecalibrating)
else:
self.events.add(EventName.calibrationInvalid)
# Lane departure warning
if self.is_ldw_enabled and self.sm.valid['driverAssistance']:
if self.sm['driverAssistance'].leftLaneDeparture or self.sm['driverAssistance'].rightLaneDeparture:
self.events.add(EventName.ldw)
# Handle lane change
if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
direction = self.sm['modelV2'].meta.laneChangeDirection
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right):
self.events.add(EventName.laneChangeBlocked)
else:
if direction == LaneChangeDirection.left:
self.events.add(EventName.preLaneChangeLeft)
else:
self.events.add(EventName.preLaneChangeRight)
elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting,
LaneChangeState.laneChangeFinishing):
self.events.add(EventName.laneChange)
for i, pandaState in enumerate(self.sm['pandaStates']):
# All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
if i < len(self.CP.safetyConfigs):
safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \
pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \
pandaState.alternativeExperience != self.CP.alternativeExperience
else:
safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES
# safety mismatch allows some time for pandad to set the safety mode and publish it back from panda
if (safety_mismatch and self.sm.frame*DT_CTRL > 10.) or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200:
self.events.add(EventName.controlsMismatch)
if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
self.events.add(EventName.relayMalfunction)
# Handle HW and system malfunctions
# Order is very intentional here. Be careful when modifying this.
# All events here should at least have NO_ENTRY and SOFT_DISABLE.
num_events = len(self.events)
not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning}
if self.sm.recv_frame['managerState'] and (not_running - IGNORE_PROCESSES):
self.events.add(EventName.processNotRunning)
if not_running != self.not_running_prev:
cloudlog.event("process_not_running", not_running=not_running, error=True)
self.not_running_prev = not_running
else:
if not SIMULATION and not self.rk.lagging:
if not self.sm.all_alive(self.camera_packets):
self.events.add(EventName.cameraMalfunction)
elif not self.sm.all_freq_ok(self.camera_packets):
self.events.add(EventName.cameraFrameRate)
if not REPLAY and self.rk.lagging:
self.events.add(EventName.selfdrivedLagging)
if len(self.sm['radarState'].radarErrors) or ((not self.rk.lagging or REPLAY) and not self.sm.all_checks(['radarState'])):
self.events.add(EventName.radarFault)
if not self.sm.valid['pandaStates']:
self.events.add(EventName.usbError)
if CS.canTimeout:
self.events.add(EventName.canBusMissing)
elif not CS.canValid:
self.events.add(EventName.canError)
# generic catch-all. ideally, a more specific event should be added above instead
has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE))
no_system_errors = (not has_disable_events) or (len(self.events) == num_events)
if not self.sm.all_checks() and no_system_errors:
if not self.sm.all_alive():
self.events.add(EventName.commIssue)
elif not self.sm.all_freq_ok():
self.events.add(EventName.commIssueAvgFreq)
else:
self.events.add(EventName.commIssue)
logs = {
'invalid': [s for s, valid in self.sm.valid.items() if not valid],
'not_alive': [s for s, alive in self.sm.alive.items() if not alive],
'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok],
}
if logs != self.logged_comm_issue:
cloudlog.event("commIssue", error=True, **logs)
self.logged_comm_issue = logs
else:
self.logged_comm_issue = None
if not self.CP.notCar:
if not self.sm['livePose'].posenetOK:
self.events.add(EventName.posenetInvalid)
if not self.sm['livePose'].inputsOK:
self.events.add(EventName.locationdTemporaryError)
if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY):
self.events.add(EventName.paramsdTemporaryError)
# conservative HW alert. if the data or frequency are off, locationd will throw an error
if any((self.sm.frame - self.sm.recv_frame[s])*DT_CTRL > 10. for s in self.sensor_packets):
self.events.add(EventName.sensorDataInvalid)
if not REPLAY:
# Check for mismatch between openpilot and car's PCM
cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
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
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
lac = getattr(self.sm['controlsState'].lateralControlState, self.sm['controlsState'].lateralControlState.which())
if lac.active and not recent_steer_pressed and not self.CP.notCar:
if self.CP.lateralTuning.which() == 'torque':
undershooting = abs(lac.desiredLateralAccel) / abs(1e-3 + lac.actualLateralAccel) > 1.2
turning = abs(lac.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.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.sm['carControl'].actuators.steeringAngleDeg
else:
steering_value = self.sm['carControl'].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
planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
if (planner_fcw or model_fcw) and not self.CP.notCar:
self.events.add(EventName.fcw)
# Camera errors from the kernel
for m in messaging.drain_sock(self.log_sock, wait_for_one=False):
try:
msg = m.androidLog.message
if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")):
csid = msg.split("CSID:")[-1].split(" ")[0]
evt = CSID_MAP.get(csid, None)
if evt is not None:
self.events.add(evt)
except UnicodeDecodeError:
pass
# TODO: fix simulator
if not SIMULATION or REPLAY:
# Not show in first 1.5 km to allow for driving out of garage. This event shows after 5 minutes
gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_CTRL < 2.0
if not gps_ok and self.sm['livePose'].inputsOK and (self.distance_traveled > 1500):
self.events.add(EventName.noGps)
if gps_ok:
self.distance_traveled = 0
self.distance_traveled += CS.vEgo * DT_CTRL
if self.sm['modelV2'].frameDropPerc > 20:
self.events.add(EventName.modeldLagging)
# decrement personality on distance button press
if self.CP.openpilotLongitudinalControl:
if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents):
self.personality = (self.personality - 1) % 3
self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
self.events.add(EventName.personalityChanged)
def data_sample(self):
car_state = messaging.recv_one(self.car_state_sock)
CS = car_state.carState if car_state else self.CS_prev
self.sm.update(0)
if not self.initialized:
all_valid = CS.canValid and self.sm.all_checks()
timed_out = self.sm.frame * DT_CTRL > 6.
if all_valid or timed_out or (SIMULATION and not REPLAY):
available_streams = VisionIpcClient.available_streams("camerad", block=False)
if VisionStreamType.VISION_STREAM_ROAD not in available_streams:
self.sm.ignore_alive.append('roadCameraState')
if VisionStreamType.VISION_STREAM_WIDE_ROAD not in available_streams:
self.sm.ignore_alive.append('wideRoadCameraState')
self.initialized = True
self.set_initial_state()
cloudlog.event(
"selfdrived.initialized",
dt=self.sm.frame*DT_CTRL,
timeout=timed_out,
canValid=CS.canValid,
invalid=[s for s, valid in self.sm.valid.items() if not valid],
not_alive=[s for s, alive in self.sm.alive.items() if not alive],
not_freq_ok=[s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok],
error=True,
)
# When the panda and selfdrived do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not self.enabled:
self.mismatch_counter = 0
# All pandas not in silent mode must have controlsAllowed when openpilot is enabled
if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates']
if ps.safetyModel not in IGNORED_SAFETY_MODES):
self.mismatch_counter += 1
return CS
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_selfdriveState(self, CS):
# selfdriveState
ss_msg = messaging.new_message('selfdriveState')
ss_msg.valid = True
ss = ss_msg.selfdriveState
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
ss.engageable = not self.events.contains(ET.NO_ENTRY)
ss.experimentalMode = self.experimental_mode
ss.personality = self.personality
self.pm.send('selfdriveState', ss_msg)
# onroadEvents - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev):
ce_send = messaging.new_message('onroadEvents', len(self.events))
ce_send.valid = True
ce_send.onroadEvents = self.events.to_msg()
self.pm.send('onroadEvents', ce_send)
self.events_prev = self.events.names.copy()
def step(self):
CS = self.data_sample()
self.update_events(CS)
if not self.CP.passive and self.initialized:
self.enabled, self.active = self.state_machine.update(self.events)
self.update_alerts(CS)
self.publish_selfdriveState(CS)
self.CS_prev = CS
def read_personality_param(self):
try:
return int(self.params.get('LongitudinalPersonality'))
except (ValueError, TypeError):
return log.LongitudinalPersonality.standard
def params_thread(self, evt):
while not evt.is_set():
self.is_metric = self.params.get_bool("IsMetric")
self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
self.personality = self.read_personality_param()
time.sleep(0.1)
def run(self):
e = threading.Event()
t = threading.Thread(target=self.params_thread, args=(e, ))
try:
t.start()
while True:
self.step()
self.rk.monitor_time()
finally:
e.set()
t.join()
def main():
config_realtime_process(4, Priority.CTRL_HIGH)
s = SelfdriveD()
s.run()
if __name__ == "__main__":
main()

2
selfdrive/controls/lib/selfdrive.py → selfdrive/selfdrived/state.py
View File

@ -1,5 +1,5 @@
from cereal import log
from openpilot.selfdrive.controls.lib.events import Events, ET
from openpilot.selfdrive.selfdrived.events import Events, ET
from openpilot.common.realtime import DT_CTRL
State = log.SelfdriveState.OpenpilotState

4
selfdrive/controls/lib/tests/test_alertmanager.py → selfdrive/selfdrived/tests/test_alertmanager.py
View File

@ -1,7 +1,7 @@
import random
from openpilot.selfdrive.controls.lib.events import Alert, EVENTS
from openpilot.selfdrive.controls.lib.alertmanager import AlertManager
from openpilot.selfdrive.selfdrived.events import Alert, EVENTS
from openpilot.selfdrive.selfdrived.alertmanager import AlertManager
class TestAlertManager:

6
selfdrive/controls/tests/test_alerts.py → selfdrive/selfdrived/tests/test_alerts.py
View File

@ -8,13 +8,13 @@ from cereal import log, car
from cereal.messaging import SubMaster
from openpilot.common.basedir import BASEDIR
from openpilot.common.params import Params
from openpilot.selfdrive.controls.lib.events import Alert, EVENTS, ET
from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
from openpilot.selfdrive.selfdrived.events import Alert, EVENTS, ET
from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS
AlertSize = log.SelfdriveState.AlertSize
OFFROAD_ALERTS_PATH = os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")
OFFROAD_ALERTS_PATH = os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json")
# TODO: add callback alerts
ALERTS = []

2
selfdrive/controls/tests/test_state_machine.py → selfdrive/selfdrived/tests/test_state_machine.py
View File

@ -1,7 +1,7 @@
from cereal import log
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.selfdrive import StateMachine, SOFT_DISABLE_TIME
from openpilot.selfdrive.controls.lib.events import Events, ET, EVENTS, NormalPermanentAlert
from openpilot.selfdrive.selfdrived.events import Events, ET, EVENTS, NormalPermanentAlert
State = log.SelfdriveState.OpenpilotState

2
selfdrive/test/process_replay/compare_logs.py
View File

@ -141,7 +141,7 @@ def format_diff(results, log_paths, ref_commit):
if __name__ == "__main__":
log1 = list(LogReader(sys.argv[1]))
log2 = list(LogReader(sys.argv[2]))
ignore_fields = sys.argv[3:] or ["logMonoTime", "controlsState.cumLagMs"]
ignore_fields = sys.argv[3:] or ["logMonoTime"]
results = {"segment": {"proc": compare_logs(log1, log2, ignore_fields)}}
log_paths = {"segment": {"proc": {"ref": sys.argv[1], "new": sys.argv[2]}}}
diff_short, diff_long, failed = format_diff(results, log_paths, None)

29
selfdrive/test/process_replay/process_replay.py
View File

@ -373,7 +373,7 @@ def get_car_params_callback(rc, pm, msgs, fingerprint):
params.put("CarParams", convert_to_capnp(CP).to_bytes())
def controlsd_rcv_callback(msg, cfg, frame):
def selfdrived_rcv_callback(msg, cfg, frame):
return (frame - 1) == 0 or msg.which() == 'carState'
@ -452,7 +452,7 @@ class FrequencyBasedRcvCallback:
return bool(len(resp_sockets))
def controlsd_config_callback(params, cfg, lr):
def selfdrived_config_callback(params, cfg, lr):
ublox = params.get_bool("UbloxAvailable")
sub_keys = ({"gpsLocation", } if ublox else {"gpsLocationExternal", })
@ -461,22 +461,33 @@ def controlsd_config_callback(params, cfg, lr):
CONFIGS = [
ProcessConfig(
proc_name="controlsd",
proc_name="selfdrived",
pubs=[
"carState", "deviceState", "pandaStates", "peripheralState", "liveCalibration", "driverMonitoringState",
"longitudinalPlan", "livePose", "liveParameters", "radarState",
"modelV2", "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState",
"testJoystick", "liveTorqueParameters", "accelerometer", "gyroscope", "carOutput",
"gpsLocationExternal", "gpsLocation", "driverAssistance"
"liveTorqueParameters", "accelerometer", "gyroscope", "carOutput",
"gpsLocationExternal", "gpsLocation", "controlsState", "carControl", "driverAssistance",
],
subs=["selfdriveState", "controlsState", "carControl", "onroadEvents"],
ignore=["logMonoTime", "controlsState.cumLagMs"],
config_callback=controlsd_config_callback,
subs=["selfdriveState", "onroadEvents"],
ignore=["logMonoTime"],
config_callback=selfdrived_config_callback,
init_callback=get_car_params_callback,
should_recv_callback=controlsd_rcv_callback,
should_recv_callback=selfdrived_rcv_callback,
tolerance=NUMPY_TOLERANCE,
processing_time=0.004,
),
ProcessConfig(
proc_name="controlsd",
pubs=["liveParameters", "liveTorqueParameters", "modelV2", "selfdriveState",
"liveCalibration", "livePose", "longitudinalPlan", "carState", "carOutput",
"driverMonitoringState", "onroadEvents", "driverAssistance"],
subs=["carControl", "controlsState"],
ignore=["logMonoTime", ],
init_callback=get_car_params_callback,
should_recv_callback=MessageBasedRcvCallback("carState"),
tolerance=NUMPY_TOLERANCE,
),
ProcessConfig(
proc_name="card",
pubs=["pandaStates", "carControl", "onroadEvents", "can"],

2
selfdrive/test/process_replay/ref_commit
View File

@ -1 +1 @@
fc1344b16b802cdb4ec542791f3b4d76a82da68b
eac137f456f25bf138677315b7c4907e2fe9971b

2
selfdrive/test/process_replay/test_fuzzy.py
View File

@ -11,7 +11,7 @@ import openpilot.selfdrive.test.process_replay.process_replay as pr
# These processes currently fail because of unrealistic data breaking assumptions
# that openpilot makes causing error with NaN, inf, int size, array indexing ...
# TODO: Make each one testable
NOT_TESTED = ['controlsd', 'card', 'plannerd', 'calibrationd', 'dmonitoringd', 'paramsd', 'dmonitoringmodeld', 'modeld']
NOT_TESTED = ['selfdrived', 'controlsd', 'card', 'plannerd', 'calibrationd', 'dmonitoringd', 'paramsd', 'dmonitoringmodeld', 'modeld']
TEST_CASES = [(cfg.proc_name, copy.deepcopy(cfg)) for cfg in pr.CONFIGS if cfg.proc_name not in NOT_TESTED]

6
selfdrive/test/process_replay/test_processes.py
View File

@ -12,7 +12,7 @@ from openpilot.common.git import get_commit
from openpilot.tools.lib.openpilotci import get_url, upload_file
from openpilot.selfdrive.test.process_replay.compare_logs import compare_logs, format_diff
from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, PROC_REPLAY_DIR, FAKEDATA, replay_process, \
check_openpilot_enabled, check_most_messages_valid
check_most_messages_valid
from openpilot.tools.lib.filereader import FileReader
from openpilot.tools.lib.logreader import LogReader, save_log
@ -103,10 +103,6 @@ def test_process(cfg, lr, segment, ref_log_path, new_log_path, ignore_fields=Non
except Exception as e:
raise Exception("failed on segment: " + segment) from e
# check to make sure openpilot is engaged in the route
if cfg.proc_name == "controlsd":
if not check_openpilot_enabled(log_msgs):
return f"Route did not enable at all or for long enough: {new_log_path}", log_msgs
if not check_most_messages_valid(log_msgs):
return f"Route did not have enough valid messages: {new_log_path}", log_msgs

6
selfdrive/test/test_onroad.py
View File

@ -19,7 +19,7 @@ from cereal.services import SERVICE_LIST
from openpilot.common.basedir import BASEDIR
from openpilot.common.timeout import Timeout
from openpilot.common.params import Params
from openpilot.selfdrive.controls.lib.events import EVENTS, ET
from openpilot.selfdrive.selfdrived.events import EVENTS, ET
from openpilot.selfdrive.test.helpers import set_params_enabled, release_only
from openpilot.system.hardware import HARDWARE
from openpilot.system.hardware.hw import Paths
@ -35,7 +35,8 @@ CPU usage budget
MAX_TOTAL_CPU = 260. # total for all 8 cores
PROCS = {
# Baseline CPU usage by process
"selfdrive.controls.controlsd": 32.0,
"selfdrive.controls.controlsd": 18.0,
"selfdrive.selfdrived.selfdrived": 21.0,
"selfdrive.car.card": 30.0,
"./loggerd": 14.0,
"./encoderd": 17.0,
@ -87,6 +88,7 @@ TIMINGS = {
"carControl": [2.5, 0.35],
"controlsState": [2.5, 0.35],
"longitudinalPlan": [2.5, 0.5],
"driverAssistance": [2.5, 0.5],
"roadCameraState": [2.5, 0.35],
"driverCameraState": [2.5, 0.35],
"modelV2": [2.5, 0.35],

5
selfdrive/test/test_time_to_onroad.py
View File

@ -20,7 +20,7 @@ def test_time_to_onroad():
proc = subprocess.Popen(["python", manager_path])
start_time = time.monotonic()
sm = messaging.SubMaster(['selfdriveState', 'controlsState', 'deviceState', 'onroadEvents'])
sm = messaging.SubMaster(['selfdriveState', 'deviceState', 'onroadEvents'])
try:
# wait for onroad. timeout assumes panda is up to date
with Timeout(10, "timed out waiting to go onroad"):
@ -34,7 +34,7 @@ def test_time_to_onroad():
while True:
sm.update(100)
if sm.seen['onroadEvents'] and not any(EventName.controlsInitializing == e.name for e in sm['onroadEvents']):
if sm.seen['onroadEvents'] and not any(EventName.selfdriveInitializing == e.name for e in sm['onroadEvents']):
initialized = True
if initialized:
@ -51,7 +51,6 @@ def test_time_to_onroad():
sm.update(100)
assert sm.all_alive(), sm.alive
assert sm['selfdriveState'].engageable, f"events: {sm['onroadEvents']}"
assert sm['controlsState'].cumLagMs < 10.
finally:
proc.terminate()
if proc.wait(20) is None:

2
selfdrive/ui/qt/widgets/offroad_alerts.cc
View File

@ -70,7 +70,7 @@ AbstractAlert::AbstractAlert(bool hasRebootBtn, QWidget *parent) : QFrame(parent
int OffroadAlert::refresh() {
// build widgets for each offroad alert on first refresh
if (alerts.empty()) {
QString json = util::read_file("../controls/lib/alerts_offroad.json").c_str();
QString json = util::read_file("../selfdrived/alerts_offroad.json").c_str();
QJsonObject obj = QJsonDocument::fromJson(json.toUtf8()).object();
// descending sort labels by severity

4
selfdrive/ui/tests/cycle_offroad_alerts.py
View File

@ -6,12 +6,12 @@ import json
from openpilot.common.basedir import BASEDIR
from openpilot.common.params import Params
from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
if __name__ == "__main__":
params = Params()
with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f:
with open(os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json")) as f:
offroad_alerts = json.load(f)
t = 10 if len(sys.argv) < 2 else int(sys.argv[1])

2
selfdrive/ui/tests/test_ui/run.py
View File

@ -17,7 +17,7 @@ from openpilot.common.basedir import BASEDIR
from openpilot.common.params import Params
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.common.transformations.camera import CameraConfig, DEVICE_CAMERAS
from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
from openpilot.selfdrive.test.helpers import with_processes
from openpilot.selfdrive.test.process_replay.migration import migrate_controlsState
from openpilot.tools.lib.logreader import LogReader

2
selfdrive/ui/update_translations.py
View File

@ -16,7 +16,7 @@ def generate_translations_include():
# offroad alerts
# TODO translate events from openpilot.selfdrive/controls/lib/events.py
content = "// THIS IS AN AUTOGENERATED FILE, PLEASE EDIT alerts_offroad.json\n"
with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f:
with open(os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json")) as f:
for alert in json.load(f).values():
content += f'QT_TRANSLATE_NOOP("OffroadAlert", R"({alert["text"]})");\n'

2
system/athena/registration.py
View File

@ -8,7 +8,7 @@ from datetime import datetime, timedelta, UTC
from openpilot.common.api import api_get
from openpilot.common.params import Params
from openpilot.common.spinner import Spinner
from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
from openpilot.system.hardware import HARDWARE, PC
from openpilot.system.hardware.hw import Paths
from openpilot.common.swaglog import cloudlog

2
system/camerad/snapshot/snapshot.py
View File

@ -10,7 +10,7 @@ from msgq.visionipc import VisionIpcClient, VisionStreamType
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL
from openpilot.system.hardware import PC
from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
from openpilot.system.manager.process_config import managed_processes

2
system/hardware/hardwared.py
View File

@ -16,7 +16,7 @@ from openpilot.common.dict_helpers import strip_deprecated_keys
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.params import Params
from openpilot.common.realtime import DT_HW
from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
from openpilot.system.hardware import HARDWARE, TICI, AGNOS
from openpilot.system.loggerd.config import get_available_percent
from openpilot.system.statsd import statlog

1
system/manager/process_config.py
View File

@ -64,6 +64,7 @@ procs = [
PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad),
PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad),
PythonProcess("controlsd", "selfdrive.controls.controlsd", only_onroad),
PythonProcess("selfdrived", "selfdrive.selfdrived.selfdrived", only_onroad),
PythonProcess("card", "selfdrive.car.card", only_onroad),
PythonProcess("deleter", "system.loggerd.deleter", always_run),
PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(not PC or WEBCAM)),

2
system/updated/updated.py
View File

@ -17,7 +17,7 @@ from openpilot.common.params import Params
from openpilot.common.time import system_time_valid
from openpilot.common.markdown import parse_markdown
from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
from openpilot.system.hardware import AGNOS, HARDWARE
from openpilot.system.version import get_build_metadata