From bc85ccd8eff0eaf2a168eee907d3436433e9b219 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sat, 1 Apr 2023 11:48:22 +0200
Subject: [PATCH] ACS CTRL fix for MEKF HK handling

---
 CHANGELOG.md                   | 4 ++++
 eive_tmtc/tmtc/acs/acs_ctrl.py | 6 +++++-
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 0fba8a6..31336c6 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -10,6 +10,10 @@ list yields a list of all related PRs for each release.
 
 # [unreleased]
 
+## Fixed
+
+- MEKF: Handle NOT_FINITE status (key 5)
+
 # [v2.20.0] 2023-03-28
 
 ## Changed
diff --git a/eive_tmtc/tmtc/acs/acs_ctrl.py b/eive_tmtc/tmtc/acs/acs_ctrl.py
index edd95f3..0ff0f4c 100644
--- a/eive_tmtc/tmtc/acs/acs_ctrl.py
+++ b/eive_tmtc/tmtc/acs/acs_ctrl.py
@@ -989,6 +989,7 @@ def handle_mekf_data(pw: PrintWrapper, hk_data: bytes):
         2: "NO_MODEL_VECTORS",
         3: "NO_SUS_MGM_STR_DATA",
         4: "COVARIANCE_INVERSION_FAILED",
+        5: "NOT_FINITE",
         10: "INITIALIZED",
         11: "RUNNING",
     }
@@ -1016,7 +1017,10 @@ def handle_mekf_data(pw: PrintWrapper, hk_data: bytes):
     current_idx += inc_len_vec
     status = struct.unpack(fmt_sts, hk_data[current_idx : current_idx + inc_len_sts])[0]
     current_idx += inc_len_sts
-    pw.dlog(f"{'MEKF Status'.ljust(25)}: {mekf_status[status]}")
+    if mekf_status.get(status) is not None:
+        pw.dlog(f"{'MEKF Status'.ljust(25)}: {mekf_status[status]}")
+    else:
+        pw.dlog(f"{'MEKF Raw Status (key unknown)'.ljust(25)}: {status}")
     pw.dlog(f"{'MEKF Quaternion'.ljust(25)}: {fmt_str_4.format(*quat)}")
     pw.dlog(f"{'MEKF Rotational Rate'.ljust(25)}: {fmt_str_3.format(*rates)}")
     pw.printer.print_validity_buffer(hk_data[current_idx:], num_vars=3)