diff --git a/cereal/log.capnp b/cereal/log.capnp index 421ae7c041..6cd7fe7534 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2304,6 +2304,11 @@ struct EncodeData { height @5 :UInt32; } +struct DebugAlert { + alertText1 @0 :Text; + alertText2 @1 :Text; +} + struct UserFlag { } @@ -2411,7 +2416,7 @@ struct Event { driverEncodeData @87 :EncodeData; wideRoadEncodeData @88 :EncodeData; qRoadEncodeData @89 :EncodeData; - # debugAlert @104 :DebugAlert; + alertDebug @133 :DebugAlert; livestreamRoadEncodeData @120 :EncodeData; livestreamWideRoadEncodeData @121 :EncodeData; diff --git a/cereal/services.py b/cereal/services.py index 926461aec2..4cc7436d92 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -75,6 +75,7 @@ _services: dict[str, tuple] = { # debug "uiDebug": (True, 0., 1), + "alertDebug": (True, 0.), "testJoystick": (True, 0.), "roadEncodeData": (False, 20.), "driverEncodeData": (False, 20.), diff --git a/examples/longitudinal_profiles.py b/examples/longitudinal_profiles.py index 36b2c822f9..c0f0910d3e 100755 --- a/examples/longitudinal_profiles.py +++ b/examples/longitudinal_profiles.py @@ -80,6 +80,10 @@ class Maneuver: def finished(self): return self._finished + @property + def active(self): + return self._active + MANEUVERS = [ Maneuver( @@ -176,7 +180,7 @@ def main(): CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) sm = messaging.SubMaster(['carState', 'controlsState', 'selfdriveState', 'modelV2'], poll='modelV2') - pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance']) + pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'alertDebug']) maneuvers = iter(MANEUVERS) maneuver = None @@ -190,6 +194,9 @@ def main(): if maneuver is None: print('We are done!') + alert_msg = messaging.new_message('alertDebug') + alert_msg.valid = True + plan_send = messaging.new_message('longitudinalPlan') plan_send.valid = sm.all_checks() @@ -200,6 +207,12 @@ def main(): if maneuver is not None: accel = maneuver.get_accel(cs.vEgo, cs.cruiseState.enabled, cs.cruiseState.standstill) + if maneuver.active: + alert_msg.alertDebug.alertText1 = 'Maneuver: Active' + else: + alert_msg.alertDebug.alertText1 = f'Reaching Target Speed: {maneuver.initial_speed * Conversions.MS_TO_MPH:0.2f} mph' + alert_msg.alertDebug.alertText2 = f'Requesting {accel:0.2f} m/s^2' + longitudinalPlan.aTarget = accel longitudinalPlan.shouldStop = cs.vEgo < CP.vEgoStopping and accel < 0 # should_stop @@ -215,6 +228,7 @@ def main(): print('finished?', maneuver is not None and maneuver.finished) print('aTarget:', longitudinalPlan.aTarget) + print('shouldStop:', longitudinalPlan.shouldStop) if maneuver is not None and maneuver.finished: maneuver = None diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 255ce080df..5524ca9a98 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -27,6 +27,10 @@ TESTING_CLOSET = "TESTING_CLOSET" in os.environ IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"} LONGITUDINAL_PERSONALITY_MAP = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()} +AlertSize = log.SelfdriveState.AlertSize +AlertStatus = log.SelfdriveState.AlertStatus +AudibleAlert = car.CarControl.HUDControl.AudibleAlert + ThermalStatus = log.DeviceState.ThermalStatus State = log.SelfdriveState.OpenpilotState PandaType = log.PandaState.PandaType @@ -61,7 +65,7 @@ class SelfdriveD: # TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches self.car_state_sock = messaging.sub_sock('carState', timeout=20) - ignore = self.sensor_packets + self.gps_packets + ignore = self.sensor_packets + self.gps_packets + ['debugAlert'] if SIMULATION: ignore += ['driverCameraState', 'managerState'] if REPLAY: @@ -70,7 +74,7 @@ class SelfdriveD: self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', - 'controlsState', 'carControl', 'driverAssistance'] + \ + 'controlsState', 'carControl', 'driverAssistance', 'alertDebug'] + \ self.camera_packets + self.sensor_packets + self.gps_packets, ignore_alive=ignore, ignore_avg_freq=ignore+['radarState',], ignore_valid=ignore, frequency=int(1/DT_CTRL)) @@ -431,6 +435,14 @@ class SelfdriveD: ss.alertType = self.AM.current_alert.alert_type ss.alertSound = self.AM.current_alert.audible_alert + if self.sm['alertDebug'].alertText1 != '': + ss.alertText1 = self.sm['alertDebug'].alertText1 + ss.alertText2 = self.sm['alertDebug'].alertText2 + ss.alertSize = AlertSize.mid + ss.alertStatus = AlertStatus.normal + ss.alertType = '' + ss.alertSound = AudibleAlert.none + self.pm.send('selfdriveState', ss_msg) # onroadEvents - logged every second or on change