Merge pull request #1150 from lk-iqt/dev-mavlink

Dev mavlink

utils/mavlink-api/README.md

@@ -26,4 +26,13 @@ sudo udevadm control --reload-rules && sudo udevadm trigger
 Test again if it is working correctly:
 ```bash
 python3 utils/mavlink_serial_test.py /dev/pixhawk_serial
+```
+
+## Running with GamutRF
+
+To run with gamutRF add the `-f utils/mavlink-api/docker-compose.yml` and include mavlink-api for the gamutRF interface, and mavlink-api-controller, mavlink-api-drone, and mqtt-publisher for the controller API, drone API, and MQTT publisher respectively.
+
+Ex:
+```bash
+docker compose -f orchestrator.yml -f torchserve-cuda.yml -f utils/mavlink-api/mavlink-api.yaml -f geolocate.yml down mqtt mavlink-api-controller mavlink-api-drone geolocate mqtt-publisher
 ```

utils/mavlink-api/mavlink-api.py

@@ -160,8 +160,12 @@ def get_latest_gps_fix_status():
 
 @app.route("/gps-data", methods=["GET"])
 def get_latest_gps_data():
-    if mavlink_gps_handler.latest_GLOBAL_POSITION_INT_msg:
+    if (
+        mavlink_gps_handler.latest_GLOBAL_POSITION_INT_msg
+        and mavlink_gps_handler.latest_GPS_RAW_INT_msg
+    ):
         mavlink_gps_handler.GLOBAL_POSITION_INT_parser()
+        mavlink_gps_handler.GPS_RAW_INT_parser()
         msg = mavlink_gps_handler.create_gps_json_payload()
         return jsonify(msg), 200
     return jsonify({"error": "No GPS data available"}), 404

utils/mavlink-api/mavlink-api.yaml

@@ -0,0 +1,54 @@
+version: '3'
+networks:
+  gamutrf:
+services:
+  mavlink-api:
+    image: iqtlabs/gamutrf-mavlink-api:latest
+    build:
+      context: .
+      dockerfile: Dockerfile.mavlink-api
+    ports:
+      - "8888:8888"
+    devices:
+      - "/dev/pixhawk_serial:/dev/tty.serial1"
+    networks:
+      - gamutrf
+    restart: unless-stopped
+  #mavlink-api-drone:
+  #  image: iqtlabs/gamutrf-mavlink-api:latest
+  #  build:
+  #    context: .
+  #    dockerfile: Dockerfile.mavlink-api
+  #  ports:
+  #    - "8889:8888"
+  #  devices:
+  #    - "/dev/ttyUSB0:/dev/tty.serial1"
+  #  networks:
+  #    - gamutrf
+  #  restart: unless-stopped
+  #mavlink-api-controller:
+  #  image: iqtlabs/gamutrf-mavlink-api:latest
+  #  build:
+  #    context: .
+  #    dockerfile: Dockerfile.mavlink-api
+  #  ports:
+  #    - "8890:8888"
+  #  devices:
+  #    - "/dev/ttyUSB1:/dev/tty.serial1"
+  #  networks:
+  #    - gamutrf
+  #  restart: unless-stopped
+  #mqtt-publisher:
+  #  image: iqtlabs/gamutrf-mavlink-mqtt-publisher:latest
+  #  build:
+  #    context: .
+  #    dockerfile: Dockerfile.mqtt-publisher
+  #  networks:
+  #    - gamutrf
+  #  environment:
+  #    - MQTT_IP=mqtt
+  #    - MQTT_PORT=1883
+  #    - MQTT_TOPIC=gamutrf/targets
+  #    - QUERY_INTERVAL=1
+  #    - URL_LIST=[["controller", "http://mavlink-api-controller:8888/gps-data"], ["drone", "http://mavlink-api-drone:8888/gps-data"]] # Example: [["target1", "https://example1.com"], ["target2", "https://example2.com"]]
+  #  restart: unless-stopped

utils/mavlink-api/mqtt-publisher.py

@@ -13,46 +13,61 @@
     os.environ.get("URL_LIST", '[["default_target", "http://127.0.0.1:8888/gps-data"]]')
 )
 
+mqtt_connect_flag = False
+
 
 def on_connect(client, userdata, flags, rc):
+    global mqtt_connect_flag
     if rc == 0:
         print("Connected to MQTT Broker")
+        mqtt_connect_flag = True
     else:
         print(f"Failed to connect, return code {rc}")
 
 
+def on_disconnect(client, userdata, flags, rc):
+    global mqtt_connect_flag
+    mqtt_connect_flag = False
+
+
 def publish_data(client, target_name, data):
     topic = f"{MQTT_TOPIC}"
     try:
-        client.publish(topic, data)
+        rc = client.publish(topic, data)
         print(f"Published data to {topic}: {data}")
 
-    except Exception as err:
+    except ConnectionError as err:
         print("could not publish data: %s", err)
 
+    else:
+        if rc != 0:
+            print(f"could not publish data RC={rc}")
+
 
 def fetch_and_publish(client, target_name, target_url):
     try:
         response = json.loads(httpx.get(f"{target_url}").text)
 
-        data = {
-            "target_name": target_name,
-            "gps_stale": response["gps_stale"],
-            "gps_fix_type": response["gps_fix_type"],
-            "time_boot_ms": response["time_boot_ms"],  # mm
-            "time_usec": response["time_usec"],
-            "latitude": response["latitude"],  # decimal degrees
-            "longitude": response["longitude"],  # decimal degrees
-            "altitude": response["altitude"],  # mm
-            "relative_alt": response["relative_alt"],  # mm
-            "heading": response["heading"],  # decimal degrees
-            "vx": response["vx"],  # meters/second
-            "vy": response["vy"],  # meters/second
-            "vz": response["vz"],  # meters/second
-        }
-
-    except Exception as err:
-        print("could not update with external GPS: %s", err)
+        if int(response["gps_fix_type"]) > 0:
+            data = {
+                "target_name": target_name,
+                "gps_stale": response["gps_stale"],
+                "gps_fix_type": response["gps_fix_type"],
+                "time_boot_ms": response["time_boot_ms"],  # mm
+                "time_usec": response["time_usec"],
+                "latitude": response["latitude"],  # decimal degrees
+                "longitude": response["longitude"],  # decimal degrees
+                "altitude": response["altitude"],  # mm
+                "relative_alt": response["relative_alt"],  # mm
+                "heading": response["heading"],  # decimal degrees
+                "vx": response["vx"],  # meters/second
+                "vy": response["vy"],  # meters/second
+                "vz": response["vz"],  # meters/second
+            }
+            publish_data(client, target_name, json.dumps(data))
+
+    except (httpx.HTTPError, json.JSONDecodeError, KeyError) as err:
+        print(f"Could not update {target_name} with error:{err}")
         data = {
             "target_name": target_name,
             "gps_stale": None,
@@ -68,30 +83,37 @@ def fetch_and_publish(client, target_name, target_url):
             "vy": None,  # meters/second
             "vz": None,  # meters/second
         }
-    publish_data(client, target_name, json.dumps(data))
 
 
 def main():
-    try:
-        client = mqtt_client.Client()
-        client.on_connect = on_connect
-        client.connect(MQTT_BROKER, int(MQTT_PORT))
-        print("Connected to MQTT Broker")
-    except Exception as err:
-        logging.error(
-            f"Could not connect to MQTT broker ({MQTT_BROKER}:{MQTT_PORT}): {err}"
-        )
-
+    global mqtt_connect_flag
     while True:
-        for target in URL_LIST:
-            if len(target) == 2:
-                print(f"Attepting to retrieve data from {target}")
-                target_name, target_url = target
-                fetch_and_publish(client, target_name, target_url)
-            else:
-                print("Invalid entry in URL_LIST. Each entry should be a 2-entry list.")
-
-        time.sleep(QUERY_INTERVAL)
+        try:
+            print(f"Attempting to connect to MQTT {MQTT_BROKER}:{MQTT_PORT}")
+            client = mqtt_client.Client()
+            client.on_connect = on_connect
+            client.on_disconnect = on_disconnect
+            client.connect(MQTT_BROKER, int(MQTT_PORT))
+        # except (ConnectionRefusedError, ConnectionError) as err:
+        except Exception as err:
+            print(
+                f"Could not connect to MQTT broker ({MQTT_BROKER}:{MQTT_PORT}): {err}"
+            )
+        time.sleep(5)
+
+        while True:  # Fix this to mqtt_connect_flag or client.is_connected()
+            print(f"Initializing with {URL_LIST}")
+            for target in URL_LIST:
+                if len(target) == 2:
+                    print(f"Attempting to retrieve data from {target}")
+                    target_name, target_url = target
+                    fetch_and_publish(client, target_name, target_url)
+                else:
+                    print(
+                        "Invalid entry in URL_LIST. Each entry should be a 2-entry list."
+                    )
+
+            time.sleep(QUERY_INTERVAL)
 
 
 if __name__ == "__main__":