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