From 00970157fc86493589d889428b6a4f13eae208f5 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Wed, 27 Sep 2023 14:32:15 +0200
Subject: [PATCH] Features for the  ground segment (#3110)

* [server] expand FP includes to have a correct replay

* [log] tools to extract a basic CSV from log file

This is specific tools used for IMAV2023, but it is still a good example
how to make a custom log extractor

* [pprzcenter] sort the session in alphabetical order
---
 sw/ground_segment/tmtc/server.ml        | 10 +++-
 sw/logalizer/log_extract.py             | 77 +++++++++++++++++++++++++
 sw/supervision/python/session_widget.py |  2 +-
 3 files changed, 87 insertions(+), 2 deletions(-)
 create mode 100755 sw/logalizer/log_extract.py

diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
index fd49d135d2..9b1186aea1 100644
--- a/sw/ground_segment/tmtc/server.ml
+++ b/sw/ground_segment/tmtc/server.ml
@@ -90,7 +90,15 @@ let expand_aicraft x =
         [Xml.Element ("generated_settings", [], Xml.children xml)]
       with _ -> []
     in
-    if List.length ac.Aircraft.xml > 0 then Xml.Element (Xml.tag x, Xml.attribs x, ac.Aircraft.xml @ settings_xml)
+    
+    (* expand procedures in flight plan and replace in aircraft conf *)
+    let fp_file = Env.paparazzi_home // "conf" // ExtXml.attrib x "flight_plan" in
+    let fp_xml = ExtXml.parse_file fp_file in
+    let dir = Filename.dirname fp_file in
+    let fp_xml = Fp_proc.process_includes dir fp_xml in
+    let ac_xml = List.map (fun e -> match Xml.tag e with "flight_plan" -> fp_xml | _ -> e) ac.Aircraft.xml in
+
+    if List.length ac.Aircraft.xml > 0 then Xml.Element (Xml.tag x, Xml.attribs x, ac_xml @ settings_xml)
     else failwith "Nothing to parse"
   with
     | Failure msg -> handle_error_message "Fail with" msg
diff --git a/sw/logalizer/log_extract.py b/sw/logalizer/log_extract.py
new file mode 100755
index 0000000000..9675fb6dcb
--- /dev/null
+++ b/sw/logalizer/log_extract.py
@@ -0,0 +1,77 @@
+#!/usr/bin/python3
+import sys
+from collections import namedtuple
+from pyproj import CRS, Proj
+import re
+from os.path import basename
+
+LEAP_SECONDS = 18
+Point = namedtuple("Point", ["time", "lat", "lon", "alt"])
+
+
+def time_from_tow(tow):
+    utc_s = (tow/1000 - LEAP_SECONDS) % (3600*24)
+    h = int(utc_s // 3600)
+    m = int((utc_s//60) % 60)
+    s = round(utc_s % 60)
+    utc_time=f"{h:02d}:{m:02d}:{s:.3f}"
+    return utc_time
+
+
+def date_from_filename(logfile):
+    m = re.match("^(?:.*/)?(\d+)_(\d+)_(\d+)__(\d+)_(\d+)_(\d+)(?:_SD)?.data$", logfile)
+    if m is not None:
+        year, month ,day, h,m,s = m.groups()
+        date = f"20{year}-{month}-{day}"
+        #time = f"{h}:{m}:{s}"
+        return date
+
+
+def main(logfile):
+    """
+    extract position data from log. Use GPS time of week.
+    """
+    with open(logfile, 'r') as log:
+        data = {}
+        projs = {}
+        start_tow = None
+
+        for line in log.readlines():
+            args = line.strip().split()
+            ac_id = args[1]
+            if args[2] == "INS_REF":
+                lat = float(args[6]) / 1e7
+                lon = float(args[7]) / 1e7
+                crs = CRS(f"+proj=ortho +lat_0={lat} +lon_0={lon}")
+                projs[ac_id] = Proj.from_crs(crs, "EPSG:4326")
+
+            if args[2] == "GPS_INT":
+                tow = int(args[15])
+                t = float(args[0])
+                start_tow = tow - t
+
+            if args[2] == "ROTORCRAFT_FP" and ac_id in projs and start_tow is not None:
+                east = int(args[3])*0.0039063
+                north = int(args[4])*0.0039063
+                up = int(args[5])*0.0039063
+                lat, lon = projs[ac_id].transform(east, north)
+                tow = float(args[0]) + start_tow
+                utc_time = time_from_tow(tow)
+                p = Point(utc_time, lat, lon, up)
+                data.setdefault(ac_id, []).append(p)
+        for ac_id, points in data.items():
+            prefix = f"{sys.argv[2]}_" if len(sys.argv) > 2 else f"{basename(logfile).strip('.data')}__"
+            filename = f"{prefix}{ac_id}.csv"
+            with open(filename, 'w') as out:
+                print(f"created {filename}")
+                out.write("time,latitude,longitude,altitude\n")
+                date = date_from_filename(logfile)
+                for p in points:
+                    line = f"{date}T{p.time},{p.lat:.07f},{p.lon:.07f},{p.alt:.01f}\n"
+                    out.write(line)
+
+if __name__ == "__main__":
+    if len(sys.argv) < 2:
+        print("Usage: ./log_extract.py the_log.data [prefix out]")
+        exit(1)
+    main(sys.argv[1])
diff --git a/sw/supervision/python/session_widget.py b/sw/supervision/python/session_widget.py
index e3646de6dd..d944379e53 100644
--- a/sw/supervision/python/session_widget.py
+++ b/sw/supervision/python/session_widget.py
@@ -62,7 +62,7 @@ class SessionWidget(QWidget, Ui_Session):
 
     def on_control_panel_changed(self):
         current_cp = self.control_panel_combo.currentText()
-        self.sessions = parse_sessions(current_cp)
+        self.sessions = sorted(parse_sessions(current_cp), key=lambda session: session.name)
         self.tools = parse_tools(current_cp)
         self.tools_changed.emit(self.tools)
         self.init_tools_menu()