diff --git a/common/params.cc b/common/params.cc index f4ffe23..edc3597 100755 --- a/common/params.cc +++ b/common/params.cc @@ -252,7 +252,8 @@ std::unordered_map keys = { // {"SpeedLimitVTSC", PERSISTENT}, {"CPTLkasButtonAction", PERSISTENT}, - {"ScreenDisaplayMode", PERSISTENT}, + {"IsDaylight", CLEAR_ON_MANAGER_START}, + {"ScreenDisplayMode", CLEAR_ON_MANAGER_START}, {"RadarDist", PERSISTENT}, {"ModelDist", PERSISTENT}, diff --git a/selfdrive/clearpilot/bench_cmd.py b/selfdrive/clearpilot/bench_cmd.py index 991062b..5c63b8f 100644 --- a/selfdrive/clearpilot/bench_cmd.py +++ b/selfdrive/clearpilot/bench_cmd.py @@ -8,6 +8,7 @@ Usage: python3 -m selfdrive.clearpilot.bench_cmd speedlimit 45 python3 -m selfdrive.clearpilot.bench_cmd cruise 55 python3 -m selfdrive.clearpilot.bench_cmd engaged 1 + python3 -m selfdrive.clearpilot.bench_cmd debugbutton (simulate LKAS debug button press) python3 -m selfdrive.clearpilot.bench_cmd dump python3 -m selfdrive.clearpilot.bench_cmd wait_ready """ @@ -105,6 +106,23 @@ def main(): elif cmd == "wait_ready": wait_ready() + elif cmd == "debugbutton": + # Simulate LKAS debug button — same state machine as controlsd.clearpilot_state_control() + current = params.get_int("ScreenDisplayMode") + gear = (params.get("BenchGear") or b"P").decode().strip().upper() + in_drive = gear in ("D", "S", "L") + + if in_drive: + transitions = {0: 4, 1: 2, 2: 3, 3: 4, 4: 2} + new_mode = transitions.get(current, 0) + else: + new_mode = 0 if current == 3 else 3 + + params.put_int("ScreenDisplayMode", new_mode) + mode_names = {0: "auto-normal", 1: "auto-nightrider", 2: "normal", 3: "screen-off", 4: "nightrider"} + print(f"ScreenDisplayMode: {current} ({mode_names.get(current, '?')}) → {new_mode} ({mode_names.get(new_mode, '?')})" + f" [gear={gear}, in_drive={in_drive}]") + elif cmd in param_map: if len(sys.argv) < 3: print(f"Usage: bench_cmd {cmd} ") diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e62f850..b0809e9 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -78,7 +78,7 @@ class Controls: self.params_storage = Params("/persist/params") self.params_memory.put_bool("CPTLkasButtonAction", False) - self.params_memory.put_bool("ScreenDisaplayMode", 0) + self.params_memory.put_int("ScreenDisplayMode", 0) self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS @@ -1251,21 +1251,17 @@ class Controls: def clearpilot_state_control(self, CC, CS): if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents): - - # if self.params_memory.get_bool("CPTLkasButtonAction"): - # self.params_memory.put_bool("CPTLkasButtonAction", False) - # else: - # self.params_memory.put_bool("CPTLkasButtonAction", True) - - # Rotate display mode. These are mostly used in the frontend ui app. - max_display_mode = 1 - current_display_mode = self.params_memory.get_int("ScreenDisaplayMode") - current_display_mode = current_display_mode + 1 - if current_display_mode > max_display_mode: - current_display_mode = 0 - self.params_memory.put_int("ScreenDisaplayMode", current_display_mode) + current = self.params_memory.get_int("ScreenDisplayMode") - # self.params_memory.put_int("SpeedLimitLatDesired", CC.actuators.speed * CV.MS_TO_MPH ) + if self.driving_gear: + # Onroad: 0→4, 1→2, 2→3, 3→4, 4→2 (never back to auto via button) + transitions = {0: 4, 1: 2, 2: 3, 3: 4, 4: 2} + new_mode = transitions.get(current, 0) + else: + # Not in drive: any except 3 → 3, state 3 → 0 + new_mode = 0 if current == 3 else 3 + + self.params_memory.put_int("ScreenDisplayMode", new_mode) return CC def main(): diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc index 0c0b2fc..ad31b34 100755 --- a/selfdrive/ui/qt/home.cc +++ b/selfdrive/ui/qt/home.cc @@ -94,8 +94,8 @@ void HomeWindow::updateState(const UIState &s) { // CLEARPILOT: honor display on/off while showing splash in park if (parked && ready->isVisible()) { - int screenMode = paramsMemory.getInt("ScreenDisaplayMode"); - if (screenMode == 1) { + int screenMode = paramsMemory.getInt("ScreenDisplayMode"); + if (screenMode == 3) { Hardware::set_display_power(false); } else { Hardware::set_display_power(true); diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 14d7d31..19a8dcb 100755 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -395,15 +395,12 @@ if (edgeColor != bgColor) { } void AnnotatedCameraWidget::drawHud(QPainter &p) { - // Blank when screenDisplayMode=1 + // CLEARPILOT: display power control based on ScreenDisplayMode p.save(); - - int screenDisaplayMode = paramsMemory.getInt("ScreenDisaplayMode"); - if (screenDisaplayMode == 1 && !alert_is_visible) { - // Draw black, filled, full-size rectangle to blank the screen - // p.fillRect(0, 0, width(), height(), Qt::black); - // p.restore(); + + if (displayMode == 3 && !alert_is_visible) { Hardware::set_display_power(false); + p.restore(); return; } else { Hardware::set_display_power(true); @@ -599,17 +596,36 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { SubMaster &sm = *(s->sm); + // CLEARPILOT: nightrider mode — outline only, no fill + bool outlineOnly = nightriderMode; + // lanelines for (int i = 0; i < std::size(scene.lane_line_vertices); ++i) { - painter.setBrush(QColor::fromRgbF(1.0, 1.0, 1.0, std::clamp(scene.lane_line_probs[i], 0.0, 0.7))); + QColor lineColor = QColor::fromRgbF(1.0, 1.0, 1.0, std::clamp(scene.lane_line_probs[i], 0.0, 0.7)); + if (outlineOnly) { + painter.setPen(QPen(lineColor, 2)); + painter.setBrush(Qt::NoBrush); + } else { + painter.setPen(Qt::NoPen); + painter.setBrush(lineColor); + } painter.drawPolygon(scene.lane_line_vertices[i]); } + if (outlineOnly) painter.setPen(Qt::NoPen); // road edges for (int i = 0; i < std::size(scene.road_edge_vertices); ++i) { - painter.setBrush(QColor::fromRgbF(1.0, 0, 0, std::clamp(1.0 - scene.road_edge_stds[i], 0.0, 1.0))); + QColor edgeCol = QColor::fromRgbF(1.0, 0, 0, std::clamp(1.0 - scene.road_edge_stds[i], 0.0, 1.0)); + if (outlineOnly) { + painter.setPen(QPen(edgeCol, 2)); + painter.setBrush(Qt::NoBrush); + } else { + painter.setPen(Qt::NoPen); + painter.setBrush(edgeCol); + } painter.drawPolygon(scene.road_edge_vertices[i]); } + if (outlineOnly) painter.setPen(Qt::NoPen); // paint center lane path // QColor bg_colors[CHANGE_LANE_PATH_COLOR]; @@ -674,12 +690,20 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { path_gradient.setColorAt(1.0, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast(CENTER_LANE_ALPHA * 255 * 0.0))); } - painter.setBrush(path_gradient); + if (outlineOnly) { + painter.setPen(QPen(QColor(center_lane_color.red(), center_lane_color.green(), + center_lane_color.blue(), 180), 2)); + painter.setBrush(Qt::NoBrush); + } else { + painter.setPen(Qt::NoPen); + painter.setBrush(path_gradient); + } painter.drawPolygon(scene.track_vertices); + if (outlineOnly) painter.setPen(Qt::NoPen); } // Paint path edges ,Use current background color - if (edgeColor != bg_colors[STATUS_DISENGAGED]) { + if (edgeColor != bg_colors[STATUS_DISENGAGED] && !outlineOnly) { QLinearGradient edge_gradient; edge_gradient.setColorAt(0.0, QColor(edgeColor.red(), edgeColor.green(), edgeColor.blue(), static_cast(255))); edge_gradient.setColorAt(0.5, QColor(edgeColor.red(), edgeColor.green(), edgeColor.blue(), static_cast(255 * 0.8) )); @@ -695,18 +719,24 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { // Paint blindspot path if (scene.blind_spot_path) { - QLinearGradient bs(0, height(), 0, 0); - bs.setColorAt(0.0, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.6)); - bs.setColorAt(0.5, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.4)); - bs.setColorAt(1.0, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.2)); - - painter.setBrush(bs); + QColor bsColor = QColor::fromHslF(0 / 360., 0.75, 0.50, 0.6); + if (outlineOnly) { + painter.setPen(QPen(bsColor, 2)); + painter.setBrush(Qt::NoBrush); + } else { + QLinearGradient bs(0, height(), 0, 0); + bs.setColorAt(0.0, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.6)); + bs.setColorAt(0.5, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.4)); + bs.setColorAt(1.0, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.2)); + painter.setBrush(bs); + } if (blindSpotLeft) { painter.drawPolygon(scene.track_adjacent_vertices[4]); } if (blindSpotRight) { painter.drawPolygon(scene.track_adjacent_vertices[5]); } + if (outlineOnly) painter.setPen(Qt::NoPen); } // Paint adjacent lane paths @@ -717,16 +747,19 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { float maxLaneWidth = laneDetectionWidth * 1.5; auto paintLane = [=](QPainter &painter, const QPolygonF &lane, float laneWidth, bool blindspot) { - QLinearGradient al(0, height(), 0, 0); - bool redPath = laneWidth < minLaneWidth || laneWidth > maxLaneWidth || blindspot; float hue = redPath ? 0.0 : 120.0 * (laneWidth - minLaneWidth) / (maxLaneWidth - minLaneWidth); - al.setColorAt(0.0, QColor::fromHslF(hue / 360.0, 0.75, 0.50, 0.6)); - al.setColorAt(0.5, QColor::fromHslF(hue / 360.0, 0.75, 0.50, 0.4)); - al.setColorAt(1.0, QColor::fromHslF(hue / 360.0, 0.75, 0.50, 0.2)); - - painter.setBrush(al); + if (outlineOnly) { + painter.setPen(QPen(QColor::fromHslF(hue / 360.0, 0.75, 0.50, 0.6), 2)); + painter.setBrush(Qt::NoBrush); + } else { + QLinearGradient al(0, height(), 0, 0); + al.setColorAt(0.0, QColor::fromHslF(hue / 360.0, 0.75, 0.50, 0.6)); + al.setColorAt(0.5, QColor::fromHslF(hue / 360.0, 0.75, 0.50, 0.4)); + al.setColorAt(1.0, QColor::fromHslF(hue / 360.0, 0.75, 0.50, 0.2)); + painter.setBrush(al); + } painter.drawPolygon(lane); painter.setFont(InterFont(30, QFont::DemiBold)); @@ -814,6 +847,10 @@ void AnnotatedCameraWidget::paintEvent(QPaintEvent *event) { const cereal::ModelDataV2::Reader &model = sm["modelV2"].getModelV2(); const float v_ego = sm["carState"].getCarState().getVEgo(); + // CLEARPILOT: read display mode early — needed for camera suppression + displayMode = paramsMemory.getInt("ScreenDisplayMode"); + nightriderMode = (displayMode == 1 || displayMode == 4); + // draw camera frame { std::lock_guard lk(frame_lock); @@ -855,8 +892,14 @@ void AnnotatedCameraWidget::paintEvent(QPaintEvent *event) { CameraWidget::updateCalibration(DEFAULT_CALIBRATION); } painter.beginNativePainting(); - CameraWidget::setFrameId(model.getFrameId()); - CameraWidget::paintGL(); + if (nightriderMode) { + // CLEARPILOT: black background, no camera feed + glClearColor(0.0f, 0.0f, 0.0f, 1.0f); + glClear(GL_COLOR_BUFFER_BIT | GL_STENCIL_BUFFER_BIT); + } else { + CameraWidget::setFrameId(model.getFrameId()); + CameraWidget::paintGL(); + } painter.endNativePainting(); } diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index df67925..3536b5d 100755 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -56,6 +56,8 @@ private: QPixmap dm_img; float speed; bool has_gps_speed = false; + bool nightriderMode = false; + int displayMode = 0; QString speedUnit; float setSpeed; float speedLimit; diff --git a/system/clearpilot/gpsd.py b/system/clearpilot/gpsd.py index e8c2f76..296975e 100644 --- a/system/clearpilot/gpsd.py +++ b/system/clearpilot/gpsd.py @@ -14,11 +14,44 @@ import datetime from cereal import log import cereal.messaging as messaging from openpilot.common.gpio import gpio_init, gpio_set +from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog from openpilot.common.time import system_time_valid from openpilot.system.hardware.tici.pins import GPIO +def is_daylight(lat: float, lon: float, utc_dt: datetime.datetime) -> bool: + """Return True if it's between sunrise and sunset at the given location. + Uses NOAA simplified solar position equations. Pure math, no external libs.""" + n = utc_dt.timetuple().tm_yday + gamma = 2 * math.pi / 365 * (n - 1 + (utc_dt.hour - 12) / 24) + # Equation of time (minutes) + eqtime = 229.18 * (0.000075 + 0.001868 * math.cos(gamma) + - 0.032077 * math.sin(gamma) + - 0.014615 * math.cos(2 * gamma) + - 0.040849 * math.sin(2 * gamma)) + # Solar declination (radians) + decl = (0.006918 - 0.399912 * math.cos(gamma) + + 0.070257 * math.sin(gamma) + - 0.006758 * math.cos(2 * gamma) + + 0.000907 * math.sin(2 * gamma) + - 0.002697 * math.cos(3 * gamma) + + 0.00148 * math.sin(3 * gamma)) + lat_rad = math.radians(lat) + zenith = math.radians(90.833) + cos_ha = (math.cos(zenith) / (math.cos(lat_rad) * math.cos(decl)) + - math.tan(lat_rad) * math.tan(decl)) + if cos_ha < -1: + return True # midnight sun + if cos_ha > 1: + return False # polar night + ha = math.degrees(math.acos(cos_ha)) + sunrise_min = 720 - 4 * (lon + ha) - eqtime + sunset_min = 720 - 4 * (lon - ha) - eqtime + now_min = utc_dt.hour * 60 + utc_dt.minute + utc_dt.second / 60 + return sunrise_min <= now_min <= sunset_min + + def at_cmd(cmd: str) -> str: try: result = subprocess.check_output( @@ -123,6 +156,9 @@ def main(): pm = messaging.PubMaster(["gpsLocation"]) clock_set = system_time_valid() + params_memory = Params("/dev/shm/params") + last_daylight_check = 0.0 + daylight_computed = False print("gpsd: entering main loop", file=sys.stderr, flush=True) while True: @@ -161,6 +197,30 @@ def main(): gps.speedAccuracy = 1.0 pm.send("gpsLocation", msg) + # CLEARPILOT: daylight calculation for auto display mode switching + if clock_set: + now_mono = time.monotonic() + interval = 1.0 if not daylight_computed else 30.0 + if (now_mono - last_daylight_check) >= interval: + last_daylight_check = now_mono + utc_now = datetime.datetime.utcfromtimestamp(fix["timestamp_ms"] / 1000) + daylight = is_daylight(fix["latitude"], fix["longitude"], utc_now) + params_memory.put_bool("IsDaylight", daylight) + + if not daylight_computed: + daylight_computed = True + cloudlog.warning("gpsd: initial daylight calc: %s", "day" if daylight else "night") + print(f"gpsd: initial daylight calc: {'day' if daylight else 'night'}", file=sys.stderr, flush=True) + + # Auto-transition: only touch states 0 and 1 + current_mode = params_memory.get_int("ScreenDisplayMode") + if current_mode == 0 and not daylight: + params_memory.put_int("ScreenDisplayMode", 1) + cloudlog.warning("gpsd: auto-switch to nightrider (sunset)") + elif current_mode == 1 and daylight: + params_memory.put_int("ScreenDisplayMode", 0) + cloudlog.warning("gpsd: auto-switch to normal (sunrise)") + time.sleep(1.0) # 1 Hz polling