use byteorder::{LittleEndian, ReadBytesExt};
use nom::*;
use std::io::Cursor;
#[derive(Clone, Debug, Default, PartialEq)]
pub struct StandardTelemetry {
pub tlm_counter: u8,
pub gps_time: u32,
pub time_subsec: u8,
pub cmd_valid_cntr: u16,
pub cmd_invalid_cntr: u16,
pub cmd_invalid_chksum_cntr: u16,
pub last_command: u8,
pub acs_mode: u8,
pub css: [u16; 6],
pub eclipse_flag: u8,
pub sun_vec_b: [i16; 3],
pub i_b_field_meas: [i16; 3],
pub bd: [f32; 3],
pub rws_speed_cmd: [i16; 3],
pub rws_speed_tach: [i16; 3],
pub rwa_torque_cmd: [f32; 3],
pub gc_rwa_torque_cmd: [i8; 3],
pub torque_coil_cmd: [f32; 3],
pub gc_torque_coil_cmd: [i8; 3],
pub qbo_cmd: [i16; 4],
pub qbo_hat: [i16; 4],
pub angle_to_go: f32,
pub q_error: [i16; 4],
pub omega_b: [f32; 3],
pub rotating_variable_a: u32,
pub rotating_variable_b: u32,
pub rotating_variable_c: u32,
pub nb: [i16; 3],
pub neci: [i16; 3],
}
impl StandardTelemetry {
pub fn new(mut msg: Vec<u8>) -> Option<Self> {
let len = msg.len() - 2;
let mut crc = Cursor::new(msg.split_off(len));
let crc = crc.read_u16::<LittleEndian>().unwrap_or(0);
let mut calc: u16 = 0;
for byte in msg.iter() {
calc += u16::from(*byte);
}
if calc == crc {
match standardtelem(&msg) {
Ok(conv) => Some(conv.1),
_ => None,
}
} else {
None
}
}
}
named!(standardtelem(&[u8]) -> StandardTelemetry,
do_parse!(
le_u16 >>
tlm_counter: le_u8 >>
gps_time: le_u32 >>
time_subsec: le_u8 >>
cmd_valid_cntr: le_u16 >>
cmd_invalid_cntr: le_u16 >>
cmd_invalid_chksum_cntr: le_u16 >>
last_command: le_u8 >>
acs_mode: le_u8 >>
css_0: le_u16 >>
css_1: le_u16 >>
css_2: le_u16 >>
css_3: le_u16 >>
css_4: le_u16 >>
css_5: le_u16 >>
eclipse_flag: le_u8 >>
sun_vec_b_0: le_i16 >>
sun_vec_b_1: le_i16 >>
sun_vec_b_2: le_i16 >>
i_b_field_meas_0: le_i16 >>
i_b_field_meas_1: le_i16 >>
i_b_field_meas_2: le_i16 >>
bd_0: le_f32 >>
bd_1: le_f32 >>
bd_2: le_f32 >>
rws_speed_cmd_0: le_i16 >>
rws_speed_cmd_1: le_i16 >>
rws_speed_cmd_2: le_i16 >>
rws_speed_tach_0: le_i16 >>
rws_speed_tach_1: le_i16 >>
rws_speed_tach_2: le_i16 >>
rwa_torque_cmd_0: le_f32 >>
rwa_torque_cmd_1: le_f32 >>
rwa_torque_cmd_2: le_f32 >>
gc_rwa_torque_cmd_0: le_i8 >>
gc_rwa_torque_cmd_1: le_i8 >>
gc_rwa_torque_cmd_2: le_i8 >>
torque_coil_cmd_0: le_f32 >>
torque_coil_cmd_1: le_f32 >>
torque_coil_cmd_2: le_f32 >>
gc_torque_coil_cmd_0: le_i8 >>
gc_torque_coil_cmd_1: le_i8 >>
gc_torque_coil_cmd_2: le_i8 >>
qbo_cmd_0: le_i16 >>
qbo_cmd_1: le_i16 >>
qbo_cmd_2: le_i16 >>
qbo_cmd_3: le_i16 >>
qbo_hat_0: le_i16 >>
qbo_hat_1: le_i16 >>
qbo_hat_2: le_i16 >>
qbo_hat_3: le_i16 >>
angle_to_go: le_f32 >>
q_error_0: le_i16 >>
q_error_1: le_i16 >>
q_error_2: le_i16 >>
q_error_3: le_i16 >>
omega_b_0: le_f32 >>
omega_b_1: le_f32 >>
omega_b_2: le_f32 >>
rotating_variable_a: le_u32 >>
rotating_variable_b: le_u32 >>
rotating_variable_c: le_u32 >>
nb_0: le_i16 >>
nb_1: le_i16 >>
nb_2: le_i16 >>
neci_0: le_i16 >>
neci_1: le_i16 >>
neci_2: le_i16 >>
(StandardTelemetry{
tlm_counter,
gps_time,
time_subsec,
cmd_valid_cntr,
cmd_invalid_cntr,
cmd_invalid_chksum_cntr,
last_command,
acs_mode,
css: [
css_0,
css_1,
css_2,
css_3,
css_4,
css_5
],
eclipse_flag,
sun_vec_b: [
sun_vec_b_0,
sun_vec_b_1,
sun_vec_b_2
],
i_b_field_meas: [
i_b_field_meas_0,
i_b_field_meas_1,
i_b_field_meas_2
],
bd: [
bd_0,
bd_1,
bd_2
],
rws_speed_cmd: [
rws_speed_cmd_0,
rws_speed_cmd_1,
rws_speed_cmd_2
],
rws_speed_tach: [
rws_speed_tach_0,
rws_speed_tach_1,
rws_speed_tach_2
],
rwa_torque_cmd: [
rwa_torque_cmd_0,
rwa_torque_cmd_1,
rwa_torque_cmd_2
],
gc_rwa_torque_cmd: [
gc_rwa_torque_cmd_0,
gc_rwa_torque_cmd_1,
gc_rwa_torque_cmd_2
],
torque_coil_cmd: [
torque_coil_cmd_0,
torque_coil_cmd_1,
torque_coil_cmd_2
],
gc_torque_coil_cmd: [
gc_torque_coil_cmd_0,
gc_torque_coil_cmd_1,
gc_torque_coil_cmd_2
],
qbo_cmd: [
qbo_cmd_0,
qbo_cmd_1,
qbo_cmd_2,
qbo_cmd_3
],
qbo_hat: [
qbo_hat_0,
qbo_hat_1,
qbo_hat_2,
qbo_hat_3
],
angle_to_go,
q_error: [
q_error_0,
q_error_1,
q_error_2,
q_error_3
],
omega_b: [
omega_b_0,
omega_b_1,
omega_b_2
],
rotating_variable_a,
rotating_variable_b,
rotating_variable_c,
nb: [
nb_0,
nb_1,
nb_2
],
neci: [
neci_0,
neci_1,
neci_2
]
})
)
);