LZY\79495
748 lines
35 KiB
Python
748 lines
35 KiB
Python
|
|
import struct
|
|
import scipy.io as spio
|
|
import math
|
|
|
|
line_buff = []
|
|
|
|
|
|
inv_dali_fix = [0.1408039507 ,-0.0140627200 ,-0.0000033598, 0.0000001592]
|
|
inv_dalo_fix = [0.0070414080 ,-0.0140834449, 0.0000011782 ,0.0000001592]
|
|
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.0028783254, 0.0143703649 ,0.0001068659 ,-0.0000027725]
|
|
inv_dfli_fix = [-0.1532449359 ,0.0146335728 ,-0.0000043779 ,0.0000000649]
|
|
inv_dflo_fix = [0.0278719692, 0.0146730933, 0.0000014963 ,-0.0000002196]
|
|
inv_dfri_fix = [0.0176016124, 0.0146706756, 0.0000019575 ,-0.0000002196]
|
|
inv_dfro_fix = [-0.1678829514 ,0.0146425231, -0.0000045724, 0.0000000649]
|
|
inv_dfali_fix = [0.1407942292 ,-0.0140664510, -0.0000031613 ,0.0000001864]
|
|
inv_dfalo_fix = [-0.0139722450, 0.0139733266 ,-0.0000011090 ,0.0000000274]
|
|
inv_dfari_fix = [0.3546473539 ,0.0139533437 ,-0.0000010483 ,-0.0000000271]
|
|
inv_dfaro_fix = [0.0422548962 ,-0.0140863505 ,-0.0000000160, 0.0000001592]
|
|
|
|
|
|
|
|
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', 1, '左尾翼指令', 0],
|
|
['der_c', 1, '右尾翼指令', 0],
|
|
['dal_c', 1, '左副翼指令', 0],
|
|
['dar_c', 1, '左副翼指令', 0],
|
|
['dfl_c', 1, '左襟翼指令', 0],
|
|
['dfr_c', 1, '左襟翼指令', 0],
|
|
['dfal_c', 1, '左襟副翼指令', 0],
|
|
['dfar_c', 1, '左襟副翼指令', 0],
|
|
['de_stimulus', 1, '升降舵激励', 0],
|
|
['da_stimulus', 1, '副翼激励', 0],
|
|
['dr_stimulus', 1, '方向舵激励', 0],
|
|
['df_stimulus', 1, '襟翼激励', 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('<If2d8f3H2BH18B'),
|
|
[['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],
|
|
],
|
|
[],
|
|
],
|
|
|
|
|
|
23: [struct.Struct('<3I5f5H'),
|
|
[['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],
|
|
],
|
|
[],
|
|
],
|
|
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) |