hm/sunny360vfe_proj
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新电调解包触发方式；更新mon包和记录模块的油位计映射；更新串口初始化波特率，与交联图相同；更新记录模块；

Browse Source
This commit is contained in:
SYTQZTS\26307
2025-07-03 21:29:44 +08:00
parent f6afe085fb
commit b9745e44f4
7 changed files with 848 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
controllers/sunny360vfe_ap_params.sldd
BIN
View File
Binary file not shown.
controllers/sunny360vfe_params.xlsx
BIN
View File
Binary file not shown.
doc/RunShow.m
+3
View File
@@ -0,0 +1,3 @@
showRecMat('00000021.DAT.mat', [1:25]);
doc/parserRec.bat
+2
View File
@@ -0,0 +1,2 @@
python parserRec.py %1
pause
doc/parserRec.py
+777
View File
@@ -0,0 +1,777 @@
import struct
import scipy.io as spio
import math
line_buff = []
inv_dalin_fix = [-0.0061075297, 0.0116297975, 0.0000028471, -0.0000004536]
inv_dalout_fix = [-0.0202131443, 0.0125861888, 0.0000298655, -0.0000013490]
inv_darin_fix = [-0.0010516139, -0.0114334432, -0.0000011885, 0.0000004852]
inv_darout_fix = [-0.0004526855, -0.0112986413, -0.0000081846, 0.0000004624]
inv_drl_fix = [0.0024652155, -0.0139869564, 0.0000143248, 0.0000004647]
inv_drr_fix = [-0.0016802808, -0.0139145523, -0.0000072242, 0.0000002906]
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', 1, '左内襟翼 舵机1', 0],
['act_can1_drli_pos', 1, '右内襟翼 舵机2', 0],
['act_can1_dfali_pos', 1, '左内襟副翼 舵机3', 0],
['act_can1_dfari_pos', 1, '右内襟副翼舵机4', 0],
['act_can1_dali_pos', 1, '左内副翼 舵机5', 0],
['act_can1_dari_pos', 1, '右内副翼 舵机6', 0],
['act_can1_deli_pos', 1, '左内尾翼 舵机7', 0],
['act_can1_deri_pos', 1, '右内尾翼 舵机8', 0],
],
[],
],
8: [struct.Struct('<I8H'),
[['boot_time', 0.005, '时间戳', 0],
['act_can2_dflo_pos', 1, '左外襟翼 舵机9', 0],
['act_can2_dfro_pos', 1, '右外襟翼 舵机10', 0],
['act_can2_dfalo_pos', 1, '左外襟副翼 舵机11', 0],
['act_can2_dfaro_pos', 1, '右外襟副翼 舵机12', 0],
['act_can2_dalo_pos', 1, '左外副翼 舵机13', 0],
['act_can2_daro_pos', 1, '右外副翼 舵机14', 0],
['act_can2_delo_pos', 1, '左外尾翼 舵机15', 0],
['act_can2_dero_pos', 1, '右外尾翼 舵机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('<Ii3H2h6B2H'),
[['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],
],
[],
],
13: [struct.Struct('<Ii3H2h6B2H'),
[['boot_time', 0.005, '时间戳', 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],
],
[],
],
14: [struct.Struct('<Ii3H2h6B2H'),
[['boot_time', 0.005, '时间戳', 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],
],
[],
],
15: [struct.Struct('<Ii3H2h6B2H'),
[['boot_time', 0.005, '时间戳', 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],
],
[],
],
16: [struct.Struct('<Ii3H2h6B2H'),
[['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],
],
[],
],
17: [struct.Struct('<Ii3H2h6B2H'),
[['boot_time', 0.005, '时间戳', 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],
],
[],
],
18: [struct.Struct('<Ii3H2h6B2H'),
[['boot_time', 0.005, '时间戳', 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],
],
[],
],
19: [struct.Struct('<Ii3H2h6B2H'),
[['boot_time', 0.005, '时间戳', 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('<I11f4fB5H12f25H'),
[['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],
['PWM1', 1, 'PWM1', 0],
['PWM2', 1, 'PWM2', 0],
['PWM3', 1, 'PWM3', 0],
['PWM4', 1, 'PWM4', 0],
['PWM5', 1, 'PWM5', 0],
['PWM6', 1, 'PWM6', 0],
['PWM7', 1, 'PWM7', 0],
['PWM8', 1, 'PWM8', 0],
['PWM9', 1, 'PWM9', 0],
['PWM10', 1, 'PWM10', 0],
['PWM11', 1, 'PWM11', 0],
['PWM12', 1, 'PWM12', 0],
['PWM13', 1, 'PWM13', 0],
['PWM14', 1, 'PWM14', 0],
['PWM15', 1, 'PWM15', 0],
['PWM16', 1, 'PWM16', 0],
['PWM17', 1, 'PWM17', 0],
['PWM18', 1, 'PWM18', 0],
['PWM19', 1, 'PWM19', 0],
['PWM20', 1, 'PWM20', 0],
['PWM21', 1, 'PWM21', 0],
['PWM22', 1, 'PWM22', 0],
['PWM23', 1, 'PWM23', 0],
['PWM24', 1, 'PWM24', 0],
['PWM25', 1, 'PWM25', 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)
doc/showRecMat.m
+66
View File
@@ -0,0 +1,66 @@
function showRecMat(filename,sheets)
Run = Simulink.sdi.Run.create;
Run.Name = filename;
for sh = sheets
p_name = sprintf('p%d',sh);
t2_name = sprintf('p%d_title2',sh);
disp(t2_name);
try
t_name = sprintf('p%d_title',sh);
S = load(filename,p_name, t_name, t2_name);
catch
t_name = sprintf('p%d_title2',sh);
S = load(filename,p_name, t_name, t2_name);
end
p = S.(p_name);
if ~isempty(p)
title = S.(t_name);
title2 = S.(t2_name);
time_name = strtrim(title2(1,:));
time_sec = p(:,1);
n = size(title,1);
for i=2:n
name = strtrim(title2(i,:));
if ~strcmp(name, time_name)
ts = timeseries(p(:,i), time_sec);
desc = strtrim(title(i,:));
ts.Name = [name ' ' desc];
Run.add('vars',ts);
switch name
case 'p'
[pdot, t1]=diff(ts.Data,ts.time,4);
ts = timeseries(pdot, t1);
ts.Name = 'pdot(deg/s2)';
Run.add('vars',ts);
case 'q'
[qdot, t1]=diff(ts.Data,ts.time,4);
% N=length(qdot);
% nfft=2^nextpow2(N);
% fft_qdot=fft(qdot,nfft)/nfft;
% f=25/2*linspace(0,1,nfft/2);
% plot(f,2*abs(fft_qdot(1:nfft/2)));
ts = timeseries(qdot, t1);
ts.Name = 'qdot(deg/s2)';
Run.add('vars',ts);
case 'r'
[rdot, t1]=diff(ts.Data,ts.time,4);
ts = timeseries(rdot, t1);
ts.Name = 'rdot(deg/s2)';
Run.add('vars',ts);
end
end
end
end
end
% 在仿真数据检查器中查看信号
Simulink.sdi.view
end
function [qdot, t1]=diff(q,t,delta)
qdot = (q(1:end-delta*2)-q(1+delta*2:end))./(t(1:end-delta*2)-t(1+delta*2:end));
t1 = t(1+delta:end-delta);
end
mainModels/sunny360vfe_ap.slx
BIN
View File
Binary file not shown.