fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 21:57:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: allow up to 4 accels, gyros, and baros and add configurable rotations for accel & gyro

Browse Source
This commit is contained in:
Daniel Agar
2020-10-08 19:01:44 -04:00
committed by GitHub
parent 28956e0399
commit eecf2e7a1e
47 changed files with 2881 additions and 1163 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/process_sensor_caldata.py
+293 -8
View File
@@ -98,6 +98,10 @@ for d in data:
sensor_gyro_2 = d.data
print('found gyro 2 data')
num_gyros += 1
elif d.multi_id == 3:
sensor_gyro_3 = d.data
print('found gyro 3 data')
num_gyros += 1
# extract accel data
num_accels = 0
@@ -115,6 +119,10 @@ for d in data:
sensor_accel_2 = d.data
print('found accel 2 data')
num_accels += 1
elif d.multi_id == 3:
sensor_accel_3 = d.data
print('found accel 3 data')
num_accels += 1
# extract baro data
num_baros = 0
@@ -132,6 +140,10 @@ for d in data:
sensor_baro_2 = d.data
print('found baro 2 data')
num_baros += 1
elif d.multi_id == 3:
sensor_baro_3 = d.data
print('found baro 3 data')
num_baros += 1
# open file to save plots to PDF
from matplotlib.backends.backend_pdf import PdfPages
@@ -464,6 +476,99 @@ if num_gyros >= 3 and not math.isnan(sensor_gyro_2['temperature'][0]):
#################################################################################
# define data dictionary of gyro 3 thermal correction parameters
gyro_3_params = {
'TC_G3_ID':0,
'TC_G3_TMIN':0.0,
'TC_G3_TMAX':0.0,
'TC_G3_TREF':0.0,
'TC_G3_X0_0':0.0,
'TC_G3_X1_0':0.0,
'TC_G3_X2_0':0.0,
'TC_G3_X3_0':0.0,
'TC_G3_X0_1':0.0,
'TC_G3_X1_1':0.0,
'TC_G3_X2_1':0.0,
'TC_G3_X3_1':0.0,
'TC_G3_X0_2':0.0,
'TC_G3_X1_2':0.0,
'TC_G3_X2_2':0.0,
'TC_G3_X3_2':0.0
}
# curve fit the data for gyro 3 corrections
if num_gyros >= 4 and not math.isnan(sensor_gyro_3['temperature'][0]):
gyro_3_params['TC_G3_ID'] = int(np.median(sensor_gyro_3['device_id']))
# find the min, max and reference temperature
gyro_3_params['TC_G3_TMIN'] = np.amin(sensor_gyro_3['temperature'])
gyro_3_params['TC_G3_TMAX'] = np.amax(sensor_gyro_3['temperature'])
gyro_3_params['TC_G3_TREF'] = 0.5 * (gyro_3_params['TC_G3_TMIN'] + gyro_3_params['TC_G3_TMAX'])
temp_rel = sensor_gyro_3['temperature'] - gyro_3_params['TC_G3_TREF']
temp_rel_resample = np.linspace(gyro_3_params['TC_G3_TMIN']-gyro_3_params['TC_G3_TREF'], gyro_3_params['TC_G3_TMAX']-gyro_3_params['TC_G3_TREF'], 100)
temp_resample = temp_rel_resample + gyro_3_params['TC_G3_TREF']
# fit X axis
coef_gyro_3_x = np.polyfit(temp_rel,sensor_gyro_3['x'],3)
gyro_3_params['TC_G3_X3_0'] = coef_gyro_3_x[0]
gyro_3_params['TC_G3_X2_0'] = coef_gyro_3_x[1]
gyro_3_params['TC_G3_X1_0'] = coef_gyro_3_x[2]
gyro_3_params['TC_G3_X0_0'] = coef_gyro_3_x[3]
fit_coef_gyro_3_x = np.poly1d(coef_gyro_3_x)
gyro_3_x_resample = fit_coef_gyro_3_x(temp_rel_resample)
# fit Y axis
coef_gyro_3_y = np.polyfit(temp_rel,sensor_gyro_3['y'],3)
gyro_3_params['TC_G3_X3_1'] = coef_gyro_3_y[0]
gyro_3_params['TC_G3_X2_1'] = coef_gyro_3_y[1]
gyro_3_params['TC_G3_X1_1'] = coef_gyro_3_y[2]
gyro_3_params['TC_G3_X0_1'] = coef_gyro_3_y[3]
fit_coef_gyro_3_y = np.poly1d(coef_gyro_3_y)
gyro_3_y_resample = fit_coef_gyro_3_y(temp_rel_resample)
# fit Z axis
coef_gyro_3_z = np.polyfit(temp_rel,sensor_gyro_3['z'],3)
gyro_3_params['TC_G3_X3_2'] = coef_gyro_3_z[0]
gyro_3_params['TC_G3_X2_2'] = coef_gyro_3_z[1]
gyro_3_params['TC_G3_X1_2'] = coef_gyro_3_z[2]
gyro_3_params['TC_G3_X0_2'] = coef_gyro_3_z[3]
fit_coef_gyro_3_z = np.poly1d(coef_gyro_3_z)
gyro_3_z_resample = fit_coef_gyro_3_z(temp_rel_resample)
# gyro3 vs temperature
plt.figure(4,figsize=(20,13))
# draw plots
plt.subplot(3,1,1)
plt.plot(sensor_gyro_3['temperature'],sensor_gyro_3['x'],'b')
plt.plot(temp_resample,gyro_3_x_resample,'r')
plt.title('Gyro 2 ({}) Bias vs Temperature'.format(gyro_3_params['TC_G3_ID']))
plt.ylabel('X bias (rad/s)')
plt.xlabel('temperature (degC)')
plt.grid()
# draw plots
plt.subplot(3,1,2)
plt.plot(sensor_gyro_3['temperature'],sensor_gyro_3['y'],'b')
plt.plot(temp_resample,gyro_3_y_resample,'r')
plt.ylabel('Y bias (rad/s)')
plt.xlabel('temperature (degC)')
plt.grid()
# draw plots
plt.subplot(3,1,3)
plt.plot(sensor_gyro_3['temperature'],sensor_gyro_3['z'],'b')
plt.plot(temp_resample,gyro_3_z_resample,'r')
plt.ylabel('Z bias (rad/s)')
plt.xlabel('temperature (degC)')
plt.grid()
pp.savefig()
#################################################################################
#################################################################################
# define data dictionary of accel 0 thermal correction parameters
accel_0_params = {
'TC_A0_ID':0,
@@ -542,7 +647,7 @@ if num_accels >= 1 and not math.isnan(sensor_accel_0['temperature'][0]):
correction_z_resample = fit_coef_accel_0_z(temp_rel_resample)
# accel 0 vs temperature
plt.figure(4,figsize=(20,13))
plt.figure(5,figsize=(20,13))
# draw plots
plt.subplot(3,1,1)
@@ -653,7 +758,7 @@ if num_accels >= 2 and not math.isnan(sensor_accel_1['temperature'][0]):
correction_z_resample = fit_coef_accel_1_z(temp_rel_resample)
# accel 1 vs temperature
plt.figure(5,figsize=(20,13))
plt.figure(6,figsize=(20,13))
# draw plots
plt.subplot(3,1,1)
@@ -765,7 +870,7 @@ if num_accels >= 3 and not math.isnan(sensor_accel_2['temperature'][0]):
correction_z_resample = fit_coef_accel_2_z(temp_rel_resample)
# accel 2 vs temperature
plt.figure(6,figsize=(20,13))
plt.figure(7,figsize=(20,13))
# draw plots
plt.subplot(3,1,1)
@@ -798,6 +903,102 @@ if num_accels >= 3 and not math.isnan(sensor_accel_2['temperature'][0]):
#################################################################################
# define data dictionary of accel 3 thermal correction parameters
accel_3_params = {
'TC_A3_ID':0,
'TC_A3_TMIN':0.0,
'TC_A3_TMAX':0.0,
'TC_A3_TREF':0.0,
'TC_A3_X0_0':0.0,
'TC_A3_X1_0':0.0,
'TC_A3_X2_0':0.0,
'TC_A3_X3_0':0.0,
'TC_A3_X0_1':0.0,
'TC_A3_X1_1':0.0,
'TC_A3_X2_1':0.0,
'TC_A3_X3_1':0.0,
'TC_A3_X0_2':0.0,
'TC_A3_X1_2':0.0,
'TC_A3_X2_2':0.0,
'TC_A3_X3_2':0.0
}
# curve fit the data for accel 2 corrections
if num_accels >= 4 and not math.isnan(sensor_accel_3['temperature'][0]):
accel_3_params['TC_A3_ID'] = int(np.median(sensor_accel_3['device_id']))
# find the min, max and reference temperature
accel_3_params['TC_A3_TMIN'] = np.amin(sensor_accel_3['temperature'])
accel_3_params['TC_A3_TMAX'] = np.amax(sensor_accel_3['temperature'])
accel_3_params['TC_A3_TREF'] = 0.5 * (accel_3_params['TC_A3_TMIN'] + accel_3_params['TC_A3_TMAX'])
temp_rel = sensor_accel_3['temperature'] - accel_3_params['TC_A3_TREF']
temp_rel_resample = np.linspace(accel_3_params['TC_A3_TMIN']-accel_3_params['TC_A3_TREF'], accel_3_params['TC_A3_TMAX']-accel_3_params['TC_A3_TREF'], 100)
temp_resample = temp_rel_resample + accel_3_params['TC_A3_TREF']
# fit X axis
correction_x = sensor_accel_3['x']-np.median(sensor_accel_3['x'])
coef_accel_3_x = np.polyfit(temp_rel,correction_x,3)
accel_3_params['TC_A3_X3_0'] = coef_accel_3_x[0]
accel_3_params['TC_A3_X2_0'] = coef_accel_3_x[1]
accel_3_params['TC_A3_X1_0'] = coef_accel_3_x[2]
accel_3_params['TC_A3_X0_0'] = coef_accel_3_x[3]
fit_coef_accel_3_x = np.poly1d(coef_accel_3_x)
correction_x_resample = fit_coef_accel_3_x(temp_rel_resample)
# fit Y axis
correction_y = sensor_accel_3['y']-np.median(sensor_accel_3['y'])
coef_accel_3_y = np.polyfit(temp_rel,correction_y,3)
accel_3_params['TC_A3_X3_1'] = coef_accel_3_y[0]
accel_3_params['TC_A3_X2_1'] = coef_accel_3_y[1]
accel_3_params['TC_A3_X1_1'] = coef_accel_3_y[2]
accel_3_params['TC_A3_X0_1'] = coef_accel_3_y[3]
fit_coef_accel_3_y = np.poly1d(coef_accel_3_y)
correction_y_resample = fit_coef_accel_3_y(temp_rel_resample)
# fit Z axis
correction_z = sensor_accel_3['z']-np.median(sensor_accel_3['z'])
coef_accel_3_z = np.polyfit(temp_rel,correction_z,3)
accel_3_params['TC_A3_X3_2'] = coef_accel_3_z[0]
accel_3_params['TC_A3_X2_2'] = coef_accel_3_z[1]
accel_3_params['TC_A3_X1_2'] = coef_accel_3_z[2]
accel_3_params['TC_A3_X0_2'] = coef_accel_3_z[3]
fit_coef_accel_3_z = np.poly1d(coef_accel_3_z)
correction_z_resample = fit_coef_accel_3_z(temp_rel_resample)
# accel 3 vs temperature
plt.figure(8,figsize=(20,13))
# draw plots
plt.subplot(3,1,1)
plt.plot(sensor_accel_3['temperature'],correction_x,'b')
plt.plot(temp_resample,correction_x_resample,'r')
plt.title('Accel 3 ({}) Bias vs Temperature'.format(accel_3_params['TC_A3_ID']))
plt.ylabel('X bias (m/s/s)')
plt.xlabel('temperature (degC)')
plt.grid()
# draw plots
plt.subplot(3,1,2)
plt.plot(sensor_accel_3['temperature'],correction_y,'b')
plt.plot(temp_resample,correction_y_resample,'r')
plt.ylabel('Y bias (m/s/s)')
plt.xlabel('temperature (degC)')
plt.grid()
# draw plots
plt.subplot(3,1,3)
plt.plot(sensor_accel_3['temperature'],correction_z,'b')
plt.plot(temp_resample,correction_z_resample,'r')
plt.ylabel('Z bias (m/s/s)')
plt.xlabel('temperature (degC)')
plt.grid()
pp.savefig()
#################################################################################
#################################################################################
# define data dictionary of baro 0 thermal correction parameters
baro_0_params = {
'TC_B0_ID':0,
@@ -841,7 +1042,7 @@ fit_coef_baro_0_x = np.poly1d(coef_baro_0_x)
baro_0_x_resample = fit_coef_baro_0_x(temp_rel_resample)
# baro 0 vs temperature
plt.figure(7,figsize=(20,13))
plt.figure(9,figsize=(20,13))
# draw plots
plt.plot(sensor_baro_0['temperature'],100*sensor_baro_0['pressure']-100*median_pressure,'b')
@@ -897,8 +1098,8 @@ if num_baros >= 2:
fit_coef_baro_1_x = np.poly1d(coef_baro_1_x)
baro_1_x_resample = fit_coef_baro_1_x(temp_rel_resample)
# baro 1 vs temperature
plt.figure(8,figsize=(20,13))
# baro 2 vs temperature
plt.figure(9,figsize=(20,13))
# draw plots
plt.plot(sensor_baro_1['temperature'],100*sensor_baro_1['pressure']-100*median_pressure,'b')
@@ -910,7 +1111,7 @@ if num_baros >= 2:
pp.savefig()
# define data dictionary of baro 1 thermal correction parameters
# define data dictionary of baro 2 thermal correction parameters
baro_2_params = {
'TC_B2_ID':0,
'TC_B2_TMIN':0.0,
@@ -956,7 +1157,7 @@ if num_baros >= 3:
baro_2_x_resample = fit_coef_baro_2_x(temp_rel_resample)
# baro 2 vs temperature
plt.figure(8,figsize=(20,13))
plt.figure(10,figsize=(20,13))
# draw plots
plt.plot(sensor_baro_2['temperature'],100*sensor_baro_2['pressure']-100*median_pressure,'b')
@@ -968,6 +1169,59 @@ if num_baros >= 3:
pp.savefig()
# define data dictionary of baro 3 thermal correction parameters
baro_3_params = {
'TC_B3_ID':0,
'TC_B3_TMIN':0.0,
'TC_B3_TMAX':0.0,
'TC_B3_TREF':0.0,
'TC_B3_X0':0.0,
'TC_B3_X1':0.0,
'TC_B3_X2':0.0,
'TC_B3_X3':0.0,
'TC_B3_X4':0.0,
'TC_B3_X5':0.0,
'TC_B3_SCL':1.0,
}
if num_baros >= 4:
# curve fit the data for baro 2 corrections
baro_3_params['TC_B3_ID'] = int(np.median(sensor_baro_3['device_id']))
# find the min, max and reference temperature
baro_3_params['TC_B3_TMIN'] = np.amin(sensor_baro_3['temperature'])
baro_3_params['TC_B3_TMAX'] = np.amax(sensor_baro_3['temperature'])
baro_3_params['TC_B3_TREF'] = 0.5 * (baro_3_params['TC_B3_TMIN'] + baro_3_params['TC_B3_TMAX'])
temp_rel = sensor_baro_3['temperature'] - baro_3_params['TC_B3_TREF']
temp_rel_resample = np.linspace(baro_3_params['TC_B3_TMIN']-baro_3_params['TC_B3_TREF'], baro_3_params['TC_B3_TMAX']-baro_3_params['TC_B3_TREF'], 100)
temp_resample = temp_rel_resample + baro_3_params['TC_B3_TREF']
# fit data
median_pressure = np.median(sensor_baro_3['pressure'])
coef_baro_3_x = np.polyfit(temp_rel,100*(sensor_baro_3['pressure']-median_pressure),5) # convert from hPa to Pa
baro_3_params['TC_B3_X5'] = coef_baro_3_x[0]
baro_3_params['TC_B3_X4'] = coef_baro_3_x[1]
baro_3_params['TC_B3_X3'] = coef_baro_3_x[2]
baro_3_params['TC_B3_X2'] = coef_baro_3_x[3]
baro_3_params['TC_B3_X1'] = coef_baro_3_x[4]
baro_3_params['TC_B3_X0'] = coef_baro_3_x[5]
fit_coef_baro_3_x = np.poly1d(coef_baro_3_x)
baro_3_x_resample = fit_coef_baro_3_x(temp_rel_resample)
# baro 3 vs temperature
plt.figure(11,figsize=(20,13))
# draw plots
plt.plot(sensor_baro_3['temperature'],100*sensor_baro_3['pressure']-100*median_pressure,'b')
plt.plot(temp_resample,baro_3_x_resample,'r')
plt.title('Baro 3 ({}) Bias vs Temperature'.format(baro_3_params['TC_B3_ID']))
plt.ylabel('Z bias (Pa)')
plt.xlabel('temperature (degC)')
plt.grid()
pp.savefig()
#################################################################################
# close the pdf file
@@ -1013,6 +1267,16 @@ for key in key_list_accel:
type = "9"
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(accel_2_params[key])+"\t"+type+"\n")
# accel 3 corrections
key_list_accel = list(accel_3_params.keys())
key_list_accel.sort
for key in key_list_accel:
if key == 'TC_A3_ID':
type = "6"
else:
type = "9"
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(accel_3_params[key])+"\t"+type+"\n")
# baro 0 corrections
key_list_baro = list(baro_0_params.keys())
key_list_baro.sort
@@ -1043,6 +1307,17 @@ for key in key_list_baro:
type = "9"
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(baro_2_params[key])+"\t"+type+"\n")
# baro 3 corrections
key_list_baro = list(baro_3_params.keys())
key_list_baro.sort
for key in key_list_baro:
if key == 'TC_B3_ID':
type = "6"
else:
type = "9"
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(baro_3_params[key])+"\t"+type+"\n")
# gyro 0 corrections
key_list_gyro = list(gyro_0_params.keys())
key_list_gyro.sort()
@@ -1073,6 +1348,16 @@ for key in key_list_gyro:
type = "9"
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(gyro_2_params[key])+"\t"+type+"\n")
# gyro 3 corrections
key_list_gyro = list(gyro_3_params.keys())
key_list_gyro.sort()
for key in key_list_gyro:
if key == 'TC_G3_ID':
type = "6"
else:
type = "9"
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(gyro_3_params[key])+"\t"+type+"\n")
file.close()
print('Correction parameters written to ' + test_results_filename)