Eskimue/ExoMy_Cuno
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GPS im Admin und offline verbesserungen

Browse Source
This commit is contained in:
Eskimue
2026-05-24 10:43:57 +02:00
parent 91fb1fcf0b
commit 1899a49f4d
5 changed files with 1019 additions and 427 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ExoMy_Software-master/src/gps_node.py
+164 -6
View File
@@ -1,5 +1,6 @@
#!/usr/bin/env python
import errno
import json
import os
import select
import termios
@@ -76,9 +77,15 @@ class GpsState(object):
self.track_deg = None
self.fix_quality = 0
self.satellites = 0
self.pdop = None
self.vdop = None
self.valid = False
self.last_sentence_time = None
self.last_fix_time = None
self.utc_datetime = None
self.visible_satellites = {}
self.visible_satellite_count = 0
self._gsv_groups = {}
def summary_text(self):
if not self.connected:
@@ -200,15 +207,129 @@ def parse_rmc(fields, state):
state.valid = True
state.last_fix_time = rospy.Time.now()
# Optional date/time parse for debugging consistency.
if fields[1] and fields[9]:
try:
datetime.strptime(fields[9] + fields[1].split('.')[0], '%d%m%y%H%M%S')
state.utc_datetime = datetime.strptime(fields[9] + fields[1].split('.')[0], '%d%m%y%H%M%S')
except ValueError:
pass
return True
def parse_gsa(fields, state):
if len(fields) < 18:
return False
try:
state.pdop = float(fields[15]) if fields[15] else None
except ValueError:
state.pdop = None
try:
state.hdop = float(fields[16]) if fields[16] else state.hdop
except ValueError:
pass
try:
state.vdop = float(fields[17]) if fields[17] else None
except ValueError:
state.vdop = None
return True
def merge_visible_satellites(state):
now = rospy.Time.now()
merged = {}
stale_talkers = []
for talker, group in state._gsv_groups.items():
completed_at = group.get('completed_at')
if completed_at is None:
continue
try:
if (now - completed_at).to_sec() > 8.0:
stale_talkers.append(talker)
continue
except rospy.ROSException:
continue
constellation = group.get('constellation', talker)
for sat_key, satellite in group.get('satellites', {}).items():
merged[sat_key] = dict(satellite, constellation=constellation)
for talker in stale_talkers:
state._gsv_groups.pop(talker, None)
state.visible_satellites = dict(sorted(merged.items()))
state.visible_satellite_count = len(state.visible_satellites)
def parse_gsv(fields, state, sentence_type):
if len(fields) < 4:
return False
try:
total_sentences = int(fields[1] or 0)
sentence_index = int(fields[2] or 0)
total_satellites = int(fields[3] or 0)
except ValueError:
return False
if sentence_index <= 0:
return False
talker = sentence_type[1:3] if len(sentence_type) >= 3 else 'GN'
group = state._gsv_groups.get(talker)
if group is None:
group = {
'constellation': talker,
'total_sentences': 0,
'working_set': {},
'satellites': {},
'completed_at': None,
'total_satellites_reported': 0,
}
state._gsv_groups[talker] = group
if sentence_index == 1 or total_sentences != group.get('total_sentences'):
group['working_set'] = {}
group['total_sentences'] = total_sentences
group['total_satellites_reported'] = total_satellites
for index in range(4, len(fields), 4):
if index + 3 >= len(fields):
break
prn = fields[index].strip()
if not prn:
continue
try:
elevation = int(fields[index + 1]) if fields[index + 1] else None
except ValueError:
elevation = None
try:
azimuth = int(fields[index + 2]) if fields[index + 2] else None
except ValueError:
azimuth = None
try:
snr = int(fields[index + 3]) if fields[index + 3] else None
except ValueError:
snr = None
satellite_key = '{0}:{1}'.format(talker, prn)
group['working_set'][satellite_key] = {
'prn': '{0}-{1}'.format(talker, prn),
'elevation': elevation,
'azimuth': azimuth,
'snr': snr
}
if sentence_index >= total_sentences:
group['satellites'] = dict(group['working_set'])
group['completed_at'] = rospy.Time.now()
merge_visible_satellites(state)
return True
def parse_sentence(sentence, state):
if not verify_checksum(sentence):
return False
@@ -224,10 +345,45 @@ def parse_sentence(sentence, state):
return parse_gga(fields, state)
if sentence_type.endswith('RMC'):
return parse_rmc(fields, state)
if sentence_type.endswith('GSA'):
return parse_gsa(fields, state)
if sentence_type.endswith('GSV'):
return parse_gsv(fields, state, sentence_type)
return False
def publish_fix(fix_publisher, overlay_publisher, state, frame_id):
def build_diagnostics_payload(state, frame_id):
last_fix_age = None
if state.last_fix_time is not None:
try:
last_fix_age = max(0.0, (rospy.Time.now() - state.last_fix_time).to_sec())
except rospy.ROSException:
last_fix_age = None
return {
'connected': state.connected,
'frame_id': frame_id,
'valid': state.valid,
'fix_quality': state.fix_quality,
'satellites_used': max(0, int(state.satellites or 0)),
'satellites_visible': max(0, int(state.visible_satellite_count or 0)),
'latitude': state.latitude,
'longitude': state.longitude,
'altitude_m': state.altitude,
'hdop': state.hdop,
'pdop': state.pdop,
'vdop': state.vdop,
'speed_mps': state.speed_mps,
'speed_kmh': (state.speed_mps or 0.0) * 3.6 if state.speed_mps is not None else None,
'track_deg': state.track_deg,
'utc_time': state.utc_datetime.strftime('%H:%M:%S') if state.utc_datetime else None,
'utc_date': state.utc_datetime.strftime('%Y-%m-%d') if state.utc_datetime else None,
'last_fix_age_s': last_fix_age,
'satellites': list(state.visible_satellites.values())
}
def publish_fix(fix_publisher, overlay_publisher, diagnostics_publisher, state, frame_id):
message = NavSatFix()
message.header.stamp = rospy.Time.now()
message.header.frame_id = frame_id
@@ -258,6 +414,7 @@ def publish_fix(fix_publisher, overlay_publisher, state, frame_id):
fix_publisher.publish(message)
overlay_publisher.publish(String(data=state.summary_text()))
diagnostics_publisher.publish(String(data=json.dumps(build_diagnostics_payload(state, frame_id), sort_keys=True)))
if __name__ == '__main__':
@@ -270,6 +427,7 @@ if __name__ == '__main__':
fix_publisher = rospy.Publisher('/fix', NavSatFix, queue_size=5)
overlay_publisher = rospy.Publisher('/gps/status_text', String, queue_size=5)
diagnostics_publisher = rospy.Publisher('/gps/diagnostics_json', String, queue_size=5)
state = GpsState()
serial_fd = None
@@ -280,14 +438,14 @@ if __name__ == '__main__':
serial_fd = open_device(port, baud)
buffer = bytearray()
state.connected = serial_fd is not None
publish_fix(fix_publisher, overlay_publisher, state, frame_id)
publish_fix(fix_publisher, overlay_publisher, diagnostics_publisher, state, frame_id)
if serial_fd is None:
break
try:
readable, _, _ = select.select([serial_fd], [], [], 0.5)
if serial_fd not in readable:
publish_fix(fix_publisher, overlay_publisher, state, frame_id)
publish_fix(fix_publisher, overlay_publisher, diagnostics_publisher, state, frame_id)
continue
chunk = os.read(serial_fd, READ_SIZE)
@@ -302,7 +460,7 @@ if __name__ == '__main__':
if not line or not line.startswith('$'):
continue
if parse_sentence(line, state):
publish_fix(fix_publisher, overlay_publisher, state, frame_id)
publish_fix(fix_publisher, overlay_publisher, diagnostics_publisher, state, frame_id)
except OSError as exc:
if exc.errno not in (errno.EAGAIN, errno.EWOULDBLOCK):