Merge pull request #811 from lk-iqt/feature/mavlink-gps

Feature/mavlink gps
IQTLabs · Jul 27, 2023 · 5e08963 · 5e08963
2 parents c1437dc + aabd12f
commit 5e08963
Show file tree

Hide file tree

Showing 2 changed files with 63 additions and 21 deletions.
diff --git a/gamutrf/api.py b/gamutrf/api.py
@@ -145,6 +145,11 @@ def argument_parser():
         action=argparse.BooleanOptionalAction,
         help="get RSSI values",
     )
+
+    parser.add_argument(
+        "--use_mavlink_gps", help="Use external Pixhawk/MAVLINK GPS", default=False, type=bool
+    )
+
     return parser
 
 
@@ -214,10 +219,11 @@ class API:
     def __init__(self, arguments):
         self.arguments = arguments
         self.mqtt_reporter = MQTTReporter(
-            self.arguments.name,
-            self.arguments.mqtt_server,
-            ORCHESTRATOR,
-            True,
+            name = self.arguments.name,
+            mqtt_server = self.arguments.mqtt_server,
+            gps_server = ORCHESTRATOR,
+            compass = True,
+            use_mavlink_gps = self.arguments.use_mavlink_gps
         )
         self.q = queue.Queue(self.arguments.qsize)
         self.sdr_recorder = get_recorder(

diff --git a/gamutrf/mqtt_reporter.py b/gamutrf/mqtt_reporter.py
@@ -2,20 +2,25 @@
 import logging
 import os
 import socket
+import time
 
 import gpsd
 import httpx
 import paho.mqtt.client as mqtt
 
 
 class MQTTReporter:
-    def __init__(self, name, mqtt_server=None, gps_server=None, compass=False):
+    def __init__(self, name, mqtt_server=None, gps_server=None, compass=False, use_mavlink_gps=False):
         self.name = name
         self.mqtt_server = mqtt_server
         self.compass = compass
         self.gps_server = gps_server
         self.mqttc = None
         self.heading = "no heading"
+        self.use_mavlink_gps = use_mavlink_gps
+        self.mavlink_gps_topic = '/MAVLINK-GPS'
+        self.mavlink_gps_msg = None
+        self.mavlink_max_wait_time_s = 3
 
     @staticmethod
     def log(path, prefix, start_time, record_args):
@@ -33,7 +38,10 @@ def connect(self):
         logging.info(f"connecting to {self.mqtt_server}")
         self.mqttc = mqtt.Client()
         self.mqttc.connect(self.mqtt_server)
-        self.mqttc.loop_start()
+        if self.use_mavlink_gps:
+            self.mqttc.subscribe(self.mavlink_gps_topic)
+            self.mqttc.on_message = self.mavlink_gps_msg_callback
+        self.mqttc.loop_start()    
 
     def get_heading(self):
         try:
@@ -57,24 +65,52 @@ def add_gps(self, publish_args):
             }
         )
         try:
-            if self.compass:
-                self.get_heading()
-            if gpsd.gpsd_stream is None:
-                gpsd.connect(host=self.gps_server, port=2947)
-            packet = gpsd.get_current()
-            publish_args.update(
-                {
-                    "position": packet.position(),
-                    "altitude": packet.altitude(),
-                    "gps_time": packet.get_time().timestamp(),
-                    "map_url": packet.map_url(),
-                    "heading": self.heading,
-                    "gps": "fix",
-                }
-            )
+            #Use external MAVLINK GPS
+            if self.use_mavlink_gps:
+                self.mqttc.publish(self.mavlink_gps_topic, json.dumps({"msg":"GPS"}))
+                start_time=time.time()
+                while self.mavlink_gps_msg == None and time.time() - start_time < self.mavlink_max_wait_time_s:
+                    time.sleep(0.001)
+                if self.mavlink_gps_msg == None:
+                    return publish_args
+                else:
+                    publish_args.update(
+                        {
+                            "position": self.mavlink_gps_msg["lat"]+","+self.mavlink_gps_msg["lon"],
+                            "altitude": self.mavlink_gps_msg["alt"],
+                            "gps_time": self.mavlink_gps_msg["time_usec"],
+                            "map_url": None,
+                            "heading": self.mavlink_gps_msg["hdg"],
+                            "gps": "fix",
+                        }
+                    )
+                self.heading=self.mavlink_gps_msg["hdg"]
+                self.mavlink_gps_msg = None
+
+            #Use internal GPIO GPS
+            else:
+                if self.compass:
+                    self.get_heading()
+                if gpsd.gpsd_stream is None:
+                    gpsd.connect(host=self.gps_server, port=2947)
+                packet = gpsd.get_current()
+                publish_args.update(
+                    {
+                        "position": packet.position(),
+                        "altitude": packet.altitude(),
+                        "gps_time": packet.get_time().timestamp(),
+                        "map_url": packet.map_url(),
+                        "heading": self.heading,
+                        "gps": "fix",
+                    }
+                )
         except (BrokenPipeError, gpsd.NoFixError, AttributeError) as err:
             logging.error("could not update with GPS: %s", err)
         return publish_args
+
+    def mavlink_gps_msg_callback(self, client, userdata, msg):
+        gps_msg=json.loads(msg)
+        self.mavlink_gps_msg
 
     def publish(self, publish_path, publish_args):
         if not self.mqtt_server: