122 lines
4.0 KiB
Python
122 lines
4.0 KiB
Python
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 |