From 8ad5e6457006e9c54b1e8e960c24d168df7fa697 Mon Sep 17 00:00:00 2001
From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
Date: Thu, 9 Jan 2025 02:07:13 -0700
Subject: [PATCH] mavproxy_soar: Add initial soaring display

Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
---
 MAVProxy/modules/mavproxy_soar.py | 96 +++++++++++++++++++++++++++++++
 1 file changed, 96 insertions(+)
 create mode 100644 MAVProxy/modules/mavproxy_soar.py

diff --git a/MAVProxy/modules/mavproxy_soar.py b/MAVProxy/modules/mavproxy_soar.py
new file mode 100644
index 0000000000..91257264d3
--- /dev/null
+++ b/MAVProxy/modules/mavproxy_soar.py
@@ -0,0 +1,96 @@
+#!/usr/bin/env python3
+'''
+Soaring Module
+Ryan Friedman
+
+This module displays the estimated soaring thermals on the map.
+A circle is drawn with the estimated radius from ArduSoar's estimated thermal location.
+
+Note that this module uses NAMED_VALUE_FLOAT messages from the autopilot,
+a temporary measure until a proper mavlink message is decided upon.
+
+AP_FLAKE8_CLEAN
+'''
+
+from MAVProxy.modules.lib import mp_module, mp_util
+from MAVProxy.modules.mavproxy_map import mp_slipmap
+
+import time
+
+
+class soar(mp_module.MPModule):
+    _SOAR_THERMAL_MAP_OBJ_ID = "soar-thermal"
+    _CLEAR_STALE_THERMAL_TIME = 5.0
+
+    def __init__(self, mpstate):
+        """Initialise module"""
+        super(soar, self).__init__(mpstate, "soar", "")
+        self._strength = None
+        self._radius = None
+        self._x = None
+        self._y = None
+        self._last_draw_time = time.time()
+
+    def mavlink_packet(self, m):
+        '''handle mavlink packets'''
+
+        if m.get_type() == 'NAMED_VALUE_FLOAT' and m.name.startswith("SOAR"):
+            if m.name == "SOAREKFX0":
+                self._strength = m.value
+            elif m.name == "SOAREKFX1":
+                self._radius = m.value
+            elif m.name == "SOAREKFX2":
+                self._x = m.value
+            elif m.name == "SOAREKFX3":
+                self._y = m.value
+            else:
+                raise NotImplementedError(m.name)
+
+            self.draw_thermal_estimate()
+
+    def idle_task(self):
+        '''called rapidly by mavproxy'''
+
+        if time.time() - self._last_draw_time > self._CLEAR_STALE_THERMAL_TIME:
+            self.clear_thermal_estimate()
+
+    def draw_thermal_estimate(self):
+
+        if self._radius is None:
+            return
+        if self._x is None:
+            return
+        if self._y is None:
+            return
+
+        wp_module = self.module('wp')
+        if wp_module is None:
+            return
+        home = wp_module.get_home()
+        if home is None:
+            return
+
+        home_lat = home.x
+        home_lng = home.y
+
+        (thermal_lat, thermal_lon) = mp_util.gps_offset(home_lat, home_lng, self._y, self._x)
+
+        slipcircle = mp_slipmap.SlipCircle(
+            self._SOAR_THERMAL_MAP_OBJ_ID, # key
+            "thermals", # layer
+            (thermal_lat, thermal_lon), # latlon
+            self._radius, # radius
+            (0, 255, 255),
+            linewidth=2)
+        for mp in self.module_matching('map*'):
+            self._last_draw_time = time.time()
+            mp.map.add_object(slipcircle)
+
+    def clear_thermal_estimate(self):
+        for mp in self.module_matching('map*'):
+            mp.map.remove_object(self._SOAR_THERMAL_MAP_OBJ_ID)
+
+
+def init(mpstate):
+    '''initialise module'''
+    return soar(mpstate)