clearpilot/system/clearpilot/gpsd.py

#!/usr/bin/env python3
"""
ClearPilot GPS daemon — reads GPS from Quectel EC25 modem via AT commands
and publishes gpsLocation messages for locationd/timed.

Replaces qcomgpsd which uses the diag interface (broken on this device).
"""
import math
import os
import subprocess
import time
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 _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(
      f"mmcli -m any --timeout 10 --command='{cmd}'",
      shell=True, encoding='utf8', stderr=subprocess.DEVNULL
    ).strip()
    # mmcli wraps AT responses: response: '+QGPSLOC: ...'
    # Strip the wrapper to get just the AT response
    if result.startswith("response: '") and result.endswith("'"):
      result = result[len("response: '"):-1]
    return result
  except subprocess.CalledProcessError:
    return ""


def wait_for_modem():
  cloudlog.warning("gpsd: waiting for modem")
  while True:
    ret = subprocess.call(
      "mmcli -m any --timeout 10 --command='AT+QGPS?'",
      stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True
    )
    if ret == 0:
      return
    time.sleep(0.5)


def parse_qgpsloc(response: str):
  """Parse AT+QGPSLOC=2 response into a dict.
  Format: +QGPSLOC: UTC,lat,lon,hdop,alt,fix,cog,spkm,spkn,date,nsat
  """
  if "+QGPSLOC:" not in response:
    return None
  try:
    data = response.split("+QGPSLOC:")[1].strip()
    fields = data.split(",")
    if len(fields) < 11:
      return None

    utc = fields[0]       # HHMMSS.S
    lat = float(fields[1])
    lon = float(fields[2])
    hdop = float(fields[3])
    alt = float(fields[4])
    fix = int(fields[5])  # 2=2D, 3=3D
    cog = float(fields[6])  # course over ground
    spkm = float(fields[7])  # speed km/h
    spkn = float(fields[8])  # speed knots
    date = fields[9]      # DDMMYY
    nsat = int(fields[10])

    # Build unix timestamp from UTC + date
    # utc: "HHMMSS.S", date: "DDMMYY"
    hh, mm = int(utc[0:2]), int(utc[2:4])
    ss = float(utc[4:])
    dd, mo, yy = int(date[0:2]), int(date[2:4]), 2000 + int(date[4:6])
    dt = datetime.datetime(yy, mo, dd, hh, mm, int(ss),
                           int((ss % 1) * 1e6), datetime.timezone.utc)

    return {
      "latitude": lat,
      "longitude": lon,
      "altitude": alt,
      "speed": spkm / 3.6,  # convert km/h to m/s
      "bearing": cog,
      "accuracy": hdop * 5,  # rough conversion from HDOP to meters
      "timestamp_ms": dt.timestamp() * 1e3,
      "satellites": nsat,
      "fix": fix,
    }
  except (ValueError, IndexError) as e:
    cloudlog.error(f"gpsd: parse error: {e}")
    return None


def main():
  import sys
  print("gpsd: starting", file=sys.stderr, flush=True)

  # Kill system gpsd which may respawn and interfere with modem access
  subprocess.run("pkill -f /usr/sbin/gpsd", shell=True,
                 stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)

  wait_for_modem()
  print("gpsd: modem ready", file=sys.stderr, flush=True)

  # Enable GPS antenna power
  gpio_init(GPIO.GNSS_PWR_EN, True)
  gpio_set(GPIO.GNSS_PWR_EN, True)
  print("gpsd: GPIO power enabled", file=sys.stderr, flush=True)

  # Don't restart GPS if already running (preserve existing fix)
  resp = at_cmd("AT+QGPS?")
  print(f"gpsd: QGPS status: {resp}", file=sys.stderr, flush=True)
  if "QGPS: 1" not in resp:
    at_cmd('AT+QGPSCFG="dpoenable",0')
    at_cmd('AT+QGPSCFG="outport","none"')
    at_cmd("AT+QGPS=1")
    print("gpsd: GPS started fresh", file=sys.stderr, flush=True)
  else:
    print("gpsd: GPS already running, keeping fix", file=sys.stderr, flush=True)

  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  # CLEARPILOT: gate IsDaylight write on change
  print("gpsd: entering main loop", file=sys.stderr, flush=True)

  while True:
    resp = at_cmd("AT+QGPSLOC=2")
    fix = parse_qgpsloc(resp)

    if fix:
      # Set system clock from GPS on first valid fix if clock is invalid
      if not clock_set:
        gps_dt = datetime.datetime.utcfromtimestamp(fix["timestamp_ms"] / 1000)
        ret = subprocess.run(["date", "-s", gps_dt.strftime("%Y-%m-%d %H:%M:%S")],
                             env={**os.environ, "TZ": "UTC"},
                             capture_output=True)
        if ret.returncode == 0:
          clock_set = True
          cloudlog.warning("gpsd: system clock set from GPS: %s", gps_dt)
          print(f"gpsd: system clock set from GPS: {gps_dt}", file=sys.stderr, flush=True)
        else:
          cloudlog.error("gpsd: failed to set clock: %s", ret.stderr.decode().strip())

      msg = messaging.new_message("gpsLocation", valid=True)
      gps = msg.gpsLocation
      gps.latitude = fix["latitude"]
      gps.longitude = fix["longitude"]
      gps.altitude = fix["altitude"]
      gps.speed = fix["speed"]
      gps.bearingDeg = fix["bearing"]
      gps.horizontalAccuracy = fix["accuracy"]
      gps.unixTimestampMillis = int(fix["timestamp_ms"])
      gps.source = log.GpsLocationData.SensorSource.qcomdiag
      gps.hasFix = fix["fix"] >= 2
      gps.flags = 1
      gps.vNED = [0.0, 0.0, 0.0]
      gps.verticalAccuracy = fix["accuracy"]
      gps.bearingAccuracyDeg = 10.0
      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)
          # CLEARPILOT: gate on change — daylight flips twice a day, don't rewrite every 30s
          if daylight != prev_daylight:
            params_memory.put_bool("IsDaylight", daylight)
            prev_daylight = 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(0.5)  # 2 Hz polling


if __name__ == "__main__":
  main()