diff --git a/Tools/process_sensor_caldata.py b/Tools/process_sensor_caldata.py index dec5ff44f1..0e6bff9cbd 100644 --- a/Tools/process_sensor_caldata.py +++ b/Tools/process_sensor_caldata.py @@ -1,29 +1,4 @@ #! /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 @@ -32,173 +7,812 @@ import os import matplotlib.pyplot as plt import numpy as np -import pyulog +from pyulog import * -class Param(dict): - def __init__(self, name, val): - """ - Initialize a param dict - """ - self.name = name - self.val = val +""" +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: -def temp_calibration(data, topic, fields, units, label): - """ - Performe a temperature calibration on a sensor. - """ +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 - # pylint: disable=no-member - params = {} +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 - 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', - } +parser = argparse.ArgumentParser(description='Analyse the sensor_gyro message data') +parser.add_argument('filename', metavar='file.ulg', help='ULog input file') - for field in float_params: params[field] = { - 'val': 0, - 'type': 'FLOAT', - } +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)) - # curve fit the data for corrections - note corrections have same sign as sensor bias and will need to be subtracted from the raw reading to remove the bias - try: - params['ID']['val'] = int(np.median(data['device_id'])) - except: - print('no device id') - pass +args = parser.parse_args() +ulog_file_name = args.filename - # 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'] +ulog = ULog(ulog_file_name, None) +data = ulog.data_list - 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) +# 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 - # 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() +# 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 - return params +# 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) -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 = {} +# 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 +} - # 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) +# curve fit the data for gyro 0 corrections +gyro_0_params['TC_G0_ID'] = int(np.median(sensor_gyro_0['device_id'])) - 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' - }, - ] +# 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'] - 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)) +# 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) - # JSON file generation - # import json - # print(json.dumps(params, indent=2)) +# 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) - 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()) +# 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) - # simple template file output - text = """# Sensor thermal compensation parameters -# -# Vehicle-Id Component-Id Name Value Type -{body:s} -""".format(body=body) +# gyro0 vs temperature +plt.figure(1,figsize=(20,13)) - with open(out_path, 'w') as f: - f.write(text) - +# 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() -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) +# 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() -# vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : +# 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 +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 +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 +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 = sensor_accel_0['x'] - np.median(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 = sensor_accel_0['y']-np.median(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 = sensor_accel_0['z']-np.median(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 +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 = sensor_accel_1['x']-np.median(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 = sensor_accel_1['y']-np.median(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 = (sensor_accel_1['z'])-np.median(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 +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 = sensor_accel_2['x']-np.median(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 = sensor_accel_2['y']-np.median(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 = sensor_accel_2['z']-np.median(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 +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 = np.median(sensor_baro_0['pressure']); +coef_baro_0_x = np.polyfit(temp_rel,100*(sensor_baro_0['pressure']-median_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']-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.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)