APID108.py

Go to the documentation of this file.

import numpy as np
from .. PacketUtils import *
from .. timestamp import *
 
'''
Attitude:  APID 108
AC_AttFilterOut_tlm_msg packet (section 7.2)
'''
 
Fields = np.dtype([
  ('timestamp', CCSDS_timestamp),
  ('Padding',   '>u4'),   # Padding to maintain packet alignment
 
  ('Est_Time',          '>f8'),         # estimate TAI timestamp
  ('q_EciToBrf_Est',    '>f8', (4,)),   # attitude estimate
  ('b_gyro_Brf_Est',    '>f8', (3,)),   # gyro bias estimate
  ('w_EciToBrf_Brf_Est','>f8', (3,)),   # angular rate estimate
  ('Covar',             '>f8', (6,6)),
  ('Covar_eigs',        '>f8', (6,)),   # covariance matrix eigenvalues
  ('Covar_cond',        '>f8'),         # covariance matrix condition number
  ('underweight',       '>f8'),         # meas underweight param
  ('gyro_sig_w',        '>f8'),         # ARW related noise param (sig_v)
  ('gyro_sig_b',        '>f8'),         # Bias drift noise param (sig_u)
 
  ('Covar_St1',         '>f8', (3,3)),
  ('Covar_St2',         '>f8', (3,3)),
  ('Covar_St3',         '>f8', (3,3)),
 
  ('att_uncert_thresh',         '>f8', (3,)), # attitude estimate uncertainty max threshold
  ('gyro_bias_uncert_thresh',   '>f8', (3,)), # gyro bias estimate uncertainty max threshold
  ('gyro_bias_est_mag_thresh',  '>f8'), # gyro bias estimate magnitude max threshold
  ('st1_innov_thresh',          '>f8'), # ST1 innov threshold (Mahalanobis^2)
  ('st2_innov_thresh',          '>f8'), # ST2 innov threshold (Mahalanobis^2)
  ('st3_innov_thresh',          '>f8'), # ST3 innov threshold (Mahalanobis^2)
  ('att_corr_thresh',           '>f8'), # attitude correction threshold
  ('gyro_bias_corr_thresh',     '>f8'), # gyro bias correction threshold
 
  ('st1_latest_accept_time',    '>f8'), # timestamp of most recent ST1 accepted
  ('st1_latest_innov',    '>f8', (6,)), # innov of most recent ST1 accepted
  ('st1_latest_mahalanobis',    '>f8'), # (Mahalanobis)^2 of most recent ST1 accepted
  ('st1_latest_reject_time',    '>f8'), # timestamp of most recent ST1 rejected
 
  ('st2_latest_accept_time',    '>f8'), # timestamp of most recent ST2 accepted
  ('st2_latest_innov',    '>f8', (6,)), # innov of most recent ST2 accepted
  ('st2_latest_mahalanobis',    '>f8'), # (Mahalanobis)^2 of most recent ST2 accepted
  ('st2_latest_reject_time',    '>f8'), # timestamp of most recent ST2 rejected
 
  ('st3_latest_accept_time',    '>f8'), # timestamp of most recent ST3 accepted
  ('st3_latest_innov',    '>f8', (6,)), # innov of most recent ST3 accepted
  ('st3_latest_mahalanobis',    '>f8'), # (Mahalanobis)^2 of most recent ST3 accepted
  ('st3_latest_reject_time',    '>f8'), # timestamp of most recent ST3 rejected
 
  ('enable_ST3',                '|u1'), # ST3 processing enabled flag
  ('gyro_bias_est_enable',      '|u1'), # gyro bias estimation enabled flag
  ('aif_st1',                   '|u1'), # ST1 Auto/Inhibit/Force flag
  ('aif_st2',                   '|u1'), # ST2 Auto/Inhibit/Force flag
  ('aif_st3',                   '|u1'), # ST3 Auto/Inhibit/Force flag
 
  ('packed_bits',               '|u1', (3,)),  # to unpack later
 
  ('st1_latest_reject_code',    '|u1'), # most recent ST1 reject code
  ('st2_latest_reject_code',    '|u1'), # most recent ST2 reject code
  ('st3_latest_reject_code',    '|u1'), # most recent ST3 reject code
  ('st1_mekf_disagree',         '|u1'), # ST1 meas disagree with MEKF est
  ('st2_mekf_disagree',         '|u1'), # ST2 meas disagree with MEKF est
  ('att_corr_excessive',        '|u1'), # 1 if attitude correction excessive
  ('gyro_bias_corr_excessive',  '|u1'), # 1 if gyro bias correction excessive
  ('gyro_bias_mag_excessive',   '|u1'), # 1 if gyro bias magnitude excessive
  ('gyro_meas_missing',         '|u1'), # number of gyro meas missing from GyroBuffer
  ('filter_is_init',            '|u1'), # filter is initialized flag
 
  ('gyro_buff_is_init',         '|u1'), # GyroBuffer is initialized flag
  ('gyro_buff_idx_new',         '|u1'), # GyroBuffer most recent entry index
  ('gyro_buff_idx_old',         '|u1'), # GyroBuffer oldest entry index
  ('Padding2',                  '|u1', (3,)),
 
]) # total length = 1018 bytes
 
 
def APID108(data):
  apid = 108
  expected = Fields.itemsize
  if len(data) != expected:
    print('APID {}: expected={}, actual={}'.format(apid,expected,len(data)))
 
  tmp = np.frombuffer(data, dtype=Fields, count=1)
  myDict = getDict(tmp)
  myDict['timestamp'] = parse_CCSDS_timestamp(myDict['timestamp'])
 
  # unpack bit fields
  (
    myDict['excessive_gyro_meas_missing'], # 1 if excessive gyro missing
    myDict['invalid_covar_eigenvalues'],   # 1 if covariance eigenvalues too small
    myDict['excessive_covar_condition'],   # 1 if covariance condition number too big
    spare,                                 # ignore
    myDict['filter_initialized'],          # 1 if filter not init
    myDict['gyro_buffer_initialized'],     # 1 if gyro buffer not init
    myDict['excessive_att_uncert'],        # 1 if attitude estimate uncertainty excessive
    myDict['excessive_bias_uncert'],       # 1 if gyro bias estimate uncertainty excessive
  ) = np.unpackbits(myDict['packed_bits'][0])
 
  (
    myDict['att_X_uncert'],      # 1 if att X estimate uncertainty excessive
    myDict['att_Y_uncert'],      # 1 if att Y estimate uncertainty excessive
    myDict['att_Z_uncert'],      # 1 if att Z estimate uncertainty excessive
  ) = np.unpackbits(myDict['packed_bits'][1])[:3] # ignore last 5 bits
 
  (
    myDict['bias_X_uncert'],     # 1 if gyro X bias estimate uncertainty excessive
    myDict['bias_Y_uncert'],     # 1 if gyro Y bias estimate uncertainty excessive
    myDict['bias_Z_uncert'],     # 1 if gyro Z bias estimate uncertainty excessive
  ) = np.unpackbits(myDict['packed_bits'][2])[:3] # ignore last 5 bits
 
  del myDict['packed_bits']
 
  return myDict