From b86380086edd7ddc9de8e17846a42a162aee5b2b Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Tue, 24 Jan 2017 18:42:15 -0500
Subject: [PATCH] Streamline python script for temp cal. (#6416)
* Streamline python script for temp cal.
* Simplify file generation for temp calibration.
---
Tools/.gitignore | 1 +
Tools/process_sensor_caldata.py | 990 ++++++--------------------------
2 files changed, 188 insertions(+), 803 deletions(-)
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,821 +1,205 @@
#! /usr/bin/env python
-
-from __future__ import print_function
-
-import argparse
-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
+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
+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
+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 <full path name to .ulog file>
+9) Run the script file 'python process_sensor_caldata.py
+ <full path name to .ulog file>
-Outputs thermal compensation parameters in a file named <inputfilename>.params which can be loaded onto the board using QGroundControl
+Outputs thermal compensation parameters in a file named
+ <inputfilename>.params which can be loaded onto the
+ board using QGroundControl
Outputs summary plots in a pdf file named <inputfilename>.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")
+from __future__ import print_function
-file.close()
+import argparse
+import os
+import matplotlib.pyplot as plt
+import numpy as np
-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 :
--
GitLab