dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Add second barometer to thermal compensation fitting script

This commit is contained in:
Mohammed Kabir 2017-06-16 13:08:31 +05:30 committed by Kabir Mohammed
parent fd47e0cbb3
commit 4d6c1b5749
1 changed files with 70 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
Tools/process_sensor_caldata.py
View File

@ -88,17 +88,17 @@ for d in data:
# extract baro data
sensor_instance = 0
num_baros = 0
for d in data:
if d.name == 'sensor_baro':
if sensor_instance == 0:
sensor_baro_0 = d.data
print('found baro 0 data')
num_baros = 1
if sensor_instance == 1:
sensor_baro_1 = d.data
print('found baro 1 data')
if sensor_instance == 2:
sensor_baro_2 = d.data
print('found baro 2 data')
num_baros = 2
sensor_instance = sensor_instance +1
# open file to save plots to PDF
@ -738,12 +738,65 @@ plt.figure(7,figsize=(20,13))
plt.plot(sensor_baro_0['temperature'],100*sensor_baro_0['pressure']-100*median_pressure,'b')
plt.plot(temp_resample,baro_0_x_resample,'r')
plt.title('Baro 0 Bias vs Temperature')
plt.ylabel('X bias (Pa)')
plt.ylabel('Z bias (Pa)')
plt.xlabel('temperature (degC)')
plt.grid()
pp.savefig()
# define data dictionary of baro 1 thermal correction parameters
baro_1_params = {
'TC_B1_ID':0,
'TC_B1_TMIN':0.0,
'TC_B1_TMAX':0.0,
'TC_B1_TREF':0.0,
'TC_B1_X0':0.0,
'TC_B1_X1':0.0,
'TC_B1_X2':0.0,
'TC_B1_X3':0.0,
'TC_B1_X4':0.0,
'TC_B1_X5':0.0,
'TC_B1_SCL':1.0,
}
if num_baros >= 2:
# curve fit the data for baro 0 corrections
baro_1_params['TC_B1_ID'] = int(np.median(sensor_baro_1['device_id']))
# find the min, max and reference temperature
baro_1_params['TC_B1_TMIN'] = np.amin(sensor_baro_1['temperature'])
baro_1_params['TC_B1_TMAX'] = np.amax(sensor_baro_1['temperature'])
baro_1_params['TC_B1_TREF'] = 0.5 * (baro_1_params['TC_B1_TMIN'] + baro_1_params['TC_B1_TMAX'])
temp_rel = sensor_baro_1['temperature'] - baro_1_params['TC_B1_TREF']
temp_rel_resample = np.linspace(baro_1_params['TC_B1_TMIN']-baro_1_params['TC_B1_TREF'], baro_1_params['TC_B1_TMAX']-baro_1_params['TC_B1_TREF'], 100)
temp_resample = temp_rel_resample + baro_1_params['TC_B1_TREF']
# fit data
median_pressure = np.median(sensor_baro_1['pressure']);
coef_baro_1_x = np.polyfit(temp_rel,100*(sensor_baro_1['pressure']-median_pressure),5) # convert from hPa to Pa
baro_1_params['TC_B1_X5'] = coef_baro_1_x[0]
baro_1_params['TC_B1_X4'] = coef_baro_1_x[1]
baro_1_params['TC_B1_X3'] = coef_baro_1_x[2]
baro_1_params['TC_B1_X2'] = coef_baro_1_x[3]
baro_1_params['TC_B1_X1'] = coef_baro_1_x[4]
baro_1_params['TC_B1_X0'] = coef_baro_1_x[5]
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))
# draw plots
plt.plot(sensor_baro_1['temperature'],100*sensor_baro_1['pressure']-100*median_pressure,'b')
plt.plot(temp_resample,baro_1_x_resample,'r')
plt.title('Baro 1 Bias vs Temperature')
plt.ylabel('Z bias (Pa)')
plt.xlabel('temperature (degC)')
plt.grid()
pp.savefig()
#################################################################################
# close the pdf file
@ -790,15 +843,25 @@ for key in key_list_accel:
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(accel_2_params[key])+"\t"+type+"\n")
# baro 0 corrections
key_list_accel = list(baro_0_params.keys())
key_list_accel.sort
for key in key_list_accel:
key_list_baro = list(baro_0_params.keys())
key_list_baro.sort
for key in key_list_baro:
if key == 'TC_B0_ID':
type = "6"
else:
type = "9"
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(baro_0_params[key])+"\t"+type+"\n")
# baro 1 corrections
key_list_baro = list(baro_1_params.keys())
key_list_baro.sort
for key in key_list_baro:
if key == 'TC_B1_ID':
type = "6"
else:
type = "9"
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(baro_1_params[key])+"\t"+type+"\n")
# gyro 0 corrections
key_list_gyro = list(gyro_0_params.keys())
key_list_gyro.sort()