fix wasted cycles in locationd and dmonitoringd

pull/2108/head
Adeeb Shihadeh 5 years ago
parent 63436373d2
commit 0fad46457c
  1. 2
      cereal
  2. 2
      selfdrive/locationd/locationd.py
  3. 63
      selfdrive/monitoring/dmonitoringd.py

@ -1 +1 @@
Subproject commit 1538183eaa3c9a22a3e675182d6e1cc8e23ad574
Subproject commit 0a13413f41d54295f618f497e5434fa64ee53313

@ -289,7 +289,7 @@ def locationd_thread(sm, pm, disabled_logs=None):
localizer = Localizer(disabled_logs=disabled_logs)
while True:
sm.update()
sm.update(wait_for="cameraOdometry")
for sock, updated in sm.updated.items():
if updated and sm.valid[sock]:

@ -40,7 +40,7 @@ def dmonitoringd_thread(sm=None, pm=None):
# 10Hz <- dmonitoringmodeld
while True:
sm.update()
sm.update(wait_for="driverState")
# Get interaction
if sm.updated['carState']:
@ -57,37 +57,36 @@ def dmonitoringd_thread(sm=None, pm=None):
driver_status.set_policy(sm['model'])
# Get data from dmonitoringmodeld
if sm.updated['driverState']:
events = Events()
driver_status.get_pose(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['carState'].cruiseState.enabled)
# Block engaging after max number of distrations
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
events.add(car.CarEvent.EventName.tooDistracted)
# Update events from driver state
driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill)
# build dMonitoringState packet
dat = messaging.new_message('dMonitoringState')
dat.dMonitoringState = {
"events": events.to_msg(),
"faceDetected": driver_status.face_detected,
"isDistracted": driver_status.driver_distracted,
"awarenessStatus": driver_status.awareness,
"isRHD": driver_status.is_rhd_region,
"posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(),
"posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n,
"poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(),
"poseYawValidCount": driver_status.pose.yaw_offseter.filtered_stat.n,
"stepChange": driver_status.step_change,
"awarenessActive": driver_status.awareness_active,
"awarenessPassive": driver_status.awareness_passive,
"isLowStd": driver_status.pose.low_std,
"hiStdCount": driver_status.hi_stds,
"isPreview": offroad,
}
pm.send('dMonitoringState', dat)
events = Events()
driver_status.get_pose(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['carState'].cruiseState.enabled)
# Block engaging after max number of distrations
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
events.add(car.CarEvent.EventName.tooDistracted)
# Update events from driver state
driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill)
# build dMonitoringState packet
dat = messaging.new_message('dMonitoringState')
dat.dMonitoringState = {
"events": events.to_msg(),
"faceDetected": driver_status.face_detected,
"isDistracted": driver_status.driver_distracted,
"awarenessStatus": driver_status.awareness,
"isRHD": driver_status.is_rhd_region,
"posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(),
"posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n,
"poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(),
"poseYawValidCount": driver_status.pose.yaw_offseter.filtered_stat.n,
"stepChange": driver_status.step_change,
"awarenessActive": driver_status.awareness_active,
"awarenessPassive": driver_status.awareness_passive,
"isLowStd": driver_status.pose.low_std,
"hiStdCount": driver_status.hi_stds,
"isPreview": offroad,
}
pm.send('dMonitoringState', dat)
def main(sm=None, pm=None):
dmonitoringd_thread(sm, pm)

Loading…
Cancel
Save