From 17486fdca7475d16c19cd2f88ee569b544208f2c Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Thu, 2 Feb 2023 11:11:22 +0100
Subject: [PATCH] updated gps data with new value. fixed validity flag output
 because someone (me) was to stupid to do it right in the first place

---
 eive_tmtc/tmtc/acs/acs_ctrl.py | 15 +++++++++++++--
 1 file changed, 13 insertions(+), 2 deletions(-)

diff --git a/eive_tmtc/tmtc/acs/acs_ctrl.py b/eive_tmtc/tmtc/acs/acs_ctrl.py
index db7edc6..8c1def8 100644
--- a/eive_tmtc/tmtc/acs/acs_ctrl.py
+++ b/eive_tmtc/tmtc/acs/acs_ctrl.py
@@ -619,7 +619,7 @@ def handle_gps_data_processed(pw: PrintWrapper, hk_data: bytes):
     fmt_vec = "!ddd"
     inc_len_scalar = struct.calcsize(fmt_scalar)
     inc_len_vec = struct.calcsize(fmt_vec)
-    if len(hk_data) < 2 * inc_len_scalar + inc_len_vec:
+    if len(hk_data) < 2 * inc_len_scalar + 2 * inc_len_vec:
         pw.dlog("Received HK set too small")
         return
     current_idx = 0
@@ -637,14 +637,22 @@ def handle_gps_data_processed(pw: PrintWrapper, hk_data: bytes):
         )
     ]
     current_idx += inc_len_scalar
-    velo = [
+    pos = [
         f"{val:8.3f}"
         for val in struct.unpack(
             fmt_vec, hk_data[current_idx : current_idx + inc_len_vec]
         )
     ]
+    velo = [
+        f"{val:8.3f}"
+        for val in struct.unpack(
+            fmt_vec, hk_data[current_idx: current_idx + inc_len_vec]
+        )
+    ]
+    current_idx += inc_len_vec
     pw.dlog(f"GPS Latitude: {lat} [rad]")
     pw.dlog(f"GPS Longitude: {long} [rad]")
+    pw.dlog(f"GPS Position: {pos} [m]")
     pw.dlog(f"GPS Velocity: {velo} [m/s]")
     pw.printer.print_validity_buffer(hk_data[current_idx:], num_vars=3)
 
@@ -672,6 +680,7 @@ def handle_mekf_data(pw: PrintWrapper, hk_data: bytes):
             fmt_vec, hk_data[current_idx : current_idx + inc_len_vec]
         )
     ]
+    current_idx += inc_len_vec
     pw.dlog(f"MEKF Quaternion: {quat}")
     pw.dlog(f"MEKF Rotational Rate: {rate}")
     pw.printer.print_validity_buffer(hk_data[current_idx:], num_vars=2)
@@ -707,6 +716,7 @@ def handle_ctrl_val_data(pw: PrintWrapper, hk_data: bytes):
             fmt_scalar, hk_data[current_idx : current_idx + inc_len_scalar]
         )
     ]
+    current_idx += inc_len_scalar
     pw.dlog(f"Control Values Target Quaternion: {tgt_quat}")
     pw.dlog(f"Control Values Error Quaternion: {err_quat}")
     pw.dlog(f"Control Values Error Angle: {err_ang} [rad]")
@@ -745,6 +755,7 @@ def handle_act_cmd_data(pw: PrintWrapper, hk_data: bytes):
             fmt_vec3_int16, hk_data[current_idx : current_idx + inc_len_vec3_int16]
         )
     ]
+    current_idx += inc_len_vec3_int16
     pw.dlog(f"Actuator Commands RW Target Torque: {rw_tgt_torque}")
     pw.dlog(f"Actuator Commands RW Target Speed: {rw_tgt_speed}")
     pw.dlog(f"Actuator Commands MTQ Target Dipole: {mtq_tgt_dipole}")