From f373ea57fed879c4551056c2e94b3cdaab1113de Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 12 Oct 2024 16:54:37 +1100
Subject: [PATCH] SIYI: added inverted and gamma

---
 MAVProxy/modules/lib/mp_image.py              |  3 +++
 MAVProxy/modules/mavproxy_SIYI/__init__.py    |  3 +++
 MAVProxy/modules/mavproxy_SIYI/raw_thermal.py | 15 +++++++++++++++
 3 files changed, 21 insertions(+)

diff --git a/MAVProxy/modules/lib/mp_image.py b/MAVProxy/modules/lib/mp_image.py
index 94b33ada3e..9ebcb7353f 100755
--- a/MAVProxy/modules/lib/mp_image.py
+++ b/MAVProxy/modules/lib/mp_image.py
@@ -221,6 +221,7 @@ def __init__(self,
         self.menu = None
         self.popup_menu = None
         self.fps = fps
+        self.inverted = False
 
         self.in_queue = multiproc.Queue()
         self.out_queue = multiproc.Queue()
@@ -427,6 +428,8 @@ def __init__(self, parent, state):
         self.seek_percentage = None
         self.seek_frame = None
         self.osd_elements = None
+        self.inverted = False
+
         state.brightness = 1.0
 
         # dragpos is the top left position in image coordinates
diff --git a/MAVProxy/modules/mavproxy_SIYI/__init__.py b/MAVProxy/modules/mavproxy_SIYI/__init__.py
index 01dd5f59de..bd170e616f 100644
--- a/MAVProxy/modules/mavproxy_SIYI/__init__.py
+++ b/MAVProxy/modules/mavproxy_SIYI/__init__.py
@@ -274,6 +274,9 @@ def __init__(self, mpstate):
                                                      ('fps_rgb', int, 20),
                                                      ('logfile', str, 'SIYI_log.bin'),
                                                      ('thermal_fov', float, 24.2),
+                                                     ('thermal_gamma', float, 0.0),
+                                                     ('thermal_square', int, 50),
+
                                                      ('zoom_fov', float, 62.0),
                                                      ('wide_fov', float, 88.0),
                                                      ('use_lidar', int, 0),
diff --git a/MAVProxy/modules/mavproxy_SIYI/raw_thermal.py b/MAVProxy/modules/mavproxy_SIYI/raw_thermal.py
index 58d5b6d086..d4bbb51494 100644
--- a/MAVProxy/modules/mavproxy_SIYI/raw_thermal.py
+++ b/MAVProxy/modules/mavproxy_SIYI/raw_thermal.py
@@ -169,6 +169,14 @@ def display_image(self, fname, data):
 
         # convert to 0 to 255
         a = (a - minv) * 255 / (maxv - minv)
+        a = a.reshape(512, 640)
+
+        maxpt = np.unravel_index(a.argmax(), a.shape)
+
+        if self.siyi.siyi_settings.thermal_gamma:
+            a = a / 255.0
+            a = np.power(a, 1.0 / self.siyi.siyi_settings.thermal_gamma)
+            a = np.uint8(a * 255)
 
         # convert to uint8 greyscale as 640x512 image
         a = a.astype(np.uint8)
@@ -177,6 +185,13 @@ def display_image(self, fname, data):
         a = cv2.cvtColor(a, cv2.COLOR_GRAY2RGB)
         if self.im is None:
             return
+
+        if self.siyi.siyi_settings.thermal_square > 0:
+            half_side = self.siyi.siyi_settings.thermal_square // 2
+            top_left = (maxpt[1] - half_side, maxpt[0] - half_side)
+            bottom_right = (maxpt[1] + half_side, maxpt[0] + half_side)
+            cv2.rectangle(a, top_left, bottom_right, (255,255,0), 2)
+
         self.im.set_image(a)
         self.image_count += 1
         self.update_title()