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}")