eive_tmtc/tmtc/acs/acs_ctrl.py

@@ -1,3 +1,4 @@
+import array
 import datetime
 import enum
 import logging
@@ -66,6 +67,7 @@ class ActionId(enum.IntEnum):
     SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0
     RESET_MEKF = 1
     RESTORE_MEKF_NONFINITE_RECOVERY = 2
+    UPDATE_TLE = 3
 
 
 CTRL_STRAT_DICT = {
@@ -90,6 +92,13 @@ CTRL_STRAT_DICT = {
     31: "PTG_RAW",
 }
 
+GPS_COURCE_DICT = {
+    0: "NONE",
+    1: "GPS",
+    2: "PROPAGATION",
+    3: "SPG4",
+}
+
 
 class OpCodes:
     OFF = ["off"]
@@ -103,6 +112,7 @@ class OpCodes:
     SAFE_PTG = ["confirm_deployment"]
     RESET_MEKF = ["reset_mekf"]
     RESTORE_MEKF_NONFINITE_RECOVERY = ["restore_mekf_nonfinite_recovery"]
+    UPDATE_TLE = ["update_tle"]
     SET_PARAMETER_SCALAR = ["set_scalar_param"]
     SET_PARAMETER_VECTOR = ["set_vector_param"]
     SET_PARAMETER_MATRIX = ["set_matrix_param"]
@@ -153,6 +163,7 @@ class Info:
     SAFE_PTG = "Confirm deployment of both solar arrays"
     RESET_MEKF = "Reset the MEKF"
     RESTORE_MEKF_NONFINITE_RECOVERY = "Restore MEKF non-finite recovery"
+    UPDATE_TLE = "Update TLE"
     SET_PARAMETER_SCALAR = "Set Scalar Parameter"
     SET_PARAMETER_VECTOR = "Set Vector Parameter"
     SET_PARAMETER_MATRIX = "Set Matrix Parameter"
@@ -220,6 +231,7 @@ def acs_cmd_defs(defs: TmtcDefinitionWrapper):
         keys=OpCodes.RESTORE_MEKF_NONFINITE_RECOVERY,
         info=Info.RESTORE_MEKF_NONFINITE_RECOVERY,
     )
+    oce.add(keys=OpCodes.UPDATE_TLE, info=Info.UPDATE_TLE)
     oce.add(keys=OpCodes.SET_PARAMETER_SCALAR, info=Info.SET_PARAMETER_SCALAR)
     oce.add(keys=OpCodes.SET_PARAMETER_VECTOR, info=Info.SET_PARAMETER_VECTOR)
     oce.add(keys=OpCodes.SET_PARAMETER_MATRIX, info=Info.SET_PARAMETER_MATRIX)
@@ -308,6 +320,24 @@ def pack_acs_ctrl_command(p: ServiceProviderParams):  # noqa C901
         q.add_pus_tc(
             create_action_cmd(ACS_CONTROLLER, ActionId.RESTORE_MEKF_NONFINITE_RECOVERY)
         )
+    elif op_code in OpCodes.UPDATE_TLE:
+        q.add_log_cmd(f"{Info.UPDATE_TLE}")
+        while True:
+            line1 = input("Please input the first line of the TLE: ")
+            if len(line1) == 69:
+                break
+            else:
+                print("The line does not have the required length of 69 characters")
+        while True:
+            line2 = input("Please input the second line of the TLE: ")
+            if len(line2) == 69:
+                break
+            else:
+                print("The line does not have the required length of 69 characters")
+        tle = line1.encode()+line2.encode()
+        q.add_pus_tc(
+            create_action_cmd(ACS_CONTROLLER, ActionId.UPDATE_TLE,tle)
+        )
     elif op_code in OpCodes.SET_PARAMETER_SCALAR:
         q.add_log_cmd(f"{Info.SET_PARAMETER_SCALAR}")
         set_acs_ctrl_param_scalar(q)
@@ -985,11 +1015,13 @@ def handle_gyr_data_processed(pw: PrintWrapper, hk_data: bytes):
 
 def handle_gps_data_processed(pw: PrintWrapper, hk_data: bytes):
     pw.dlog("Received GPS Processed Set")
+    fmt_source = "!B"
     fmt_scalar = "!d"
     fmt_vec = "!ddd"
+    inc_len_source = struct.calcsize(fmt_source)
     inc_len_scalar = struct.calcsize(fmt_scalar)
     inc_len_vec = struct.calcsize(fmt_vec)
-    if len(hk_data) < 2 * inc_len_scalar + 2 * inc_len_vec:
+    if len(hk_data) < 2 * inc_len_scalar + 2 * inc_len_vec + inc_len_source:
         pw.dlog("Received HK set too small")
         return
     current_idx = 0
@@ -1028,12 +1060,20 @@ def handle_gps_data_processed(pw: PrintWrapper, hk_data: bytes):
         )
     ]
     current_idx += inc_len_vec
+    source = [
+        f"{val:8.3f}"
+        for val in struct.unpack(
+            fmt_vec, hk_data[current_idx: current_idx + inc_len_source]
+        )
+    ]
+    current_idx += inc_len_source
+    pw.dlog(f"GPS Source: {GPS_COURCE_DICT[source]}")
     pw.dlog(f"GPS Latitude: {lat} [deg]")
     pw.dlog(f"GPS Longitude: {long} [deg]")
     pw.dlog(f"GPS Altitude: {alt} [m]")
     pw.dlog(f"GPS Position: {pos} [m]")
     pw.dlog(f"GPS Velocity: {velo} [m/s]")
-    FsfwTmTcPrinter.get_validity_buffer(hk_data[current_idx:], num_vars=5)
+    FsfwTmTcPrinter.get_validity_buffer(hk_data[current_idx:], num_vars=6)
 
 
 def handle_mekf_data(pw: PrintWrapper, hk_data: bytes):