From 8ec0d87de06c265dcb0526b9a3bce8a397248dc9 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Mon, 4 Mar 2024 12:53:42 -0500 Subject: [PATCH] card: prepare for separate process (#31660) * Card * update ref * bump cpu * sub to caroutput * update ref --- cereal | 2 +- selfdrive/controls/controlsd.py | 64 ++++++++++--------- selfdrive/locationd/torqued.py | 5 +- selfdrive/test/process_replay/ref_commit | 2 +- selfdrive/test/test_onroad.py | 2 +- .../plotjuggler/layouts/max-torque-debug.xml | 2 +- .../plotjuggler/layouts/torque-controller.xml | 4 +- tools/tuning/measure_steering_accuracy.py | 4 +- 8 files changed, 46 insertions(+), 39 deletions(-) diff --git a/cereal b/cereal index bfbb0cab83..0172e60275 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit bfbb0cab83e3ad49d85ad1a34ee1241bf1ff782f +Subproject commit 0172e60275b11074ff4e6b65378a12f3936fa95a diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index bd3dd08179..fc68db14e5 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -68,11 +68,15 @@ class CarD: def __init__(self, CI=None): self.can_sock = messaging.sub_sock('can', timeout=20) self.sm = messaging.SubMaster(['pandaStates']) - self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams']) + self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput']) self.can_rcv_timeout_counter = 0 # conseuctive timeout count self.can_rcv_cum_timeout_counter = 0 # cumulative timeout count + self.CC_prev = car.CarState.new_message() + + self.last_actuators = None + self.params = Params() if CI is None: @@ -118,14 +122,12 @@ class CarD: """Initialize CarInterface, once controls are ready""" self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) - def state_update(self, CC: car.CarControl): + def state_update(self): """carState update loop, driven by can""" - # TODO: This should not depend on carControl - # Update carState from CAN can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) - self.CS = self.CI.update(CC, can_strs) + self.CS = self.CI.update(self.CC_prev, can_strs) self.sm.update(0) @@ -143,18 +145,17 @@ class CarD: if can_rcv_valid and REPLAY: self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime + self.state_publish() + return self.CS - def state_publish(self, car_events): + def state_publish(self): """carState and carParams publish loop""" - # TODO: carState should be independent of the event loop - # carState cs_send = messaging.new_message('carState') cs_send.valid = self.CS.canValid cs_send.carState = self.CS - cs_send.carState.events = car_events self.pm.send('carState', cs_send) # carParams - logged every 50 seconds (> 1 per segment) @@ -164,25 +165,36 @@ class CarD: cp_send.carParams = self.CP self.pm.send('carParams', cp_send) + # publish new carOutput + co_send = messaging.new_message('carOutput') + co_send.valid = True + if self.last_actuators is not None: + co_send.carOutput.actuatorsOutput = self.last_actuators + self.pm.send('carOutput', co_send) + def controls_update(self, CC: car.CarControl): """control update loop, driven by carControl""" # send car controls over can now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) - actuators_output, can_sends = self.CI.apply(CC, now_nanos) + self.last_actuators, can_sends = self.CI.apply(CC, now_nanos) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=self.CS.canValid)) - return actuators_output + self.CC_prev = CC class Controls: def __init__(self, CI=None): self.card = CarD(CI) - self.CP = self.card.CP + self.params = Params() + + with car.CarParams.from_bytes(self.params.get("CarParams", block=True)) as msg: + # TODO: this shouldn't need to be a builder + self.CP = msg.as_builder() + self.CI = self.card.CI - config_realtime_process(4, Priority.CTRL_HIGH) # Ensure the current branch is cached, otherwise the first iteration of controlsd lags self.branch = get_short_branch() @@ -195,12 +207,11 @@ class Controls: self.log_sock = messaging.sub_sock('androidLog') - self.params = Params() ignore = self.sensor_packets + ['testJoystick'] if SIMULATION: ignore += ['driverCameraState', 'managerState'] self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', - 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', + 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'testJoystick'] + self.camera_packets + self.sensor_packets, ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ], @@ -212,15 +223,12 @@ class Controls: self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") self.is_metric = self.params.get_bool("IsMetric") self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") - openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") # detect sound card presence and ensure successful init sounds_available = HARDWARE.get_sound_card_online() car_recognized = self.CP.carName != 'mock' - controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly - # cleanup old params if not self.CP.experimentalLongitudinalAvailable: self.params.remove("ExperimentalLongitudinalEnabled") @@ -267,7 +275,7 @@ class Controls: self.can_log_mono_time = 0 - self.startup_event = get_startup_event(car_recognized, controller_available, len(self.CP.carFw) > 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) @@ -513,7 +521,7 @@ class Controls: def data_sample(self): """Receive data from sockets and update carState""" - CS = self.card.state_update(self.CC) + CS = self.card.state_update() self.sm.update(0) @@ -771,6 +779,8 @@ class Controls: def publish_logs(self, CS, start_time, CC, lac_log): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" + CO = self.sm['carOutput'] + # Orientation and angle rates can be useful for carcontroller # Only calibrated (car) frame is relevant for the carcontroller orientation_value = list(self.sm['liveLocationKalman'].calibratedOrientationNED.value) @@ -833,13 +843,12 @@ class Controls: hudControl.visualAlert = current_alert.visual_alert if not self.CP.passive and self.initialized: - self.last_actuators = self.card.controls_update(CC) - CC.actuatorsOutput = self.last_actuators + self.card.controls_update(CC) if self.CP.steerControlType == car.CarParams.SteerControlType.angle: - self.steer_limited = abs(CC.actuators.steeringAngleDeg - CC.actuatorsOutput.steeringAngleDeg) > \ + self.steer_limited = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ STEER_ANGLE_SATURATION_THRESHOLD else: - self.steer_limited = abs(CC.actuators.steer - CC.actuatorsOutput.steer) > 1e-2 + self.steer_limited = abs(CC.actuators.steer - CO.actuatorsOutput.steer) > 1e-2 force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \ (self.state == State.softDisabling) @@ -896,15 +905,11 @@ class Controls: self.pm.send('controlsState', dat) - car_events = self.events.to_msg() - - self.card.state_publish(car_events) - # 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 = car_events + ce_send.onroadEvents = self.events.to_msg() self.pm.send('onroadEvents', ce_send) self.events_prev = self.events.names.copy() @@ -961,6 +966,7 @@ class Controls: def main(): + config_realtime_process(4, Priority.CTRL_HIGH) controls = Controls() controls.controlsd_thread() diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 69bab8d1fa..b49784d13f 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -159,8 +159,9 @@ class TorqueEstimator(ParameterEstimator): def handle_log(self, t, which, msg): if which == "carControl": self.raw_points["carControl_t"].append(t + self.lag) - self.raw_points["steer_torque"].append(-msg.actuatorsOutput.steer) self.raw_points["active"].append(msg.latActive) + elif which == "carOutput": + self.raw_points["steer_torque"].append(-msg.actuatorsOutput.steer) elif which == "carState": self.raw_points["carState_t"].append(t + self.lag) self.raw_points["vego"].append(msg.vEgo) @@ -218,7 +219,7 @@ def main(demo=False): config_realtime_process([0, 1, 2, 3], 5) pm = messaging.PubMaster(['liveTorqueParameters']) - sm = messaging.SubMaster(['carControl', 'carState', 'liveLocationKalman'], poll='liveLocationKalman') + sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveLocationKalman'], poll='liveLocationKalman') params = Params() with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP: diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 1dfdca0d89..46d684211c 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -99a50fe1b645bc1dcbf621e9cb72d151c6896429 +43efe1cf08cba8c86bc1ae8234b3d3d084a40e5d diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 8f8c93ecff..4be9b8a430 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -29,7 +29,7 @@ from openpilot.tools.lib.logreader import LogReader # Baseline CPU usage by process PROCS = { - "selfdrive.controls.controlsd": 41.0, + "selfdrive.controls.controlsd": 46.0, "./loggerd": 14.0, "./encoderd": 17.0, "./camerad": 14.5, diff --git a/tools/plotjuggler/layouts/max-torque-debug.xml b/tools/plotjuggler/layouts/max-torque-debug.xml index 20a49c2181..8cfd30599e 100644 --- a/tools/plotjuggler/layouts/max-torque-debug.xml +++ b/tools/plotjuggler/layouts/max-torque-debug.xml @@ -24,7 +24,7 @@ - + diff --git a/tools/plotjuggler/layouts/torque-controller.xml b/tools/plotjuggler/layouts/torque-controller.xml index 443255968a..d6e4684d63 100644 --- a/tools/plotjuggler/layouts/torque-controller.xml +++ b/tools/plotjuggler/layouts/torque-controller.xml @@ -24,8 +24,8 @@ - - + + diff --git a/tools/tuning/measure_steering_accuracy.py b/tools/tuning/measure_steering_accuracy.py index f804b328de..6abf1338dc 100755 --- a/tools/tuning/measure_steering_accuracy.py +++ b/tools/tuning/measure_steering_accuracy.py @@ -47,7 +47,7 @@ class SteeringAccuracyTool: v_ego = sm['carState'].vEgo active = sm['controlsState'].active - steer = sm['carControl'].actuatorsOutput.steer + steer = sm['carOutput'].actuatorsOutput.steer standstill = sm['carState'].standstill steer_limited = abs(sm['carControl'].actuators.steer - sm['carControl'].actuatorsOutput.steer) > 1e-2 overriding = sm['carState'].steeringPressed @@ -150,7 +150,7 @@ if __name__ == "__main__": messaging.context = messaging.Context() carControl = messaging.sub_sock('carControl', addr=args.addr, conflate=True) - sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'modelV2'], addr=args.addr) + sm = messaging.SubMaster(['carState', 'carControl', 'carOutput', 'controlsState', 'modelV2'], addr=args.addr) time.sleep(1) # Make sure all submaster data is available before going further print("waiting for messages...")