gpsd: re-add day/night auto display-mode switch
Restores the IsDaylight + ScreenDisplayMode auto-switch logic that was stripped during the initial gpsd port (since the display-modes feature hadn't been touched yet). NOAA solar-position calc against the current GPS fix: - First calc immediately when system clock is valid + GPS has fix - Every 30s thereafter - Writes IsDaylight memory param on change (twice/day, gated) - ScreenDisplayMode 0 + sunset → 1 (auto-nightrider) - ScreenDisplayMode 1 + sunrise → 0 (auto-normal) - Manual modes 2/3/4 are never touched Print-to-stderr instead of cloudlog per CLAUDE.md.
This commit is contained in:
@@ -6,10 +6,14 @@ and publishes gpsLocation messages.
|
|||||||
Replaces qcomgpsd (which uses the diag interface — broken on this device).
|
Replaces qcomgpsd (which uses the diag interface — broken on this device).
|
||||||
|
|
||||||
Used solely for: setting system clock on first fix, an on-screen UI
|
Used solely for: setting system clock on first fix, an on-screen UI
|
||||||
speed indicator, and per-segment GPS metadata for the dashcam. Self-
|
speed indicator, per-segment GPS metadata for the dashcam, and driving
|
||||||
driving code does NOT consume this output — locationd is patched to not
|
the auto day/night display-mode switch (ScreenDisplayMode 0 ↔ 1) via
|
||||||
subscribe to gpsLocation, so liveLocationKalman.gpsOK stays false.
|
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 os
|
||||||
import subprocess
|
import subprocess
|
||||||
import sys
|
import sys
|
||||||
@@ -19,10 +23,69 @@ import datetime
|
|||||||
from cereal import log
|
from cereal import log
|
||||||
import cereal.messaging as messaging
|
import cereal.messaging as messaging
|
||||||
from openpilot.common.gpio import gpio_init, gpio_set
|
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.common.time import system_time_valid
|
||||||
from openpilot.system.hardware.tici.pins import GPIO
|
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:
|
def at_cmd(cmd: str) -> str:
|
||||||
try:
|
try:
|
||||||
result = subprocess.check_output(
|
result = subprocess.check_output(
|
||||||
@@ -123,6 +186,10 @@ def main():
|
|||||||
|
|
||||||
pm = messaging.PubMaster(["gpsLocation"])
|
pm = messaging.PubMaster(["gpsLocation"])
|
||||||
clock_set = system_time_valid()
|
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)
|
print("CLP gpsd: entering main loop", file=sys.stderr, flush=True)
|
||||||
|
|
||||||
while True:
|
while True:
|
||||||
@@ -160,6 +227,33 @@ def main():
|
|||||||
gps.speedAccuracy = 1.0
|
gps.speedAccuracy = 1.0
|
||||||
pm.send("gpsLocation", msg)
|
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
|
time.sleep(0.5) # 2 Hz polling
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user