import struct
import scipy.io as spio
import math

line_buff = []


inv_dali_fix = [0.1835114759, -0.0139180047 ,0.0000104651 ,-0.0000001076]
inv_dalo_fix = [0.0605854762, -0.0134594170, 0.0000014590, -0.0000005228]
inv_dari_fix = [-0.0628936466, 0.0139820955, -0.0000013965, 0.0000000274]
inv_daro_fix = [0.0760199848, 0.0140838792 ,-0.0000000214, -0.0000002133]
inv_deli_fix = [0.0279715821 ,-0.0139882897 ,0.0000012014 ,0.0000000240]
inv_delo_fix = [-0.1674419142 ,-0.0141767262, -0.0000105237 ,-0.0000001360]
inv_deri_fix = [-0.0913878050 ,0.0140764858, -0.0000026448 ,0.0000000087]
inv_dero_fix = [-0.0028181221, 0.0140918517 ,-0.0000062197, 0.0000000745]
inv_dfli_fix = [0.1593846077 ,-0.0144023415, -0.0000012018 ,-0.0000006111]
inv_dflo_fix = [0.0533200329, 0.0139748253 ,0.0000733247, -0.0000011213]
inv_dfri_fix = [0.0165723758, 0.0139446481 ,0.0001093034, -0.0000022020]
inv_dfro_fix = [0.1713424769, -0.0142385147, -0.0000310481, -0.0000022970]
inv_dfali_fix = [0.1608325503, -0.0145932118 ,0.0000025537, -0.0000004630]
inv_dfalo_fix = [0.0116635936 ,0.0145875729 ,0.0000099810, -0.0000001501]
inv_dfari_fix = [0.3798064479, 0.0148503457 ,-0.0000142067, -0.0000004002]
inv_dfaro_fix = [0.0384463888 ,-0.0143005142, 0.0000221575, 0.0000001764]




def printline(txt):
    global line_buff
    line_buff.append(txt)
    if len(line_buff) >= 40:
        print(' '.join(line_buff))
        line_buff.clear()


def chksum(buff):
    s = 0
    for i in buff:
        s = s+i
    return s % 256

def pwm2pos(pwm):
    if pwm > 0x1000:
        return 0
    elif pwm > 0x7FF:
        return pwm - 4095
    else:
        return pwm 
def act2deg_new(para):
    def wrap(p):
        x = p
        if p > 32767:
         x = p - 65536
        x = x / 10
        act =(para[0] + para[1] * x + para[2] * x * x + para[3] * x * x * x )*57.3
        return act
    return wrap


def parseHJ(filename):

    hj = open(filename, 'rb')
    pkgs = \
        {   1: [struct.Struct('<2I2d24f3B'),
                [['boot_time',0.005,'时间戳',0],
                 ['tick',0.005,'时间戳',0],
                 ['lat_ins_inn',1,'内部惯导纬度(deg)',0],
                 ['lon_ins_inn',1,'内部惯导经度(deg)',0],
                 ['alt_ins_inn',1,'内部惯导高度(m)',0],
                 ['phi_ins_inn',57.29,'内部惯导滚转角(deg)',0],
                 ['tht_ins_inn',57.29,'内部惯导俯仰角(deg)',0],
                 ['psi_ins_inn',57.29,'内部惯导偏航角(deg)',0],
                 ['quad1',1,'四元数',0],
                 ['quad2',1,'四元数',0],
                 ['quad3',1,'四元数',0],
                 ['quad4',1,'四元数',0],
                 ['vn_ins_inn',1,'内部惯导北向速度(m/s)',0],
                 ['ve_ins_inn',1,'内部惯导东向速度(m/s)',0],
                 ['vu_ins_inn',-1,'内部惯导天向速度(m/s)',0],
                 ['ax_ins_1',1,'主IMU前向加速度(m/s2)',0],
                 ['ay_ins_1',1,'主IMU侧向加速度(m/s2)',0],
                 ['az_ins_1',-1,'主IMU上向加速度(m/s2)',0],
                 ['ax_ins_2',1,'副IMU前向加速度(m/s2)',0],
                 ['ay_ins_2',1,'副IMU侧向加速度(m/s2)',0],
                 ['az_ins_2',-1,'副IMU上向加速度(m/s2)',0],                
                 ['p_ins_1',57.29,'主IMU滚转角速度(deg/sec)',0],
                 ['q_ins_1',57.29,'主IMU俯仰角速度(deg/sec)',0],
                 ['r_ins_1',57.29,'主IMU偏航角速度(deg/sec)',0],
                 ['p_ins_2',57.29,'副IMU滚转角速度(deg/sec)',0],
                 ['q_ins_2',57.29,'副IMU俯仰角速度(deg/sec)',0],
                 ['r_ins_2',57.29,'副IMU偏航角速度(deg/sec)',0],                 
                 ['acc_g',1,'重力加速度(deg/sec)',0],
                 ['fuse_vel_cnt',1,'速度组合计数',0],
                 ['fuse_hdg_cnt',1,'航向组合计数',0],
                 ['integrated_flag',1,'惯导组合状态',0],
                ],
                [],
            ],
            2: [struct.Struct('<2I35f'),
                [['boot_time',0.005,'时间戳',0],
                 ['tick',0.005,'时间戳',0],
                 ['gyro_bias1',1,'gyro_bias',0],
                 ['gyro_bias2',1,'gyro_bias',0],
                 ['gyro_bias3',1,'gyro_bias',0],
                 ['acc_bias1',1,'acc_bias',0],
                 ['acc_bias2',1,'acc_bias',0],
                 ['acc_bias3',1,'acc_bias',0],
                 ['hdg_bias',1,'hdg_bias',0],
                 ['euler_var1',1,'euler_var',0],
                 ['euler_var2',1,'euler_var',0],
                 ['euler_var3',1,'euler_var',0],
                 ['vn_var',1,'vned_var',0],
                 ['ve_var',1,'vned_var',0],
                 ['vd_var',1,'vned_var',0],
                 ['gyro_bias_var1',1,'gyro_bias_var',0],
                 ['gyro_bias_var2',1,'gyro_bias_var',0],
                 ['gyro_bias_var3',1,'gyro_bias_var',0],
                 ['acc_bias_var1',1,'acc_bias_var',0],
                 ['acc_bias_var2',1,'acc_bias_var',0],
                 ['acc_bias_var3',1,'acc_bias_var',0],
                 ['hdg_bias_var',1,'hdg_bias_var',0],
                 ['vel_innov1',1,'vel_innov',0],
                 ['vel_innov2',1,'vel_innov',0],
                 ['vel_innov3',1,'vel_innov',0],
                 ['vel_innov_var1',1,'vel_innov_var',0],
                 ['vel_innov_var2',1,'vel_innov_var',0],
                 ['vel_innov_var3',1,'vel_innov_var',0],
                 ['pos_innov1',1,'pos_innov',0],
                 ['pos_innov2',1,'pos_innov',0],
                 ['pos_innov3',1,'pos_innov',0],
                 ['pos_innov_var1',1,'pos_innov_var',0],
                 ['pos_innov_var2',1,'pos_innov_var',0],
                 ['pos_innov_var3',1,'pos_innov_var',0],
                 ['LLA_var',1,'LLA_var',0],
                 ['LLA_var',1,'LLA_var',0],
                 ['LLA_var',1,'LLA_var',0],
                ],
                [],
            ],
            3: [struct.Struct('<3I2df3f3H7B'),
                [['boot_time',0.005,'时间戳',0],
                 ['tick',0.005,'时间戳',0],
                 ['tow',1,'GPS周期计数',0],
                 ['lat_gps_inn',1,'内部GPS纬度(deg)',0],
                 ['lon_gps_inn',1,'内部GPS经度(deg)',0],
                 ['alt_gps_inn',1,'内部GPS高度(m)',0],
                 ['vn_gps_inn',1,'内部GPS北向速度(m/s)',0],
                 ['ve_gps_inn',1,'内部GPS东向速度(m/s)',0],
                 ['vu_gps_inn',-1,'内部GPS天向速度(m/s)',0],
                 ['year',1,'年份',0],
                 ['msec',1,'毫秒',0],
                 ['pdop',1,'PDOP值',0],
                 ['month',1,'月份',0],
                 ['day',1,'日期',0],
                 ['hour',1,'小时',0],
                 ['minute',1,'分钟',0],
                 ['second',1,'秒',0],
                 ['fixtype',1,'内部惯导定位模式',0],
                 ['satnum',1,'内部惯导定位星数',0],
                ],
                [],
            ],
            4: [struct.Struct('<3I6f9hH'),
                [['boot_time',0.005,'时间戳',0],
                 ['tick',0.005,'时间戳',0],
                 ['health',1,'传感器健康状态',0],
                 ['ps_raw_1',1,'主静压(Pa)',0],
                 ['ps_raw_2',1,'副静压(Pa)',0],
                 ['qbar_raw_1',1,'主动压(Pa)',0],
                 ['qbar_raw_2',1,'副动压(Pa)',0],
                 ['mag_hdg',57.29,'磁航向(Deg)',0],
                 ['gps_hdg',57.29,'GPS航向(Deg)',0],
                 ['temp_imu_1',0.01,'主IMU温度(Deg)',0],
                 ['temp_imu_2',0.01,'副IMU温度(Deg)',0],                
                 ['temp_qbar_1',0.01,'主IMU温度(Deg)',0],
                 ['temp_qbar_2',0.01,'副IMU温度(Deg)',0],
                 ['temp_ps_1',0.01,'主IMU温度(Deg)',0],
                 ['temp_ps_2',0.01,'副IMU温度(Deg)',0],
                 ['magx_raw',1,'磁力计',0],
                 ['magy_raw',1,'磁力计',0],
                 ['magz_raw',1,'磁力计',0],
                 ['cpu_load',1,'导航',0],
                ],
                [],
            ],
            5: [struct.Struct('<2IB9f2d10f8B5Hb'),
                [['boot_time',0.005,'时间戳',0],
                 ['tick',1,'桢计数器',0],
                 ['state',1,'传感器状态',0],
                 ['tht_ins_ext',57.29,'外部惯导俯仰角(deg)',0],     
                 ['phi_ins_ext',57.29,'外部惯导滚转角(deg)',0],             
                 ['psi_ins_ext',57.29,'外部惯导偏航角(deg)',0],
                 ['psit_ins_ext',57.29,'外部GPS航迹偏航角(deg)',0],             
                 ['q_ahrs_ext',57.29,'外部惯导俯仰角速度(deg/sec)',0],
                 ['p_ahrs_ext',57.29,'外部惯导滚转角速度(deg/sec)',0],
                 ['r_ahrs_ext',57.29,'外部惯导偏航角速度(deg/sec)',0],         
                 ['gps_hdg',57.29,'外部GPS双天线航向(deg)',0], 
                 ['gps_hdg_dev',57.29,'外部GPS双天线航向标准差(deg)',0], 
                 ['lon_gps_ext',1,'外部GPS经度(deg)',0],
                 ['lat_gps_ext',1,'外部GPS纬度(deg)',0],
                 ['alt_gps_ext',1,'外部GPS高度(m)',0],
                 ['vn_ins_ext',1,'外部惯导北向速度(m/s)',0],
                 ['ve_ins_ext',1,'外部惯导东向速度(m/s)',0],                
                 ['vu_ins_ext',-1,'外部惯导天向速度(m/s)',0],      
                 ['acc_n_ext',1,'外部惯导北向加速度(m/s2)',0], 
                 ['acc_e_ext',1,'外部惯导东向加速度(m/s2)',0],                
                 ['acc_d_ext',1,'外部惯导地向加速度(m/s2)',0],     
                 ['vn_gps_ext',1,'外部gps北向速度(m/s)',0],    
                 ['ve_gps_ext',1,'外部gps东向速度(m/s)',0],                
                 ['vu_gps_ext',-1,'外部gps天向速度(m/s)',0],     
                 ['satnum_ext',1,'外部卫星星数',0],
                 ['gps_fixtype',1,'外部惯导定位类型',0],
                 ['gps_hous',1,'GPS时',0],
                 ['gps_minute',1,'GPS分',0],
                 ['gps_second',1,'GPS秒',0],    
                 ['gps_day',1,'GPS周内天',0],
                 ['work_state',1,'工作状态',0],
                 ['gps_month',1,'GPS月',0],
                 ['gps_year',1,'GPS年',0],
                 ['gps_week',1,'GPS周',0], 
                 ['gps_msec',1,'GPS秒内毫秒',0], 
                 ['hdop',1,'水平精度因子',0], 
                 ['vdop',1,'垂直精度因子',0],            
                 ['temperature',1,'摄氏度',0], 
                ],
                [],
            ],
            6: [struct.Struct('<4IHd6f'),
                [['boot_time',0.005,'时间戳',0],
                 ['tow_ms',1,'tow_ms',0],
                 ['GPSWeek_uint32',1,'GPSWeek_uint32',0],
                 ['imu_status',1,'imu_status',0],
                 ['GPSWeek_uint16',1,'GPSWeek_uint16',0],
                 ['tow_s',1,'tow_s',0],   
                 ['az_ins_ext',-1,'外部惯导上向加速度(m/s2)',0],                                 
                 ['ax_ins_ext',1,'外部惯导前向加速度(m/s2)',0],
                 ['ay_ins_ext',1,'外部惯导侧向加速度(m/s2)',0],
                 ['r_ins_ext',57.29,'外部惯导偏航角速度(deg/sec)',0],         
                 ['p_ins_ext',57.29,'外部惯导滚转角速度(deg/sec)',0],
                 ['q_ins_ext',57.29,'外部惯导俯仰角速度(deg/sec)',0],
                ],
                [],
            ],
            7: [struct.Struct('<I8H'),
                [['boot_time', 0.005, '时间戳', 0],
                 ['act_can1_dfli_pos', act2deg_new(inv_dfli_fix), '左内襟翼 舵机1', 0],
                 ['act_can1_dfri_pos', act2deg_new(inv_dfri_fix), '右内襟翼 舵机2', 0],
                 ['act_can1_dfali_pos', act2deg_new(inv_dfali_fix), '左内襟副翼 舵机3', 0],
                 ['act_can1_dfari_pos', act2deg_new(inv_dfari_fix), '右内襟副翼舵机4', 0],
                 ['act_can1_dali_pos', act2deg_new(inv_dali_fix), '左内副翼 舵机5', 0],
                 ['act_can1_dari_pos', act2deg_new(inv_dari_fix), '右内副翼 舵机6', 0],
                 ['act_can1_deli_pos', act2deg_new(inv_deli_fix), '左内尾翼 舵机7', 0],
                 ['act_can1_deri_pos', act2deg_new(inv_deri_fix), '右内尾翼 舵机8', 0],
                 ],
                [],
                ],
            8: [struct.Struct('<I8H'),
                [['boot_time', 0.005, '时间戳', 0],
                 ['act_can2_dflo_pos', act2deg_new(inv_dflo_fix), '左外襟翼 舵机9', 0],
                 ['act_can2_dfro_pos', act2deg_new(inv_dfro_fix), '右外襟翼 舵机10', 0],
                 ['act_can2_dfalo_pos', act2deg_new(inv_dfalo_fix), '左外襟副翼 舵机11', 0],
                 ['act_can2_dfaro_pos', act2deg_new(inv_dfaro_fix), '右外襟副翼 舵机12', 0],
                 ['act_can2_dalo_pos', act2deg_new(inv_dalo_fix), '左外副翼 舵机13', 0],
                 ['act_can2_daro_pos', act2deg_new(inv_daro_fix), '右外副翼 舵机14', 0],
                 ['act_can2_delo_pos', act2deg_new(inv_delo_fix), '左外尾翼 舵机15', 0],
                 ['act_can2_dero_pos', act2deg_new(inv_dero_fix), '右外尾翼 舵机16', 0],
                 ],
                [],
                ],
            9:  [struct.Struct('<I4fH'),  # sdas
                [['boot_time',0.005,'时间戳',0],
                 ['sdas_Ps',1,'外部大气机静压',0],
                 ['sdas_temp',1,'外部大气机温度',0],
                 ['sdas_qbar',1,'外部大气机动压',0],
                 ['sdas_temp_qbar',1,'外部动压温度',0],
                 ['sdas_stats',1,'外部大气机状态',0],
                ],
                [],
            ],
            10:  [struct.Struct('<I5B25H'),
                [['boot_time',0.005,'时间戳',0],

                ['ignite1_on_b',1,'后发CDI1开关状态',0],
                ['pump_on_b',1,'后发油泵开关状态',0],
                ['choke_on_b',1,'后发强制喷油开关',0],
                ['ignite2_on_b',1,'后发CDI2开关状态',0],                
                ['throt_off_b',1,'后发风门舵机开关状态',0],

                ['temperature_b',1,'后发外界温度',0],
                ['pressure_b', 1,'后发外界气压',0],
                ['actual_fuel_pressure_b',1,'后发实际油压',0],
                ['set_fuel_pressure_b',1,'后发预期的油压',0],   
                ['pump_duty_cycle_b',1,'后发油泵占空比',0],
                ['set_throttle_value_b',1,'后发预期线性油门位置',0],
                ['actual_jet1_duty_cycle_b',1,'后发实际喷油1脉宽',0],
                ['actual_jet2_duty_cycle_b',1,'后发实际喷油2脉宽',0],
                ['pre_throt_position_b',1,'后发预热时风门位置',0],
                ['expect_rpm_b',1,'后发预热时风门位置',0],
                ['set_value_throttle_plate_b',1,'后发设置风门舵机位置',0],
                ['RPM_b',1,'后发转速',0],
                ['expect_throttle_b',1,'后发期望的油门位置',0],
                ['ch1_temp_b',1,'后发通道1温度',0],
                ['ch2_temp_b',1,'后发通道2温度',0],
                ['ch3_temp_b',1,'后发通道3温度',0],
                ['ch4_temp_b',1,'后发通道4温度',0],     
                ['main_current_b',0.1,'后发系统总电流',0],    
                ['I_ignition1_b',0.1,'后发喷油嘴1电流',0], 
                ['I_ignition2_b',0.1,'后发喷油嘴2电流',0], 
                ['I_servos_b',0.1,'后发舵机电流',0], 
                ['I_pump_b',0.1,'后发油泵电流',0], 
                ['main_voltage_b',0.1,'后发系统输入电压',0], 
                ['actual_value_throttle_plate_b',1,'后发实际风门舵机位置',0],        
                ['2nd_rpm_value_b',1,'后发第二转速',0],                              
                ],
                [],
            ],
            11: [struct.Struct('<Iff'),  # oil_level
                 [['boot_time', 0.005, '时间戳', 0],
                  ['oil_level_value', 1, '液位传感器采集值', 0],
                  ['oil_level_mass', 1, '剩余油量', 0],
                  ],
                 [],
            ],
            12: [struct.Struct('<Ii3H2h6B2Hi3H2h6B2Hi3H2h6B2Hi3H2h6B2H'),
                 [['boot_time', 0.005, '时间戳', 0],
                  ['rpm1', 1, '电机转速1', 0],
                  ['recv_pwm1', 1, '0.1us电调接收PWM1', 0],
                  ['comm_pwm1', 1, '0.1us电调接收的通信PWM1', 0],
                  ['voltage1', 1, '0.1v母线电压1', 0],
                  ['bus_current1', 1, '0.1A母线电流1', 0],
                  ['current1', 1, '0.1A电机线电流1', 0],
                  ['v_modulation1', 1, '调制比1', 0],
                  ['esc_index1', 1, '电调编号1', 0],
                  ['mos_temp1', 1, 'MOS温度1', 0],
                  ['cap_temp1', 1, '电容温度1', 0],
                  ['mcu_temp', 1, 'MCU温度1', 0],
                  ['motor_temp', 1, '电机温度', 0],
                  ['running_error1', 1, '运行错误1', 0],
                  ['throttle_cmd_freq', 1, '通信油门指令的帧率1', 0],
                  ['rpm2', 1, '电机转速2', 0],
                  ['recv_pwm2', 1, '0.1us电调接收PWM2', 0],
                  ['comm_pwm2', 1, '0.1us电调接收的通信PWM2', 0],
                  ['voltage2', 1, '0.1v母线电压2', 0],
                  ['bus_current2', 1, '0.1A母线电流2', 0],
                  ['current2', 1, '0.1A电机线电流2', 0],
                  ['v_modulation2', 1, '调制比2', 0],
                  ['esc_index2', 1, '电调编号2', 0],
                  ['mos_temp2', 1, 'MOS温度2', 0],
                  ['cap_temp2', 1, '电容温度2', 0],
                  ['mcu_temp2', 1, 'MCU温度2', 0],
                  ['motor_temp2', 1, '电机温度2', 0],
                  ['running_error2', 1, '运行错误2', 0],
                  ['throttle_cmd_freq2', 1, '通信油门指令的帧率2', 0],
                  ['rpm3', 1, '电机转速3', 0],
                  ['recv_pwm3', 1, '0.1us电调接收PWM3', 0],
                  ['comm_pwm3', 1, '0.1us电调接收的通信PWM3', 0],
                  ['voltage3', 1, '0.1v母线电压3', 0],
                  ['bus_current3', 1, '0.1A母线电流3', 0],
                  ['current3', 1, '0.1A电机线电流3', 0],
                  ['v_modulation3', 1, '调制比3', 0],
                  ['esc_index3', 1, '电调编号3', 0],
                  ['mos_temp3', 1, 'MOS温度3', 0],
                  ['cap_temp3', 1, '电容温度3', 0],
                  ['mcu_temp3', 1, 'MCU温度3', 0],
                  ['motor_temp3', 1, '电机温度3', 0],
                  ['running_error3', 1, '运行错误3', 0],
                  ['throttle_cmd_freq3', 1, '通信油门指令的帧率3', 0],
                  ['rpm4', 1, '电机转速4', 0],
                  ['recv_pwm4', 1, '0.1us电调接收PWM4', 0],
                  ['comm_pwm4', 1, '0.1us电调接收的通信PWM4', 0],
                  ['voltage4', 1, '0.1v母线电压4', 0],
                  ['bus_current4', 1, '0.1A母线电流4', 0],
                  ['current4', 1, '0.1A电机线电流4', 0],
                  ['v_modulation4', 1, '调制比4', 0],
                  ['esc_index4', 1, '电调编号4', 0],
                  ['mos_temp4', 1, 'MOS温度4', 0],
                  ['cap_temp4', 1, '电容温度4', 0],
                  ['mcu_temp4', 1, 'MCU温度4', 0],
                  ['motor_temp4', 1, '电机温度4', 0],
                  ['running_error4', 1, '运行错误4', 0],
                  ['throttle_cmd_freq4', 1, '通信油门指令的帧率4', 0],
                  ],
                 [],
                 ],
            13: [struct.Struct('<Ii3H2h6B2Hi3H2h6B2Hi3H2h6B2Hi3H2h6B2H'),
                 [['boot_time', 0.005, '时间戳', 0],
                  ['rpm5', 1, '电机转速5', 0],
                  ['recv_pwm5', 1, '0.1us电调接收PWM5', 0],
                  ['comm_pwm5', 1, '0.1us电调接收的通信PWM5', 0],
                  ['voltage5', 1, '0.1v母线电压5', 0],
                  ['bus_current5', 1, '0.1A母线电流5', 0],
                  ['current5', 1, '0.1A电机线电流5', 0],
                  ['v_modulation5', 1, '调制比5', 0],
                  ['esc_index5', 1, '电调编号5', 0],
                  ['mos_temp5', 1, 'MOS温度5', 0],
                  ['cap_temp5', 1, '电容温度5', 0],
                  ['mcu_temp5', 1, 'MCU温度5', 0],
                  ['motor_temp5', 1, '电机温度5', 0],
                  ['running_error5', 1, '运行错误5', 0],
                  ['throttle_cmd_freq5', 1, '通信油门指令的帧率5', 0],
                  ['rpm6', 1, '电机转速6', 0],
                  ['recv_pwm6', 1, '0.1us电调接收PWM6', 0],
                  ['comm_pwm6', 1, '0.1us电调接收的通信PWM6', 0],
                  ['voltage6', 1, '0.1v母线电压6', 0],
                  ['bus_current6', 1, '0.1A母线电流6', 0],
                  ['current6', 1, '0.1A电机线电流6', 0],
                  ['v_modulation6', 1, '调制比6', 0],
                  ['esc_index6', 1, '电调编号6', 0],
                  ['mos_temp6', 1, 'MOS温度6', 0],
                  ['cap_temp6', 1, '电容温度6', 0],
                  ['mcu_temp6', 1, 'MCU温度6', 0],
                  ['motor_temp6', 1, '电机温度6', 0],
                  ['running_error6', 1, '运行错误6', 0],
                  ['throttle_cmd_freq6', 1, '通信油门指令的帧率6', 0],
                  ['rpm7', 1, '电机转速7', 0],
                  ['recv_pwm7', 1, '0.1us电调接收PWM7', 0],
                  ['comm_pwm7', 1, '0.1us电调接收的通信PWM7', 0],
                  ['voltage7', 1, '0.1v母线电压7', 0],
                  ['bus_current7', 1, '0.1A母线电流7', 0],
                  ['current7', 1, '0.1A电机线电流7', 0],
                  ['v_modulation7', 1, '调制比7', 0],
                  ['esc_index7', 1, '电调编号7', 0],
                  ['mos_temp7', 1, 'MOS温度7', 0],
                  ['cap_temp7', 1, '电容温度7', 0],
                  ['mcu_temp7', 1, 'MCU温度7', 0],
                  ['motor_temp7', 1, '电机温度7', 0],
                  ['running_error7', 1, '运行错误7', 0],
                  ['throttle_cmd_freq7', 1, '通信油门指令的帧率7', 0],
                  ['rpm8', 1, '电机转速8', 0],
                  ['recv_pwm8', 1, '0.1us电调接收PWM8', 0],
                  ['comm_pwm8', 1, '0.1us电调接收的通信PWM8', 0],
                  ['voltage8', 1, '0.1v母线电压8', 0],
                  ['bus_current8', 1, '0.1A母线电流8', 0],
                  ['current8', 1, '0.1A电机线电流8', 0],
                  ['v_modulation8', 1, '调制比8', 0],
                  ['esc_index8', 1, '电调编号8', 0],
                  ['mos_temp8', 1, 'MOS温度8', 0],
                  ['cap_temp8', 1, '电容温度8', 0],
                  ['mcu_temp8', 1, 'MCU温度8', 0],
                  ['motor_temp8', 1, '电机温度8', 0],
                  ['running_error8', 1, '运行错误8', 0],
                  ['throttle_cmd_freq8', 1, '通信油门指令的帧率8', 0],
                  ],
                 [],
                 ],
            20: [struct.Struct('<I11f4fB5H12f12f'),
                 [['boot_time', 0.005, '时间戳', 0],
                  ['q_c', 57.3, '俯仰角速度指令(deg/s)', 0],
                  ['p_c', 57.3, '滚转角速度指令(deg/s)', 0],
                  ['r_c', 57.3, '偏航角速度指令(deg/s)', 0],
                  ['p', 57.3, '滚转角速度(deg/s)', 0],
                  ['q', 57.3, '俯仰角速度(deg/s)', 0],
                  ['r', 57.3, '偏航角速度(deg/s)', 0],
                  ['ax', 1, '前向加速度(m/s2)', 0],
                  ['ay', 1, '右向加速度(m/s2)', 0],
                  ['az', 1, '下向加速度(m/s2)', 0],
                  ['ay_c', 1, '右向加速度指令(m/s2)', 0],
                  ['az_c', 1, '下向加速度指令(m/s2)', 0],

                  ['de_c', 57.3, '升降舵指令(deg)', 0],
                  ['da_c', 57.3, '副翼舵指令(deg)', 0],
                  ['dr_c', 57.3, '方向舵指令(deg)', 0],
                  ['steer_c', 57.3, '前轮转向指令(deg)', 0],

                  ['brake', 1, '刹车', 0],

                  ['RPM1', 1, '转速1', 0],
                  ['RPM2', 1, '转速2', 0],
                  ['RPM3', 1, '转速3', 0],
                  ['RPM4', 1, '转速4', 0],
                  ['RPM5', 1, '转速5', 0],

                  ['rot_heave', 1, '悬停转速指令rot_heave', 0],
                  ['rot_pitch', 1, '俯仰转速指令rot_pitch', 0],
                  ['rot_roll', 1, '滚转转速指令rot_roll', 0],
                  ['rot_yaw', 1, '偏航转速指令rot_yaw', 0],
                  ['rot_lfo', 1, 'rot_lfo(rpm)', 0],
                  ['rot_lfi', 1, 'rot_lfi(rpm)', 0],
                  ['rot_rfi', 1, 'rot_rfi(rpm)', 0],
                  ['rot_rfo', 1, 'rot_rfo(rpm)', 0],
                  ['rot_rbo', 1, 'rot_rbo(rpm)', 0],
                  ['rot_rbi', 1, 'rot_rbi(rpm)', 0],
                  ['rot_lbi', 1, 'rot_lbi(rpm)', 0],
                  ['rot_lbo', 1, 'rot_lbo(rpm)', 0],

                  ['del_c', 57.3, '左尾翼指令', 0],
                  ['der_c', 57.3, '右尾翼指令', 0],
                  ['dal_c', 57.3, '左副翼指令', 0],
                  ['dar_c', 57.3, '左副翼指令', 0],
                  ['dfl_c', 57.3, '左襟翼指令', 0],
                  ['dfr_c', 57.3, '左襟翼指令', 0],
                  ['dfal_c',57.3, '左襟副翼指令', 0],
                  ['dfar_c', 57.3, '左襟副翼指令', 0],
                  ['de_stimulus', 57.3, '升降舵激励', 0],
                  ['da_stimulus', 57.3, '副翼激励', 0],
                  ['dr_stimulus', 57.3, '方向舵激励', 0],
                  ['df_stimulus', 57.3, '襟翼激励', 0],
            
                  ],
                 [],
                 ],

            21: [struct.Struct('<I11ff4f7f'),
                 [['boot_time', 0.005, '时间戳', 0],

                  ['phi_c', 57.3, '滚转角指令(deg)', 0],
                  ['tht_c', 57.3, '俯仰角指令(deg)', 0],
                  ['hdot_c', 1, '升降速率指令(m/s)', 0],
                  ['phi', 57.3, '滚转角(deg)', 0],
                  ['tht', 57.3, '俯仰角(deg)', 0],
                  ['psi', 57.3, '偏航角(deg)', 0],
                  ['v_n', 1, '北向速度(m/s)', 0],
                  ['v_e', 1, '东向速度(m/s)', 0],
                  ['v_d', 1, '下向速度(m/s)', 0],
                  ['v_x', 1, '前向速度(m/s)', 0],
                  ['v_y', 1, '侧向速度(m/s)', 0],

                  ['thr_c', 1, '油门指令', 0],

                  ['rc_roll', 1, '遥控滚转指令', 0],
                  ['rc_pitch', 1, '遥控俯仰指令', 0],
                  ['rc_yaw', 1, '遥控偏航指令', 0],
                  ['rc_thr', 1, '遥控油门指令', 0],

                  ['as_c', 1, '空速指令', 0],
                  ['gamma_c', 1, '爬升角指令', 0],
                  ['gs_c', 1, '地速指令', 0],
                  ['h_c', 1, '高度指令', 0],
                  ['vx_c', 1, '前向速度指令', 0],
                  ['vy_c', 1, '右向速度指令', 0],
                  ['psidot_c', 1, '偏航角速率指令', 0],
                  ],
                 [],
            ],
            22: [struct.Struct('<If2d8f3H2BH19B'),
                 [['boot_time', 0.005, '时间戳', 0],

                  ['msl', 1, '海拔高度(m)', 0],

                  ['lat', 1e-7, '纬度(deg)', 0],
                  ['lon', 1e-7, '经度(deg)', 0],

                  ['baro', 1, '静压(Pa)', 0],
                  ['qbar', 1, '动压(Pa)', 0],
                  ['asl_baro', 1, '气压高度(m)', 0],
                  ['IAS', 1, '表速(m/s)', 0],
                  ['TAS', 1, '真空速(m/s)', 0],
                  ['temp', 1, '静温(degC)', 0],
                  ['alpha', 57.3, '迎角(deg)', 0],
                  ['beta', 57.3, '侧滑角(deg)', 0],

                  ['load', 1, 'cpu负载', 0],
                  ['HDOP', 1, '横向定位', 0],
                  ['VDOP', 1, '纵向定位', 0],

                  ['satnum', 1, '星数', 0],
                  ['gps_fixtype', 1, 'GPS定位模式', 0],

                  ['WP_cur', 1, '当前航点', 0],

                  ['rc_auto', 1, '遥控主模式', 0],
                  ['rc_aux', 1, '遥控副模式', 0],
                  ['rc_conn', 1, '遥控上行通畅', 0],
                  ['linked', 1, '数据链通畅', 0],
                  ['level1_lgx', 1, '模式1', 0],
                  ['level2_lgx', 1, '模式2', 0],
                  ['level3_lgx', 1, '模式3', 0],
                  ['level3_auto_lgx', 1, '模式3a', 0],
                  ['level4_lgx', 1, '模式4', 0],
                  ['landed_state', 1, '着陆模式', 0],
                  ['AFCS_LAT', 1, '横向导引', 0],
                  ['AFCS_LON', 1, '纵向导引', 0],
                  ['AG', 1, '旋翼通道', 0],
                  ['AT', 1, '速度通道', 0],
                  ['CSAS_PITCH', 1, '俯仰通道', 0],
                  ['CSAS_ROLL', 1, '滚转通道', 0],
                  ['CSAS_YAW', 1, '偏航通道', 0],
                  ['vtol_state', 1, 'vtol模式', 0],
                  ['armed', 1, '动力锁定', 0],
                  ],
                 [],
                 ],


            23: [struct.Struct('<3I5f5HH'),
                 [['boot_time', 0.005, '时间戳',0],
                  ['health_ap', 1, '飞控健康字',0],
                  ['health_nav', 1, '惯导健康字',0],

                  ['volt1', 1, '电压采集1(V)',0],
                  ['volt2', 1, '电压采集2(V)',0],
                  ['volt3', 1, '电压采集3(V)',0],
                  ['volt4', 1, '电压采集4(V)',0],
                  ['volt5', 1, '电压采集5(V)',0],

                  ['monitor1', 1, 'INS',0],
                  ['monitor2', 1, 'INS',0],
                  ['monitor3', 1, 'INS',0],
                  ['monitor4', 1, 'INS',0],
                  ['monitor5', 1, 'INS',0],

                  ['rally_idx', 1, '选择迫降点',0],
                  ],
                 [],
            ],
            24: [struct.Struct('<I18f'),
                 [['boot_time', 0.005, '时间戳', 0],
                  ['q_tht_hat', 1, 'q_tht_hat', 0],
                  ['q_sigma_hat', 1, 'q_sigma_hat', 0],
                  ['q_omega_hat', 1, 'q_omega_hat', 0],
                  ['q_hat', 57.3, 'q_hat', 0],
                  ['p_tht_hat', 1, 'p_tht_hat', 0],
                  ['p_sigma_hat', 1, 'p_sigma_hat', 0],
                  ['p_omega_hat', 1, 'p_omega_hat', 0],
                  ['p_hat', 57.3, 'p_hat', 0],
                  ['pdot_c', 57.31, 'pdot_c', 0],
                  ['qdot_c', 57.31, 'qdot_c', 0],
                  ['qdot_c_out', 57.31, 'qdot_c_out', 0],
                  ['pdot_c_out', 57.31, 'pdot_c_out', 0],
                  ['rdot_c', 57.31, 'rdot_c', 0],
                  ['r_hat', 57.3, 'r_hat', 0],
                  ['r_sigma_hat', 1, 'r_sigma_hat', 0],
                  ['r_omega_hat', 1, 'r_omega_hat', 0],
                  ['r_tht_hat', 1, 'r_tht_hat', 0],
                  ['rdot_c_out', 57.31, 'rdot_c_out', 0],
                  ],
                 [],
            ],
            25: [struct.Struct('<I22fH'),
                [['boot_time', 0.005, '时间戳', 0],

                 ['dist', 1, '航点距离(m)', 0],
                 ['dist_offset', 1, '航线待飞距(m)', 0],
                 ['vert_offset', 1, '航线纵向距(m)', 0],
                 ['horiz_offset', 1, '航线侧向距(m)', 0],
                 ['ht_diff', 1, '航点高度差(m)', 0],
                 ['abs_alt', 1, '绝对高度(m)', 0],
                 ['rel_alt', 1, '相对高度(m)', 0],
                 ['home_dist', 1, '到HOME点距离(m)', 0],
                 ['home_bearing', 57.29, 'HOME点方位(deg)', 0],
                 ['v_g', 1, '地速(m/s)', 0],
                 ['hdot', 1, '升降速度(m/s)', 0],
                 ['rdot', 1, '接近速度(m/s)', 0],
                 ['elevation', 57.29, '航点倾角(deg)', 0],
                 ['elevation_basesline', 57.29, '航线倾角(deg)', 0],
                 ['bearing', 57.29, '航点方位(deg)', 0],
                 ['bearing_basesline', 57.29, '航线方向角(deg)', 0],
                 ['gamma', 57.3, '航迹倾角', 0],
                 ['gamma_c', 57.3, '航迹倾角指令', 0],
                 ['qbar', 1, '动压', 0],
                 ['baro', 1, 'baro', 0],
                 ['h_c', 1, '高度指令', 0],
                 ['TAS', 1, '真空速', 0],

                 ['WP_cur', 1, '当前航点', 0],
                 ],
                [],
            ],

        }
    cnt = 0
    while True:
        try:
            b = hj.read(1)
            cnt += 1
            if len(b) != 1:
                raise EOFError
            while b != b'\xEB':
                printline('X')
                b = hj.read(1)
                cnt += 1
                if len(b) != 1:
                    raise EOFError

            b = hj.read(1)
            cnt += 1
            if len(b) != 1:
                raise EOFError
            while b != b'\x90':
                printline('x')
                b = hj.read(1)
                cnt += 1
                if len(b) != 1:
                    raise EOFError

            b = hj.read(1)
            cnt += 1
            if len(b) != 1:
                raise EOFError
            Id = b[0]
            printline('{}'.format(Id))
            if Id not in pkgs.keys():
                continue

            b = hj.read(1)
            cnt += 1
            if len(b) != 1:
                raise EOFError
            Len = b[0]

            buff = hj.read(Len)
            cnt += Len
            if len(buff) != Len:
                raise EOFError

            # chksum
            b = hj.read(1)
            cnt += 1
            if len(b) != 1:
                raise EOFError
            if b[0] == chksum(buff):
                try:
                    items = pkgs[Id][0].unpack_from(buff)
                    items = [pkgs[Id][1][j][1](i) if callable(pkgs[Id][1][j][1]) else i*pkgs[Id][1][j][1]+pkgs[Id][1][j][3] for j,i in enumerate(items)]
                    for num in items:
                        if math.isnan(num):
                            printline('A')
                    pkgs[Id][2].append(items)
                except:
                    printline('E')
            else:
                printline('C')

        except EOFError:
            break

    print(' '.join(line_buff))
    print('read {}'.format(cnt))

    mat_filename = filename+'.mat'
    mat_data = {'p{}'.format(i): pkgs[i][2] for i in pkgs.keys()}
    mat_data.update({'p{}_title'.format(i): [j[2] for j in pkgs[i][1]] for i in pkgs.keys()})
    mat_data.update({'p{}_title2'.format(i): [j[0] for j in pkgs[i][1]] for i in pkgs.keys()})
    spio.savemat(mat_filename, mat_data)

    return mat_filename


if __name__ == '__main__':
    import argparse
    import glob

    parser = argparse.ArgumentParser(description='Process some integers.')

    parser.add_argument('filename', nargs='+',
                        help='filename to be parsed')
    args = parser.parse_args()
    for i in args.filename:
        fs = glob.glob(i)
        for j in fs:
            parseHJ(j)