def test_rhd_parser(self):
cities = [[32.7, -117.1, 0],\
[51.5, 0.129, 1],\
[35.7, 139.7, 1],\
[-37.8, 144.9, 1],\
[32.1, 41.74, 0],\
[55.7, 12.69, 0]]
result = []
for city in cities:
result.append(int(is_rhd_region(city[0], city[1])))
self.assertEqual(result, [int(city[2]) for city in cities])
def dmonitoringd_thread(sm=None, pm=None):
gc.disable()
# start the loop
set_realtime_priority(3)
params = Params()
# Pub/Sub Sockets
if pm is None:
pm = messaging.PubMaster(['dMonitoringState'])
if sm is None:
sm = messaging.SubMaster([
'driverState', 'liveCalibration', 'carState', 'model',
'gpsLocation'
],
ignore_alive=['gpsLocation'])
driver_status = DriverStatus()
is_rhd = params.get("IsRHD")
if is_rhd is not None:
driver_status.is_rhd_region = bool(int(is_rhd))
sm['liveCalibration'].calStatus = Calibration.INVALID
sm['carState'].vEgo = 0.
sm['carState'].cruiseState.enabled = False
sm['carState'].cruiseState.speed = 0.
sm['carState'].buttonEvents = []
sm['carState'].steeringPressed = False
sm['carState'].standstill = True
cal_rpy = [0, 0, 0]
v_cruise_last = 0
driver_engaged = False
# 10Hz <- dmonitoringmodeld
while True:
sm.update()
# GPS coords RHD parsing, once every restart
if not driver_status.is_rhd_region_checked and sm.updated[
'gpsLocation']:
is_rhd = is_rhd_region(sm['gpsLocation'].latitude,
sm['gpsLocation'].longitude)
driver_status.is_rhd_region = is_rhd
driver_status.is_rhd_region_checked = True
put_nonblocking("IsRHD", "1" if is_rhd else "0")
# Handle calibration
if sm.updated['liveCalibration']:
if sm['liveCalibration'].calStatus == Calibration.CALIBRATED:
if len(sm['liveCalibration'].rpyCalib) == 3:
cal_rpy = sm['liveCalibration'].rpyCalib
# Get interaction
if sm.updated['carState']:
v_cruise = sm['carState'].cruiseState.speed
driver_engaged = len(sm['carState'].buttonEvents) > 0 or \
v_cruise != v_cruise_last or \
sm['carState'].steeringPressed
v_cruise_last = v_cruise
# Get model meta
if sm.updated['model']:
driver_status.set_policy(sm['model'])
# Get data from dmonitoringmodeld
if sm.updated['driverState']:
events = []
driver_status.get_pose(sm['driverState'], cal_rpy,
sm['carState'].vEgo,
sm['carState'].cruiseState.enabled)
# Block any engage after certain distrations
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
# Update events from driver state
events = driver_status.update(events, driver_engaged,
sm['carState'].cruiseState.enabled,
sm['carState'].standstill)
# dMonitoringState packet
dat = messaging.new_message()
dat.init('dMonitoringState')
dat.dMonitoringState = {
"events":
events,
"faceDetected":
driver_status.face_detected,
"isDistracted":
driver_status.driver_distracted,
"awarenessStatus":
driver_status.awareness,
"isRHD":
driver_status.is_rhd_region,
"rhdChecked":
driver_status.is_rhd_region_checked,
"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,
}
pm.send('dMonitoringState', dat)
def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp,
free_space, low_battery, driver_status, state,
mismatch_counter, params):
"""Receive data from sockets and create events for battery, temperature and disk space"""
# Update carstate from CAN and create events
can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True)
CS = CI.update(CC, can_strs)
sm.update(0)
events = list(CS.events)
enabled = isEnabled(state)
# Check for CAN timeout
if not can_strs:
events.append(
create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if sm.updated['thermal']:
overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red
free_space = sm[
'thermal'].freeSpace < 0.07 # under 7% of space free no enable allowed
low_battery = sm['thermal'].batteryPercent < 1 and sm[
'thermal'].chargingError # at zero percent battery, while discharging, OP should not allowed
# Create events for battery, temperature and disk space
if low_battery:
events.append(
create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if overtemp:
events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if free_space:
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
# GPS coords RHD parsing, once every restart
if sm.updated['gpsLocation'] and not driver_status.is_rhd_region_checked:
is_rhd = is_rhd_region(sm['gpsLocation'].latitude,
sm['gpsLocation'].longitude)
driver_status.is_rhd_region = is_rhd
driver_status.is_rhd_region_checked = True
put_nonblocking("IsRHD", str(int(is_rhd)))
# Handle calibration
if sm.updated['liveCalibration']:
cal_status = sm['liveCalibration'].calStatus
cal_perc = sm['liveCalibration'].calPerc
cal_rpy = [0, 0, 0]
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
events.append(
create_event('calibrationIncomplete',
[ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
else:
events.append(
create_event('calibrationInvalid',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
else:
rpy = sm['liveCalibration'].rpyCalib
if len(rpy) == 3:
cal_rpy = rpy
# 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 enabled:
mismatch_counter = 0
if sm.updated['health']:
controls_allowed = sm['health'].controlsAllowed
if not controls_allowed and enabled:
mismatch_counter += 1
if mismatch_counter >= 2:
events.append(
create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
# Driver monitoring
if sm.updated['driverMonitoring']:
driver_status.get_pose(sm['driverMonitoring'], cal_rpy, CS.vEgo,
enabled)
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS:
events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter
