From af44da25f0fcbc597150c252d47872439160eb0e Mon Sep 17 00:00:00 2001 From: mcsauder Date: Tue, 27 Sep 2022 18:40:40 -0600 Subject: [PATCH] Use accel of the same instance or primary baro for gyro instances that do not have valid temperature readings in temperature calibration data, use primary baro for magnetometers without valid temperature. --- Tools/process_sensor_caldata.py | 1283 +++++++++-------- .../magnetometer/isentek/ist8308/IST8308.cpp | 19 - .../magnetometer/isentek/ist8308/IST8308.hpp | 28 +- .../magnetometer/isentek/ist8310/IST8310.cpp | 20 - .../magnetometer/isentek/ist8310/IST8310.hpp | 30 +- src/modules/logger/logged_topics.cpp | 12 +- .../VehicleAcceleration.cpp | 6 +- .../VehicleAngularVelocity.cpp | 5 +- .../VehicleMagnetometer.cpp | 16 +- .../VehicleMagnetometer.hpp | 2 + .../TemperatureCompensationModule.cpp | 88 +- .../temp_comp_params_mag.c | 2 +- .../temp_comp_params_mag_0.c | 2 +- .../temp_comp_params_mag_1.c | 2 +- .../temp_comp_params_mag_2.c | 2 +- .../temp_comp_params_mag_3.c | 2 +- .../temperature_calibration/mag.cpp | 2 +- .../temperature_calibration/mag.h | 2 +- .../temperature_calibration/task.cpp | 8 +- 19 files changed, 814 insertions(+), 717 deletions(-) diff --git a/Tools/process_sensor_caldata.py b/Tools/process_sensor_caldata.py index 0ec667ccc1..15ef1c84ce 100755 --- a/Tools/process_sensor_caldata.py +++ b/Tools/process_sensor_caldata.py @@ -17,7 +17,7 @@ Reads in IMU data from a static thermal calibration test and performs a curve fi Data can be gathered using the following sequence: 1) Power up the board and set the TC_A_ENABLE, TC_B_ENABLE and TC_G_ENABLE parameters to 1 -2) Set all CAL_GYR and CAL_ACC parameters to defaults +2) Set all CAL_ACC*, CAL_GYR*, and CAL_MAG* parameters to defaults 3) Set the parameter SDLOG_MODE to 2, and SDLOG_PROFILE "Thermal calibration" bit (2) 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, still air, constant pressure environment. @@ -31,7 +31,6 @@ Data can be gathered using the following sequence: 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 - """ def resampleWithDeltaX(x,y): @@ -68,7 +67,7 @@ def resampleWithDeltaX(x,y): def median_filter(data): return medfilt(data, 31) -parser = argparse.ArgumentParser(description='Reads in IMU data from a static thermal calibration test and performs a curve fit of accel, gyro, mag, and baro bias vs temperature') +parser = argparse.ArgumentParser(description='Reads in IMU data from a static thermal calibration test and performs a curve fit of gyro, accel and baro bias vs temperature') parser.add_argument('filename', metavar='file.ulg', help='ULog input file') parser.add_argument('--no_resample', dest='noResample', action='store_const', const=True, default=False, help='skip resampling and use raw data') @@ -210,6 +209,7 @@ if num_accels >= 1 and not math.isnan(sensor_accel_0['temperature'][0]): 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'] @@ -325,6 +325,7 @@ if num_accels >= 2 and not math.isnan(sensor_accel_1['temperature'][0]): 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'] @@ -441,6 +442,7 @@ if num_accels >= 3 and not math.isnan(sensor_accel_2['temperature'][0]): 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'] @@ -556,6 +558,7 @@ if num_accels >= 4 and not math.isnan(sensor_accel_3['temperature'][0]): accel_3_params['TC_A3_TMIN'] = np.amin(sensor_accel_3['temperature']) accel_3_params['TC_A3_TMAX'] = np.amax(sensor_accel_3['temperature']) accel_3_params['TC_A3_TREF'] = 0.5 * (accel_3_params['TC_A3_TMIN'] + accel_3_params['TC_A3_TMAX']) + temp_rel = sensor_accel_3['temperature'] - accel_3_params['TC_A3_TREF'] temp_rel_resample = np.linspace(accel_3_params['TC_A3_TMIN']-accel_3_params['TC_A3_TREF'], accel_3_params['TC_A3_TMAX']-accel_3_params['TC_A3_TREF'], 100) temp_resample = temp_rel_resample + accel_3_params['TC_A3_TREF'] @@ -650,92 +653,104 @@ gyro_0_params = { } # curve fit the data for gyro 0 corrections -if num_gyros >= 1 and not math.isnan(sensor_gyro_0['temperature'][0]): - gyro_0_params['TC_G0_ID'] = int(np.median(sensor_gyro_0['device_id'])) +if num_gyros >= 1: - # 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'] + #if the gyro has no temperature data, use the corresponding accel instance or the primary baro temp. + if math.isnan(sensor_gyro_0['temperature'][0]): + if not math.isnan(sensor_accel_0['temperature'][0]): + sensor_gyro_0['temperature'] = sensor_accel_0['temperature'] + elif not math.isnan(sensor_baro_0['temperature'][0]): + sensor_gyro_0['temperature'] = sensor_baro_0['temperature'] - sensor_gyro_0['x'] = median_filter(sensor_gyro_0['x']) - sensor_gyro_0['y'] = median_filter(sensor_gyro_0['y']) - sensor_gyro_0['z'] = median_filter(sensor_gyro_0['z']) + if not math.isnan(sensor_gyro_0['temperature'][0]): - # fit X axis - if noResample: - coef_gyro_0_x = np.polyfit(temp_rel, sensor_gyro_0['x'], 3) - else: - temp, sens = resampleWithDeltaX(temp_rel, sensor_gyro_0['x']) - coef_gyro_0_x = np.polyfit(temp, sens, 3) + gyro_0_params['TC_G0_ID'] = int(np.median(sensor_gyro_0['device_id'])) - 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) + # 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']) - # fit Y axis - if noResample: - coef_gyro_0_y = np.polyfit(temp_rel, sensor_gyro_0['y'], 3) - else: - temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_0['y']) - coef_gyro_0_y = np.polyfit(temp, sens, 3) + 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'] - 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) + sensor_gyro_0['x'] = median_filter(sensor_gyro_0['x']) + sensor_gyro_0['y'] = median_filter(sensor_gyro_0['y']) + sensor_gyro_0['z'] = median_filter(sensor_gyro_0['z']) - # fit Z axis - if noResample: - coef_gyro_0_z = np.polyfit(temp_rel, sensor_gyro_0['z'],3) - else: - temp, sens = resampleWithDeltaX(temp_rel, sensor_gyro_0['z']) - coef_gyro_0_z = np.polyfit(temp, sens ,3) + # fit X axis + if noResample: + coef_gyro_0_x = np.polyfit(temp_rel, sensor_gyro_0['x'], 3) + else: + temp, sens = resampleWithDeltaX(temp_rel, sensor_gyro_0['x']) + coef_gyro_0_x = np.polyfit(temp, sens, 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) + 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) - # gyro0 vs temperature - plt.figure(5,figsize=(20,13)) + # fit Y axis + if noResample: + coef_gyro_0_y = np.polyfit(temp_rel, sensor_gyro_0['y'], 3) + else: + temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_0['y']) + coef_gyro_0_y = np.polyfit(temp, sens, 3) - # 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'.format(gyro_0_params['TC_G0_ID'])) - plt.ylabel('X bias (rad/s)') - plt.xlabel('temperature (degC)') - plt.grid() + 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) - # 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() + # fit Z axis + if noResample: + coef_gyro_0_z = np.polyfit(temp_rel, sensor_gyro_0['z'],3) + else: + temp, sens = resampleWithDeltaX(temp_rel, sensor_gyro_0['z']) + coef_gyro_0_z = np.polyfit(temp, sens ,3) - # 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() + 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(5,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'.format(gyro_0_params['TC_G0_ID'])) + 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() - pp.savefig() ################################################################################# @@ -762,92 +777,103 @@ gyro_1_params = { } # curve fit the data for gyro 1 corrections -if num_gyros >= 2 and not math.isnan(sensor_gyro_1['temperature'][0]): - gyro_1_params['TC_G1_ID'] = int(np.median(sensor_gyro_1['device_id'])) +if num_gyros >= 2: - # 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'] + #if the gyro has no temperature data, use the corresponding accel instance or the primary baro temp. + if math.isnan(sensor_gyro_1['temperature'][0]): + if not math.isnan(sensor_accel_1['temperature'][0]): + sensor_gyro_1['temperature'] = sensor_accel_1['temperature'] + elif not math.isnan(sensor_baro_0['temperature'][0]): + sensor_gyro_1['temperature'] = sensor_baro_0['temperature'] - sensor_gyro_1['x'] = median_filter(sensor_gyro_1['x']) - sensor_gyro_1['y'] = median_filter(sensor_gyro_1['y']) - sensor_gyro_1['z'] = median_filter(sensor_gyro_1['z']) + if not math.isnan(sensor_gyro_1['temperature'][0]): - # fit X axis - if noResample: - coef_gyro_1_x = np.polyfit(temp_rel,sensor_gyro_1['x'],3) - else: - temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_1['x']) - coef_gyro_1_x = np.polyfit(temp, sens ,3) + gyro_1_params['TC_G1_ID'] = int(np.median(sensor_gyro_1['device_id'])) - 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) + # 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']) - # fit Y axis - if noResample: - coef_gyro_1_y = np.polyfit(temp_rel,sensor_gyro_1['y'],3) - else: - temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_1['y']) - coef_gyro_1_y = np.polyfit(temp, sens ,3) + 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'] - 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) + sensor_gyro_1['x'] = median_filter(sensor_gyro_1['x']) + sensor_gyro_1['y'] = median_filter(sensor_gyro_1['y']) + sensor_gyro_1['z'] = median_filter(sensor_gyro_1['z']) - # fit Z axis - if noResample: - coef_gyro_1_z = np.polyfit(temp_rel,sensor_gyro_1['z'],3) - else: - temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_1['z']) - coef_gyro_1_z = np.polyfit(temp, sens ,3) + # fit X axis + if noResample: + coef_gyro_1_x = np.polyfit(temp_rel,sensor_gyro_1['x'],3) + else: + temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_1['x']) + coef_gyro_1_x = np.polyfit(temp, sens ,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) + 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) - # gyro1 vs temperature - plt.figure(6,figsize=(20,13)) + # fit Y axis + if noResample: + coef_gyro_1_y = np.polyfit(temp_rel,sensor_gyro_1['y'],3) + else: + temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_1['y']) + coef_gyro_1_y = np.polyfit(temp, sens ,3) - # 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'.format(gyro_1_params['TC_G1_ID'])) - plt.ylabel('X bias (rad/s)') - plt.xlabel('temperature (degC)') - plt.grid() + 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) - # 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() + # fit Z axis + if noResample: + coef_gyro_1_z = np.polyfit(temp_rel,sensor_gyro_1['z'],3) + else: + temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_1['z']) + coef_gyro_1_z = np.polyfit(temp, sens ,3) - # 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() + 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) - pp.savefig() + # gyro1 vs temperature + plt.figure(6,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'.format(gyro_1_params['TC_G1_ID'])) + 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() ################################################################################# @@ -874,92 +900,103 @@ gyro_2_params = { } # curve fit the data for gyro 2 corrections -if num_gyros >= 3 and not math.isnan(sensor_gyro_2['temperature'][0]): - gyro_2_params['TC_G2_ID'] = int(np.median(sensor_gyro_2['device_id'])) +if num_gyros >= 3: - # 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'] + #if the gyro has no temperature data, use the corresponding accel instance or the primary baro temp. + if math.isnan(sensor_gyro_2['temperature'][0]): + if not math.isnan(sensor_accel_2['temperature'][0]): + sensor_gyro_2['temperature'] = sensor_accel_2['temperature'] + elif not math.isnan(sensor_baro_0['temperature'][0]): + sensor_gyro_2['temperature'] = sensor_baro_0['temperature'] - sensor_gyro_2['x'] = median_filter(sensor_gyro_2['x']) - sensor_gyro_2['y'] = median_filter(sensor_gyro_2['y']) - sensor_gyro_2['z'] = median_filter(sensor_gyro_2['z']) + if not math.isnan(sensor_gyro_2['temperature'][0]): - # fit X axis - if noResample: - coef_gyro_2_x = np.polyfit(temp_rel,sensor_gyro_2['x'],3) - else: - temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_2['x']) - coef_gyro_2_x = np.polyfit(temp, sens ,3) + gyro_2_params['TC_G2_ID'] = int(np.median(sensor_gyro_2['device_id'])) - 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) + # 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']) - # fit Y axis - if noResample: - coef_gyro_2_y = np.polyfit(temp_rel, sensor_gyro_2['y'], 3) - else: - temp, sens = resampleWithDeltaX(temp_rel, sensor_gyro_2['y']) - coef_gyro_2_y = np.polyfit(temp, sens, 3) + 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'] - 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) + sensor_gyro_2['x'] = median_filter(sensor_gyro_2['x']) + sensor_gyro_2['y'] = median_filter(sensor_gyro_2['y']) + sensor_gyro_2['z'] = median_filter(sensor_gyro_2['z']) - # fit Z axis - if noResample: - coef_gyro_2_z = np.polyfit(temp_rel,sensor_gyro_2['z'], 3) - else: - temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_2['z']) - coef_gyro_2_z = np.polyfit(temp, sens, 3) + # fit X axis + if noResample: + coef_gyro_2_x = np.polyfit(temp_rel,sensor_gyro_2['x'],3) + else: + temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_2['x']) + coef_gyro_2_x = np.polyfit(temp, sens ,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) + 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) - # gyro2 vs temperature - plt.figure(7,figsize=(20,13)) + # fit Y axis + if noResample: + coef_gyro_2_y = np.polyfit(temp_rel, sensor_gyro_2['y'], 3) + else: + temp, sens = resampleWithDeltaX(temp_rel, sensor_gyro_2['y']) + coef_gyro_2_y = np.polyfit(temp, sens, 3) - # 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'.format(gyro_2_params['TC_G2_ID'])) - plt.ylabel('X bias (rad/s)') - plt.xlabel('temperature (degC)') - plt.grid() + 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) - # 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() + # fit Z axis + if noResample: + coef_gyro_2_z = np.polyfit(temp_rel,sensor_gyro_2['z'], 3) + else: + temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_2['z']) + coef_gyro_2_z = np.polyfit(temp, sens, 3) - # 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() + 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) - pp.savefig() + # gyro2 vs temperature + plt.figure(7,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'.format(gyro_2_params['TC_G2_ID'])) + 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() ################################################################################# @@ -986,83 +1023,94 @@ gyro_3_params = { } # curve fit the data for gyro 3 corrections -if num_gyros >= 4 and not math.isnan(sensor_gyro_3['temperature'][0]): - gyro_3_params['TC_G3_ID'] = int(np.median(sensor_gyro_3['device_id'])) +if num_gyros >= 4: - # find the min, max and reference temperature - gyro_3_params['TC_G3_TMIN'] = np.amin(sensor_gyro_3['temperature']) - gyro_3_params['TC_G3_TMAX'] = np.amax(sensor_gyro_3['temperature']) - gyro_3_params['TC_G3_TREF'] = 0.5 * (gyro_3_params['TC_G3_TMIN'] + gyro_3_params['TC_G3_TMAX']) - temp_rel = sensor_gyro_3['temperature'] - gyro_3_params['TC_G3_TREF'] - temp_rel_resample = np.linspace(gyro_3_params['TC_G3_TMIN']-gyro_3_params['TC_G3_TREF'], gyro_3_params['TC_G3_TMAX']-gyro_3_params['TC_G3_TREF'], 100) - temp_resample = temp_rel_resample + gyro_3_params['TC_G3_TREF'] + #if the gyro has no temperature data, use the corresponding accel instance or the primary baro temp. + if math.isnan(sensor_gyro_3['temperature'][0]): + if not math.isnan(sensor_accel_3['temperature'][0]): + sensor_gyro_3['temperature'] = sensor_accel_3['temperature'] + elif not math.isnan(sensor_baro_0['temperature'][0]): + sensor_gyro_3['temperature'] = sensor_baro_0['temperature'] - sensor_gyro_3['x'] = median_filter(sensor_gyro_3['x']) - sensor_gyro_3['y'] = median_filter(sensor_gyro_3['y']) - sensor_gyro_3['z'] = median_filter(sensor_gyro_3['z']) + if not math.isnan(sensor_gyro_3['temperature'][0]): - # fit X axis - coef_gyro_3_x = np.polyfit(temp_rel,sensor_gyro_3['x'], 3) - gyro_3_params['TC_G3_X3_0'] = coef_gyro_3_x[0] - gyro_3_params['TC_G3_X2_0'] = coef_gyro_3_x[1] - gyro_3_params['TC_G3_X1_0'] = coef_gyro_3_x[2] - gyro_3_params['TC_G3_X0_0'] = coef_gyro_3_x[3] - fit_coef_gyro_3_x = np.poly1d(coef_gyro_3_x) - gyro_3_x_resample = fit_coef_gyro_3_x(temp_rel_resample) + gyro_3_params['TC_G3_ID'] = int(np.median(sensor_gyro_3['device_id'])) - # fit Y axis - coef_gyro_3_y = np.polyfit(temp_rel,sensor_gyro_3['y'], 3) - gyro_3_params['TC_G3_X3_1'] = coef_gyro_3_y[0] - gyro_3_params['TC_G3_X2_1'] = coef_gyro_3_y[1] - gyro_3_params['TC_G3_X1_1'] = coef_gyro_3_y[2] - gyro_3_params['TC_G3_X0_1'] = coef_gyro_3_y[3] - fit_coef_gyro_3_y = np.poly1d(coef_gyro_3_y) - gyro_3_y_resample = fit_coef_gyro_3_y(temp_rel_resample) + # find the min, max and reference temperature + gyro_3_params['TC_G3_TMIN'] = np.amin(sensor_gyro_3['temperature']) + gyro_3_params['TC_G3_TMAX'] = np.amax(sensor_gyro_3['temperature']) + gyro_3_params['TC_G3_TREF'] = 0.5 * (gyro_2_params['TC_G3_TMIN'] + gyro_2_params['TC_G3_TMAX']) - # fit Z axis - coef_gyro_3_z = np.polyfit(temp_rel,sensor_gyro_3['z'], 3) - gyro_3_params['TC_G3_X3_2'] = coef_gyro_3_z[0] - gyro_3_params['TC_G3_X2_2'] = coef_gyro_3_z[1] - gyro_3_params['TC_G3_X1_2'] = coef_gyro_3_z[2] - gyro_3_params['TC_G3_X0_2'] = coef_gyro_3_z[3] - fit_coef_gyro_3_z = np.poly1d(coef_gyro_3_z) - gyro_3_z_resample = fit_coef_gyro_3_z(temp_rel_resample) + temp_rel = sensor_gyro_3['temperature'] - gyro_2_params['TC_G3_TREF'] + temp_rel_resample = np.linspace(gyro_3_params['TC_G3_TMIN']-gyro_3_params['TC_G3_TREF'], gyro_3_params['TC_G3_TMAX']-gyro_3_params['TC_G3_TREF'], 100) + temp_resample = temp_rel_resample + gyro_3_params['TC_G3_TREF'] - # gyro3 vs temperature - plt.figure(8,figsize=(20,13)) + sensor_gyro_3['x'] = median_filter(sensor_gyro_3['x']) + sensor_gyro_3['y'] = median_filter(sensor_gyro_3['y']) + sensor_gyro_3['z'] = median_filter(sensor_gyro_3['z']) - # draw plots - plt.subplot(3,1,1) - plt.plot(sensor_gyro_3['temperature'],sensor_gyro_3['x'], 'b') - plt.plot(temp_resample,gyro_3_x_resample, 'r') - plt.title('Gyro 2 ({}) Bias vs Temperature'.format(gyro_3_params['TC_G3_ID'])) - plt.ylabel('X bias (rad/s)') - plt.xlabel('temperature (degC)') - plt.grid() + # fit X axis + coef_gyro_3_x = np.polyfit(temp_rel,sensor_gyro_3['x'], 3) + gyro_3_params['TC_G3_X3_0'] = coef_gyro_3_x[0] + gyro_3_params['TC_G3_X2_0'] = coef_gyro_3_x[1] + gyro_3_params['TC_G3_X1_0'] = coef_gyro_3_x[2] + gyro_3_params['TC_G3_X0_0'] = coef_gyro_3_x[3] + fit_coef_gyro_3_x = np.poly1d(coef_gyro_3_x) + gyro_3_x_resample = fit_coef_gyro_3_x(temp_rel_resample) - # draw plots - plt.subplot(3,1,2) - plt.plot(sensor_gyro_3['temperature'],sensor_gyro_3['y'],'b') - plt.plot(temp_resample,gyro_3_y_resample,'r') - plt.ylabel('Y bias (rad/s)') - plt.xlabel('temperature (degC)') - plt.grid() + # fit Y axis + coef_gyro_3_y = np.polyfit(temp_rel,sensor_gyro_3['y'], 3) + gyro_3_params['TC_G3_X3_1'] = coef_gyro_3_y[0] + gyro_3_params['TC_G3_X2_1'] = coef_gyro_3_y[1] + gyro_3_params['TC_G3_X1_1'] = coef_gyro_3_y[2] + gyro_3_params['TC_G3_X0_1'] = coef_gyro_3_y[3] + fit_coef_gyro_3_y = np.poly1d(coef_gyro_3_y) + gyro_3_y_resample = fit_coef_gyro_3_y(temp_rel_resample) - # draw plots - plt.subplot(3,1,3) - plt.plot(sensor_gyro_3['temperature'],sensor_gyro_3['z'],'b') - plt.plot(temp_resample,gyro_3_z_resample,'r') - plt.ylabel('Z bias (rad/s)') - plt.xlabel('temperature (degC)') - plt.grid() + # fit Z axis + coef_gyro_3_z = np.polyfit(temp_rel,sensor_gyro_3['z'], 3) + gyro_3_params['TC_G3_X3_2'] = coef_gyro_3_z[0] + gyro_3_params['TC_G3_X2_2'] = coef_gyro_3_z[1] + gyro_3_params['TC_G3_X1_2'] = coef_gyro_3_z[2] + gyro_3_params['TC_G3_X0_2'] = coef_gyro_3_z[3] + fit_coef_gyro_3_z = np.poly1d(coef_gyro_3_z) + gyro_3_z_resample = fit_coef_gyro_3_z(temp_rel_resample) - pp.savefig() + # gyro3 vs temperature + plt.figure(8,figsize=(20,13)) + + # draw plots + plt.subplot(3,1,1) + plt.plot(sensor_gyro_3['temperature'],sensor_gyro_3['x'], 'b') + plt.plot(temp_resample,gyro_3_x_resample, 'r') + plt.title('Gyro 2 ({}) Bias vs Temperature'.format(gyro_3_params['TC_G3_ID'])) + plt.ylabel('X bias (rad/s)') + plt.xlabel('temperature (degC)') + plt.grid() + + # draw plots + plt.subplot(3,1,2) + plt.plot(sensor_gyro_3['temperature'],sensor_gyro_3['y'],'b') + plt.plot(temp_resample,gyro_3_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_3['temperature'],sensor_gyro_3['z'],'b') + plt.plot(temp_resample,gyro_3_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 +# define data dictionary of mag 0 thermal correction parameters mag_0_params = { 'TC_M0_ID':0, 'TC_M0_TMIN':0.0, @@ -1082,96 +1130,107 @@ mag_0_params = { 'TC_M0_X3_2':0.0 } -# curve fit the data for accel 0 corrections -if num_mags >= 1 and not math.isnan(sensor_mag_0['temperature'][0]): - mag_0_params['TC_M0_ID'] = int(np.median(sensor_mag_0['device_id'])) +# curve fit the data for mag 0 corrections +if num_mags >= 1: + if math.isnan(sensor_mag_0['temperature'][0]): + if not math.isnan(sensor_baro_0['temperature'][0]): + sensor_mag_0['temperature'] = sensor_baro_0['temperature'] - # find the min, max and reference temperature - mag_0_params['TC_M0_TMIN'] = np.amin(sensor_mag_0['temperature']) - mag_0_params['TC_M0_TMAX'] = np.amax(sensor_mag_0['temperature']) - mag_0_params['TC_M0_TREF'] = 0.5 * (mag_0_params['TC_M0_TMIN'] + mag_0_params['TC_M0_TMAX']) - temp_rel = sensor_mag_0['temperature'] - mag_0_params['TC_M0_TREF'] - temp_rel_resample = np.linspace(mag_0_params['TC_M0_TMIN']-mag_0_params['TC_M0_TREF'], mag_0_params['TC_M0_TMAX']-mag_0_params['TC_M0_TREF'], 100) - temp_resample = temp_rel_resample + mag_0_params['TC_M0_TREF'] + if not math.isnan(sensor_mag_0['temperature'][0]): - sensor_mag_0['x'] = median_filter(sensor_mag_0['x']) - sensor_mag_0['y'] = median_filter(sensor_mag_0['y']) - sensor_mag_0['z'] = median_filter(sensor_mag_0['z']) + # find the min, max and reference temperature + mag_0_params['TC_M0_TMIN'] = np.amin(sensor_mag_0['temperature']) + mag_0_params['TC_M0_TMAX'] = np.amax(sensor_mag_0['temperature']) + mag_0_params['TC_M0_TREF'] = 0.5 * (mag_0_params['TC_M0_TMIN'] + mag_0_params['TC_M0_TMAX']) - # fit X axis - correction_x = sensor_mag_0['x'] - np.median(sensor_mag_0['x']) - if noResample: - coef_mag_0_x = np.polyfit(temp_rel,correction_x, 3) - else: - temp, sens = resampleWithDeltaX(temp_rel,correction_x) - coef_mag_0_x = np.polyfit(temp, sens, 3) + temp_rel = sensor_mag_0['temperature'] - mag_0_params['TC_M0_TREF'] + temp_rel_resample = np.linspace(mag_0_params['TC_M0_TMIN']-mag_0_params['TC_M0_TREF'], mag_0_params['TC_M0_TMAX']-mag_0_params['TC_M0_TREF'], 100) + temp_resample = temp_rel_resample + mag_0_params['TC_M0_TREF'] - mag_0_params['TC_M0_X3_0'] = coef_mag_0_x[0] - mag_0_params['TC_M0_X2_0'] = coef_mag_0_x[1] - mag_0_params['TC_M0_X1_0'] = coef_mag_0_x[2] - mag_0_params['TC_M0_X0_0'] = coef_mag_0_x[3] - fit_coef_mag_0_x = np.poly1d(coef_mag_0_x) - correction_x_resample = fit_coef_mag_0_x(temp_rel_resample) + sensor_mag_0['x'] = median_filter(sensor_mag_0['x']) + sensor_mag_0['y'] = median_filter(sensor_mag_0['y']) + sensor_mag_0['z'] = median_filter(sensor_mag_0['z']) - # fit Y axis - correction_y = sensor_mag_0['y'] - np.median(sensor_mag_0['y']) - if noResample: - coef_mag_0_y = np.polyfit(temp_rel, correction_y, 3) - else: - temp, sens = resampleWithDeltaX(temp_rel,correction_y) - coef_mag_0_y = np.polyfit(temp, sens, 3) + # fit X axis + correction_x = sensor_mag_0['x'] - np.median(sensor_mag_0['x']) - mag_0_params['TC_M0_X3_1'] = coef_mag_0_y[0] - mag_0_params['TC_M0_X2_1'] = coef_mag_0_y[1] - mag_0_params['TC_M0_X1_1'] = coef_mag_0_y[2] - mag_0_params['TC_M0_X0_1'] = coef_mag_0_y[3] - fit_coef_mag_0_y = np.poly1d(coef_mag_0_y) - correction_y_resample = fit_coef_mag_0_y(temp_rel_resample) + if noResample: + coef_mag_0_x = np.polyfit(temp_rel,correction_x, 3) + else: + temp, sens = resampleWithDeltaX(temp_rel,correction_x) + coef_mag_0_x = np.polyfit(temp, sens, 3) - # fit Z axis - correction_z = sensor_mag_0['z'] - np.median(sensor_mag_0['z']) - if noResample: - coef_mag_0_z = np.polyfit(temp_rel,correction_z, 3) - else: - temp, sens = resampleWithDeltaX(temp_rel,correction_z) - coef_mag_0_z = np.polyfit(temp, sens, 3) + mag_0_params['TC_M0_X3_0'] = coef_mag_0_x[0] + mag_0_params['TC_M0_X2_0'] = coef_mag_0_x[1] + mag_0_params['TC_M0_X1_0'] = coef_mag_0_x[2] + mag_0_params['TC_M0_X0_0'] = coef_mag_0_x[3] - mag_0_params['TC_M0_X3_2'] = coef_mag_0_z[0] - mag_0_params['TC_M0_X2_2'] = coef_mag_0_z[1] - mag_0_params['TC_M0_X1_2'] = coef_mag_0_z[2] - mag_0_params['TC_M0_X0_2'] = coef_mag_0_z[3] - fit_coef_mag_0_z = np.poly1d(coef_mag_0_z) - correction_z_resample = fit_coef_mag_0_z(temp_rel_resample) + fit_coef_mag_0_x = np.poly1d(coef_mag_0_x) + correction_x_resample = fit_coef_mag_0_x(temp_rel_resample) - # mag 0 vs temperature - plt.figure(9,figsize=(20,13)) + # fit Y axis + correction_y = sensor_mag_0['y'] - np.median(sensor_mag_0['y']) - # draw plots - plt.subplot(3,1,1) - plt.plot(sensor_mag_0['temperature'],correction_x,'b') - plt.plot(temp_resample,correction_x_resample,'r') - plt.title('Mag 0 ({}) Bias vs Temperature'.format(mag_0_params['TC_M0_ID'])) - plt.ylabel('X bias (Gauss)') - plt.xlabel('temperature (degC)') - plt.grid() + if noResample: + coef_mag_0_y = np.polyfit(temp_rel, correction_y, 3) + else: + temp, sens = resampleWithDeltaX(temp_rel,correction_y) + coef_mag_0_y = np.polyfit(temp, sens, 3) - # draw plots - plt.subplot(3,1,2) - plt.plot(sensor_mag_0['temperature'],correction_y,'b') - plt.plot(temp_resample,correction_y_resample,'r') - plt.ylabel('Y bias (Gauss)') - plt.xlabel('temperature (degC)') - plt.grid() + mag_0_params['TC_M0_X3_1'] = coef_mag_0_y[0] + mag_0_params['TC_M0_X2_1'] = coef_mag_0_y[1] + mag_0_params['TC_M0_X1_1'] = coef_mag_0_y[2] + mag_0_params['TC_M0_X0_1'] = coef_mag_0_y[3] - # draw plots - plt.subplot(3,1,3) - plt.plot(sensor_mag_0['temperature'],correction_z,'b') - plt.plot(temp_resample,correction_z_resample,'r') - plt.ylabel('Z bias (Gauss)') - plt.xlabel('temperature (degC)') - plt.grid() + fit_coef_mag_0_y = np.poly1d(coef_mag_0_y) + correction_y_resample = fit_coef_mag_0_y(temp_rel_resample) - pp.savefig() + # fit Z axis + correction_z = sensor_mag_0['z'] - np.median(sensor_mag_0['z']) + + if noResample: + coef_mag_0_z = np.polyfit(temp_rel,correction_z, 3) + else: + temp, sens = resampleWithDeltaX(temp_rel,correction_z) + coef_mag_0_z = np.polyfit(temp, sens, 3) + + mag_0_params['TC_M0_X3_2'] = coef_mag_0_z[0] + mag_0_params['TC_M0_X2_2'] = coef_mag_0_z[1] + mag_0_params['TC_M0_X1_2'] = coef_mag_0_z[2] + mag_0_params['TC_M0_X0_2'] = coef_mag_0_z[3] + + fit_coef_mag_0_z = np.poly1d(coef_mag_0_z) + correction_z_resample = fit_coef_mag_0_z(temp_rel_resample) + + # mag 0 vs temperature + plt.figure(9,figsize=(20,13)) + + # draw plots + plt.subplot(3,1,1) + plt.plot(sensor_mag_0['temperature'],correction_x,'b') + plt.plot(temp_resample,correction_x_resample,'r') + plt.title('Mag 0 ({}) Bias vs Temperature'.format(mag_0_params['TC_M0_ID'])) + plt.ylabel('X bias (Gauss)') + plt.xlabel('temperature (degC)') + plt.grid() + + # draw plots + plt.subplot(3,1,2) + plt.plot(sensor_mag_0['temperature'],correction_y,'b') + plt.plot(temp_resample,correction_y_resample,'r') + plt.ylabel('Y bias (Gauss)') + plt.xlabel('temperature (degC)') + plt.grid() + + # draw plots + plt.subplot(3,1,3) + plt.plot(sensor_mag_0['temperature'],correction_z,'b') + plt.plot(temp_resample,correction_z_resample,'r') + plt.ylabel('Z bias (Gauss)') + plt.xlabel('temperature (degC)') + plt.grid() + + pp.savefig() ################################################################################# @@ -1198,95 +1257,108 @@ mag_1_params = { } # curve fit the data for mag 1 corrections -if num_mags >= 2 and not math.isnan(sensor_mag_1['temperature'][0]): - mag_1_params['TC_M1_ID'] = int(np.median(sensor_mag_1['device_id'])) +if num_mags >= 2: + if math.isnan(sensor_mag_1['temperature'][0]): + if not math.isnan(sensor_baro_0['temperature'][0]): + sensor_mag_1['temperature'] = sensor_baro_0['temperature'] - # find the min, max and reference temperature - mag_1_params['TC_M1_TMIN'] = np.amin(sensor_mag_1['temperature']) - mag_1_params['TC_M1_TMAX'] = np.amax(sensor_mag_1['temperature']) - mag_1_params['TC_M1_TREF'] = 0.5 * (mag_1_params['TC_M1_TMIN'] + mag_1_params['TC_M1_TMAX']) - temp_rel = sensor_mag_1['temperature'] - mag_1_params['TC_M1_TREF'] - temp_rel_resample = np.linspace(mag_1_params['TC_M1_TMIN']-mag_1_params['TC_M1_TREF'], mag_1_params['TC_M1_TMAX']-mag_1_params['TC_M1_TREF'], 100) - temp_resample = temp_rel_resample + mag_1_params['TC_M1_TREF'] + if not math.isnan(sensor_mag_1['temperature'][0]): - sensor_mag_1['x'] = median_filter(sensor_mag_1['x']) - sensor_mag_1['y'] = median_filter(sensor_mag_1['y']) - sensor_mag_1['z'] = median_filter(sensor_mag_1['z']) + mag_1_params['TC_M1_ID'] = int(np.median(sensor_mag_1['device_id'])) - # fit X axis - correction_x = sensor_mag_1['x'] - np.median(sensor_mag_1['x']) - if noResample: - coef_mag_1_x = np.polyfit(temp_rel, correction_x, 3) - else: - temp, sens = resampleWithDeltaX(temp_rel, correction_x) - coef_mag_1_x = np.polyfit(temp, sens, 3) + # find the min, max and reference temperature + mag_1_params['TC_M1_TMIN'] = np.amin(sensor_mag_1['temperature']) + mag_1_params['TC_M1_TMAX'] = np.amax(sensor_mag_1['temperature']) + mag_1_params['TC_M1_TREF'] = 0.5 * (mag_1_params['TC_M1_TMIN'] + mag_1_params['TC_M1_TMAX']) - mag_1_params['TC_M1_X3_0'] = coef_mag_1_x[0] - mag_1_params['TC_M1_X2_0'] = coef_mag_1_x[1] - mag_1_params['TC_M1_X1_0'] = coef_mag_1_x[2] - mag_1_params['TC_M1_X0_0'] = coef_mag_1_x[3] - fit_coef_mag_1_x = np.poly1d(coef_mag_1_x) - correction_x_resample = fit_coef_mag_1_x(temp_rel_resample) + temp_rel = sensor_mag_1['temperature'] - mag_1_params['TC_M1_TREF'] + temp_rel_resample = np.linspace(mag_1_params['TC_M1_TMIN']-mag_1_params['TC_M1_TREF'], mag_1_params['TC_M1_TMAX']-mag_1_params['TC_M1_TREF'], 100) + temp_resample = temp_rel_resample + mag_1_params['TC_M1_TREF'] - # fit Y axis - correction_y = sensor_mag_1['y'] - np.median(sensor_mag_1['y']) - if noResample: - coef_mag_1_y = np.polyfit(temp_rel,correction_y,3) - else: - temp, sens = resampleWithDeltaX(temp_rel,correction_y) - coef_mag_1_y = np.polyfit(temp, sens ,3) + sensor_mag_1['x'] = median_filter(sensor_mag_1['x']) + sensor_mag_1['y'] = median_filter(sensor_mag_1['y']) + sensor_mag_1['z'] = median_filter(sensor_mag_1['z']) - mag_1_params['TC_M1_X3_1'] = coef_mag_1_y[0] - mag_1_params['TC_M1_X2_1'] = coef_mag_1_y[1] - mag_1_params['TC_M1_X1_1'] = coef_mag_1_y[2] - mag_1_params['TC_M1_X0_1'] = coef_mag_1_y[3] - fit_coef_mag_1_y = np.poly1d(coef_mag_1_y) - correction_y_resample = fit_coef_mag_1_y(temp_rel_resample) + # fit X axis + correction_x = sensor_mag_1['x'] - np.median(sensor_mag_1['x']) - # fit Z axis - correction_z = sensor_mag_1['z'] - np.median(sensor_mag_1['z']) - if noResample: - coef_mag_1_z = np.polyfit(temp_rel,correction_z, 3) - else: - temp, sens = resampleWithDeltaX(temp_rel,correction_z) - coef_mag_1_z = np.polyfit(temp, sens, 3) + if noResample: + coef_mag_1_x = np.polyfit(temp_rel, correction_x, 3) + else: + temp, sens = resampleWithDeltaX(temp_rel, correction_x) + coef_mag_1_x = np.polyfit(temp, sens, 3) - mag_1_params['TC_M1_X3_2'] = coef_mag_1_z[0] - mag_1_params['TC_M1_X2_2'] = coef_mag_1_z[1] - mag_1_params['TC_M1_X1_2'] = coef_mag_1_z[2] - mag_1_params['TC_M1_X0_2'] = coef_mag_1_z[3] - fit_coef_mag_1_z = np.poly1d(coef_mag_1_z) - correction_z_resample = fit_coef_mag_1_z(temp_rel_resample) + mag_1_params['TC_M1_X3_0'] = coef_mag_1_x[0] + mag_1_params['TC_M1_X2_0'] = coef_mag_1_x[1] + mag_1_params['TC_M1_X1_0'] = coef_mag_1_x[2] + mag_1_params['TC_M1_X0_0'] = coef_mag_1_x[3] - # mag 1 vs temperature - plt.figure(10,figsize=(20,13)) + fit_coef_mag_1_x = np.poly1d(coef_mag_1_x) + correction_x_resample = fit_coef_mag_1_x(temp_rel_resample) - # draw plots - plt.subplot(3,1,1) - plt.plot(sensor_mag_1['temperature'],correction_x,'b') - plt.plot(temp_resample,correction_x_resample,'r') - plt.title('Mag 1 ({}) Bias vs Temperature'.format(mag_1_params['TC_M1_ID'])) - plt.ylabel('X bias (Gauss)') - plt.xlabel('temperature (degC)') - plt.grid() + # fit Y axis + correction_y = sensor_mag_1['y'] - np.median(sensor_mag_1['y']) - # draw plots - plt.subplot(3,1,2) - plt.plot(sensor_mag_1['temperature'],correction_y,'b') - plt.plot(temp_resample,correction_y_resample,'r') - plt.ylabel('Y bias (Gauss)') - plt.xlabel('temperature (degC)') - plt.grid() + if noResample: + coef_mag_1_y = np.polyfit(temp_rel,correction_y,3) + else: + temp, sens = resampleWithDeltaX(temp_rel,correction_y) + coef_mag_1_y = np.polyfit(temp, sens ,3) - # draw plots - plt.subplot(3,1,3) - plt.plot(sensor_mag_1['temperature'],correction_z,'b') - plt.plot(temp_resample,correction_z_resample,'r') - plt.ylabel('Z bias (Gauss)') - plt.xlabel('temperature (degC)') - plt.grid() + mag_1_params['TC_M1_X3_1'] = coef_mag_1_y[0] + mag_1_params['TC_M1_X2_1'] = coef_mag_1_y[1] + mag_1_params['TC_M1_X1_1'] = coef_mag_1_y[2] + mag_1_params['TC_M1_X0_1'] = coef_mag_1_y[3] - pp.savefig() + fit_coef_mag_1_y = np.poly1d(coef_mag_1_y) + correction_y_resample = fit_coef_mag_1_y(temp_rel_resample) + + # fit Z axis + correction_z = sensor_mag_1['z'] - np.median(sensor_mag_1['z']) + + if noResample: + coef_mag_1_z = np.polyfit(temp_rel,correction_z, 3) + else: + temp, sens = resampleWithDeltaX(temp_rel,correction_z) + coef_mag_1_z = np.polyfit(temp, sens, 3) + + mag_1_params['TC_M1_X3_2'] = coef_mag_1_z[0] + mag_1_params['TC_M1_X2_2'] = coef_mag_1_z[1] + mag_1_params['TC_M1_X1_2'] = coef_mag_1_z[2] + mag_1_params['TC_M1_X0_2'] = coef_mag_1_z[3] + + fit_coef_mag_1_z = np.poly1d(coef_mag_1_z) + correction_z_resample = fit_coef_mag_1_z(temp_rel_resample) + + # mag 1 vs temperature + plt.figure(10,figsize=(20,13)) + + # draw plots + plt.subplot(3,1,1) + plt.plot(sensor_mag_1['temperature'],correction_x,'b') + plt.plot(temp_resample,correction_x_resample,'r') + plt.title('Mag 1 ({}) Bias vs Temperature'.format(mag_1_params['TC_M1_ID'])) + plt.ylabel('X bias (Gauss)') + plt.xlabel('temperature (degC)') + plt.grid() + + # draw plots + plt.subplot(3,1,2) + plt.plot(sensor_mag_1['temperature'],correction_y,'b') + plt.plot(temp_resample,correction_y_resample,'r') + plt.ylabel('Y bias (Gauss)') + plt.xlabel('temperature (degC)') + plt.grid() + + # draw plots + plt.subplot(3,1,3) + plt.plot(sensor_mag_1['temperature'],correction_z,'b') + plt.plot(temp_resample,correction_z_resample,'r') + plt.ylabel('Z bias (Gauss)') + plt.xlabel('temperature (degC)') + plt.grid() + + pp.savefig() ################################################################################# @@ -1314,95 +1386,108 @@ mag_2_params = { } # curve fit the data for mag 2 corrections -if num_mags >= 3 and not math.isnan(sensor_mag_2['temperature'][0]): - mag_2_params['TC_M2_ID'] = int(np.median(sensor_mag_2['device_id'])) +if num_mags >= 3: + if math.isnan(sensor_mag_2['temperature'][0]): + if not math.isnan(sensor_baro_0['temperature'][0]): + sensor_mag_2['temperature'] = sensor_baro_0['temperature'] - # find the min, max and reference temperature - mag_2_params['TC_M2_TMIN'] = np.amin(sensor_mag_2['temperature']) - mag_2_params['TC_M2_TMAX'] = np.amax(sensor_mag_2['temperature']) - mag_2_params['TC_M2_TREF'] = 0.5 * (mag_2_params['TC_M2_TMIN'] + mag_2_params['TC_M2_TMAX']) - temp_rel = sensor_mag_2['temperature'] - mag_2_params['TC_M2_TREF'] - temp_rel_resample = np.linspace(mag_2_params['TC_M2_TMIN']-mag_2_params['TC_M2_TREF'], mag_2_params['TC_M2_TMAX']-mag_2_params['TC_M2_TREF'], 100) - temp_resample = temp_rel_resample + mag_2_params['TC_M2_TREF'] + if not math.isnan(sensor_mag_2['temperature'][0]): - sensor_mag_2['x'] = median_filter(sensor_mag_2['x']) - sensor_mag_2['y'] = median_filter(sensor_mag_2['y']) - sensor_mag_2['z'] = median_filter(sensor_mag_2['z']) + mag_2_params['TC_M2_ID'] = int(np.median(sensor_mag_2['device_id'])) - # fit X axis - correction_x = sensor_mag_2['x'] - np.median(sensor_mag_2['x']) - if noResample: - coef_mag_2_x = np.polyfit(temp_rel,correction_x, 3) - else: - temp, sens = resampleWithDeltaX(temp_rel, correction_x) - coef_mag_2_x = np.polyfit(temp, sens, 3) + # find the min, max and reference temperature + mag_2_params['TC_M2_TMIN'] = np.amin(sensor_mag_2['temperature']) + mag_2_params['TC_M2_TMAX'] = np.amax(sensor_mag_2['temperature']) + mag_2_params['TC_M2_TREF'] = 0.5 * (mag_2_params['TC_M2_TMIN'] + mag_2_params['TC_M2_TMAX']) - mag_2_params['TC_M2_X3_0'] = coef_mag_2_x[0] - mag_2_params['TC_M2_X2_0'] = coef_mag_2_x[1] - mag_2_params['TC_M2_X1_0'] = coef_mag_2_x[2] - mag_2_params['TC_M2_X0_0'] = coef_mag_2_x[3] - fit_coef_mag_2_x = np.poly1d(coef_mag_2_x) - correction_x_resample = fit_coef_mag_2_x(temp_rel_resample) + temp_rel = sensor_mag_2['temperature'] - mag_2_params['TC_M2_TREF'] + temp_rel_resample = np.linspace(mag_2_params['TC_M2_TMIN']-mag_2_params['TC_M2_TREF'], mag_2_params['TC_M2_TMAX']-mag_2_params['TC_M2_TREF'], 100) + temp_resample = temp_rel_resample + mag_2_params['TC_M2_TREF'] - # fit Y axis - correction_y = sensor_mag_2['y'] - np.median(sensor_mag_2['y']) - if noResample: - coef_mag_2_y = np.polyfit(temp_rel,correction_y,3) - else: - temp, sens = resampleWithDeltaX(temp_rel,correction_y) - coef_mag_2_y = np.polyfit(temp, sens ,3) + sensor_mag_2['x'] = median_filter(sensor_mag_2['x']) + sensor_mag_2['y'] = median_filter(sensor_mag_2['y']) + sensor_mag_2['z'] = median_filter(sensor_mag_2['z']) - mag_2_params['TC_M2_X3_1'] = coef_mag_2_y[0] - mag_2_params['TC_M2_X2_1'] = coef_mag_2_y[1] - mag_2_params['TC_M2_X1_1'] = coef_mag_2_y[2] - mag_2_params['TC_M2_X0_1'] = coef_mag_2_y[3] - fit_coef_mag_2_y = np.poly1d(coef_mag_2_y) - correction_y_resample = fit_coef_mag_2_y(temp_rel_resample) + # fit X axis + correction_x = sensor_mag_2['x'] - np.median(sensor_mag_2['x']) - # fit Z axis - correction_z = sensor_mag_2['z'] - np.median(sensor_mag_2['z']) - if noResample: - coef_mag_2_z = np.polyfit(temp_rel,correction_z,3) - else: - temp, sens = resampleWithDeltaX(temp_rel,correction_z) - coef_mag_2_z = np.polyfit(temp, sens ,3) + if noResample: + coef_mag_2_x = np.polyfit(temp_rel,correction_x, 3) + else: + temp, sens = resampleWithDeltaX(temp_rel, correction_x) + coef_mag_2_x = np.polyfit(temp, sens, 3) - mag_2_params['TC_M2_X3_2'] = coef_mag_2_z[0] - mag_2_params['TC_M2_X2_2'] = coef_mag_2_z[1] - mag_2_params['TC_M2_X1_2'] = coef_mag_2_z[2] - mag_2_params['TC_M2_X0_2'] = coef_mag_2_z[3] - fit_coef_mag_2_z = np.poly1d(coef_mag_2_z) - correction_z_resample = fit_coef_mag_2_z(temp_rel_resample) + mag_2_params['TC_M2_X3_0'] = coef_mag_2_x[0] + mag_2_params['TC_M2_X2_0'] = coef_mag_2_x[1] + mag_2_params['TC_M2_X1_0'] = coef_mag_2_x[2] + mag_2_params['TC_M2_X0_0'] = coef_mag_2_x[3] - # mag 2 vs temperature - plt.figure(11,figsize=(20,13)) + fit_coef_mag_2_x = np.poly1d(coef_mag_2_x) + correction_x_resample = fit_coef_mag_2_x(temp_rel_resample) - # draw plots - plt.subplot(3,1,1) - plt.plot(sensor_mag_2['temperature'],correction_x,'b') - plt.plot(temp_resample,correction_x_resample,'r') - plt.title('Mag 2 ({}) Bias vs Temperature'.format(mag_2_params['TC_M2_ID'])) - plt.ylabel('X bias (Gauss)') - plt.xlabel('temperature (degC)') - plt.grid() + # fit Y axis + correction_y = sensor_mag_2['y'] - np.median(sensor_mag_2['y']) - # draw plots - plt.subplot(3,1,2) - plt.plot(sensor_mag_2['temperature'],correction_y,'b') - plt.plot(temp_resample,correction_y_resample,'r') - plt.ylabel('Y bias (Gauss)') - plt.xlabel('temperature (degC)') - plt.grid() + if noResample: + coef_mag_2_y = np.polyfit(temp_rel,correction_y,3) + else: + temp, sens = resampleWithDeltaX(temp_rel,correction_y) + coef_mag_2_y = np.polyfit(temp, sens ,3) - # draw plots - plt.subplot(3,1,3) - plt.plot(sensor_mag_2['temperature'],correction_z,'b') - plt.plot(temp_resample,correction_z_resample,'r') - plt.ylabel('Z bias (Gauss)') - plt.xlabel('temperature (degC)') - plt.grid() + mag_2_params['TC_M2_X3_1'] = coef_mag_2_y[0] + mag_2_params['TC_M2_X2_1'] = coef_mag_2_y[1] + mag_2_params['TC_M2_X1_1'] = coef_mag_2_y[2] + mag_2_params['TC_M2_X0_1'] = coef_mag_2_y[3] - pp.savefig() + fit_coef_mag_2_y = np.poly1d(coef_mag_2_y) + correction_y_resample = fit_coef_mag_2_y(temp_rel_resample) + + # fit Z axis + correction_z = sensor_mag_2['z'] - np.median(sensor_mag_2['z']) + + if noResample: + coef_mag_2_z = np.polyfit(temp_rel,correction_z,3) + else: + temp, sens = resampleWithDeltaX(temp_rel,correction_z) + coef_mag_2_z = np.polyfit(temp, sens ,3) + + mag_2_params['TC_M2_X3_2'] = coef_mag_2_z[0] + mag_2_params['TC_M2_X2_2'] = coef_mag_2_z[1] + mag_2_params['TC_M2_X1_2'] = coef_mag_2_z[2] + mag_2_params['TC_M2_X0_2'] = coef_mag_2_z[3] + + fit_coef_mag_2_z = np.poly1d(coef_mag_2_z) + correction_z_resample = fit_coef_mag_2_z(temp_rel_resample) + + # mag 2 vs temperature + plt.figure(11,figsize=(20,13)) + + # draw plots + plt.subplot(3,1,1) + plt.plot(sensor_mag_2['temperature'],correction_x,'b') + plt.plot(temp_resample,correction_x_resample,'r') + plt.title('Mag 2 ({}) Bias vs Temperature'.format(mag_2_params['TC_M2_ID'])) + plt.ylabel('X bias (Gauss)') + plt.xlabel('temperature (degC)') + plt.grid() + + # draw plots + plt.subplot(3,1,2) + plt.plot(sensor_mag_2['temperature'],correction_y,'b') + plt.plot(temp_resample,correction_y_resample,'r') + plt.ylabel('Y bias (Gauss)') + plt.xlabel('temperature (degC)') + plt.grid() + + # draw plots + plt.subplot(3,1,3) + plt.plot(sensor_mag_2['temperature'],correction_z,'b') + plt.plot(temp_resample,correction_z_resample,'r') + plt.ylabel('Z bias (Gauss)') + plt.xlabel('temperature (degC)') + plt.grid() + + pp.savefig() ################################################################################# @@ -1428,81 +1513,113 @@ mag_3_params = { 'TC_M3_X3_2':0.0 } -# curve fit the data for mag 2 corrections -if num_mags >= 4 and not math.isnan(sensor_mag_3['temperature'][0]): - mag_3_params['TC_M3_ID'] = int(np.median(sensor_mag_3['device_id'])) +# curve fit the data for mag 3 corrections +if num_mags >= 4: + if math.isnan(sensor_mag_3['temperature'][0]): + if not math.isnan(sensor_baro_0['temperature'][0]): + sensor_mag_3['temperature'] = sensor_baro_0['temperature'] - # find the min, max and reference temperature - mag_3_params['TC_M3_TMIN'] = np.amin(sensor_mag_3['temperature']) - mag_3_params['TC_M3_TMAX'] = np.amax(sensor_mag_3['temperature']) - mag_3_params['TC_M3_TREF'] = 0.5 * (mag_3_params['TC_M3_TMIN'] + mag_3_params['TC_M3_TMAX']) - temp_rel = sensor_mag_3['temperature'] - mag_3_params['TC_M3_TREF'] - temp_rel_resample = np.linspace(mag_3_params['TC_M3_TMIN']-mag_3_params['TC_M3_TREF'], mag_3_params['TC_M3_TMAX']-mag_3_params['TC_M3_TREF'], 100) - temp_resample = temp_rel_resample + mag_3_params['TC_M3_TREF'] + if not math.isnan(sensor_mag_3['temperature'][0]): - sensor_mag_3['x'] = median_filter(sensor_mag_3['x']) - sensor_mag_3['y'] = median_filter(sensor_mag_3['y']) - sensor_mag_3['z'] = median_filter(sensor_mag_3['z']) + mag_3_params['TC_M3_ID'] = int(np.median(sensor_mag_3['device_id'])) - # fit X axis - correction_x = sensor_mag_3['x'] - np.median(sensor_mag_3['x']) - coef_mag_3_x = np.polyfit(temp_rel, correction_x, 3) - mag_3_params['TC_M3_X3_0'] = coef_mag_3_x[0] - mag_3_params['TC_M3_X2_0'] = coef_mag_3_x[1] - mag_3_params['TC_M3_X1_0'] = coef_mag_3_x[2] - mag_3_params['TC_M3_X0_0'] = coef_mag_3_x[3] - fit_coef_mag_3_x = np.poly1d(coef_mag_3_x) - correction_x_resample = fit_coef_mag_3_x(temp_rel_resample) + # find the min, max and reference temperature + mag_3_params['TC_M3_TMIN'] = np.amin(sensor_mag_3['temperature']) + mag_3_params['TC_M3_TMAX'] = np.amax(sensor_mag_3['temperature']) + mag_3_params['TC_M3_TREF'] = 0.5 * (mag_3_params['TC_M3_TMIN'] + mag_3_params['TC_M3_TMAX']) - # fit Y axis - correction_y = sensor_mag_3['y'] - np.median(sensor_mag_3['y']) - coef_mag_3_y = np.polyfit(temp_rel, correction_y, 3) - mag_3_params['TC_M3_X3_1'] = coef_mag_3_y[0] - mag_3_params['TC_M3_X2_1'] = coef_mag_3_y[1] - mag_3_params['TC_M3_X1_1'] = coef_mag_3_y[2] - mag_3_params['TC_M3_X0_1'] = coef_mag_3_y[3] - fit_coef_mag_3_y = np.poly1d(coef_mag_3_y) - correction_y_resample = fit_coef_mag_3_y(temp_rel_resample) + temp_rel = sensor_mag_3['temperature'] - mag_3_params['TC_M3_TREF'] + temp_rel_resample = np.linspace(mag_3_params['TC_M3_TMIN']-mag_3_params['TC_M3_TREF'], mag_3_params['TC_M3_TMAX']-mag_3_params['TC_M3_TREF'], 100) + temp_resample = temp_rel_resample + mag_3_params['TC_M3_TREF'] - # fit Z axis - correction_z = sensor_mag_3['z'] - np.median(sensor_mag_3['z']) - coef_mag_3_z = np.polyfit(temp_rel, correction_z, 3) - mag_3_params['TC_M3_X3_2'] = coef_mag_3_z[0] - mag_3_params['TC_M3_X2_2'] = coef_mag_3_z[1] - mag_3_params['TC_M3_X1_2'] = coef_mag_3_z[2] - mag_3_params['TC_M3_X0_2'] = coef_mag_3_z[3] - fit_coef_mag_3_z = np.poly1d(coef_mag_3_z) - correction_z_resample = fit_coef_mag_3_z(temp_rel_resample) + # Delete the dataset first and last 10 seconds of data + sensor_mag_3 = np.delete(sensor_mag_3, range(0,1000), axis=['x']) + sensor_mag_3 = np.delete(sensor_mag_3, range(-1000,), axis=['x']) - # mag 3 vs temperature - plt.figure(12,figsize=(20,13)) + sensor_mag_3['x'] = median_filter(sensor_mag_3['x']) + sensor_mag_3['y'] = median_filter(sensor_mag_3['y']) + sensor_mag_3['z'] = median_filter(sensor_mag_3['z']) - # draw plots - plt.subplot(3,1,1) - plt.plot(sensor_mag_3['temperature'],correction_x,'b') - plt.plot(temp_resample,correction_x_resample,'r') - plt.title('Mag 3 ({}) Bias vs Temperature'.format(mag_3_params['TC_M3_ID'])) - plt.ylabel('X bias (Gauss)') - plt.xlabel('temperature (degC)') - plt.grid() + # fit X axis + correction_x = sensor_mag_3['x'] - np.median(sensor_mag_3['x']) - # draw plots - plt.subplot(3,1,2) - plt.plot(sensor_mag_3['temperature'],correction_y,'b') - plt.plot(temp_resample,correction_y_resample,'r') - plt.ylabel('Y bias (Gauss)') - plt.xlabel('temperature (degC)') - plt.grid() + if noResample: + coef_mag_3_x = np.polyfit(temp_rel,correction_x, 3) + else: + temp, sens = resampleWithDeltaX(temp_rel,correction_x) + coef_mag_3_x = np.polyfit(temp, sens, 3) - # draw plots - plt.subplot(3,1,3) - plt.plot(sensor_mag_3['temperature'],correction_z,'b') - plt.plot(temp_resample,correction_z_resample,'r') - plt.ylabel('Z bias (Gauss)') - plt.xlabel('temperature (degC)') - plt.grid() + mag_3_params['TC_M3_X3_0'] = coef_mag_3_x[0] + mag_3_params['TC_M3_X2_0'] = coef_mag_3_x[1] + mag_3_params['TC_M3_X1_0'] = coef_mag_3_x[2] + mag_3_params['TC_M3_X0_0'] = coef_mag_3_x[3] - pp.savefig() + fit_coef_mag_3_x = np.poly1d(coef_mag_3_x) + correction_x_resample = fit_coef_mag_3_x(temp_rel_resample) + + # fit Y axis + correction_y = sensor_mag_3['y'] - np.median(sensor_mag_3['y']) + + if noResample: + coef_mag_3_y = np.polyfit(temp_rel, correction_y, 3) + else: + temp, sens = resampleWithDeltaX(temp_rel,correction_y) + coef_mag_3_y = np.polyfit(temp, sens, 3) + + mag_3_params['TC_M3_X3_1'] = coef_mag_3_y[0] + mag_3_params['TC_M3_X2_1'] = coef_mag_3_y[1] + mag_3_params['TC_M3_X1_1'] = coef_mag_3_y[2] + mag_3_params['TC_M3_X0_1'] = coef_mag_3_y[3] + + fit_coef_mag_3_y = np.poly1d(coef_mag_3_y) + correction_y_resample = fit_coef_mag_3_y(temp_rel_resample) + + # fit Z axis + correction_z = sensor_mag_3['z'] - np.median(sensor_mag_3['z']) + + if noResample: + coef_mag_3_z = np.polyfit(temp_rel,correction_z, 3) + else: + temp, sens = resampleWithDeltaX(temp_rel,correction_z) + coef_mag_3_z = np.polyfit(temp, sens, 3) + + mag_3_params['TC_M3_X3_2'] = coef_mag_3_z[0] + mag_3_params['TC_M3_X2_2'] = coef_mag_3_z[1] + mag_3_params['TC_M3_X1_2'] = coef_mag_3_z[2] + mag_3_params['TC_M3_X0_2'] = coef_mag_3_z[3] + + fit_coef_mag_3_z = np.poly1d(coef_mag_3_z) + correction_z_resample = fit_coef_mag_3_z(temp_rel_resample) + + # mag 3 vs temperature + plt.figure(12,figsize=(20,13)) + + # draw plots + plt.subplot(3,1,1) + plt.plot(sensor_mag_3['temperature'],correction_x,'b') + plt.plot(temp_resample,correction_x_resample,'r') + plt.title('Mag 3 ({}) Bias vs Temperature'.format(mag_3_params['TC_M3_ID'])) + plt.ylabel('X bias (Gauss)') + plt.xlabel('temperature (degC)') + plt.grid() + + # draw plots + plt.subplot(3,1,2) + plt.plot(sensor_mag_3['temperature'],correction_y,'b') + plt.plot(temp_resample,correction_y_resample,'r') + plt.ylabel('Y bias (Gauss)') + plt.xlabel('temperature (degC)') + plt.grid() + + # draw plots + plt.subplot(3,1,3) + plt.plot(sensor_mag_3['temperature'],correction_z,'b') + plt.plot(temp_resample,correction_z_resample,'r') + plt.ylabel('Z bias (Gauss)') + plt.xlabel('temperature (degC)') + plt.grid() + + pp.savefig() ################################################################################# @@ -1529,6 +1646,7 @@ baro_0_params['TC_B0_ID'] = int(np.median(sensor_baro_0['device_id'])) 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'] @@ -1549,6 +1667,7 @@ 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) @@ -1588,6 +1707,7 @@ if num_baros >= 2: baro_1_params['TC_B1_TMIN'] = np.amin(sensor_baro_1['temperature']) baro_1_params['TC_B1_TMAX'] = np.amax(sensor_baro_1['temperature']) baro_1_params['TC_B1_TREF'] = 0.5 * (baro_1_params['TC_B1_TMIN'] + baro_1_params['TC_B1_TMAX']) + temp_rel = sensor_baro_1['temperature'] - baro_1_params['TC_B1_TREF'] temp_rel_resample = np.linspace(baro_1_params['TC_B1_TMIN']-baro_1_params['TC_B1_TREF'], baro_1_params['TC_B1_TMAX']-baro_1_params['TC_B1_TREF'], 100) temp_resample = temp_rel_resample + baro_1_params['TC_B1_TREF'] @@ -1608,6 +1728,7 @@ if num_baros >= 2: baro_1_params['TC_B1_X2'] = coef_baro_1_x[3] baro_1_params['TC_B1_X1'] = coef_baro_1_x[4] baro_1_params['TC_B1_X0'] = coef_baro_1_x[5] + fit_coef_baro_1_x = np.poly1d(coef_baro_1_x) baro_1_x_resample = fit_coef_baro_1_x(temp_rel_resample) @@ -1636,7 +1757,6 @@ baro_2_params = { 'TC_B2_X3':0.0, 'TC_B2_X4':0.0, 'TC_B2_X5':0.0, -'TC_B2_SCL':1.0, } if num_baros >= 3: @@ -1648,6 +1768,7 @@ if num_baros >= 3: baro_2_params['TC_B2_TMIN'] = np.amin(sensor_baro_2['temperature']) baro_2_params['TC_B2_TMAX'] = np.amax(sensor_baro_2['temperature']) baro_2_params['TC_B2_TREF'] = 0.5 * (baro_2_params['TC_B2_TMIN'] + baro_2_params['TC_B2_TMAX']) + temp_rel = sensor_baro_2['temperature'] - baro_2_params['TC_B2_TREF'] temp_rel_resample = np.linspace(baro_2_params['TC_B2_TMIN']-baro_2_params['TC_B2_TREF'], baro_2_params['TC_B2_TMAX']-baro_2_params['TC_B2_TREF'], 100) temp_resample = temp_rel_resample + baro_2_params['TC_B2_TREF'] @@ -1668,6 +1789,7 @@ if num_baros >= 3: baro_2_params['TC_B2_X2'] = coef_baro_2_x[3] baro_2_params['TC_B2_X1'] = coef_baro_2_x[4] baro_2_params['TC_B2_X0'] = coef_baro_2_x[5] + fit_coef_baro_2_x = np.poly1d(coef_baro_2_x) baro_2_x_resample = fit_coef_baro_2_x(temp_rel_resample) @@ -1696,7 +1818,6 @@ baro_3_params = { 'TC_B3_X3':0.0, 'TC_B3_X4':0.0, 'TC_B3_X5':0.0, -'TC_B3_SCL':1.0, } if num_baros >= 4: @@ -1708,6 +1829,7 @@ if num_baros >= 4: baro_3_params['TC_B3_TMIN'] = np.amin(sensor_baro_3['temperature']) baro_3_params['TC_B3_TMAX'] = np.amax(sensor_baro_3['temperature']) baro_3_params['TC_B3_TREF'] = 0.5 * (baro_3_params['TC_B3_TMIN'] + baro_3_params['TC_B3_TMAX']) + temp_rel = sensor_baro_3['temperature'] - baro_3_params['TC_B3_TREF'] temp_rel_resample = np.linspace(baro_3_params['TC_B3_TMIN']-baro_3_params['TC_B3_TREF'], baro_3_params['TC_B3_TMAX']-baro_3_params['TC_B3_TREF'], 100) temp_resample = temp_rel_resample + baro_3_params['TC_B3_TREF'] @@ -1723,6 +1845,7 @@ if num_baros >= 4: baro_3_params['TC_B3_X2'] = coef_baro_3_x[3] baro_3_params['TC_B3_X1'] = coef_baro_3_x[4] baro_3_params['TC_B3_X0'] = coef_baro_3_x[5] + fit_coef_baro_3_x = np.poly1d(coef_baro_3_x) baro_3_x_resample = fit_coef_baro_3_x(temp_rel_resample) diff --git a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp index 566d3a22ba..73507df074 100644 --- a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp +++ b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp @@ -220,13 +220,6 @@ void IST8308::RunImpl() perf_count(_bad_register_perf); Reset(); } - - } else { - // periodically update temperature (~1 Hz) - if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { - UpdateTemperature(); - _temperature_update_timestamp = now; - } } } @@ -298,15 +291,3 @@ void IST8308::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t cle RegisterWrite(reg, val); } } - -void IST8308::UpdateTemperature() -{ - // Isentek magnetometers do not have a temperature sensor, so we will to use the baro's value. - sensor_baro_s sensor_baro; - - if (_sensor_baro_sub[0].update(&sensor_baro)) { - if (PX4_ISFINITE(sensor_baro.temperature)) { - _px4_mag.set_temperature(sensor_baro.temperature); - } - } -} \ No newline at end of file diff --git a/src/drivers/magnetometer/isentek/ist8308/IST8308.hpp b/src/drivers/magnetometer/isentek/ist8308/IST8308.hpp index abd02f24d7..1a958b8e47 100644 --- a/src/drivers/magnetometer/isentek/ist8308/IST8308.hpp +++ b/src/drivers/magnetometer/isentek/ist8308/IST8308.hpp @@ -46,10 +46,7 @@ #include #include #include -#include #include -#include -#include using namespace iSentek_IST8308; @@ -64,17 +61,9 @@ public: void RunImpl(); int init() override; - void print_status() override; private: - enum class STATE : uint8_t { - RESET, - WAIT_FOR_RESET, - CONFIGURE, - READ, - } _state{STATE::RESET}; - // Sensor Configuration struct register_config_t { Register reg; @@ -91,15 +80,9 @@ private: bool RegisterCheck(const register_config_t ®_cfg); uint8_t RegisterRead(Register reg); - void RegisterWrite(Register reg, uint8_t value); - void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); - void UpdateTemperature(); - - uORB::SubscriptionMultiArray _sensor_baro_sub{ORB_ID::sensor_baro}; - PX4Magnetometer _px4_mag; perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; @@ -108,14 +91,17 @@ private: hrt_abstime _reset_timestamp{0}; hrt_abstime _last_config_check_timestamp{0}; - hrt_abstime _temperature_update_timestamp{0}; - int _failure_count{0}; + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + READ, + } _state{STATE::RESET}; + uint8_t _checked_register{0}; - static constexpr uint8_t size_register_cfg{6}; - register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits { Register::ACTR, 0, ACTR_BIT::SUSPEND_EN }, diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp index d7967af511..b7d8512ce9 100644 --- a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp @@ -188,7 +188,6 @@ void IST8310::RunImpl() _px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf)); _px4_mag.update(now, x, y, z); - UpdateTemperature(); success = true; @@ -223,13 +222,6 @@ void IST8310::RunImpl() Reset(); return; } - - } else { - // periodically update temperature (~1 Hz) - if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { - UpdateTemperature(); - _temperature_update_timestamp = now; - } } // initiate next measurement @@ -304,15 +296,3 @@ void IST8310::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t cle RegisterWrite(reg, val); } } - -void IST8310::UpdateTemperature() -{ - // Isentek magnetometers do not have a temperature sensor, so we will to use the baro's value. - sensor_baro_s sensor_baro; - - if (_sensor_baro_sub[0].update(&sensor_baro)) { - if (PX4_ISFINITE(sensor_baro.temperature)) { - _px4_mag.set_temperature(sensor_baro.temperature); - } - } -} \ No newline at end of file diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp index 6ea5a78162..a251f49cf0 100644 --- a/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp @@ -46,10 +46,7 @@ #include #include #include -#include #include -#include -#include using namespace iSentek_IST8310; @@ -64,18 +61,9 @@ public: void RunImpl(); int init() override; - void print_status() override; private: - enum class STATE : uint8_t { - RESET, - WAIT_FOR_RESET, - CONFIGURE, - MEASURE, - READ, - } _state{STATE::RESET}; - // Sensor Configuration struct register_config_t { Register reg; @@ -92,15 +80,9 @@ private: bool RegisterCheck(const register_config_t ®_cfg); uint8_t RegisterRead(Register reg); - void RegisterWrite(Register reg, uint8_t value); - void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); - void UpdateTemperature(); - - uORB::SubscriptionMultiArray _sensor_baro_sub{ORB_ID::sensor_baro}; - PX4Magnetometer _px4_mag; perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; @@ -109,14 +91,18 @@ private: hrt_abstime _reset_timestamp{0}; hrt_abstime _last_config_check_timestamp{0}; - hrt_abstime _temperature_update_timestamp{0}; - int _failure_count{0}; + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + MEASURE, + READ, + } _state{STATE::RESET}; + uint8_t _checked_register{0}; - static constexpr uint8_t size_register_cfg{4}; - register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits { Register::CNTL2, 0, CNTL2_BIT::SRST }, diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index c2c6e94275..3e729489a9 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -344,17 +344,17 @@ void LoggedTopics::add_estimator_replay_topics() void LoggedTopics::add_thermal_calibration_topics() { - add_topic_multi("sensor_accel", 100, 3); - add_topic_multi("sensor_baro", 100, 3); - add_topic_multi("sensor_gyro", 100, 3); + add_topic_multi("sensor_accel", 100, 4); + add_topic_multi("sensor_baro", 100, 4); + add_topic_multi("sensor_gyro", 100, 4); add_topic_multi("sensor_mag", 100, 4); } void LoggedTopics::add_sensor_comparison_topics() { - add_topic_multi("sensor_accel", 100, 3); - add_topic_multi("sensor_baro", 100, 3); - add_topic_multi("sensor_gyro", 100, 3); + add_topic_multi("sensor_accel", 100, 4); + add_topic_multi("sensor_baro", 100, 4); + add_topic_multi("sensor_gyro", 100, 4); add_topic_multi("sensor_mag", 100, 4); } diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index af2f948f3a..05373e0e22 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -215,10 +215,12 @@ void VehicleAcceleration::Run() // update corrections first to set _selected_sensor bool selection_updated = SensorSelectionUpdate(); - _calibration.SensorCorrectionsUpdate(selection_updated); - SensorBiasUpdate(selection_updated); ParametersUpdate(); + _calibration.SensorCorrectionsUpdate(selection_updated); + + SensorBiasUpdate(selection_updated); + // require valid sensor sample rate to run if (!PX4_ISFINITE(_filter_sample_rate)) { CheckAndUpdateFilters(); diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index e688a788e9..0264c930de 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -790,6 +790,8 @@ void VehicleAngularVelocity::Run() const hrt_abstime time_now_us = hrt_absolute_time(); + ParametersUpdate(); + // update corrections first to set _selected_sensor const bool selection_updated = SensorSelectionUpdate(time_now_us); @@ -801,9 +803,8 @@ void VehicleAngularVelocity::Run() } } - ParametersUpdate(); - _calibration.SensorCorrectionsUpdate(selection_updated); + SensorBiasUpdate(selection_updated); if (_reset_filters) { diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 8ee9380f30..a0ce7de6ae 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -369,8 +369,8 @@ void VehicleMagnetometer::UpdatePowerCompensation() if (_vehicle_thrust_setpoint_0_sub.update(&vehicle_thrust_setpoint)) { const matrix::Vector3f thrust_setpoint = matrix::Vector3f(vehicle_thrust_setpoint.xyz); - for (auto &cal : _calibration) { - cal.UpdatePower(thrust_setpoint.length()); + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + _calibration[i].UpdatePower(thrust_setpoint.length()); } } @@ -382,14 +382,14 @@ void VehicleMagnetometer::UpdatePowerCompensation() if (_battery_status_sub.update(&bat_stat)) { float power = bat_stat.current_a * 0.001f; // current in [kA] - for (auto &cal : _calibration) { - cal.UpdatePower(power); + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + _calibration[i].UpdatePower(power); } } } else { - for (auto &cal : _calibration) { - cal.UpdatePower(0.f); + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + _calibration[i].UpdatePower(0.f); } } } @@ -412,6 +412,10 @@ void VehicleMagnetometer::Run() } } + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + _calibration[i].SensorCorrectionsUpdate(); + } + UpdatePowerCompensation(); UpdateMagBiasEstimate(); diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp index 042a401476..c90398fe1f 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp @@ -146,6 +146,7 @@ private: Current_inst0, Current_inst1 }; + MagCompensationType _mag_comp_type{MagCompensationType::Disabled}; perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; @@ -158,6 +159,7 @@ private: uint64_t _timestamp_sample_sum[MAX_SENSOR_COUNT] {}; matrix::Vector3f _data_sum[MAX_SENSOR_COUNT] {}; + int _data_sum_count[MAX_SENSOR_COUNT] {}; hrt_abstime _last_publication_timestamp[MAX_SENSOR_COUNT] {}; diff --git a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp index 5a8321e6a0..1ed3283c32 100644 --- a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp +++ b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp @@ -144,16 +144,16 @@ void TemperatureCompensationModule::accelPoll() // For each accel instance for (uint8_t uorb_index = 0; uorb_index < ACCEL_COUNT_MAX; uorb_index++) { - sensor_accel_s report; + sensor_accel_s sensor_accel; - // Grab temperature from report - if (_accel_subs[uorb_index].update(&report)) { - if (PX4_ISFINITE(report.temperature)) { + // Grab temperature from accel + if (_accel_subs[uorb_index].update(&sensor_accel)) { + if (PX4_ISFINITE(sensor_accel.temperature)) { // Update the offsets and mark for publication if they've changed - if (_temperature_compensation.update_offsets_accel(uorb_index, report.temperature, offsets[uorb_index]) == 2) { + if (_temperature_compensation.update_offsets_accel(uorb_index, sensor_accel.temperature, offsets[uorb_index]) == 2) { - _corrections.accel_device_ids[uorb_index] = report.device_id; - _corrections.accel_temperature[uorb_index] = report.temperature; + _corrections.accel_device_ids[uorb_index] = sensor_accel.device_id; + _corrections.accel_temperature[uorb_index] = sensor_accel.temperature; _corrections_changed = true; } } @@ -167,16 +167,28 @@ void TemperatureCompensationModule::gyroPoll() // For each gyro instance for (uint8_t uorb_index = 0; uorb_index < GYRO_COUNT_MAX; uorb_index++) { - sensor_gyro_s report; + sensor_gyro_s sensor_gyro; - // Grab temperature from report - if (_gyro_subs[uorb_index].update(&report)) { - if (PX4_ISFINITE(report.temperature)) { + // Grab temperature from gyro + if (_gyro_subs[uorb_index].update(&sensor_gyro)) { + if (PX4_ISFINITE(sensor_gyro.temperature)) { // Update the offsets and mark for publication if they've changed - if (_temperature_compensation.update_offsets_gyro(uorb_index, report.temperature, offsets[uorb_index]) == 2) { + if (_temperature_compensation.update_offsets_gyro(uorb_index, sensor_gyro.temperature, offsets[uorb_index]) == 2) { - _corrections.gyro_device_ids[uorb_index] = report.device_id; - _corrections.gyro_temperature[uorb_index] = report.temperature; + _corrections.gyro_device_ids[uorb_index] = sensor_gyro.device_id; + _corrections.gyro_temperature[uorb_index] = sensor_gyro.temperature; + _corrections_changed = true; + } + + } else { + + _corrections.gyro_device_ids[uorb_index] = sensor_gyro.device_id; + + // Use accelerometer of the same instance if gyro temperature was NAN. + sensor_accel_s sensor_accel; + + if (_accel_subs[uorb_index].update(&sensor_accel)) { + _corrections.gyro_temperature[uorb_index] = sensor_accel.temperature; _corrections_changed = true; } } @@ -190,16 +202,28 @@ void TemperatureCompensationModule::magPoll() // For each mag instance for (uint8_t uorb_index = 0; uorb_index < MAG_COUNT_MAX; uorb_index++) { - sensor_mag_s report; + sensor_mag_s sensor_mag; // Grab temperature from report - if (_mag_subs[uorb_index].update(&report)) { - if (PX4_ISFINITE(report.temperature)) { + if (_mag_subs[uorb_index].update(&sensor_mag)) { + if (PX4_ISFINITE(sensor_mag.temperature)) { // Update the offsets and mark for publication if they've changed - if (_temperature_compensation.update_offsets_mag(uorb_index, report.temperature, offsets[uorb_index]) == 2) { + if (_temperature_compensation.update_offsets_mag(uorb_index, sensor_mag.temperature, offsets[uorb_index]) == 2) { - _corrections.mag_device_ids[uorb_index] = report.device_id; - _corrections.mag_temperature[uorb_index] = report.temperature; + _corrections.mag_device_ids[uorb_index] = sensor_mag.device_id; + _corrections.mag_temperature[uorb_index] = sensor_mag.temperature; + _corrections_changed = true; + } + + } else { + + _corrections.mag_device_ids[uorb_index] = sensor_mag.device_id; + + // Use primary baro instance if mag temperature was NAN. + sensor_baro_s sensor_baro; + + if (_accel_subs[0].update(&sensor_baro)) { + _corrections.mag_temperature[uorb_index] = sensor_baro.temperature; _corrections_changed = true; } } @@ -213,16 +237,28 @@ void TemperatureCompensationModule::baroPoll() // For each baro instance for (uint8_t uorb_index = 0; uorb_index < BARO_COUNT_MAX; uorb_index++) { - sensor_baro_s report; + sensor_baro_s sensor_baro; // Grab temperature from report - if (_baro_subs[uorb_index].update(&report)) { - if (PX4_ISFINITE(report.temperature)) { + if (_baro_subs[uorb_index].update(&sensor_baro)) { + if (PX4_ISFINITE(sensor_baro.temperature)) { // Update the offsets and mark for publication if they've changed - if (_temperature_compensation.update_offsets_baro(uorb_index, report.temperature, offsets[uorb_index]) == 2) { + if (_temperature_compensation.update_offsets_baro(uorb_index, sensor_baro.temperature, offsets[uorb_index]) == 2) { - _corrections.baro_device_ids[uorb_index] = report.device_id; - _corrections.baro_temperature[uorb_index] = report.temperature; + _corrections.baro_device_ids[uorb_index] = sensor_baro.device_id; + _corrections.baro_temperature[uorb_index] = sensor_baro.temperature; + _corrections_changed = true; + } + + } else { + + _corrections.baro_device_ids[uorb_index] = sensor_baro.device_id; + + // Use primary accelerometer instance if baro temperature was NAN. + sensor_accel_s sensor_accel; + + if (_accel_subs[0].update(&sensor_accel)) { + _corrections.baro_temperature[uorb_index] = sensor_accel.temperature; _corrections_changed = true; } } diff --git a/src/modules/temperature_compensation/temp_comp_params_mag.c b/src/modules/temperature_compensation/temp_comp_params_mag.c index 1f1f31cb46..c71cbb6345 100644 --- a/src/modules/temperature_compensation/temp_comp_params_mag.c +++ b/src/modules/temperature_compensation/temp_comp_params_mag.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/temperature_compensation/temp_comp_params_mag_0.c b/src/modules/temperature_compensation/temp_comp_params_mag_0.c index 621fcfaaaa..68f7240e5c 100644 --- a/src/modules/temperature_compensation/temp_comp_params_mag_0.c +++ b/src/modules/temperature_compensation/temp_comp_params_mag_0.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/temperature_compensation/temp_comp_params_mag_1.c b/src/modules/temperature_compensation/temp_comp_params_mag_1.c index b7e38db737..a34b452999 100644 --- a/src/modules/temperature_compensation/temp_comp_params_mag_1.c +++ b/src/modules/temperature_compensation/temp_comp_params_mag_1.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/temperature_compensation/temp_comp_params_mag_2.c b/src/modules/temperature_compensation/temp_comp_params_mag_2.c index 8c71906f21..8843b64dff 100644 --- a/src/modules/temperature_compensation/temp_comp_params_mag_2.c +++ b/src/modules/temperature_compensation/temp_comp_params_mag_2.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/temperature_compensation/temp_comp_params_mag_3.c b/src/modules/temperature_compensation/temp_comp_params_mag_3.c index 7b44d3f59b..021e47dfa7 100644 --- a/src/modules/temperature_compensation/temp_comp_params_mag_3.c +++ b/src/modules/temperature_compensation/temp_comp_params_mag_3.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/temperature_compensation/temperature_calibration/mag.cpp b/src/modules/temperature_compensation/temperature_calibration/mag.cpp index 2eeba2dc57..3609d94f12 100644 --- a/src/modules/temperature_compensation/temperature_calibration/mag.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/mag.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/temperature_compensation/temperature_calibration/mag.h b/src/modules/temperature_compensation/temperature_calibration/mag.h index 3494531054..2e4d4e78f2 100644 --- a/src/modules/temperature_compensation/temperature_calibration/mag.h +++ b/src/modules/temperature_compensation/temperature_calibration/mag.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/temperature_compensation/temperature_calibration/task.cpp b/src/modules/temperature_compensation/temperature_calibration/task.cpp index 396bd81b1c..4262b4a0a2 100644 --- a/src/modules/temperature_compensation/temperature_calibration/task.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/task.cpp @@ -69,17 +69,13 @@ class TemperatureCalibration { public: - TemperatureCalibration(bool accel, bool gyro, bool mag, bool baro) - : _accel(accel) - , _gyro(gyro) - , _mag(mag) - , _baro(baro) {}; + TemperatureCalibration(bool accel, bool gyro, bool mag, bool baro) : + _accel(accel), _gyro(gyro), _mag(mag), _baro(baro) {}; ~TemperatureCalibration() = default; /** * Start task. - * * @return OK on success. */ int start();