diff --git a/system/clearpilot/gpsd.py b/system/clearpilot/gpsd.py index 76273e3..8852cc6 100644 --- a/system/clearpilot/gpsd.py +++ b/system/clearpilot/gpsd.py @@ -6,10 +6,14 @@ and publishes gpsLocation messages. Replaces qcomgpsd (which uses the diag interface — broken on this device). Used solely for: setting system clock on first fix, an on-screen UI -speed indicator, and per-segment GPS metadata for the dashcam. Self- -driving code does NOT consume this output — locationd is patched to not -subscribe to gpsLocation, so liveLocationKalman.gpsOK stays false. +speed indicator, per-segment GPS metadata for the dashcam, and driving +the auto day/night display-mode switch (ScreenDisplayMode 0 ↔ 1) via +NOAA solar-position calc against the current fix. + +Self-driving code does NOT consume this output — locationd is patched +to not subscribe to gpsLocation, so liveLocationKalman.gpsOK stays false. """ +import math import os import subprocess import sys @@ -19,10 +23,69 @@ 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.time import system_time_valid from openpilot.system.hardware.tici.pins import GPIO +def _sunrise_sunset_min(lat: float, lon: float, utc_dt: datetime.datetime): + """Compute (sunrise_min, sunset_min) in UTC minutes since midnight of utc_dt's day. + Values can be negative or >1440 for western/eastern longitudes. Returns + (None, None) for polar night, ('always', 'always') for midnight sun.""" + n = utc_dt.timetuple().tm_yday + gamma = 2 * math.pi / 365 * (n - 1 + (utc_dt.hour - 12) / 24) + 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)) + 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 ('always', 'always') # midnight sun + if cos_ha > 1: + return (None, None) # polar night + ha = math.degrees(math.acos(cos_ha)) + sunrise_min = 720 - 4 * (lon + ha) - eqtime + sunset_min = 720 - 4 * (lon - ha) - eqtime + return (sunrise_min, sunset_min) + + +def is_daylight(lat: float, lon: float, utc_dt: datetime.datetime) -> bool: + """Return True if the sun is currently above the horizon at (lat, lon). + + Handles west-of-Greenwich correctly: at UTC midnight it may still be + evening local time, and the relevant sunset is the PREVIOUS UTC day's + value (which is >1440 min if we re-ref to that day, i.e. it's past + midnight UTC). Symmetric case for east-of-Greenwich at the other end. + + Strategy: compute sunrise/sunset for yesterday, today, and tomorrow (each + relative to its own UTC midnight), shift them all onto today's clock + (yesterday = -1440, tomorrow = +1440), and check if now_min falls inside + any of the three [sunrise, sunset] intervals. + """ + now_min = utc_dt.hour * 60 + utc_dt.minute + utc_dt.second / 60 + for day_offset in (-1, 0, 1): + d = utc_dt + datetime.timedelta(days=day_offset) + sr, ss = _sunrise_sunset_min(lat, lon, d) + if sr == 'always': + return True + if sr is None: + continue # polar night this day + sr += day_offset * 1440 + ss += day_offset * 1440 + if sr <= now_min <= ss: + return True + return False + + def at_cmd(cmd: str) -> str: try: result = subprocess.check_output( @@ -123,6 +186,10 @@ 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 + prev_daylight = None # gate IsDaylight write on change (twice/day, no point rewriting every 30s) print("CLP gpsd: entering main loop", file=sys.stderr, flush=True) while True: @@ -160,6 +227,33 @@ def main(): gps.speedAccuracy = 1.0 pm.send("gpsLocation", msg) + # Daylight calc + auto display-mode switch (only touches modes 0 and 1). + # First calc happens immediately once clock is set; thereafter every 30s. + 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) + if daylight != prev_daylight: + params_memory.put_bool("IsDaylight", daylight) + prev_daylight = daylight + + if not daylight_computed: + daylight_computed = True + print(f"CLP gpsd: initial daylight calc: {'day' if daylight else 'night'}", + file=sys.stderr, flush=True) + + # Auto-transition: only touch states 0 and 1 (manual modes 2/3/4 stay) + current_mode = params_memory.get_int("ScreenDisplayMode") + if current_mode == 0 and not daylight: + params_memory.put_int("ScreenDisplayMode", 1) + print("CLP gpsd: auto-switch to nightrider (sunset)", file=sys.stderr, flush=True) + elif current_mode == 1 and daylight: + params_memory.put_int("ScreenDisplayMode", 0) + print("CLP gpsd: auto-switch to normal (sunrise)", file=sys.stderr, flush=True) + time.sleep(0.5) # 2 Hz polling