Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Offboard temp cal: check topic instance #14897

Merged
merged 2 commits into from
May 14, 2020
Merged

Offboard temp cal: check topic instance #14897

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
7c8df20
Offboard temp cal: check topic instance
bresch May 13, 2020
f938b17
Offboard temp cal: add support for 3rd baro
bresch May 13, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
107 changes: 84 additions & 23 deletions Tools/process_sensor_caldata.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,57 +48,55 @@ def is_valid_directory(parser, arg):
data = ulog.data_list

# extract gyro data
sensor_instance = 0
num_gyros = 0
for d in data:
if d.name == 'sensor_gyro':
if sensor_instance == 0:
if d.multi_id == 0:
sensor_gyro_0 = d.data
print('found gyro 0 data')
num_gyros = 1
if sensor_instance == 1:
num_gyros += 1
elif d.multi_id == 1:
sensor_gyro_1 = d.data
print('found gyro 1 data')
num_gyros = 2
if sensor_instance == 2:
num_gyros += 1
elif d.multi_id == 2:
sensor_gyro_2 = d.data
print('found gyro 2 data')
num_gyros = 3
sensor_instance = sensor_instance +1
num_gyros += 1

# extract accel data
sensor_instance = 0
num_accels = 0
for d in data:
if d.name == 'sensor_accel':
if sensor_instance == 0:
if d.multi_id == 0:
sensor_accel_0 = d.data
print('found accel 0 data')
num_accels = 1
if sensor_instance == 1:
num_accels += 1
elif d.multi_id == 1:
sensor_accel_1 = d.data
print('found accel 1 data')
num_accels = 2
if sensor_instance == 2:
num_accels += 1
elif d.multi_id == 2:
sensor_accel_2 = d.data
print('found accel 2 data')
num_accels = 3
sensor_instance = sensor_instance +1
num_accels += 1

# extract baro data
sensor_instance = 0
num_baros = 0
for d in data:
if d.name == 'sensor_baro':
if sensor_instance == 0:
if d.multi_id == 0:
sensor_baro_0 = d.data
print('found baro 0 data')
num_baros = 1
if sensor_instance == 1:
num_baros += 1
elif d.multi_id == 1:
sensor_baro_1 = d.data
print('found baro 1 data')
num_baros = 2
sensor_instance = sensor_instance +1
num_baros += 1
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You might as well add sensor_baro 2. We're already carrying the parameters. https://github.com/PX4/Firmware/blob/c96b5246ffc50eeceef5aad0a31068a00f2b672b/src/modules/temperature_compensation/temp_comp_params_baro.c#L233-L319

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I added that in f938b17 . I followed the copy-paste pattern...

elif d.multi_id == 2:
sensor_baro_2 = d.data
print('found baro 2 data')
num_baros += 1

# open file to save plots to PDF
from matplotlib.backends.backend_pdf import PdfPages
Expand Down Expand Up @@ -760,7 +758,7 @@ def is_valid_directory(parser, arg):

if num_baros >= 2:

# curve fit the data for baro 0 corrections
# curve fit the data for baro 1 corrections
baro_1_params['TC_B1_ID'] = int(np.median(sensor_baro_1['device_id']))

# find the min, max and reference temperature
Expand Down Expand Up @@ -796,6 +794,59 @@ def is_valid_directory(parser, arg):

pp.savefig()

# define data dictionary of baro 1 thermal correction parameters
baro_2_params = {
'TC_B2_ID':0,
'TC_B2_TMIN':0.0,
'TC_B2_TMAX':0.0,
'TC_B2_TREF':0.0,
'TC_B2_X0':0.0,
'TC_B2_X1':0.0,
'TC_B2_X2':0.0,
'TC_B2_X3':0.0,
'TC_B2_X4':0.0,
'TC_B2_X5':0.0,
'TC_B2_SCL':1.0,
}

if num_baros >= 3:

# curve fit the data for baro 2 corrections
baro_2_params['TC_B2_ID'] = int(np.median(sensor_baro_2['device_id']))

# find the min, max and reference temperature
baro_2_params['TC_B2_TMIN'] = np.amin(sensor_baro_2['temperature'])
baro_2_params['TC_B2_TMAX'] = np.amax(sensor_baro_2['temperature'])
baro_2_params['TC_B2_TREF'] = 0.5 * (baro_2_params['TC_B2_TMIN'] + baro_2_params['TC_B2_TMAX'])
temp_rel = sensor_baro_2['temperature'] - baro_2_params['TC_B2_TREF']
temp_rel_resample = np.linspace(baro_2_params['TC_B2_TMIN']-baro_2_params['TC_B2_TREF'], baro_2_params['TC_B2_TMAX']-baro_2_params['TC_B2_TREF'], 100)
temp_resample = temp_rel_resample + baro_2_params['TC_B2_TREF']

# fit data
median_pressure = np.median(sensor_baro_2['pressure']);
coef_baro_2_x = np.polyfit(temp_rel,100*(sensor_baro_2['pressure']-median_pressure),5) # convert from hPa to Pa
baro_2_params['TC_B2_X5'] = coef_baro_2_x[0]
baro_2_params['TC_B2_X4'] = coef_baro_2_x[1]
baro_2_params['TC_B2_X3'] = coef_baro_2_x[2]
baro_2_params['TC_B2_X2'] = coef_baro_2_x[3]
baro_2_params['TC_B2_X1'] = coef_baro_2_x[4]
baro_2_params['TC_B2_X0'] = coef_baro_2_x[5]
fit_coef_baro_2_x = np.poly1d(coef_baro_2_x)
baro_2_x_resample = fit_coef_baro_2_x(temp_rel_resample)

# baro 2 vs temperature
plt.figure(8,figsize=(20,13))

# draw plots
plt.plot(sensor_baro_2['temperature'],100*sensor_baro_2['pressure']-100*median_pressure,'b')
plt.plot(temp_resample,baro_2_x_resample,'r')
plt.title('Baro 2 Bias vs Temperature')
plt.ylabel('Z bias (Pa)')
plt.xlabel('temperature (degC)')
plt.grid()

pp.savefig()

#################################################################################

# close the pdf file
Expand Down Expand Up @@ -861,6 +912,16 @@ def is_valid_directory(parser, arg):
type = "9"
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(baro_1_params[key])+"\t"+type+"\n")

# baro 2 corrections
key_list_baro = list(baro_2_params.keys())
key_list_baro.sort
for key in key_list_baro:
if key == 'TC_B2_ID':
type = "6"
else:
type = "9"
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(baro_2_params[key])+"\t"+type+"\n")

# gyro 0 corrections
key_list_gyro = list(gyro_0_params.keys())
key_list_gyro.sort()
Expand Down