diff --git a/Tools/.gitignore b/Tools/.gitignore index 7628bda821..1f1adab2d2 100644 --- a/Tools/.gitignore +++ b/Tools/.gitignore @@ -1,2 +1,3 @@ parameters.wiki parameters.xml +*.pdf diff --git a/Tools/process_sensor_caldata.py b/Tools/process_sensor_caldata.py index cdae5468dd..4ebf2e1a29 100644 --- a/Tools/process_sensor_caldata.py +++ b/Tools/process_sensor_caldata.py @@ -1,4 +1,29 @@ #! /usr/bin/env python +""" +Reads in IMU data from a static thermal calibration test and performs +a curve fit of gyro, accel and baro bias vs temperature +Data can be gathered using the following sequence: + +1) Set the TC_A_ENABLE, TC_B_ENABLE and TC_G_ENABLE parameters to 0 to + thermal compensation and reboot +2) Perform a gyro and accel cal +2) Set the SYS_LOGGER parameter to 1 to use the new system logger +3) Set the SDLOG_MODE parameter to 3 to enable logging of sensor data + for calibration and power off +4) Cold soak the board for 30 minutes +5) Move to a warm dry environment. +6) Apply power for 45 minutes, keeping the board still. +7) Remove power and extract the .ulog file +8) Open a terminal window in the script file directory +9) Run the script file 'python process_sensor_caldata.py + + +Outputs thermal compensation parameters in a file named + .params which can be loaded onto the + board using QGroundControl +Outputs summary plots in a pdf file named .pdf + +""" from __future__ import print_function @@ -7,815 +32,174 @@ import os import matplotlib.pyplot as plt import numpy as np -from pyulog import * - -""" -Reads in IMU data from a static thermal calibration test and performs a curve fit of gyro, accel and baro bias vs temperature -Data can be gathered using the following sequence: - -1) Set the TC_A_ENABLE, TC_B_ENABLE and TC_G_ENABLE parameters to 0 to thermal compensation and reboot -2) Perform a gyro and accel cal -2) Set the SYS_LOGGER parameter to 1 to use the new system logger -3) Set the SDLOG_MODE parameter to 3 to enable logging of sensor data for calibration and power off -4) Cold soak the board for 30 minutes -5) Move to a warm dry environment. -6) Apply power for 45 minutes, keeping the board still. -7) Remove power and extract the .ulog file -8) Open a terminal window in the script file directory -9) Run the script file 'python process_sensor_caldata.py - -Outputs thermal compensation parameters in a file named .params which can be loaded onto the board using QGroundControl -Outputs summary plots in a pdf file named .pdf - -""" - -parser = argparse.ArgumentParser(description='Analyse the sensor_gyro message data') -parser.add_argument('filename', metavar='file.ulg', help='ULog input file') - -def is_valid_directory(parser, arg): - if os.path.isdir(arg): - # Directory exists so return the directory - return arg - else: - parser.error('The directory {} does not exist'.format(arg)) - -args = parser.parse_args() -ulog_file_name = args.filename - -ulog = ULog(ulog_file_name, None) -data = ulog.data_list - -# define constants -gravity = 9.80665 - -# extract gyro data -sensor_instance = 0 -for d in data: - if d.name == 'sensor_gyro': - if sensor_instance == 0: - sensor_gyro_0 = d.data - print('found gyro 0 data') - if sensor_instance == 1: - sensor_gyro_1 = d.data - print('found gyro 1 data') - if sensor_instance == 2: - sensor_gyro_2 = d.data - print('found gyro 2 data') - sensor_instance = sensor_instance +1 - -# extract accel data -sensor_instance = 0 -for d in data: - if d.name == 'sensor_accel': - if sensor_instance == 0: - sensor_accel_0 = d.data - print('found accel 0 data') - if sensor_instance == 1: - sensor_accel_1 = d.data - print('found accel 1 data') - if sensor_instance == 2: - sensor_accel_2 = d.data - print('found accel 2 data') - sensor_instance = sensor_instance +1 - -# extract baro data -sensor_instance = 0 -for d in data: - if d.name == 'sensor_baro': - if sensor_instance == 0: - sensor_baro_0 = d.data - print('found baro 0 data') - 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') - sensor_instance = sensor_instance +1 - -# open file to save plots to PDF -from matplotlib.backends.backend_pdf import PdfPages -output_plot_filename = ulog_file_name + ".pdf" -pp = PdfPages(output_plot_filename) - -################################################################################# - -# define data dictionary of gyro 0 thermal correction parameters -gyro_0_params = { -'TC_G0_ID':0, -'TC_G0_TMIN':0.0, -'TC_G0_TMAX':0.0, -'TC_G0_TREF':0.0, -'TC_G0_X0_0':0.0, -'TC_G0_X1_0':0.0, -'TC_G0_X2_0':0.0, -'TC_G0_X3_0':0.0, -'TC_G0_X0_1':0.0, -'TC_G0_X1_1':0.0, -'TC_G0_X2_1':0.0, -'TC_G0_X3_1':0.0, -'TC_G0_X0_2':0.0, -'TC_G0_X1_2':0.0, -'TC_G0_X2_2':0.0, -'TC_G0_X3_2':0.0, -'TC_G0_SCL_0':1.0, -'TC_G0_SCL_1':1.0, -'TC_G0_SCL_2':1.0 -} - -# curve fit the data for gyro 0 corrections - note corrections have oppsite sign to sensor bias -gyro_0_params['TC_G0_ID'] = int(np.median(sensor_gyro_0['device_id'])) - -# find the min, max and reference temperature -gyro_0_params['TC_G0_TMIN'] = np.amin(sensor_gyro_0['temperature']) -gyro_0_params['TC_G0_TMAX'] = np.amax(sensor_gyro_0['temperature']) -gyro_0_params['TC_G0_TREF'] = 0.5 * (gyro_0_params['TC_G0_TMIN'] + gyro_0_params['TC_G0_TMAX']) -temp_rel = sensor_gyro_0['temperature'] - gyro_0_params['TC_G0_TREF'] -temp_rel_resample = np.linspace(gyro_0_params['TC_G0_TMIN']-gyro_0_params['TC_G0_TREF'], gyro_0_params['TC_G0_TMAX']-gyro_0_params['TC_G0_TREF'], 100) -temp_resample = temp_rel_resample + gyro_0_params['TC_G0_TREF'] - -# fit X axis -coef_gyro_0_x = np.polyfit(temp_rel,-sensor_gyro_0['x'],3) -gyro_0_params['TC_G0_X3_0'] = coef_gyro_0_x[0] -gyro_0_params['TC_G0_X2_0'] = coef_gyro_0_x[1] -gyro_0_params['TC_G0_X1_0'] = coef_gyro_0_x[2] -gyro_0_params['TC_G0_X0_0'] = coef_gyro_0_x[3] -fit_coef_gyro_0_x = np.poly1d(coef_gyro_0_x) -gyro_0_x_resample = fit_coef_gyro_0_x(temp_rel_resample) - -# fit Y axis -coef_gyro_0_y = np.polyfit(temp_rel,-sensor_gyro_0['y'],3) -gyro_0_params['TC_G0_X3_1'] = coef_gyro_0_y[0] -gyro_0_params['TC_G0_X2_1'] = coef_gyro_0_y[1] -gyro_0_params['TC_G0_X1_1'] = coef_gyro_0_y[2] -gyro_0_params['TC_G0_X0_1'] = coef_gyro_0_y[3] -fit_coef_gyro_0_y = np.poly1d(coef_gyro_0_y) -gyro_0_y_resample = fit_coef_gyro_0_y(temp_rel_resample) - -# fit Z axis -coef_gyro_0_z = np.polyfit(temp_rel,-sensor_gyro_0['z'],3) -gyro_0_params['TC_G0_X3_2'] = coef_gyro_0_z[0] -gyro_0_params['TC_G0_X2_2'] = coef_gyro_0_z[1] -gyro_0_params['TC_G0_X1_2'] = coef_gyro_0_z[2] -gyro_0_params['TC_G0_X0_2'] = coef_gyro_0_z[3] -fit_coef_gyro_0_z = np.poly1d(coef_gyro_0_z) -gyro_0_z_resample = fit_coef_gyro_0_z(temp_rel_resample) - -# gyro0 vs temperature -plt.figure(1,figsize=(20,13)) - -# draw plots -plt.subplot(3,1,1) -plt.plot(sensor_gyro_0['temperature'],sensor_gyro_0['x'],'b') -plt.plot(temp_resample,-gyro_0_x_resample,'r') -plt.title('Gyro 0 Bias vs Temperature') -plt.ylabel('X bias (rad/s)') -plt.xlabel('temperature (degC)') -plt.grid() - -# draw plots -plt.subplot(3,1,2) -plt.plot(sensor_gyro_0['temperature'],sensor_gyro_0['y'],'b') -plt.plot(temp_resample,-gyro_0_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_0['temperature'],sensor_gyro_0['z'],'b') -plt.plot(temp_resample,-gyro_0_z_resample,'r') -plt.ylabel('Z bias (rad/s)') -plt.xlabel('temperature (degC)') -plt.grid() - -pp.savefig() - -################################################################################# - -################################################################################# - -# define data dictionary of gyro 1 thermal correction parameters -gyro_1_params = { -'TC_G1_ID':0, -'TC_G1_TMIN':0.0, -'TC_G1_TMAX':0.0, -'TC_G1_TREF':0.0, -'TC_G1_X0_0':0.0, -'TC_G1_X1_0':0.0, -'TC_G1_X2_0':0.0, -'TC_G1_X3_0':0.0, -'TC_G1_X0_1':0.0, -'TC_G1_X1_1':0.0, -'TC_G1_X2_1':0.0, -'TC_G1_X3_1':0.0, -'TC_G1_X0_2':0.0, -'TC_G1_X1_2':0.0, -'TC_G1_X2_2':0.0, -'TC_G1_X3_2':0.0, -'TC_G1_SCL_0':1.0, -'TC_G1_SCL_1':1.0, -'TC_G1_SCL_2':1.0 -} - -# curve fit the data for gyro 1 corrections - note corrections have oppsite sign to sensor bias -gyro_1_params['TC_G1_ID'] = int(np.median(sensor_gyro_1['device_id'])) - -# find the min, max and reference temperature -gyro_1_params['TC_G1_TMIN'] = np.amin(sensor_gyro_1['temperature']) -gyro_1_params['TC_G1_TMAX'] = np.amax(sensor_gyro_1['temperature']) -gyro_1_params['TC_G1_TREF'] = 0.5 * (gyro_1_params['TC_G1_TMIN'] + gyro_1_params['TC_G1_TMAX']) -temp_rel = sensor_gyro_1['temperature'] - gyro_1_params['TC_G1_TREF'] -temp_rel_resample = np.linspace(gyro_1_params['TC_G1_TMIN']-gyro_1_params['TC_G1_TREF'], gyro_1_params['TC_G1_TMAX']-gyro_1_params['TC_G1_TREF'], 100) -temp_resample = temp_rel_resample + gyro_1_params['TC_G1_TREF'] - -# fit X axis -coef_gyro_1_x = np.polyfit(temp_rel,-sensor_gyro_1['x'],3) -gyro_1_params['TC_G1_X3_0'] = coef_gyro_1_x[0] -gyro_1_params['TC_G1_X2_0'] = coef_gyro_1_x[1] -gyro_1_params['TC_G1_X1_0'] = coef_gyro_1_x[2] -gyro_1_params['TC_G1_X0_0'] = coef_gyro_1_x[3] -fit_coef_gyro_1_x = np.poly1d(coef_gyro_1_x) -gyro_1_x_resample = fit_coef_gyro_1_x(temp_rel_resample) - -# fit Y axis -coef_gyro_1_y = np.polyfit(temp_rel,-sensor_gyro_1['y'],3) -gyro_1_params['TC_G1_X3_1'] = coef_gyro_1_y[0] -gyro_1_params['TC_G1_X2_1'] = coef_gyro_1_y[1] -gyro_1_params['TC_G1_X1_1'] = coef_gyro_1_y[2] -gyro_1_params['TC_G1_X0_1'] = coef_gyro_1_y[3] -fit_coef_gyro_1_y = np.poly1d(coef_gyro_1_y) -gyro_1_y_resample = fit_coef_gyro_1_y(temp_rel_resample) - -# fit Z axis -coef_gyro_1_z = np.polyfit(temp_rel,-sensor_gyro_1['z'],3) -gyro_1_params['TC_G1_X3_2'] = coef_gyro_1_z[0] -gyro_1_params['TC_G1_X2_2'] = coef_gyro_1_z[1] -gyro_1_params['TC_G1_X1_2'] = coef_gyro_1_z[2] -gyro_1_params['TC_G1_X0_2'] = coef_gyro_1_z[3] -fit_coef_gyro_1_z = np.poly1d(coef_gyro_1_z) -gyro_1_z_resample = fit_coef_gyro_1_z(temp_rel_resample) - -# gyro1 vs temperature -plt.figure(2,figsize=(20,13)) - -# draw plots -plt.subplot(3,1,1) -plt.plot(sensor_gyro_1['temperature'],sensor_gyro_1['x'],'b') -plt.plot(temp_resample,-gyro_1_x_resample,'r') -plt.title('Gyro 1 Bias vs Temperature') -plt.ylabel('X bias (rad/s)') -plt.xlabel('temperature (degC)') -plt.grid() - -# draw plots -plt.subplot(3,1,2) -plt.plot(sensor_gyro_1['temperature'],sensor_gyro_1['y'],'b') -plt.plot(temp_resample,-gyro_1_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_1['temperature'],sensor_gyro_1['z'],'b') -plt.plot(temp_resample,-gyro_1_z_resample,'r') -plt.ylabel('Z bias (rad/s)') -plt.xlabel('temperature (degC)') -plt.grid() - -pp.savefig() - -################################################################################# - -################################################################################# - -# define data dictionary of gyro 2 thermal correction parameters -gyro_2_params = { -'TC_G2_ID':0, -'TC_G2_TMIN':0.0, -'TC_G2_TMAX':0.0, -'TC_G2_TREF':0.0, -'TC_G2_X0_0':0.0, -'TC_G2_X1_0':0.0, -'TC_G2_X2_0':0.0, -'TC_G2_X3_0':0.0, -'TC_G2_X0_1':0.0, -'TC_G2_X1_1':0.0, -'TC_G2_X2_1':0.0, -'TC_G2_X3_1':0.0, -'TC_G2_X0_2':0.0, -'TC_G2_X1_2':0.0, -'TC_G2_X2_2':0.0, -'TC_G2_X3_2':0.0, -'TC_G2_SCL_0':1.0, -'TC_G2_SCL_1':1.0, -'TC_G2_SCL_2':1.0 -} - -# curve fit the data for gyro 2 corrections - note corrections have oppsite sign to sensor bias -gyro_2_params['TC_G2_ID'] = int(np.median(sensor_gyro_2['device_id'])) - -# find the min, max and reference temperature -gyro_2_params['TC_G2_TMIN'] = np.amin(sensor_gyro_2['temperature']) -gyro_2_params['TC_G2_TMAX'] = np.amax(sensor_gyro_2['temperature']) -gyro_2_params['TC_G2_TREF'] = 0.5 * (gyro_2_params['TC_G2_TMIN'] + gyro_2_params['TC_G2_TMAX']) -temp_rel = sensor_gyro_2['temperature'] - gyro_2_params['TC_G2_TREF'] -temp_rel_resample = np.linspace(gyro_2_params['TC_G2_TMIN']-gyro_2_params['TC_G2_TREF'], gyro_2_params['TC_G2_TMAX']-gyro_2_params['TC_G2_TREF'], 100) -temp_resample = temp_rel_resample + gyro_2_params['TC_G2_TREF'] - -# fit X axis -coef_gyro_2_x = np.polyfit(temp_rel,-sensor_gyro_2['x'],3) -gyro_2_params['TC_G2_X3_0'] = coef_gyro_2_x[0] -gyro_2_params['TC_G2_X2_0'] = coef_gyro_2_x[1] -gyro_2_params['TC_G2_X1_0'] = coef_gyro_2_x[2] -gyro_2_params['TC_G2_X0_0'] = coef_gyro_2_x[3] -fit_coef_gyro_2_x = np.poly1d(coef_gyro_2_x) -gyro_2_x_resample = fit_coef_gyro_2_x(temp_rel_resample) - -# fit Y axis -coef_gyro_2_y = np.polyfit(temp_rel,-sensor_gyro_2['y'],3) -gyro_2_params['TC_G2_X3_1'] = coef_gyro_2_y[0] -gyro_2_params['TC_G2_X2_1'] = coef_gyro_2_y[1] -gyro_2_params['TC_G2_X1_1'] = coef_gyro_2_y[2] -gyro_2_params['TC_G2_X0_1'] = coef_gyro_2_y[3] -fit_coef_gyro_2_y = np.poly1d(coef_gyro_2_y) -gyro_2_y_resample = fit_coef_gyro_2_y(temp_rel_resample) - -# fit Z axis -coef_gyro_2_z = np.polyfit(temp_rel,-sensor_gyro_2['z'],3) -gyro_2_params['TC_G2_X3_2'] = coef_gyro_2_z[0] -gyro_2_params['TC_G2_X2_2'] = coef_gyro_2_z[1] -gyro_2_params['TC_G2_X1_2'] = coef_gyro_2_z[2] -gyro_2_params['TC_G2_X0_2'] = coef_gyro_2_z[3] -fit_coef_gyro_2_z = np.poly1d(coef_gyro_2_z) -gyro_2_z_resample = fit_coef_gyro_2_z(temp_rel_resample) - -# gyro2 vs temperature -plt.figure(3,figsize=(20,13)) - -# draw plots -plt.subplot(3,1,1) -plt.plot(sensor_gyro_2['temperature'],sensor_gyro_2['x'],'b') -plt.plot(temp_resample,-gyro_2_x_resample,'r') -plt.title('Gyro 2 Bias vs Temperature') -plt.ylabel('X bias (rad/s)') -plt.xlabel('temperature (degC)') -plt.grid() - -# draw plots -plt.subplot(3,1,2) -plt.plot(sensor_gyro_2['temperature'],sensor_gyro_2['y'],'b') -plt.plot(temp_resample,-gyro_2_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_2['temperature'],sensor_gyro_2['z'],'b') -plt.plot(temp_resample,-gyro_2_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, -'TC_A0_TMIN':0.0, -'TC_A0_TMAX':0.0, -'TC_A0_TREF':0.0, -'TC_A0_X0_0':0.0, -'TC_A0_X1_0':0.0, -'TC_A0_X2_0':0.0, -'TC_A0_X3_0':0.0, -'TC_A0_X0_1':0.0, -'TC_A0_X1_1':0.0, -'TC_A0_X2_1':0.0, -'TC_A0_X3_1':0.0, -'TC_A0_X0_2':0.0, -'TC_A0_X1_2':0.0, -'TC_A0_X2_2':0.0, -'TC_A0_X3_2':0.0, -'TC_A0_SCL_0':1.0, -'TC_A0_SCL_1':1.0, -'TC_A0_SCL_2':1.0 -} - -# curve fit the data for accel 0 corrections - note corrections have oppsite sign to sensor bias -accel_0_params['TC_A0_ID'] = int(np.median(sensor_accel_0['device_id'])) - -# find the min, max and reference temperature -accel_0_params['TC_A0_TMIN'] = np.amin(sensor_accel_0['temperature']) -accel_0_params['TC_A0_TMAX'] = np.amax(sensor_accel_0['temperature']) -accel_0_params['TC_A0_TREF'] = 0.5 * (accel_0_params['TC_A0_TMIN'] + accel_0_params['TC_A0_TMAX']) -temp_rel = sensor_accel_0['temperature'] - accel_0_params['TC_A0_TREF'] -temp_rel_resample = np.linspace(accel_0_params['TC_A0_TMIN']-accel_0_params['TC_A0_TREF'], accel_0_params['TC_A0_TMAX']-accel_0_params['TC_A0_TREF'], 100) -temp_resample = temp_rel_resample + accel_0_params['TC_A0_TREF'] - -# fit X axis -correction_x = np.median(sensor_accel_0['x'])-sensor_accel_0['x'] -coef_accel_0_x = np.polyfit(temp_rel,correction_x,3) -accel_0_params['TC_A0_X3_0'] = coef_accel_0_x[0] -accel_0_params['TC_A0_X2_0'] = coef_accel_0_x[1] -accel_0_params['TC_A0_X1_0'] = coef_accel_0_x[2] -accel_0_params['TC_A0_X0_0'] = coef_accel_0_x[3] -fit_coef_accel_0_x = np.poly1d(coef_accel_0_x) -correction_x_resample = fit_coef_accel_0_x(temp_rel_resample) - -# fit Y axis -correction_y = np.median(sensor_accel_0['y'])-sensor_accel_0['y'] -coef_accel_0_y = np.polyfit(temp_rel,correction_y,3) -accel_0_params['TC_A0_X3_1'] = coef_accel_0_y[0] -accel_0_params['TC_A0_X2_1'] = coef_accel_0_y[1] -accel_0_params['TC_A0_X1_1'] = coef_accel_0_y[2] -accel_0_params['TC_A0_X0_1'] = coef_accel_0_y[3] -fit_coef_accel_0_y = np.poly1d(coef_accel_0_y) -correction_y_resample = fit_coef_accel_0_y(temp_rel_resample) - -# fit Z axis -correction_z = np.median(sensor_accel_0['z'])-sensor_accel_0['z'] -coef_accel_0_z = np.polyfit(temp_rel,correction_z,3) -accel_0_params['TC_A0_X3_2'] = coef_accel_0_z[0] -accel_0_params['TC_A0_X2_2'] = coef_accel_0_z[1] -accel_0_params['TC_A0_X1_2'] = coef_accel_0_z[2] -accel_0_params['TC_A0_X0_2'] = coef_accel_0_z[3] -fit_coef_accel_0_z = np.poly1d(coef_accel_0_z) -correction_z_resample = fit_coef_accel_0_z(temp_rel_resample) - -# accel 0 vs temperature -plt.figure(4,figsize=(20,13)) - -# draw plots -plt.subplot(3,1,1) -plt.plot(sensor_accel_0['temperature'],-correction_x,'b') -plt.plot(temp_resample,-correction_x_resample,'r') -plt.title('Accel 0 Bias vs Temperature') -plt.ylabel('X bias (m/s/s)') -plt.xlabel('temperature (degC)') -plt.grid() - -# draw plots -plt.subplot(3,1,2) -plt.plot(sensor_accel_0['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_0['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 accel 1 thermal correction parameters -accel_1_params = { -'TC_A1_ID':0, -'TC_A1_TMIN':0.0, -'TC_A1_TMAX':0.0, -'TC_A1_TREF':0.0, -'TC_A1_X0_0':0.0, -'TC_A1_X1_0':0.0, -'TC_A1_X2_0':0.0, -'TC_A1_X3_0':0.0, -'TC_A1_X0_1':0.0, -'TC_A1_X1_1':0.0, -'TC_A1_X2_1':0.0, -'TC_A1_X3_1':0.0, -'TC_A1_X0_2':0.0, -'TC_A1_X1_2':0.0, -'TC_A1_X2_2':0.0, -'TC_A1_X3_2':0.0, -'TC_A1_SCL_0':1.0, -'TC_A1_SCL_1':1.0, -'TC_A1_SCL_2':1.0 -} - -# curve fit the data for accel 1 corrections - note corrections have oppsite sign to sensor bias -accel_1_params['TC_A1_ID'] = int(np.median(sensor_accel_1['device_id'])) - -# find the min, max and reference temperature -accel_1_params['TC_A1_TMIN'] = np.amin(sensor_accel_1['temperature']) -accel_1_params['TC_A1_TMAX'] = np.amax(sensor_accel_1['temperature']) -accel_1_params['TC_A1_TREF'] = 0.5 * (accel_1_params['TC_A1_TMIN'] + accel_1_params['TC_A1_TMAX']) -temp_rel = sensor_accel_1['temperature'] - accel_1_params['TC_A1_TREF'] -temp_rel_resample = np.linspace(accel_1_params['TC_A1_TMIN']-accel_1_params['TC_A1_TREF'], accel_1_params['TC_A1_TMAX']-accel_1_params['TC_A1_TREF'], 100) -temp_resample = temp_rel_resample + accel_1_params['TC_A1_TREF'] - -# fit X axis -correction_x = np.median(sensor_accel_1['x'])-sensor_accel_1['x'] -coef_accel_1_x = np.polyfit(temp_rel,correction_x,3) -accel_1_params['TC_A1_X3_0'] = coef_accel_1_x[0] -accel_1_params['TC_A1_X2_0'] = coef_accel_1_x[1] -accel_1_params['TC_A1_X1_0'] = coef_accel_1_x[2] -accel_1_params['TC_A1_X0_0'] = coef_accel_1_x[3] -fit_coef_accel_1_x = np.poly1d(coef_accel_1_x) -correction_x_resample = fit_coef_accel_1_x(temp_rel_resample) - -# fit Y axis -correction_y = np.median(sensor_accel_1['y'])-sensor_accel_1['y'] -coef_accel_1_y = np.polyfit(temp_rel,correction_y,3) -accel_1_params['TC_A1_X3_1'] = coef_accel_1_y[0] -accel_1_params['TC_A1_X2_1'] = coef_accel_1_y[1] -accel_1_params['TC_A1_X1_1'] = coef_accel_1_y[2] -accel_1_params['TC_A1_X0_1'] = coef_accel_1_y[3] -fit_coef_accel_1_y = np.poly1d(coef_accel_1_y) -correction_y_resample = fit_coef_accel_1_y(temp_rel_resample) - -# fit Z axis -correction_z = np.median(sensor_accel_1['z'])-(sensor_accel_1['z']) -coef_accel_1_z = np.polyfit(temp_rel,correction_z,3) -accel_1_params['TC_A1_X3_2'] = coef_accel_1_z[0] -accel_1_params['TC_A1_X2_2'] = coef_accel_1_z[1] -accel_1_params['TC_A1_X1_2'] = coef_accel_1_z[2] -accel_1_params['TC_A1_X0_2'] = coef_accel_1_z[3] -fit_coef_accel_1_z = np.poly1d(coef_accel_1_z) -correction_z_resample = fit_coef_accel_1_z(temp_rel_resample) - -# accel 1 vs temperature -plt.figure(5,figsize=(20,13)) - -# draw plots -plt.subplot(3,1,1) -plt.plot(sensor_accel_1['temperature'],-correction_x,'b') -plt.plot(temp_resample,-correction_x_resample,'r') -plt.title('Accel 1 Bias vs Temperature') -plt.ylabel('X bias (m/s/s)') -plt.xlabel('temperature (degC)') -plt.grid() - -# draw plots -plt.subplot(3,1,2) -plt.plot(sensor_accel_1['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_1['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 accel 2 thermal correction parameters -accel_2_params = { -'TC_A2_ID':0, -'TC_A2_TMIN':0.0, -'TC_A2_TMAX':0.0, -'TC_A2_TREF':0.0, -'TC_A2_X0_0':0.0, -'TC_A2_X1_0':0.0, -'TC_A2_X2_0':0.0, -'TC_A2_X3_0':0.0, -'TC_A2_X0_1':0.0, -'TC_A2_X1_1':0.0, -'TC_A2_X2_1':0.0, -'TC_A2_X3_1':0.0, -'TC_A2_X0_2':0.0, -'TC_A2_X1_2':0.0, -'TC_A2_X2_2':0.0, -'TC_A2_X3_2':0.0, -'TC_A2_SCL_0':1.0, -'TC_A2_SCL_1':1.0, -'TC_A2_SCL_2':1.0 -} - -# curve fit the data for accel 2 corrections - note corrections have oppsite sign to sensor bias -accel_2_params['TC_A2_ID'] = int(np.median(sensor_accel_2['device_id'])) - -# find the min, max and reference temperature -accel_2_params['TC_A2_TMIN'] = np.amin(sensor_accel_2['temperature']) -accel_2_params['TC_A2_TMAX'] = np.amax(sensor_accel_2['temperature']) -accel_2_params['TC_A2_TREF'] = 0.5 * (accel_2_params['TC_A2_TMIN'] + accel_2_params['TC_A2_TMAX']) -temp_rel = sensor_accel_2['temperature'] - accel_2_params['TC_A2_TREF'] -temp_rel_resample = np.linspace(accel_2_params['TC_A2_TMIN']-accel_2_params['TC_A2_TREF'], accel_2_params['TC_A2_TMAX']-accel_2_params['TC_A2_TREF'], 100) -temp_resample = temp_rel_resample + accel_2_params['TC_A2_TREF'] - -# fit X axis -correction_x = np.median(sensor_accel_2['x'])-sensor_accel_2['x'] -coef_accel_2_x = np.polyfit(temp_rel,correction_x,3) -accel_2_params['TC_A2_X3_0'] = coef_accel_2_x[0] -accel_2_params['TC_A2_X2_0'] = coef_accel_2_x[1] -accel_2_params['TC_A2_X1_0'] = coef_accel_2_x[2] -accel_2_params['TC_A2_X0_0'] = coef_accel_2_x[3] -fit_coef_accel_2_x = np.poly1d(coef_accel_2_x) -correction_x_resample = fit_coef_accel_2_x(temp_rel_resample) - -# fit Y axis -correction_y = np.median(sensor_accel_2['y'])-sensor_accel_2['y'] -coef_accel_2_y = np.polyfit(temp_rel,correction_y,3) -accel_2_params['TC_A2_X3_1'] = coef_accel_2_y[0] -accel_2_params['TC_A2_X2_1'] = coef_accel_2_y[1] -accel_2_params['TC_A2_X1_1'] = coef_accel_2_y[2] -accel_2_params['TC_A2_X0_1'] = coef_accel_2_y[3] -fit_coef_accel_2_y = np.poly1d(coef_accel_2_y) -correction_y_resample = fit_coef_accel_2_y(temp_rel_resample) - -# fit Z axis -correction_z = np.median(sensor_accel_2['z'])-sensor_accel_2['z'] -coef_accel_2_z = np.polyfit(temp_rel,correction_z,3) -accel_2_params['TC_A2_X3_2'] = coef_accel_2_z[0] -accel_2_params['TC_A2_X2_2'] = coef_accel_2_z[1] -accel_2_params['TC_A2_X1_2'] = coef_accel_2_z[2] -accel_2_params['TC_A2_X0_2'] = coef_accel_2_z[3] -fit_coef_accel_2_z = np.poly1d(coef_accel_2_z) -correction_z_resample = fit_coef_accel_2_z(temp_rel_resample) - -# accel 2 vs temperature -plt.figure(6,figsize=(20,13)) - -# draw plots -plt.subplot(3,1,1) -plt.plot(sensor_accel_2['temperature'],-correction_x,'b') -plt.plot(temp_resample,-correction_x_resample,'r') -plt.title('Accel 2 Bias vs Temperature') -plt.ylabel('X bias (m/s/s)') -plt.xlabel('temperature (degC)') -plt.grid() - -# draw plots -plt.subplot(3,1,2) -plt.plot(sensor_accel_2['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_2['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, -'TC_B0_TMIN':0.0, -'TC_B0_TMAX':0.0, -'TC_B0_TREF':0.0, -'TC_B0_X0':0.0, -'TC_B0_X1':0.0, -'TC_B0_X2':0.0, -'TC_B0_X3':0.0, -'TC_B0_X4':0.0, -'TC_B0_X5':0.0, -'TC_B0_SCL':1.0, -} - -# curve fit the data for baro 0 corrections - note corrections have oppsite sign to sensor bias -baro_0_params['TC_B0_ID'] = int(np.median(sensor_baro_0['device_id'])) - -# find the min, max and reference temperature -baro_0_params['TC_B0_TMIN'] = np.amin(sensor_baro_0['temperature']) -baro_0_params['TC_B0_TMAX'] = np.amax(sensor_baro_0['temperature']) -baro_0_params['TC_B0_TREF'] = 0.5 * (baro_0_params['TC_B0_TMIN'] + baro_0_params['TC_B0_TMAX']) -temp_rel = sensor_baro_0['temperature'] - baro_0_params['TC_B0_TREF'] -temp_rel_resample = np.linspace(baro_0_params['TC_B0_TMIN']-baro_0_params['TC_B0_TREF'], baro_0_params['TC_B0_TMAX']-baro_0_params['TC_B0_TREF'], 100) -temp_resample = temp_rel_resample + baro_0_params['TC_B0_TREF'] - -# fit data -median_pressure =100*np.median(sensor_baro_0['pressure']); -coef_baro_0_x = np.polyfit(temp_rel,median_pressure-100*sensor_baro_0['pressure'],5) # convert from hPa to Pa -baro_0_params['TC_B0_X5'] = coef_baro_0_x[0] -baro_0_params['TC_B0_X4'] = coef_baro_0_x[1] -baro_0_params['TC_B0_X3'] = coef_baro_0_x[2] -baro_0_params['TC_B0_X2'] = coef_baro_0_x[3] -baro_0_params['TC_B0_X1'] = coef_baro_0_x[4] -baro_0_params['TC_B0_X0'] = coef_baro_0_x[5] -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)) - -# draw plots -plt.plot(sensor_baro_0['temperature'],100*sensor_baro_0['pressure']-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.xlabel('temperature (degC)') -plt.grid() - -pp.savefig() - -################################################################################# - -# close the pdf file -pp.close() - -# clase all figures -plt.close("all") - -# write correction parameters to file -test_results_filename = ulog_file_name + ".params" -file = open(test_results_filename,"w") -file.write("# Sensor thermal compensation parameters\n") -file.write("#\n") -file.write("# Vehicle-Id Component-Id Name Value Type\n") - -# accel 0 corrections -key_list_accel = list(accel_0_params.keys()) -key_list_accel.sort -for key in key_list_accel: - if key == 'TC_A0_ID': - type = "6" - else: - type = "9" - file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(accel_0_params[key])+"\t"+type+"\n") - -# accel 1 corrections -key_list_accel = list(accel_1_params.keys()) -key_list_accel.sort -for key in key_list_accel: - if key == 'TC_A1_ID': - type = "6" - else: - type = "9" - file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(accel_1_params[key])+"\t"+type+"\n") - -# accel 2 corrections -key_list_accel = list(accel_2_params.keys()) -key_list_accel.sort -for key in key_list_accel: - if key == 'TC_A2_ID': - type = "6" - else: - type = "9" - 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: - 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") - -# gyro 0 corrections -key_list_gyro = list(gyro_0_params.keys()) -key_list_gyro.sort() -for key in key_list_gyro: - if key == 'TC_G0_ID': - type = "6" - else: - type = "9" - file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(gyro_0_params[key])+"\t"+type+"\n") - -# gyro 1 corrections -key_list_gyro = list(gyro_1_params.keys()) -key_list_gyro.sort() -for key in key_list_gyro: - if key == 'TC_G1_ID': - type = "6" - else: - type = "9" - file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(gyro_1_params[key])+"\t"+type+"\n") - -# gyro 2 corrections -key_list_gyro = list(gyro_2_params.keys()) -key_list_gyro.sort() -for key in key_list_gyro: - if key == 'TC_G2_ID': - type = "6" - else: - type = "9" - file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(gyro_2_params[key])+"\t"+type+"\n") - -file.close() - -print('Correction parameters written to ' + test_results_filename) -print('Plots saved to ' + output_plot_filename) +import pyulog + +class Param(dict): + def __init__(self, name, val): + """ + Initialize a param dict + """ + self.name = name + self.val = val + +def temp_calibration(data, topic, fields, units, label): + """ + Performe a temperature calibration on a sensor. + """ + + # pylint: disable=no-member + params = {} + + int_params = ['ID'] + float_params = [ + 'TMIN', 'TMAX', 'TREF', + 'X0_0', 'X1_0', 'X2_0', 'X3_0', + 'X0_1', 'X1_1', 'X2_1', 'X3_1', + 'X0_2', 'X1_2', 'X2_2', 'X3_2', + 'SCL_0', 'SCL_1', 'SCL_2' + ] + + # define data dictionary of thermal correction parameters + for field in int_params: + params[field] = { + 'val': 0, + 'type': 'INT', + } + + for field in float_params: params[field] = { + 'val': 0, + 'type': 'FLOAT', + } + + # curve fit the data for corrections - note + # corrections have oppsite sign to sensor bias + try: + params['ID']['val'] = int(np.median(data['device_id'])) + except: + print('no device id') + pass + + # find the min, max and reference temperature + params['TMIN']['val'] = float(np.amin(data['temperature'])) + params['TMAX']['val'] = float(np.amax(data['temperature'])) + params['TREF']['val'] = float(0.5 * (params['TMIN']['val'] + params['TMAX']['val'])) + temp_rel = data['temperature'] - params['TREF']['val'] + temp_rel_resample = np.linspace( + float(params['TMIN']['val'] - params['TREF']['val']), + float(params['TMAX']['val'] - params['TREF']['val']), 100) + temp_resample = temp_rel_resample + params['TREF']['val'] + + for i, field in enumerate(fields): + coef = np.polyfit(temp_rel, -data[field], 3) + for j in range(3): + params['X{:d}_{:d}'.format(3-j, i)]['val'] = float(coef[j]) + fit_coef = np.poly1d(coef) + resample = fit_coef(temp_rel_resample) + + # draw plots + plt.subplot(len(fields), 1, i + 1) + plt.plot(data['temperature'], data[field], 'b') + plt.plot(temp_resample, -resample, 'r') + plt.title('{:s} Bias vs Temperature'.format(topic)) + plt.ylabel('{:s} bias {:s}'.format(field, units)) + plt.xlabel('temperature (degC)') + plt.grid() + + return params + + +def process_file(log_path, out_path, template_path): + """ + Command line interface to temperature calibration. + """ + log = pyulog.ULog(log_path, 'sensor_gyro, sensor_accel, sensor_baro') + data = {} + for d in log.data_list: + data['{:s}_{:d}'.format(d.name, d.multi_id)] = d.data + + params = {} + + # open file to save plots to PDF + # from matplotlib.backends.backend_pdf import PdfPages + # output_plot_filename = ulog_file_name + ".pdf" + # pp = PdfPages(output_plot_filename) + + configs = [ + { + 'msg': 'sensor_gyro', + 'fields': ['x', 'y', 'z'], + 'units': 'rad/s', + 'label': 'TC_G' + }, + { + 'msg': 'sensor_accel', + 'fields': ['x', 'y', 'z'], + 'units': 'm/s^2', + 'label': 'TC_A' + }, + { + 'msg': 'sensor_baro', + 'fields': ['pressure'], + 'units': 'm', + 'label': 'TC_B' + }, + ] + + for config in configs: + for d in log.data_list: + if d.name == config['msg']: + plt.figure(figsize=(20, 13)) + topic = '{:s}_{:d}'.format(d.name, d.multi_id) + print('found {:s} data'.format(topic)) + label='{:s}{:d}'.format( + config['label'], d.multi_id) + params[topic] = { + 'params': temp_calibration( + data=d.data, topic=topic, + fields=config['fields'], + units=config['units'], + label=label), + 'label': label + } + plt.savefig('{:s}_cal.pdf'.format(topic)) + + # JSON file generation + # import json + # print(json.dumps(params, indent=2)) + + body = '' + for sensor in sorted(params.keys()): + for param in sorted(params[sensor]['params'].keys()): + label = params[sensor]['label'] + pdict = params[sensor]['params'] + if pdict[param]['type'] == 'INT': + type_id = 6 + elif pdict[param]['type'] == 'FLOAT': + type_id = 9 + val = pdict[param]['val'] + name = '{:s}_{:s}'.format(label, param) + body += "1\t1\t{name:20s}\t{val:15g}\t{type_id:5d}\n".format(**locals()) + + # simple template file output + text = """# Sensor thermal compensation parameters +# +# Vehicle-Id Component-Id Name Value Type +{body:s} +""".format(body=body) + + with open(out_path, 'w') as f: + f.write(text) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description='Analyse the sensor_gyro message data') + parser.add_argument('filename', metavar='file.ulg', help='ULog input file') + args = parser.parse_args() + ulog_file_name = args.filename + template_path = os.path.join(os.path.dirname( + os.path.realpath(__file__)), 'templates') + process_file(log_path=args.filename, out_path=ulog_file_name.replace('ulg', 'params'), + template_path=template_path) + +# vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 :