5-state display mode: auto day/night, nightrider, screen off
New ScreenDisplayMode param (fixes ScreenDisaplayMode typo): 0=auto-normal, 1=auto-nightrider, 2=normal, 3=screen-off, 4=nightrider Nightrider mode: black background (no camera feed), path/lane lines drawn as 2px outlines only. Auto mode uses NOAA solar position calc in gpsd to switch between day/night based on GPS lat/lon and UTC time. First calc on GPS fix, then every 30 seconds. Button cycle onroad: 0→4, 1→2, 2→3, 3→4, 4→2 (never back to auto). Offroad: any→3, 3→0. bench_cmd debugbutton simulates the button press. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
@@ -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
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user