dragonpilot beta3

date: 2023-10-09T10:55:55
commit: 91b6e3aecd7170f24bccacb10c515ec281c30295

system/sensord/pigeond.py

@@ -10,11 +10,11 @@ from datetime import datetime
 from typing import List, Optional, Tuple
 
 from cereal import messaging
-from common.params import Params
-from system.swaglog import cloudlog
-from system.hardware import TICI
-from common.gpio import gpio_init, gpio_set
-from system.hardware.tici.pins import GPIO
+from openpilot.common.params import Params
+from openpilot.system.swaglog import cloudlog
+from openpilot.system.hardware import TICI
+from openpilot.common.gpio import gpio_init, gpio_set
+from openpilot.system.hardware.tici.pins import GPIO
 
 UBLOX_TTY = "/dev/ttyHS0"
 
@@ -27,11 +27,11 @@ UBLOX_ASSIST_ACK = b"\xb5\x62\x13\x60\x08\x00"
 
 def set_power(enabled: bool) -> None:
   gpio_init(GPIO.UBLOX_SAFEBOOT_N, True)
-  gpio_init(GPIO.UBLOX_PWR_EN, True)
+  gpio_init(GPIO.GNSS_PWR_EN, True)
   gpio_init(GPIO.UBLOX_RST_N, True)
 
   gpio_set(GPIO.UBLOX_SAFEBOOT_N, True)
-  gpio_set(GPIO.UBLOX_PWR_EN, enabled)
+  gpio_set(GPIO.GNSS_PWR_EN, enabled)
   gpio_set(GPIO.UBLOX_RST_N, enabled)
 
 def add_ubx_checksum(msg: bytes) -> bytes:
@@ -164,12 +164,12 @@ def initialize_pigeon(pigeon: TTYPigeon) -> bool:
       pigeon.send_with_ack(b"\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10")
 
       # UBX-CFG-NAV5 (0x06 0x24)
-      pigeon.send_with_ack(b"\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63") # noqa: E501
+      pigeon.send_with_ack(b"\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63")
 
       # UBX-CFG-ODO (0x06 0x1E)
       pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37")
       pigeon.send_with_ack(b"\xB5\x62\x06\x39\x08\x00\xFF\xAD\x62\xAD\x1E\x63\x00\x00\x83\x0C")
-      pigeon.send_with_ack(b"\xB5\x62\x06\x23\x28\x00\x00\x00\x00\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x56\x24") # noqa: E501
+      pigeon.send_with_ack(b"\xB5\x62\x06\x23\x28\x00\x00\x00\x00\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x56\x24")
 
       # UBX-CFG-NAV5 (0x06 0x24)
       pigeon.send_with_ack(b"\xB5\x62\x06\x24\x00\x00\x2A\x84")

system/sensord/rawgps/nmeaport.py

@@ -0,0 +1,169 @@
+import os
+import sys
+from dataclasses import dataclass, fields
+from subprocess import check_output, CalledProcessError
+from time import sleep
+from typing import NoReturn
+
+DEBUG = int(os.environ.get("DEBUG", "0"))
+
+@dataclass
+class GnssClockNmeaPort:
+  # flags bit mask:
+  # 0x01 = leap_seconds valid
+  # 0x02 = time_uncertainty_ns valid
+  # 0x04 = full_bias_ns valid
+  # 0x08 = bias_ns valid
+  # 0x10 = bias_uncertainty_ns valid
+  # 0x20 = drift_nsps valid
+  # 0x40 = drift_uncertainty_nsps valid
+  flags: int
+  leap_seconds: int
+  time_ns: int
+  time_uncertainty_ns: int # 1-sigma
+  full_bias_ns: int
+  bias_ns: float
+  bias_uncertainty_ns: float # 1-sigma
+  drift_nsps: float
+  drift_uncertainty_nsps: float # 1-sigma
+
+  def __post_init__(self):
+    for field in fields(self):
+      val = getattr(self, field.name)
+      setattr(self, field.name, field.type(val) if val else None)
+
+@dataclass
+class GnssMeasNmeaPort:
+  messageCount: int
+  messageNum: int
+  svCount: int
+  # constellation enum:
+  # 1 = GPS
+  # 2 = SBAS
+  # 3 = GLONASS
+  # 4 = QZSS
+  # 5 = BEIDOU
+  # 6 = GALILEO
+  constellation: int
+  svId: int
+  flags: int # always zero
+  time_offset_ns: int
+  # state bit mask:
+  # 0x0001 = CODE LOCK
+  # 0x0002 = BIT SYNC
+  # 0x0004 = SUBFRAME SYNC
+  # 0x0008 = TIME OF WEEK DECODED
+  # 0x0010 = MSEC AMBIGUOUS
+  # 0x0020 = SYMBOL SYNC
+  # 0x0040 = GLONASS STRING SYNC
+  # 0x0080 = GLONASS TIME OF DAY DECODED
+  # 0x0100 = BEIDOU D2 BIT SYNC
+  # 0x0200 = BEIDOU D2 SUBFRAME SYNC
+  # 0x0400 = GALILEO E1BC CODE LOCK
+  # 0x0800 = GALILEO E1C 2ND CODE LOCK
+  # 0x1000 = GALILEO E1B PAGE SYNC
+  # 0x2000 = GALILEO E1B PAGE SYNC
+  state: int
+  time_of_week_ns: int
+  time_of_week_uncertainty_ns: int # 1-sigma
+  carrier_to_noise_ratio: float
+  pseudorange_rate: float
+  pseudorange_rate_uncertainty: float # 1-sigma
+
+  def __post_init__(self):
+    for field in fields(self):
+      val = getattr(self, field.name)
+      setattr(self, field.name, field.type(val) if val else None)
+
+def nmea_checksum_ok(s):
+  checksum = 0
+  for i, c in enumerate(s[1:]):
+    if c == "*":
+      if i != len(s) - 4: # should be 3rd to last character
+        print("ERROR: NMEA string does not have checksum delimiter in correct location:", s)
+        return False
+      break
+    checksum ^= ord(c)
+  else:
+    print("ERROR: NMEA string does not have checksum delimiter:", s)
+    return False
+
+  return True
+
+def process_nmea_port_messages(device:str="/dev/ttyUSB1") -> NoReturn:
+  while True:
+    try:
+      with open(device, "r") as nmeaport:
+        for line in nmeaport:
+          line = line.strip()
+          if DEBUG:
+            print(line)
+          if not line.startswith("$"): # all NMEA messages start with $
+            continue
+          if not nmea_checksum_ok(line):
+            continue
+
+          fields = line.split(",")
+          match fields[0]:
+            case "$GNCLK":
+              # fields at end are reserved (not used)
+              gnss_clock = GnssClockNmeaPort(*fields[1:10]) # type: ignore[arg-type]
+              print(gnss_clock)
+            case "$GNMEAS":
+              # fields at end are reserved (not used)
+              gnss_meas = GnssMeasNmeaPort(*fields[1:14]) # type: ignore[arg-type]
+              print(gnss_meas)
+    except Exception as e:
+      print(e)
+      sleep(1)
+
+def main() -> NoReturn:
+  from openpilot.common.gpio import gpio_init, gpio_set
+  from openpilot.system.hardware.tici.pins import GPIO
+  from openpilot.system.sensord.rawgps.rawgpsd import at_cmd
+
+  try:
+    check_output(["pidof", "rawgpsd"])
+    print("rawgpsd is running, please kill openpilot before running this script! (aborted)")
+    sys.exit(1)
+  except CalledProcessError as e:
+    if e.returncode != 1: # 1 == no process found (boardd not running)
+      raise e
+
+  print("power up antenna ...")
+  gpio_init(GPIO.GNSS_PWR_EN, True)
+  gpio_set(GPIO.GNSS_PWR_EN, True)
+
+  if b"+QGPS: 0" not in (at_cmd("AT+QGPS?") or b""):
+    print("stop location tracking ...")
+    at_cmd("AT+QGPSEND")
+
+  if b'+QGPSCFG: "outport",usbnmea' not in (at_cmd('AT+QGPSCFG="outport"') or b""):
+    print("configure outport ...")
+    at_cmd('AT+QGPSCFG="outport","usbnmea"') # usbnmea = /dev/ttyUSB1
+
+  if b'+QGPSCFG: "gnssrawdata",3,0' not in (at_cmd('AT+QGPSCFG="gnssrawdata"') or b""):
+    print("configure gnssrawdata ...")
+    # AT+QGPSCFG="gnssrawdata",<constellation-mask>,<port>'
+    # <constellation-mask> values:
+    # 0x01 = GPS
+    # 0x02 = GLONASS
+    # 0x04 = BEIDOU
+    # 0x08 = GALILEO
+    # 0x10 = QZSS
+    # <port> values:
+    # 0 = NMEA port
+    # 1 = AT port
+    at_cmd('AT+QGPSCFG="gnssrawdata",3,0') # enable all constellations, output data to NMEA port
+    print("rebooting ...")
+    at_cmd('AT+CFUN=1,1')
+    print("re-run this script when it is back up")
+    sys.exit(2)
+
+  print("starting location tracking ...")
+  at_cmd("AT+QGPS=1")
+
+  process_nmea_port_messages()
+
+if __name__ == "__main__":
+  main()

system/sensord/rawgps/rawgpsd.py

@@ -15,14 +15,14 @@ from struct import unpack_from, calcsize, pack
 
 from cereal import log
 import cereal.messaging as messaging
-from common.gpio import gpio_init, gpio_set
+from openpilot.common.gpio import gpio_init, gpio_set
 from laika.gps_time import GPSTime, utc_to_gpst, get_leap_seconds
 from laika.helpers import get_prn_from_nmea_id
 from laika.constants import SECS_IN_HR, SECS_IN_DAY, SECS_IN_WEEK
-from system.hardware.tici.pins import GPIO
-from system.swaglog import cloudlog
-from system.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
-from system.sensord.rawgps.structs import (dict_unpacker, position_report, relist,
+from openpilot.system.hardware.tici.pins import GPIO
+from openpilot.system.swaglog import cloudlog
+from openpilot.system.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
+from openpilot.system.sensord.rawgps.structs import (dict_unpacker, position_report, relist,
                                               gps_measurement_report, gps_measurement_report_sv,
                                               glonass_measurement_report, glonass_measurement_report_sv,
                                               oemdre_measurement_report, oemdre_measurement_report_sv, oemdre_svpoly_report,
@@ -271,7 +271,7 @@ def main() -> NoReturn:
   def cleanup(sig, frame):
     cloudlog.warning("caught sig disabling quectel gps")
 
-    gpio_set(GPIO.UBLOX_PWR_EN, False)
+    gpio_set(GPIO.GNSS_PWR_EN, False)
     teardown_quectel(diag)
     cloudlog.warning("quectel cleanup done")
 
@@ -289,8 +289,8 @@ def main() -> NoReturn:
   want_assistance = not r
   current_gps_time = utc_to_gpst(GPSTime.from_datetime(datetime.utcnow()))
   cloudlog.warning("quectel setup done")
-  gpio_init(GPIO.UBLOX_PWR_EN, True)
-  gpio_set(GPIO.UBLOX_PWR_EN, True)
+  gpio_init(GPIO.GNSS_PWR_EN, True)
+  gpio_set(GPIO.GNSS_PWR_EN, True)
 
   pm = messaging.PubMaster(['qcomGnss', 'gpsLocation'])
 

system/sensord/rawgps/test_rawgps.py

@@ -8,10 +8,10 @@ import subprocess
 import numpy as np
 
 import cereal.messaging as messaging
-from system.hardware import TICI
-from system.sensord.rawgps.rawgpsd import at_cmd, wait_for_modem
-from selfdrive.manager.process_config import managed_processes
-from common.transformations.coordinates import ecef_from_geodetic
+from openpilot.system.hardware import TICI
+from openpilot.system.sensord.rawgps.rawgpsd import at_cmd, wait_for_modem
+from openpilot.selfdrive.manager.process_config import managed_processes
+from openpilot.common.transformations.coordinates import ecef_from_geodetic
 
 GOOD_SIGNAL = bool(int(os.getenv("GOOD_SIGNAL", '0')))
 

system/sensord/sensors/file_sensor.h

@@ -1,19 +0,0 @@
-#pragma once
-
-#include <fstream>
-#include <string>
-
-#include "cereal/gen/cpp/log.capnp.h"
-#include "system/sensord/sensors/sensor.h"
-
-class FileSensor : public Sensor {
-protected:
-  std::ifstream file;
-
-public:
-  FileSensor(std::string filename);
-  ~FileSensor();
-  int init();
-  bool has_interrupt_enabled();
-  virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
-};

system/sensord/sensors/i2c_sensor.h

@@ -2,6 +2,7 @@
 
 #include <cstdint>
 #include <unistd.h>
+#include <vector>
 #include "cereal/gen/cpp/log.capnp.h"
 
 #include "common/i2c.h"

system/sensord/sensors/light_sensor.h

@@ -1,9 +0,0 @@
-#pragma once
-#include "file_sensor.h"
-
-class LightSensor : public FileSensor {
-public:
-  LightSensor(std::string filename);
-  bool get_event(MessageBuilder &msg, uint64_t ts = 0);
-  int shutdown() { return 0; }
-};

system/sensord/sensors/mmc5603nj_magn.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <vector>
+
 #include "system/sensord/sensors/i2c_sensor.h"
 
 // Address of the chip on the bus
@@ -19,9 +21,14 @@
 #define MMC5603NJ_AUTO_SR_EN     (1 << 5)
 #define MMC5603NJ_CMM_EN         (1 << 4)
 #define MMC5603NJ_EN_PRD_SET     (1 << 3)
+#define MMC5603NJ_SET            (1 << 3)
+#define MMC5603NJ_RESET          (1 << 4)
 
 class MMC5603NJ_Magn : public I2CSensor {
+private:
   uint8_t get_device_address() {return MMC5603NJ_I2C_ADDR;}
+  void start_measurement();
+  std::vector<float> read_measurement();
 public:
   MMC5603NJ_Magn(I2CBus *bus);
   int init();

system/sensord/sensors/sensor.h

@@ -5,14 +5,18 @@
 class Sensor {
 public:
   int gpio_fd = -1;
+  uint64_t start_ts = 0;
   uint64_t init_delay = 500e6; // default dealy 500ms
-  virtual ~Sensor() {};
+  virtual ~Sensor() {}
   virtual int init() = 0;
   virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
   virtual bool has_interrupt_enabled() = 0;
   virtual int shutdown() = 0;
 
-  virtual bool is_data_valid(uint64_t st, uint64_t ct) {
-    return (ct - st) > init_delay;
+  virtual bool is_data_valid(uint64_t current_ts) {
+    if (start_ts == 0) {
+      start_ts = current_ts;
+    }
+    return (current_ts - start_ts) > init_delay;
   }
 };