diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index f137b406b..293904a8e 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -13,6 +13,7 @@ def dmonitoringd_thread(): sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'selfdriveState', 'modelV2'], poll='driverStateV2') DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM")) + demo_mode=False # 20Hz <- dmonitoringmodeld while True: @@ -22,8 +23,10 @@ def dmonitoringd_thread(): continue valid = sm.all_checks() - if valid: - DM.run_step(sm) + if demo_mode and sm.valid['driverStateV2']: + DM.run_step(sm, demo=demo_mode) + elif valid: + DM.run_step(sm, demo=demo_mode) # publish dat = DM.get_state_packet(valid=valid) @@ -32,6 +35,7 @@ def dmonitoringd_thread(): # load live always-on toggle if sm['driverStateV2'].frameId % 40 == 1: DM.always_on = params.get_bool("AlwaysOnDM") + demo_mode = params.get_bool("IsDriverViewEnabled") # save rhd virtual toggle every 5 mins if (sm['driverStateV2'].frameId % 6000 == 0 and diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py index f405eba53..463bf2b7f 100644 --- a/selfdrive/monitoring/helpers.py +++ b/selfdrive/monitoring/helpers.py @@ -211,8 +211,8 @@ class DriverMonitoring: self.step_change = self.settings._DT_DMON / self.settings._AWARENESS_TIME self.active_monitoring_mode = False - def _set_policy(self, model_data, car_speed): - bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s + def _set_policy(self, brake_disengage_prob, car_speed): + bp = brake_disengage_prob k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2) bp_normal = max(min(bp / k1, 0.5),0) self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5], @@ -417,27 +417,42 @@ class DriverMonitoring: } return dat - def run_step(self, sm): - # Set strictness + def run_step(self, sm, demo=False): + if demo: + highway_speed = 30 + enabled = True + wrong_gear = False + standstill = False + driver_engaged = False + brake_disengage_prob = 1.0 + rpyCalib = [0., 0., 0.] + else: + highway_speed = sm['carState'].vEgo + enabled = sm['selfdriveState'].enabled + wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low) + standstill = sm['carState'].standstill + driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed + brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s + rpyCalib = sm['liveCalibration'].rpyCalib self._set_policy( - model_data=sm['modelV2'], - car_speed=sm['carState'].vEgo + brake_disengage_prob=brake_disengage_prob, + car_speed=highway_speed, ) # Parse data from dmonitoringmodeld self._update_states( driver_state=sm['driverStateV2'], - cal_rpy=sm['liveCalibration'].rpyCalib, - car_speed=sm['carState'].vEgo, - op_engaged=sm['selfdriveState'].enabled, - standstill=sm['carState'].standstill, + cal_rpy=rpyCalib, + car_speed=highway_speed, + op_engaged=enabled, + standstill=standstill, ) # Update distraction events self._update_events( - driver_engaged=sm['carState'].steeringPressed or sm['carState'].gasPressed, - op_engaged=sm['selfdriveState'].enabled, - standstill=sm['carState'].standstill, - wrong_gear=sm['carState'].gearShifter in [car.CarState.GearShifter.reverse, car.CarState.GearShifter.park], - car_speed=sm['carState'].vEgo + driver_engaged=driver_engaged, + op_engaged=enabled, + standstill=standstill, + wrong_gear=wrong_gear, + car_speed=highway_speed )