ACS Ctrl fixes #129

Merged
muellerr merged 2 commits from acs-ctrl-validitiy-flag-fixes into main 2023-02-02 17:28:46 +01:00
Showing only changes of commit 17486fdca7 - Show all commits

View File

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