UART & Sensors

orchestrateur/sensors/gps.py

@@ -0,0 +1,122 @@
+import serial
+import time
+import threading
+from shared.logging import log
+from sensors.lock import serial_lock
+
+def calculate_nmea_checksum(line: str) -> bool:
+    """Validates standard NMEA 0183 sentence checksum ($...*HH)."""
+    if not line.startswith('$') or '*' not in line:
+        return False
+    
+    try:
+        content, checksum_str = line[1:].split('*', 1)
+        calculated_checksum = 0
+        for char in content:
+            calculated_checksum ^= ord(char)
+        
+        return calculated_checksum == int(checksum_str[:2], 16)
+    except Exception:
+        return False
+
+
+class GROVEGPS:
+    def __init__(self, port='/dev/ttyAMA0', baud=9600, timeout=1):
+        self.ser = serial.Serial(port, baud, timeout=timeout)
+        self.clean_data()
+
+    def clean_data(self):
+        self.timestamp = ""
+        self.quality = 0
+        self.satellites = 0
+        self.altitude = -1.0
+        self.latitude = -1.0
+        self.longitude = -1.0
+
+    def read(self):
+        """Reads the latest GGA sentence from serial, thread-safely."""
+        with serial_lock:
+            # 1. Flush accumulated stale data in the UART buffer
+            if self.ser.in_waiting > 0:
+                self.ser.reset_input_buffer()
+
+            # 2. Try reading up to 15 lines to catch the freshest GGA sentence
+            for _ in range(5):
+                raw_bytes = self.ser.readline()
+                try:
+                    line = raw_bytes.decode('utf-8', errors='ignore').strip()
+                    # log(f"GPS: Read line: {line}")
+                except Exception:
+                    continue
+
+                # Supports both $GPGGA and modern $GNGGA sentences
+                if (line.startswith('$GPGGA') or line.startswith('$GNGGA')) and calculate_nmea_checksum(line):
+                    if self.parse_gga(line):
+                        return True
+        return False
+
+    def parse_gga(self, line):
+        self.clean_data()
+        gga = line.split(',')
+        
+        if len(gga) < 10:
+            return False
+
+        try:
+            self.timestamp = gga[1]
+            self.quality = int(gga[6]) if gga[6] != "" else 0
+            self.satellites = int(gga[7]) if gga[7] != "" else 0
+
+            # If quality > 0 and coordinates exist, convert NMEA DDDMM.MMMM to decimal degrees
+            if self.quality > 0 and gga[2] != "" and gga[4] != "":
+                lat_raw = float(gga[2])
+                ns = gga[3]
+                lon_raw = float(gga[4])
+                ew = gga[5]
+
+                # Latitude calculation
+                lat_deg = lat_raw // 100
+                lat_min = lat_raw % 100
+                self.latitude = lat_deg + (lat_min / 60.0)
+                if ns == 'S':
+                    self.latitude = -self.latitude
+
+                # Longitude calculation
+                lon_deg = lon_raw // 100
+                lon_min = lon_raw % 100
+                self.longitude = lon_deg + (lon_min / 60.0)
+                if ew == 'W':
+                    self.longitude = -self.longitude
+
+                self.altitude = float(gga[9]) if gga[9] != "" else -1.0
+                return True
+            else:
+                # No lock on this line
+                return True
+
+        except (ValueError, IndexError):
+            return False
+
+
+# Shared instance
+gps = GROVEGPS()
+
+def get_gps_data():
+    """Returns GPS dictionary if fix is valid, otherwise returns None."""
+    has_data = gps.read()
+    
+    # Strictly check that we have a valid GPS lock (quality > 0 and valid coordinates)
+    if has_data and gps.quality > 0 and gps.latitude != -1.0:
+        return {
+            "timestamp": gps.timestamp,
+            "latitude": round(gps.latitude, 6),
+            "longitude": round(gps.longitude, 6),
+            "altitude": gps.altitude,
+            "quality": gps.quality,
+            "satellites": gps.satellites
+        }
+    else:
+        log(f"GPS: No valid fix or data available. Satellites: {gps.satellites}, Quality: {gps.quality}")
+    
+    # Return None so main.py doesn't process or log empty GPS data
+    return None