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

card: prepare for separate process (#31660)

Browse Source 
* Card

* update ref

* bump cpu

* sub to caroutput

* update ref
pull/31693/head
Justin Newberry 1 year ago committed by GitHub
parent 8cd3bc65bf
commit 8ec0d87de0
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
8 changed files with 46 additions and 39 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      cereal
  2. 64
      selfdrive/controls/controlsd.py
  3. 5
      selfdrive/locationd/torqued.py
  4. 2
      selfdrive/test/process_replay/ref_commit
  5. 2
      selfdrive/test/test_onroad.py
  6. 2
      tools/plotjuggler/layouts/max-torque-debug.xml
  7. 4
      tools/plotjuggler/layouts/torque-controller.xml
  8. 4
      tools/tuning/measure_steering_accuracy.py

2
cereal

@ -1 +1 @@
Subproject commit bfbb0cab83e3ad49d85ad1a34ee1241bf1ff782f
Subproject commit 0172e60275b11074ff4e6b65378a12f3936fa95a

64
selfdrive/controls/controlsd.py
Unescape View File

@ -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()

5
selfdrive/locationd/torqued.py
Unescape View File

@ -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:

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
99a50fe1b645bc1dcbf621e9cb72d151c6896429
43efe1cf08cba8c86bc1ae8234b3d3d084a40e5d

2
selfdrive/test/test_onroad.py
Unescape View File

@ -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,

2
tools/plotjuggler/layouts/max-torque-debug.xml
Unescape View File

@ -24,7 +24,7 @@
<range left="0.000450" top="1.050000" right="2483.624998" bottom="-1.050000"/>
<limitY/>
<curve color="#0097ff" name="/carState/steeringPressed"/>
<curve color="#d62728" name="/carControl/actuatorsOutput/steer"/>
<curve color="#d62728" name="/carOutput/actuatorsOutput/steer"/>
</plot>
</DockArea>
<DockArea name="...">

4
tools/plotjuggler/layouts/torque-controller.xml
Unescape View File

@ -24,8 +24,8 @@
<plot style="Lines" flip_x="false" flip_y="false" mode="TimeSeries">
<range bottom="-1.134948" top="1.052072" left="825.563261" right="1415.827546"/>
<limitY/>
<curve name="/carControl/actuatorsOutput/steer" color="#9467bd">
<transform name="Scale/Offset" alias="/carControl/actuatorsOutput/steer[Scale/Offset]">
<curve name="/carOutput/actuatorsOutput/steer" color="#9467bd">
<transform name="Scale/Offset" alias="/carOutput/actuatorsOutput/steer[Scale/Offset]">
<options time_offset="0" value_scale="-1" value_offset="0"/>
</transform>
</curve>

4
tools/tuning/measure_steering_accuracy.py
Unescape View File

@ -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...")

Write Preview
Loading…
Cancel
Save