SGP4 Propagator Prep #227

Merged
muellerr merged 9 commits from spg4-propagator into main 2023-08-15 10:46:28 +02:00

View File

@ -66,6 +66,7 @@ class ActionId(enum.IntEnum):
SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0 SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0
RESET_MEKF = 1 RESET_MEKF = 1
RESTORE_MEKF_NONFINITE_RECOVERY = 2 RESTORE_MEKF_NONFINITE_RECOVERY = 2
UPDATE_TLE = 3
CTRL_STRAT_DICT = { CTRL_STRAT_DICT = {
@ -90,6 +91,13 @@ CTRL_STRAT_DICT = {
31: "PTG_RAW", 31: "PTG_RAW",
} }
GPS_COURCE_DICT = {
0: "NONE",
1: "GPS",
2: "GPS_EXTRAPOLATED",
3: "SGP4",
}
class OpCodes: class OpCodes:
OFF = ["off"] OFF = ["off"]
@ -103,6 +111,7 @@ class OpCodes:
SAFE_PTG = ["confirm_deployment"] SAFE_PTG = ["confirm_deployment"]
RESET_MEKF = ["reset_mekf"] RESET_MEKF = ["reset_mekf"]
RESTORE_MEKF_NONFINITE_RECOVERY = ["restore_mekf_nonfinite_recovery"] RESTORE_MEKF_NONFINITE_RECOVERY = ["restore_mekf_nonfinite_recovery"]
UPDATE_TLE = ["update_tle"]
SET_PARAMETER_SCALAR = ["set_scalar_param"] SET_PARAMETER_SCALAR = ["set_scalar_param"]
SET_PARAMETER_VECTOR = ["set_vector_param"] SET_PARAMETER_VECTOR = ["set_vector_param"]
SET_PARAMETER_MATRIX = ["set_matrix_param"] SET_PARAMETER_MATRIX = ["set_matrix_param"]
@ -153,6 +162,7 @@ class Info:
SAFE_PTG = "Confirm deployment of both solar arrays" SAFE_PTG = "Confirm deployment of both solar arrays"
RESET_MEKF = "Reset the MEKF" RESET_MEKF = "Reset the MEKF"
RESTORE_MEKF_NONFINITE_RECOVERY = "Restore MEKF non-finite recovery" RESTORE_MEKF_NONFINITE_RECOVERY = "Restore MEKF non-finite recovery"
UPDATE_TLE = "Update TLE"
SET_PARAMETER_SCALAR = "Set Scalar Parameter" SET_PARAMETER_SCALAR = "Set Scalar Parameter"
SET_PARAMETER_VECTOR = "Set Vector Parameter" SET_PARAMETER_VECTOR = "Set Vector Parameter"
SET_PARAMETER_MATRIX = "Set Matrix Parameter" SET_PARAMETER_MATRIX = "Set Matrix Parameter"
@ -220,6 +230,7 @@ def acs_cmd_defs(defs: TmtcDefinitionWrapper):
keys=OpCodes.RESTORE_MEKF_NONFINITE_RECOVERY, keys=OpCodes.RESTORE_MEKF_NONFINITE_RECOVERY,
info=Info.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_SCALAR, info=Info.SET_PARAMETER_SCALAR)
oce.add(keys=OpCodes.SET_PARAMETER_VECTOR, info=Info.SET_PARAMETER_VECTOR) oce.add(keys=OpCodes.SET_PARAMETER_VECTOR, info=Info.SET_PARAMETER_VECTOR)
oce.add(keys=OpCodes.SET_PARAMETER_MATRIX, info=Info.SET_PARAMETER_MATRIX) oce.add(keys=OpCodes.SET_PARAMETER_MATRIX, info=Info.SET_PARAMETER_MATRIX)
@ -308,6 +319,24 @@ def pack_acs_ctrl_command(p: ServiceProviderParams): # noqa C901
q.add_pus_tc( q.add_pus_tc(
create_action_cmd(ACS_CONTROLLER, ActionId.RESTORE_MEKF_NONFINITE_RECOVERY) 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: elif op_code in OpCodes.SET_PARAMETER_SCALAR:
q.add_log_cmd(f"{Info.SET_PARAMETER_SCALAR}") q.add_log_cmd(f"{Info.SET_PARAMETER_SCALAR}")
set_acs_ctrl_param_scalar(q) set_acs_ctrl_param_scalar(q)
@ -985,11 +1014,13 @@ def handle_gyr_data_processed(pw: PrintWrapper, hk_data: bytes):
def handle_gps_data_processed(pw: PrintWrapper, hk_data: bytes): def handle_gps_data_processed(pw: PrintWrapper, hk_data: bytes):
pw.dlog("Received GPS Processed Set") pw.dlog("Received GPS Processed Set")
fmt_source = "!B"
fmt_scalar = "!d" fmt_scalar = "!d"
fmt_vec = "!ddd" fmt_vec = "!ddd"
inc_len_source = struct.calcsize(fmt_source)
inc_len_scalar = struct.calcsize(fmt_scalar) inc_len_scalar = struct.calcsize(fmt_scalar)
inc_len_vec = struct.calcsize(fmt_vec) 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") pw.dlog("Received HK set too small")
return return
current_idx = 0 current_idx = 0
@ -1028,12 +1059,18 @@ def handle_gps_data_processed(pw: PrintWrapper, hk_data: bytes):
) )
] ]
current_idx += inc_len_vec current_idx += inc_len_vec
source = struct.unpack(fmt_source, hk_data[current_idx: current_idx + inc_len_source])[0]
current_idx += inc_len_source
if GPS_COURCE_DICT.get(source) is not None:
pw.dlog(f"GPS Source: {GPS_COURCE_DICT[source]}")
else:
pw.dlog(f"'GPS Source (key unknown)': {source}")
pw.dlog(f"GPS Latitude: {lat} [deg]") pw.dlog(f"GPS Latitude: {lat} [deg]")
pw.dlog(f"GPS Longitude: {long} [deg]") pw.dlog(f"GPS Longitude: {long} [deg]")
pw.dlog(f"GPS Altitude: {alt} [m]") pw.dlog(f"GPS Altitude: {alt} [m]")
pw.dlog(f"GPS Position: {pos} [m]") pw.dlog(f"GPS Position: {pos} [m]")
pw.dlog(f"GPS Velocity: {velo} [m/s]") 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): def handle_mekf_data(pw: PrintWrapper, hk_data: bytes):