diff --git a/Tools/process_sensor_caldata.py b/Tools/process_sensor_caldata.py index 625e5da426..0ec667ccc1 100755 --- a/Tools/process_sensor_caldata.py +++ b/Tools/process_sensor_caldata.py @@ -68,7 +68,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 gyro, accel 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 accel, gyro, mag, 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') @@ -87,6 +87,27 @@ noResample = args.noResample ulog = ULog(ulog_file_name, None) data = ulog.data_list +# extract accel data +num_accels = 0 +for d in data: + if d.name == 'sensor_accel': + if d.multi_id == 0: + sensor_accel_0 = d.data + print('found accel 0 data') + num_accels += 1 + elif d.multi_id == 1: + sensor_accel_1 = d.data + print('found accel 1 data') + num_accels += 1 + elif d.multi_id == 2: + sensor_accel_2 = d.data + print('found accel 2 data') + num_accels += 1 + elif d.multi_id == 3: + sensor_accel_3 = d.data + print('found accel 3 data') + num_accels += 1 + # extract gyro data num_gyros = 0 for d in data: @@ -108,26 +129,26 @@ for d in data: print('found gyro 3 data') num_gyros += 1 -# extract accel data -num_accels = 0 +# extract mag data +num_mags = 0 for d in data: - if d.name == 'sensor_accel': + if d.name == 'sensor_mag': if d.multi_id == 0: - sensor_accel_0 = d.data - print('found accel 0 data') - num_accels += 1 + sensor_mag_0 = d.data + print('found mag 0 data') + num_mags += 1 elif d.multi_id == 1: - sensor_accel_1 = d.data - print('found accel 1 data') - num_accels += 1 + sensor_mag_1 = d.data + print('found mag 1 data') + num_mags += 1 elif d.multi_id == 2: - sensor_accel_2 = d.data - print('found accel 2 data') - num_accels += 1 + sensor_mag_2 = d.data + print('found mag 2 data') + num_mags += 1 elif d.multi_id == 3: - sensor_accel_3 = d.data - print('found accel 3 data') - num_accels += 1 + sensor_mag_3 = d.data + print('found mag 3 data') + num_mags += 1 # extract baro data num_baros = 0 @@ -155,436 +176,7 @@ 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 -} - -# 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'])) - - # 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'] - - 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 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_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 - 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) - - 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 - 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) - - 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'.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() - -################################################################################# - -################################################################################# - -# 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 -} - -# 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'])) - - # 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'] - - 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 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_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 - 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) - - 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 - 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) - - 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'.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() - -################################################################################# - -################################################################################# - -# 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 -} - -# 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'])) - - # 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'] - - 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 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_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 - 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) - - 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 - 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) - - 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'.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() - -################################################################################# - -################################################################################# - -# define data dictionary of gyro 3 thermal correction parameters -gyro_3_params = { -'TC_G3_ID':0, -'TC_G3_TMIN':0.0, -'TC_G3_TMAX':0.0, -'TC_G3_TREF':0.0, -'TC_G3_X0_0':0.0, -'TC_G3_X1_0':0.0, -'TC_G3_X2_0':0.0, -'TC_G3_X3_0':0.0, -'TC_G3_X0_1':0.0, -'TC_G3_X1_1':0.0, -'TC_G3_X2_1':0.0, -'TC_G3_X3_1':0.0, -'TC_G3_X0_2':0.0, -'TC_G3_X1_2':0.0, -'TC_G3_X2_2':0.0, -'TC_G3_X3_2':0.0 -} - -# 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'])) - - # 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'] - - 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']) - - # 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) - - # 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) - - # 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) - - # gyro3 vs temperature - plt.figure(4,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() ################################################################################# @@ -672,7 +264,7 @@ if num_accels >= 1 and not math.isnan(sensor_accel_0['temperature'][0]): correction_z_resample = fit_coef_accel_0_z(temp_rel_resample) # accel 0 vs temperature - plt.figure(5,figsize=(20,13)) + plt.figure(1,figsize=(20,13)) # draw plots plt.subplot(3,1,1) @@ -787,7 +379,7 @@ if num_accels >= 2 and not math.isnan(sensor_accel_1['temperature'][0]): correction_z_resample = fit_coef_accel_1_z(temp_rel_resample) # accel 1 vs temperature - plt.figure(6,figsize=(20,13)) + plt.figure(2,figsize=(20,13)) # draw plots plt.subplot(3,1,1) @@ -903,7 +495,7 @@ if num_accels >= 3 and not math.isnan(sensor_accel_2['temperature'][0]): correction_z_resample = fit_coef_accel_2_z(temp_rel_resample) # accel 2 vs temperature - plt.figure(7,figsize=(20,13)) + plt.figure(3,figsize=(20,13)) # draw plots plt.subplot(3,1,1) @@ -1003,7 +595,7 @@ if num_accels >= 4 and not math.isnan(sensor_accel_3['temperature'][0]): correction_z_resample = fit_coef_accel_3_z(temp_rel_resample) # accel 3 vs temperature - plt.figure(8,figsize=(20,13)) + plt.figure(4,figsize=(20,13)) # draw plots plt.subplot(3,1,1) @@ -1032,6 +624,886 @@ if num_accels >= 4 and not math.isnan(sensor_accel_3['temperature'][0]): pp.savefig() + +################################################################################# + +################################################################################# + +# 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 +} + +# 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'])) + + # 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'] + + 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 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_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 + 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) + + 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 + 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) + + 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() + +################################################################################# + +################################################################################# + +# 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 +} + +# 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'])) + + # 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'] + + 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 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_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 + 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) + + 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 + 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) + + 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(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() + +################################################################################# + +################################################################################# + +# 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 +} + +# 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'])) + + # 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'] + + 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 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_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 + 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) + + 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 + 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) + + 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(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() + +################################################################################# + +################################################################################# + +# define data dictionary of gyro 3 thermal correction parameters +gyro_3_params = { +'TC_G3_ID':0, +'TC_G3_TMIN':0.0, +'TC_G3_TMAX':0.0, +'TC_G3_TREF':0.0, +'TC_G3_X0_0':0.0, +'TC_G3_X1_0':0.0, +'TC_G3_X2_0':0.0, +'TC_G3_X3_0':0.0, +'TC_G3_X0_1':0.0, +'TC_G3_X1_1':0.0, +'TC_G3_X2_1':0.0, +'TC_G3_X3_1':0.0, +'TC_G3_X0_2':0.0, +'TC_G3_X1_2':0.0, +'TC_G3_X2_2':0.0, +'TC_G3_X3_2':0.0 +} + +# 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'])) + + # 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'] + + 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']) + + # 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) + + # 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) + + # 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) + + # 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 +mag_0_params = { +'TC_M0_ID':0, +'TC_M0_TMIN':0.0, +'TC_M0_TMAX':0.0, +'TC_M0_TREF':0.0, +'TC_M0_X0_0':0.0, +'TC_M0_X1_0':0.0, +'TC_M0_X2_0':0.0, +'TC_M0_X3_0':0.0, +'TC_M0_X0_1':0.0, +'TC_M0_X1_1':0.0, +'TC_M0_X2_1':0.0, +'TC_M0_X3_1':0.0, +'TC_M0_X0_2':0.0, +'TC_M0_X1_2':0.0, +'TC_M0_X2_2':0.0, +'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'])) + + # 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'] + + 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 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) + + 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) + + # 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) + + 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) + + # 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() + +################################################################################# + +################################################################################# + +# define data dictionary of mag 1 thermal correction parameters +mag_1_params = { +'TC_M1_ID':0, +'TC_M1_TMIN':0.0, +'TC_M1_TMAX':0.0, +'TC_M1_TREF':0.0, +'TC_M1_X0_0':0.0, +'TC_M1_X1_0':0.0, +'TC_M1_X2_0':0.0, +'TC_M1_X3_0':0.0, +'TC_M1_X0_1':0.0, +'TC_M1_X1_1':0.0, +'TC_M1_X2_1':0.0, +'TC_M1_X3_1':0.0, +'TC_M1_X0_2':0.0, +'TC_M1_X1_2':0.0, +'TC_M1_X2_2':0.0, +'TC_M1_X3_2':0.0 +} + +# 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'])) + + # 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'] + + 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']) + + # 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) + + 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) + + # 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) + + 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 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() + + +################################################################################# + +################################################################################# + +# define data dictionary of mag 2 thermal correction parameters +mag_2_params = { +'TC_M2_ID':0, +'TC_M2_TMIN':0.0, +'TC_M2_TMAX':0.0, +'TC_M2_TREF':0.0, +'TC_M2_X0_0':0.0, +'TC_M2_X1_0':0.0, +'TC_M2_X2_0':0.0, +'TC_M2_X3_0':0.0, +'TC_M2_X0_1':0.0, +'TC_M2_X1_1':0.0, +'TC_M2_X2_1':0.0, +'TC_M2_X3_1':0.0, +'TC_M2_X0_2':0.0, +'TC_M2_X1_2':0.0, +'TC_M2_X2_2':0.0, +'TC_M2_X3_2':0.0 +} + +# 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'])) + + # 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'] + + 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']) + + # 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) + + 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) + + # 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) + + 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 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() + +################################################################################# + +################################################################################# + +# define data dictionary of mag 3 thermal correction parameters +mag_3_params = { +'TC_M3_ID':0, +'TC_M3_TMIN':0.0, +'TC_M3_TMAX':0.0, +'TC_M3_TREF':0.0, +'TC_M3_X0_0':0.0, +'TC_M3_X1_0':0.0, +'TC_M3_X2_0':0.0, +'TC_M3_X3_0':0.0, +'TC_M3_X0_1':0.0, +'TC_M3_X1_1':0.0, +'TC_M3_X2_1':0.0, +'TC_M3_X3_1':0.0, +'TC_M3_X0_2':0.0, +'TC_M3_X1_2':0.0, +'TC_M3_X2_2':0.0, +'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'])) + + # 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'] + + 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']) + + # 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) + + # 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) + + # 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) + + # 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() + ################################################################################# ################################################################################# @@ -1081,7 +1553,7 @@ 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(9,figsize=(20,13)) +plt.figure(13,figsize=(20,13)) # draw plots plt.plot(sensor_baro_0['temperature'],100*sensor_baro_0['pressure']-100*median_pressure,'b') @@ -1140,7 +1612,7 @@ if num_baros >= 2: baro_1_x_resample = fit_coef_baro_1_x(temp_rel_resample) # baro 2 vs temperature - plt.figure(10,figsize=(20,13)) + plt.figure(14,figsize=(20,13)) # draw plots plt.plot(sensor_baro_1['temperature'],100*sensor_baro_1['pressure']-100*median_pressure,'b') @@ -1200,7 +1672,7 @@ if num_baros >= 3: baro_2_x_resample = fit_coef_baro_2_x(temp_rel_resample) # baro 2 vs temperature - plt.figure(11,figsize=(20,13)) + plt.figure(15,figsize=(20,13)) # draw plots plt.plot(sensor_baro_2['temperature'],100*sensor_baro_2['pressure']-100*median_pressure,'b') @@ -1255,7 +1727,7 @@ if num_baros >= 4: baro_3_x_resample = fit_coef_baro_3_x(temp_rel_resample) # baro 3 vs temperature - plt.figure(12,figsize=(20,13)) + plt.figure(16,figsize=(20,13)) # draw plots plt.plot(sensor_baro_3['temperature'],100*sensor_baro_3['pressure']-100*median_pressure,'b') @@ -1272,7 +1744,7 @@ if num_baros >= 4: # close the pdf file pp.close() -# clase all figures +# close all figures plt.close("all") # write correction parameters to file @@ -1322,47 +1794,6 @@ for key in key_list_accel: type = "9" file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(accel_3_params[key])+"\t"+type+"\n") -# baro 0 corrections -key_list_baro = list(baro_0_params.keys()) -key_list_baro.sort -for key in key_list_baro: - 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") - -# baro 1 corrections -key_list_baro = list(baro_1_params.keys()) -key_list_baro.sort -for key in key_list_baro: - if key == 'TC_B1_ID': - type = "6" - else: - type = "9" - file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(baro_1_params[key])+"\t"+type+"\n") - -# baro 2 corrections -key_list_baro = list(baro_2_params.keys()) -key_list_baro.sort -for key in key_list_baro: - if key == 'TC_B2_ID': - type = "6" - else: - type = "9" - file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(baro_2_params[key])+"\t"+type+"\n") - -# baro 3 corrections -key_list_baro = list(baro_3_params.keys()) -key_list_baro.sort -for key in key_list_baro: - if key == 'TC_B3_ID': - type = "6" - else: - type = "9" - file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(baro_3_params[key])+"\t"+type+"\n") - - # gyro 0 corrections key_list_gyro = list(gyro_0_params.keys()) key_list_gyro.sort() @@ -1403,6 +1834,86 @@ for key in key_list_gyro: type = "9" file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(gyro_3_params[key])+"\t"+type+"\n") +# accel 0 corrections +key_list_mag = list(mag_0_params.keys()) +key_list_mag.sort +for key in key_list_mag: + if key == 'TC_M0_ID': + type = "6" + else: + type = "9" + file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(mag_0_params[key])+"\t"+type+"\n") + +# mag 1 corrections +key_list_mag = list(mag_1_params.keys()) +key_list_mag.sort +for key in key_list_mag: + if key == 'TC_M1_ID': + type = "6" + else: + type = "9" + file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(mag_1_params[key])+"\t"+type+"\n") + +# mag 2 corrections +key_list_mag = list(mag_2_params.keys()) +key_list_mag.sort +for key in key_list_mag: + if key == 'TC_M2_ID': + type = "6" + else: + type = "9" + file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(mag_2_params[key])+"\t"+type+"\n") + +# mag 3 corrections +key_list_mag = list(mag_3_params.keys()) +key_list_mag.sort +for key in key_list_mag: + if key == 'TC_M3_ID': + type = "6" + else: + type = "9" + file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(mag_3_params[key])+"\t"+type+"\n") + +# baro 0 corrections +key_list_baro = list(baro_0_params.keys()) +key_list_baro.sort +for key in key_list_baro: + 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") + +# baro 1 corrections +key_list_baro = list(baro_1_params.keys()) +key_list_baro.sort +for key in key_list_baro: + if key == 'TC_B1_ID': + type = "6" + else: + type = "9" + file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(baro_1_params[key])+"\t"+type+"\n") + +# baro 2 corrections +key_list_baro = list(baro_2_params.keys()) +key_list_baro.sort +for key in key_list_baro: + if key == 'TC_B2_ID': + type = "6" + else: + type = "9" + file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(baro_2_params[key])+"\t"+type+"\n") + +# baro 3 corrections +key_list_baro = list(baro_3_params.keys()) +key_list_baro.sort +for key in key_list_baro: + if key == 'TC_B3_ID': + type = "6" + else: + type = "9" + file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(baro_3_params[key])+"\t"+type+"\n") + file.close() print('Correction parameters written to ' + test_results_filename) diff --git a/msg/SensorCorrection.msg b/msg/SensorCorrection.msg index 45ee23135d..bfbc8e2eb5 100644 --- a/msg/SensorCorrection.msg +++ b/msg/SensorCorrection.msg @@ -4,15 +4,6 @@ uint64 timestamp # time since system start (microseconds) -# Corrections for gyro angular rate outputs where corrected_rate = raw_rate * gyro_scale + gyro_offset -# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame -uint32[4] gyro_device_ids -float32[4] gyro_temperature -float32[3] gyro_offset_0 # gyro 0 XYZ offsets in the sensor frame in rad/s -float32[3] gyro_offset_1 # gyro 1 XYZ offsets in the sensor frame in rad/s -float32[3] gyro_offset_2 # gyro 2 XYZ offsets in the sensor frame in rad/s -float32[3] gyro_offset_3 # gyro 3 XYZ offsets in the sensor frame in rad/s - # Corrections for acceleromter acceleration outputs where corrected_accel = raw_accel * accel_scale + accel_offset # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame uint32[4] accel_device_ids @@ -22,6 +13,24 @@ float32[3] accel_offset_1 # accelerometer 1 offsets in the FRD board frame XYZ-a float32[3] accel_offset_2 # accelerometer 2 offsets in the FRD board frame XYZ-axis in m/s^s float32[3] accel_offset_3 # accelerometer 3 offsets in the FRD board frame XYZ-axis in m/s^s +# Corrections for gyro angular rate outputs where corrected_rate = raw_rate * gyro_scale + gyro_offset +# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame +uint32[4] gyro_device_ids +float32[4] gyro_temperature +float32[3] gyro_offset_0 # gyro 0 XYZ offsets in the sensor frame in rad/s +float32[3] gyro_offset_1 # gyro 1 XYZ offsets in the sensor frame in rad/s +float32[3] gyro_offset_2 # gyro 2 XYZ offsets in the sensor frame in rad/s +float32[3] gyro_offset_3 # gyro 3 XYZ offsets in the sensor frame in rad/s + +# Corrections for magnetometer measurement outputs where corrected_mag = raw_mag * mag_scale + mag_offset +# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame +uint32[4] mag_device_ids +float32[4] mag_temperature +float32[3] mag_offset_0 # magnetometer 0 offsets in the FRD board frame XYZ-axis in m/s^s +float32[3] mag_offset_1 # magnetometer 1 offsets in the FRD board frame XYZ-axis in m/s^s +float32[3] mag_offset_2 # magnetometer 2 offsets in the FRD board frame XYZ-axis in m/s^s +float32[3] mag_offset_3 # magnetometer 3 offsets in the FRD board frame XYZ-axis in m/s^s + # Corrections for barometric pressure outputs where corrected_pressure = raw_pressure * pressure_scale + pressure_offset # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame uint32[4] baro_device_ids diff --git a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp index 73507df074..566d3a22ba 100644 --- a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp +++ b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp @@ -220,6 +220,13 @@ 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; + } } } @@ -291,3 +298,15 @@ 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 1a958b8e47..abd02f24d7 100644 --- a/src/drivers/magnetometer/isentek/ist8308/IST8308.hpp +++ b/src/drivers/magnetometer/isentek/ist8308/IST8308.hpp @@ -46,7 +46,10 @@ #include #include #include +#include #include +#include +#include using namespace iSentek_IST8308; @@ -61,9 +64,17 @@ 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; @@ -80,9 +91,15 @@ 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")}; @@ -91,17 +108,14 @@ 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 b7d8512ce9..d7967af511 100644 --- a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp @@ -188,6 +188,7 @@ 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; @@ -222,6 +223,13 @@ 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 @@ -296,3 +304,15 @@ 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 a251f49cf0..6ea5a78162 100644 --- a/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp @@ -46,7 +46,10 @@ #include #include #include +#include #include +#include +#include using namespace iSentek_IST8310; @@ -61,9 +64,18 @@ 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; @@ -80,9 +92,15 @@ 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")}; @@ -91,18 +109,14 @@ 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/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index efb2b8c1b2..c7306283b3 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/src/lib/sensor_calibration/Magnetometer.cpp @@ -65,6 +65,46 @@ void Magnetometer::set_device_id(uint32_t device_id) Reset(); ParametersUpdate(); + SensorCorrectionsUpdate(true); + } +} + +void Magnetometer::SensorCorrectionsUpdate(bool force) +{ + // check if the selected sensor has updated + if (_sensor_correction_sub.updated() || force) { + + // valid device id required + if (_device_id == 0) { + return; + } + + sensor_correction_s corrections; + + if (_sensor_correction_sub.copy(&corrections)) { + // find sensor_corrections index + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if (corrections.mag_device_ids[i] == _device_id) { + switch (i) { + case 0: + _thermal_offset = Vector3f{corrections.mag_offset_0}; + return; + case 1: + _thermal_offset = Vector3f{corrections.mag_offset_1}; + return; + case 2: + _thermal_offset = Vector3f{corrections.mag_offset_2}; + return; + case 3: + _thermal_offset = Vector3f{corrections.mag_offset_3}; + return; + } + } + } + } + + // zero thermal offset if not found + _thermal_offset.zero(); } } diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 481cc58e8d..047cd3398b 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -39,6 +39,7 @@ #include #include #include +#include namespace calibration { @@ -98,15 +99,21 @@ public: void Reset(); + void SensorCorrectionsUpdate(bool force = false); + void UpdatePower(float power) { _power = power; } private: + uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; + Rotation _rotation_enum{ROTATION_NONE}; matrix::Dcmf _rotation; matrix::Vector3f _offset; matrix::Matrix3f _scale; + matrix::Vector3f _thermal_offset; matrix::Vector3f _power_compensation; + float _power{0.f}; int8_t _calibration_index{-1}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 31ea1bad68..c2c6e94275 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -347,6 +347,7 @@ 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_mag", 100, 4); } void LoggedTopics::add_sensor_comparison_topics() diff --git a/src/modules/temperature_compensation/CMakeLists.txt b/src/modules/temperature_compensation/CMakeLists.txt index 50e6a181e0..df20d1d407 100644 --- a/src/modules/temperature_compensation/CMakeLists.txt +++ b/src/modules/temperature_compensation/CMakeLists.txt @@ -40,6 +40,7 @@ px4_add_module( temperature_calibration/accel.cpp temperature_calibration/baro.cpp temperature_calibration/gyro.cpp + temperature_calibration/mag.cpp temperature_calibration/task.cpp DEPENDS mathlib diff --git a/src/modules/temperature_compensation/TemperatureCompensation.cpp b/src/modules/temperature_compensation/TemperatureCompensation.cpp index cab7630bfd..bcc8a2e3d3 100644 --- a/src/modules/temperature_compensation/TemperatureCompensation.cpp +++ b/src/modules/temperature_compensation/TemperatureCompensation.cpp @@ -52,6 +52,37 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles ¶ char nbuf[16] {}; int ret = PX4_ERROR; + /* accelerometer calibration parameters */ + parameter_handles.accel_tc_enable = param_find("TC_A_ENABLE"); + int32_t accel_tc_enabled = 0; + ret = param_get(parameter_handles.accel_tc_enable, &accel_tc_enabled); + + + if (ret == PX4_OK && accel_tc_enabled) { + for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) { + snprintf(nbuf, sizeof(nbuf), "TC_A%d_ID", j); + parameter_handles.accel_cal_handles[j].ID = param_find(nbuf); + + for (unsigned i = 0; i < 3; i++) { + snprintf(nbuf, sizeof(nbuf), "TC_A%d_X3_%d", j, i); + parameter_handles.accel_cal_handles[j].x3[i] = param_find(nbuf); + snprintf(nbuf, sizeof(nbuf), "TC_A%d_X2_%d", j, i); + parameter_handles.accel_cal_handles[j].x2[i] = param_find(nbuf); + snprintf(nbuf, sizeof(nbuf), "TC_A%d_X1_%d", j, i); + parameter_handles.accel_cal_handles[j].x1[i] = param_find(nbuf); + snprintf(nbuf, sizeof(nbuf), "TC_A%d_X0_%d", j, i); + parameter_handles.accel_cal_handles[j].x0[i] = param_find(nbuf); + } + + snprintf(nbuf, sizeof(nbuf), "TC_A%d_TREF", j); + parameter_handles.accel_cal_handles[j].ref_temp = param_find(nbuf); + snprintf(nbuf, sizeof(nbuf), "TC_A%d_TMIN", j); + parameter_handles.accel_cal_handles[j].min_temp = param_find(nbuf); + snprintf(nbuf, sizeof(nbuf), "TC_A%d_TMAX", j); + parameter_handles.accel_cal_handles[j].max_temp = param_find(nbuf); + } + } + /* rate gyro calibration parameters */ parameter_handles.gyro_tc_enable = param_find("TC_G_ENABLE"); int32_t gyro_tc_enabled = 0; @@ -82,33 +113,33 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles ¶ } } - /* accelerometer calibration parameters */ - parameter_handles.accel_tc_enable = param_find("TC_A_ENABLE"); - int32_t accel_tc_enabled = 0; - ret = param_get(parameter_handles.accel_tc_enable, &accel_tc_enabled); + /* magnetometer calibration parameters */ + parameter_handles.mag_tc_enable = param_find("TC_M_ENABLE"); + int32_t mag_tc_enabled = 0; + ret = param_get(parameter_handles.mag_tc_enable, &mag_tc_enabled); - if (ret == PX4_OK && accel_tc_enabled) { - for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) { - snprintf(nbuf, sizeof(nbuf), "TC_A%d_ID", j); - parameter_handles.accel_cal_handles[j].ID = param_find(nbuf); + if (ret == PX4_OK && mag_tc_enabled) { + for (unsigned j = 0; j < MAG_COUNT_MAX; j++) { + snprintf(nbuf, sizeof(nbuf), "TC_M%d_ID", j); + parameter_handles.mag_cal_handles[j].ID = param_find(nbuf); for (unsigned i = 0; i < 3; i++) { - snprintf(nbuf, sizeof(nbuf), "TC_A%d_X3_%d", j, i); - parameter_handles.accel_cal_handles[j].x3[i] = param_find(nbuf); - snprintf(nbuf, sizeof(nbuf), "TC_A%d_X2_%d", j, i); - parameter_handles.accel_cal_handles[j].x2[i] = param_find(nbuf); - snprintf(nbuf, sizeof(nbuf), "TC_A%d_X1_%d", j, i); - parameter_handles.accel_cal_handles[j].x1[i] = param_find(nbuf); - snprintf(nbuf, sizeof(nbuf), "TC_A%d_X0_%d", j, i); - parameter_handles.accel_cal_handles[j].x0[i] = param_find(nbuf); + snprintf(nbuf, sizeof(nbuf), "TC_M%d_X3_%d", j, i); + parameter_handles.mag_cal_handles[j].x3[i] = param_find(nbuf); + snprintf(nbuf, sizeof(nbuf), "TC_M%d_X2_%d", j, i); + parameter_handles.mag_cal_handles[j].x2[i] = param_find(nbuf); + snprintf(nbuf, sizeof(nbuf), "TC_M%d_X1_%d", j, i); + parameter_handles.mag_cal_handles[j].x1[i] = param_find(nbuf); + snprintf(nbuf, sizeof(nbuf), "TC_M%d_X0_%d", j, i); + parameter_handles.mag_cal_handles[j].x0[i] = param_find(nbuf); } - snprintf(nbuf, sizeof(nbuf), "TC_A%d_TREF", j); - parameter_handles.accel_cal_handles[j].ref_temp = param_find(nbuf); - snprintf(nbuf, sizeof(nbuf), "TC_A%d_TMIN", j); - parameter_handles.accel_cal_handles[j].min_temp = param_find(nbuf); - snprintf(nbuf, sizeof(nbuf), "TC_A%d_TMAX", j); - parameter_handles.accel_cal_handles[j].max_temp = param_find(nbuf); + snprintf(nbuf, sizeof(nbuf), "TC_M%d_TREF", j); + parameter_handles.mag_cal_handles[j].ref_temp = param_find(nbuf); + snprintf(nbuf, sizeof(nbuf), "TC_M%d_TMIN", j); + parameter_handles.mag_cal_handles[j].min_temp = param_find(nbuf); + snprintf(nbuf, sizeof(nbuf), "TC_M%d_TMAX", j); + parameter_handles.mag_cal_handles[j].max_temp = param_find(nbuf); } } @@ -154,6 +185,33 @@ int TemperatureCompensation::parameters_update() return ret; } + /* accelerometer calibration parameters */ + param_get(parameter_handles.accel_tc_enable, &_parameters.accel_tc_enable); + + if (_parameters.accel_tc_enable == 1) { + for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) { + if (param_get(parameter_handles.accel_cal_handles[j].ID, &(_parameters.accel_cal_data[j].ID)) == PX4_OK) { + param_get(parameter_handles.accel_cal_handles[j].ref_temp, &(_parameters.accel_cal_data[j].ref_temp)); + param_get(parameter_handles.accel_cal_handles[j].min_temp, &(_parameters.accel_cal_data[j].min_temp)); + param_get(parameter_handles.accel_cal_handles[j].max_temp, &(_parameters.accel_cal_data[j].max_temp)); + + for (unsigned int i = 0; i < 3; i++) { + param_get(parameter_handles.accel_cal_handles[j].x3[i], &(_parameters.accel_cal_data[j].x3[i])); + param_get(parameter_handles.accel_cal_handles[j].x2[i], &(_parameters.accel_cal_data[j].x2[i])); + param_get(parameter_handles.accel_cal_handles[j].x1[i], &(_parameters.accel_cal_data[j].x1[i])); + param_get(parameter_handles.accel_cal_handles[j].x0[i], &(_parameters.accel_cal_data[j].x0[i])); + } + + } else { + // Set all cal values to zero + memset(&_parameters.accel_cal_data[j], 0, sizeof(_parameters.accel_cal_data[j])); + + PX4_WARN("FAIL ACCEL %d CAL PARAM LOAD - USING DEFAULTS", j); + ret = PX4_ERROR; + } + } + } + /* rate gyro calibration parameters */ param_get(parameter_handles.gyro_tc_enable, &_parameters.gyro_tc_enable); @@ -181,28 +239,28 @@ int TemperatureCompensation::parameters_update() } } - /* accelerometer calibration parameters */ - param_get(parameter_handles.accel_tc_enable, &_parameters.accel_tc_enable); + /* magnetometer calibration parameters */ + param_get(parameter_handles.mag_tc_enable, &_parameters.mag_tc_enable); - if (_parameters.accel_tc_enable == 1) { - for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) { - if (param_get(parameter_handles.accel_cal_handles[j].ID, &(_parameters.accel_cal_data[j].ID)) == PX4_OK) { - param_get(parameter_handles.accel_cal_handles[j].ref_temp, &(_parameters.accel_cal_data[j].ref_temp)); - param_get(parameter_handles.accel_cal_handles[j].min_temp, &(_parameters.accel_cal_data[j].min_temp)); - param_get(parameter_handles.accel_cal_handles[j].max_temp, &(_parameters.accel_cal_data[j].max_temp)); + if (_parameters.mag_tc_enable == 1) { + for (unsigned j = 0; j < MAG_COUNT_MAX; j++) { + if (param_get(parameter_handles.mag_cal_handles[j].ID, &(_parameters.mag_cal_data[j].ID)) == PX4_OK) { + param_get(parameter_handles.mag_cal_handles[j].ref_temp, &(_parameters.mag_cal_data[j].ref_temp)); + param_get(parameter_handles.mag_cal_handles[j].min_temp, &(_parameters.mag_cal_data[j].min_temp)); + param_get(parameter_handles.mag_cal_handles[j].max_temp, &(_parameters.mag_cal_data[j].max_temp)); for (unsigned int i = 0; i < 3; i++) { - param_get(parameter_handles.accel_cal_handles[j].x3[i], &(_parameters.accel_cal_data[j].x3[i])); - param_get(parameter_handles.accel_cal_handles[j].x2[i], &(_parameters.accel_cal_data[j].x2[i])); - param_get(parameter_handles.accel_cal_handles[j].x1[i], &(_parameters.accel_cal_data[j].x1[i])); - param_get(parameter_handles.accel_cal_handles[j].x0[i], &(_parameters.accel_cal_data[j].x0[i])); + param_get(parameter_handles.mag_cal_handles[j].x3[i], &(_parameters.mag_cal_data[j].x3[i])); + param_get(parameter_handles.mag_cal_handles[j].x2[i], &(_parameters.mag_cal_data[j].x2[i])); + param_get(parameter_handles.mag_cal_handles[j].x1[i], &(_parameters.mag_cal_data[j].x1[i])); + param_get(parameter_handles.mag_cal_handles[j].x0[i], &(_parameters.mag_cal_data[j].x0[i])); } } else { // Set all cal values to zero - memset(&_parameters.accel_cal_data[j], 0, sizeof(_parameters.accel_cal_data[j])); + memset(&_parameters.mag_cal_data[j], 0, sizeof(_parameters.mag_cal_data[j])); - PX4_WARN("FAIL ACCEL %d CAL PARAM LOAD - USING DEFAULTS", j); + PX4_WARN("FAIL MAG %d CAL PARAM LOAD - USING DEFAULTS", j); ret = PX4_ERROR; } } @@ -311,6 +369,15 @@ bool TemperatureCompensation::calc_thermal_offsets_3D(const SensorCalData3D &coe return ret; } +int TemperatureCompensation::set_sensor_id_accel(uint32_t device_id, int topic_instance) +{ + if (_parameters.accel_tc_enable != 1) { + return 0; + } + + return set_sensor_id(device_id, topic_instance, _accel_data, _parameters.accel_cal_data, ACCEL_COUNT_MAX); +} + int TemperatureCompensation::set_sensor_id_gyro(uint32_t device_id, int topic_instance) { if (_parameters.gyro_tc_enable != 1) { @@ -320,13 +387,13 @@ int TemperatureCompensation::set_sensor_id_gyro(uint32_t device_id, int topic_in return set_sensor_id(device_id, topic_instance, _gyro_data, _parameters.gyro_cal_data, GYRO_COUNT_MAX); } -int TemperatureCompensation::set_sensor_id_accel(uint32_t device_id, int topic_instance) +int TemperatureCompensation::set_sensor_id_mag(uint32_t device_id, int topic_instance) { - if (_parameters.accel_tc_enable != 1) { + if (_parameters.mag_tc_enable != 1) { return 0; } - return set_sensor_id(device_id, topic_instance, _accel_data, _parameters.accel_cal_data, ACCEL_COUNT_MAX); + return set_sensor_id(device_id, topic_instance, _mag_data, _parameters.mag_cal_data, MAG_COUNT_MAX); } int TemperatureCompensation::set_sensor_id_baro(uint32_t device_id, int topic_instance) @@ -352,6 +419,32 @@ int TemperatureCompensation::set_sensor_id(uint32_t device_id, int topic_instanc return -1; } +int TemperatureCompensation::update_offsets_accel(int topic_instance, float temperature, float *offsets) +{ + // Check if temperature compensation is enabled + if (_parameters.accel_tc_enable != 1) { + return 0; + } + + // Map device ID to uORB topic instance + uint8_t mapping = _accel_data.device_mapping[topic_instance]; + + if (mapping == 255) { + return -1; + } + + // Calculate and update the offsets + calc_thermal_offsets_3D(_parameters.accel_cal_data[mapping], temperature, offsets); + + // Check if temperature delta is large enough to warrant a new publication + if (fabsf(temperature - _accel_data.last_temperature[topic_instance]) > 1.0f) { + _accel_data.last_temperature[topic_instance] = temperature; + return 2; + } + + return 1; +} + int TemperatureCompensation::update_offsets_gyro(int topic_instance, float temperature, float *offsets) { // Check if temperature compensation is enabled @@ -378,26 +471,26 @@ int TemperatureCompensation::update_offsets_gyro(int topic_instance, float tempe return 1; } -int TemperatureCompensation::update_offsets_accel(int topic_instance, float temperature, float *offsets) +int TemperatureCompensation::update_offsets_mag(int topic_instance, float temperature, float *offsets) { // Check if temperature compensation is enabled - if (_parameters.accel_tc_enable != 1) { + if (_parameters.mag_tc_enable != 1) { return 0; } // Map device ID to uORB topic instance - uint8_t mapping = _accel_data.device_mapping[topic_instance]; + uint8_t mapping = _mag_data.device_mapping[topic_instance]; if (mapping == 255) { return -1; } // Calculate and update the offsets - calc_thermal_offsets_3D(_parameters.accel_cal_data[mapping], temperature, offsets); + calc_thermal_offsets_3D(_parameters.mag_cal_data[mapping], temperature, offsets); // Check if temperature delta is large enough to warrant a new publication - if (fabsf(temperature - _accel_data.last_temperature[topic_instance]) > 1.0f) { - _accel_data.last_temperature[topic_instance] = temperature; + if (fabsf(temperature - _mag_data.last_temperature[topic_instance]) > 1.0f) { + _mag_data.last_temperature[topic_instance] = temperature; return 2; } @@ -433,6 +526,19 @@ int TemperatureCompensation::update_offsets_baro(int topic_instance, float tempe void TemperatureCompensation::print_status() { PX4_INFO("Temperature Compensation:"); + + PX4_INFO(" accel: enabled: %" PRId32, _parameters.accel_tc_enable); + + if (_parameters.accel_tc_enable == 1) { + for (int i = 0; i < ACCEL_COUNT_MAX; ++i) { + uint8_t mapping = _accel_data.device_mapping[i]; + + if (_accel_data.device_mapping[i] != 255) { + PX4_INFO(" using device ID %" PRId32 " for topic instance %i", _parameters.accel_cal_data[mapping].ID, i); + } + } + } + PX4_INFO(" gyro: enabled: %" PRId32, _parameters.gyro_tc_enable); if (_parameters.gyro_tc_enable == 1) { @@ -445,14 +551,14 @@ void TemperatureCompensation::print_status() } } - PX4_INFO(" accel: enabled: %" PRId32, _parameters.accel_tc_enable); + PX4_INFO(" mag: enabled: %" PRId32, _parameters.mag_tc_enable); - if (_parameters.accel_tc_enable == 1) { - for (int i = 0; i < ACCEL_COUNT_MAX; ++i) { - uint8_t mapping = _accel_data.device_mapping[i]; + if (_parameters.mag_tc_enable == 1) { + for (int i = 0; i < MAG_COUNT_MAX; ++i) { + uint8_t mapping = _mag_data.device_mapping[i]; - if (_accel_data.device_mapping[i] != 255) { - PX4_INFO(" using device ID %" PRId32 " for topic instance %i", _parameters.accel_cal_data[mapping].ID, i); + if (_mag_data.device_mapping[i] != 255) { + PX4_INFO(" using device ID %" PRId32 " for topic instance %i", _parameters.mag_cal_data[mapping].ID, i); } } } diff --git a/src/modules/temperature_compensation/TemperatureCompensation.h b/src/modules/temperature_compensation/TemperatureCompensation.h index 051bc73496..7975e2465f 100644 --- a/src/modules/temperature_compensation/TemperatureCompensation.h +++ b/src/modules/temperature_compensation/TemperatureCompensation.h @@ -49,13 +49,15 @@ namespace temperature_compensation { -static constexpr uint8_t GYRO_COUNT_MAX = 4; static constexpr uint8_t ACCEL_COUNT_MAX = 4; -static constexpr uint8_t BARO_COUNT_MAX = 4; +static constexpr uint8_t GYRO_COUNT_MAX = 4; +static constexpr uint8_t MAG_COUNT_MAX = 4; +static constexpr uint8_t BARO_COUNT_MAX = 4; -static_assert(GYRO_COUNT_MAX == 4, "GYRO_COUNT_MAX must be 4 (if changed, add/remove TC_* params to match the count)"); static_assert(ACCEL_COUNT_MAX == 4, "ACCEL_COUNT_MAX must be 4 (if changed, add/remove TC_* params to match the count)"); +static_assert(GYRO_COUNT_MAX == 4, "GYRO_COUNT_MAX must be 4 (if changed, add/remove TC_* params to match the count)"); +static_assert(MAG_COUNT_MAX == 4, "MAG_COUNT_MAX must be 4 (if changed, add/remove TC_* params to match the count)"); static_assert(BARO_COUNT_MAX == 4, "BARO_COUNT_MAX must be 4 (if changed, add/remove TC_* params to match the count)"); static constexpr uint8_t SENSOR_COUNT_MAX = 4; @@ -74,12 +76,13 @@ public: /** supply information which device_id matches a specific uORB topic_instance * (needed if a system has multiple sensors of the same type) * @return index for compensation parameter entry containing matching device ID on success, <0 otherwise */ - int set_sensor_id_gyro(uint32_t device_id, int topic_instance); int set_sensor_id_accel(uint32_t device_id, int topic_instance); + int set_sensor_id_gyro(uint32_t device_id, int topic_instance); + int set_sensor_id_mag(uint32_t device_id, int topic_instance); int set_sensor_id_baro(uint32_t device_id, int topic_instance); /** - * Apply Thermal corrections to gyro (& other) sensor data. + * Apply Thermal corrections to accel, gyro, mag, and baro sensor data. * @param topic_instance uORB topic instance * @param sensor_data input sensor data, output sensor data with applied corrections * @param temperature measured current temperature @@ -89,8 +92,9 @@ public: * 1: corrections applied but no changes to offsets, * 2: corrections applied and offsets updated */ - int update_offsets_gyro(int topic_instance, float temperature, float *offsets); int update_offsets_accel(int topic_instance, float temperature, float *offsets); + int update_offsets_gyro(int topic_instance, float temperature, float *offsets); + int update_offsets_mag(int topic_instance, float temperature, float *offsets); int update_offsets_baro(int topic_instance, float temperature, float *offsets); /** output current configuration status to console */ @@ -178,11 +182,14 @@ private: // create a struct containing all thermal calibration parameters struct Parameters { + int32_t accel_tc_enable{0}; + SensorCalData3D accel_cal_data[ACCEL_COUNT_MAX] {}; + int32_t gyro_tc_enable{0}; SensorCalData3D gyro_cal_data[GYRO_COUNT_MAX] {}; - int32_t accel_tc_enable{0}; - SensorCalData3D accel_cal_data[ACCEL_COUNT_MAX] {}; + int32_t mag_tc_enable{0}; + SensorCalData3D mag_cal_data[MAG_COUNT_MAX] {}; int32_t baro_tc_enable{0}; SensorCalData1D baro_cal_data[BARO_COUNT_MAX] {}; @@ -190,11 +197,14 @@ private: // create a struct containing the handles required to access all calibration parameters struct ParameterHandles { + param_t accel_tc_enable{PARAM_INVALID}; + SensorCalHandles3D accel_cal_handles[ACCEL_COUNT_MAX] {}; + param_t gyro_tc_enable{PARAM_INVALID}; SensorCalHandles3D gyro_cal_handles[GYRO_COUNT_MAX] {}; - param_t accel_tc_enable{PARAM_INVALID}; - SensorCalHandles3D accel_cal_handles[ACCEL_COUNT_MAX] {}; + param_t mag_tc_enable{PARAM_INVALID}; + SensorCalHandles3D mag_cal_handles[MAG_COUNT_MAX] {}; param_t baro_tc_enable{PARAM_INVALID}; SensorCalHandles1D baro_cal_handles[BARO_COUNT_MAX] {}; @@ -271,8 +281,9 @@ private: float last_temperature[SENSOR_COUNT_MAX] {}; }; - PerSensorData _gyro_data; PerSensorData _accel_data; + PerSensorData _gyro_data; + PerSensorData _mag_data; PerSensorData _baro_data; template diff --git a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp index a0df447791..5a8321e6a0 100644 --- a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp +++ b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp @@ -65,6 +65,24 @@ void TemperatureCompensationModule::parameters_update() { _temperature_compensation.parameters_update(); + // Accel + for (uint8_t uorb_index = 0; uorb_index < ACCEL_COUNT_MAX; uorb_index++) { + sensor_accel_s report; + + if (_accel_subs[uorb_index].copy(&report)) { + int temp = _temperature_compensation.set_sensor_id_accel(report.device_id, uorb_index); + + if (temp < 0) { + PX4_INFO("No temperature calibration available for accel %" PRIu8 " (device id %" PRIu32 ")", uorb_index, + report.device_id); + _corrections.accel_device_ids[uorb_index] = 0; + + } else { + _corrections.accel_device_ids[uorb_index] = report.device_id; + } + } + } + // Gyro for (uint8_t uorb_index = 0; uorb_index < GYRO_COUNT_MAX; uorb_index++) { sensor_gyro_s report; @@ -83,20 +101,20 @@ void TemperatureCompensationModule::parameters_update() } } - // Accel - for (uint8_t uorb_index = 0; uorb_index < ACCEL_COUNT_MAX; uorb_index++) { - sensor_accel_s report; + // Mag + for (uint8_t uorb_index = 0; uorb_index < MAG_COUNT_MAX; uorb_index++) { + sensor_mag_s report; - if (_accel_subs[uorb_index].copy(&report)) { + if (_mag_subs[uorb_index].copy(&report)) { int temp = _temperature_compensation.set_sensor_id_accel(report.device_id, uorb_index); if (temp < 0) { - PX4_INFO("No temperature calibration available for accel %" PRIu8 " (device id %" PRIu32 ")", uorb_index, + PX4_INFO("No temperature calibration available for mag %" PRIu8 " (device id %" PRIu32 ")", uorb_index, report.device_id); - _corrections.accel_device_ids[uorb_index] = 0; + _corrections.mag_device_ids[uorb_index] = 0; } else { - _corrections.accel_device_ids[uorb_index] = report.device_id; + _corrections.mag_device_ids[uorb_index] = report.device_id; } } } @@ -166,6 +184,29 @@ void TemperatureCompensationModule::gyroPoll() } } +void TemperatureCompensationModule::magPoll() +{ + float *offsets[] = {_corrections.mag_offset_0, _corrections.mag_offset_1, _corrections.mag_offset_2, _corrections.mag_offset_3 }; + + // For each mag instance + for (uint8_t uorb_index = 0; uorb_index < MAG_COUNT_MAX; uorb_index++) { + sensor_mag_s report; + + // Grab temperature from report + if (_mag_subs[uorb_index].update(&report)) { + if (PX4_ISFINITE(report.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) { + + _corrections.mag_device_ids[uorb_index] = report.device_id; + _corrections.mag_temperature[uorb_index] = report.temperature; + _corrections_changed = true; + } + } + } + } +} + void TemperatureCompensationModule::baroPoll() { float *offsets[] = {&_corrections.baro_offset_0, &_corrections.baro_offset_1, &_corrections.baro_offset_2, &_corrections.baro_offset_3 }; @@ -204,16 +245,22 @@ void TemperatureCompensationModule::Run() if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION) { bool got_temperature_calibration_command = false; bool accel = false; - bool baro = false; bool gyro = false; + bool mag = false; + bool baro = false; + + if ((int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { + accel = true; + got_temperature_calibration_command = true; + } if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { gyro = true; got_temperature_calibration_command = true; } - if ((int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { - accel = true; + if ((int)(cmd.param2) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { + mag = true; got_temperature_calibration_command = true; } @@ -223,7 +270,7 @@ void TemperatureCompensationModule::Run() } if (got_temperature_calibration_command) { - int ret = run_temperature_calibration(accel, baro, gyro); + int ret = run_temperature_calibration(accel, baro, gyro, mag); // publish ACK vehicle_command_ack_s command_ack{}; @@ -258,6 +305,7 @@ void TemperatureCompensationModule::Run() accelPoll(); gyroPoll(); + magPoll(); baroPoll(); // publish sensor corrections if necessary @@ -306,27 +354,34 @@ int TemperatureCompensationModule::custom_command(int argc, char *argv[]) if (!strcmp(argv[0], "calibrate")) { bool accel_calib = false; - bool baro_calib = false; bool gyro_calib = false; + bool mag_calib = false; + bool baro_calib = false; bool calib_all = true; int myoptind = 1; int ch; + const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "abg", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "agmb", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'a': accel_calib = true; calib_all = false; break; - case 'b': - baro_calib = true; + case 'g': + gyro_calib = true; calib_all = false; break; - case 'g': - gyro_calib = true; + case 'm': + mag_calib = true; + calib_all = false; + break; + + case 'b': + baro_calib = true; calib_all = false; break; @@ -349,7 +404,8 @@ int TemperatureCompensationModule::custom_command(int argc, char *argv[]) vcmd.timestamp = hrt_absolute_time(); vcmd.param1 = (float)((gyro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN); - vcmd.param2 = NAN; + vcmd.param2 = (float)((mag_calib + || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN); vcmd.param3 = NAN; vcmd.param4 = NAN; vcmd.param5 = ((accel_calib @@ -398,8 +454,9 @@ a temperature cycle. PRINT_MODULE_USAGE_NAME("temperature_compensation", "system"); PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the module, which monitors the sensors and updates the sensor_correction topic"); PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate", "Run temperature calibration process"); - PRINT_MODULE_USAGE_PARAM_FLAG('g', "calibrate the gyro", true); PRINT_MODULE_USAGE_PARAM_FLAG('a', "calibrate the accel", true); + PRINT_MODULE_USAGE_PARAM_FLAG('g', "calibrate the gyro", true); + PRINT_MODULE_USAGE_PARAM_FLAG('m', "calibrate the mag", true); PRINT_MODULE_USAGE_PARAM_FLAG('b', "calibrate the baro (if none of these is given, all will be calibrated)", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); diff --git a/src/modules/temperature_compensation/TemperatureCompensationModule.h b/src/modules/temperature_compensation/TemperatureCompensationModule.h index 24e9ea5f49..496c20b614 100644 --- a/src/modules/temperature_compensation/TemperatureCompensationModule.h +++ b/src/modules/temperature_compensation/TemperatureCompensationModule.h @@ -54,6 +54,7 @@ #include #include #include +#include #include #include @@ -97,6 +98,7 @@ private: void accelPoll(); void gyroPoll(); + void magPoll(); void baroPoll(); /** @@ -119,6 +121,13 @@ private: {ORB_ID(sensor_gyro), 3}, }; + uORB::Subscription _mag_subs[MAG_COUNT_MAX] { + {ORB_ID(sensor_mag), 0}, + {ORB_ID(sensor_mag), 1}, + {ORB_ID(sensor_mag), 2}, + {ORB_ID(sensor_mag), 3}, + }; + uORB::Subscription _baro_subs[BARO_COUNT_MAX] { {ORB_ID(sensor_baro), 0}, {ORB_ID(sensor_baro), 1}, diff --git a/src/modules/temperature_compensation/temp_comp_params_mag.c b/src/modules/temperature_compensation/temp_comp_params_mag.c new file mode 100644 index 0000000000..1f1f31cb46 --- /dev/null +++ b/src/modules/temperature_compensation/temp_comp_params_mag.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Thermal compensation for magnetometer sensors. + * + * @group Thermal Compensation + * @reboot_required true + * @boolean + */ +PARAM_DEFINE_INT32(TC_M_ENABLE, 0); diff --git a/src/modules/temperature_compensation/temp_comp_params_mag_0.c b/src/modules/temperature_compensation/temp_comp_params_mag_0.c new file mode 100644 index 0000000000..621fcfaaaa --- /dev/null +++ b/src/modules/temperature_compensation/temp_comp_params_mag_0.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2020 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* Magnetometer 0 */ + +/** + * ID of Magnetometer that the calibration is for. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_INT32(TC_M0_ID, 0); + +/** + * Magnetometer offset temperature ^3 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M0_X3_0, 0.0f); + +/** + * Magnetometer offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M0_X3_1, 0.0f); + +/** + * Magnetometer offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M0_X3_2, 0.0f); + +/** + * Magnetometer offset temperature ^2 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M0_X2_0, 0.0f); + +/** + * Magnetometer offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M0_X2_1, 0.0f); + +/** + * Magnetometer offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M0_X2_2, 0.0f); + +/** + * Magnetometer offset temperature ^1 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M0_X1_0, 0.0f); + +/** + * Magnetometer offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M0_X1_1, 0.0f); + +/** + * Magnetometer offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M0_X1_2, 0.0f); + +/** + * Magnetometer offset temperature ^0 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M0_X0_0, 0.0f); + +/** + * Magnetometer offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M0_X0_1, 0.0f); + +/** + * Magnetometer offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M0_X0_2, 0.0f); + +/** + * Magnetometer calibration reference temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M0_TREF, 25.0f); + +/** + * Magnetometer calibration minimum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M0_TMIN, 0.0f); + +/** + * Magnetometer calibration maximum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M0_TMAX, 100.0f); diff --git a/src/modules/temperature_compensation/temp_comp_params_mag_1.c b/src/modules/temperature_compensation/temp_comp_params_mag_1.c new file mode 100644 index 0000000000..b7e38db737 --- /dev/null +++ b/src/modules/temperature_compensation/temp_comp_params_mag_1.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2020 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* Magnetometer 1 */ + +/** + * ID of Magnetometer that the calibration is for. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_INT32(TC_M1_ID, 0); + +/** + * Magnetometer offset temperature ^3 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M1_X3_0, 0.0f); + +/** + * Magnetometer offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M1_X3_1, 0.0f); + +/** + * Magnetometer offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M1_X3_2, 0.0f); + +/** + * Magnetometer offset temperature ^2 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M1_X2_0, 0.0f); + +/** + * Magnetometer offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M1_X2_1, 0.0f); + +/** + * Magnetometer offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M1_X2_2, 0.0f); + +/** + * Magnetometer offset temperature ^1 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M1_X1_0, 0.0f); + +/** + * Magnetometer offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M1_X1_1, 0.0f); + +/** + * Magnetometer offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M1_X1_2, 0.0f); + +/** + * Magnetometer offset temperature ^0 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M1_X0_0, 0.0f); + +/** + * Magnetometer offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M1_X0_1, 0.0f); + +/** + * Magnetometer offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M1_X0_2, 0.0f); + +/** + * Magnetometer calibration reference temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M1_TREF, 25.0f); + +/** + * Magnetometer calibration minimum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M1_TMIN, 0.0f); + +/** + * Magnetometer calibration maximum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M1_TMAX, 100.0f); diff --git a/src/modules/temperature_compensation/temp_comp_params_mag_2.c b/src/modules/temperature_compensation/temp_comp_params_mag_2.c new file mode 100644 index 0000000000..8c71906f21 --- /dev/null +++ b/src/modules/temperature_compensation/temp_comp_params_mag_2.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2020 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* Magnetometer 2 */ + +/** + * ID of Magnetometer that the calibration is for. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_INT32(TC_M2_ID, 0); + +/** + * Magnetometer offset temperature ^3 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M2_X3_0, 0.0f); + +/** + * Magnetometer offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M2_X3_1, 0.0f); + +/** + * Magnetometer offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M2_X3_2, 0.0f); + +/** + * Magnetometer offset temperature ^2 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M2_X2_0, 0.0f); + +/** + * Magnetometer offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M2_X2_1, 0.0f); + +/** + * Magnetometer offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M2_X2_2, 0.0f); + +/** + * Magnetometer offset temperature ^1 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M2_X1_0, 0.0f); + +/** + * Magnetometer offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M2_X1_1, 0.0f); + +/** + * Magnetometer offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M2_X1_2, 0.0f); + +/** + * Magnetometer offset temperature ^0 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M2_X0_0, 0.0f); + +/** + * Magnetometer offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M2_X0_1, 0.0f); + +/** + * Magnetometer offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M2_X0_2, 0.0f); + +/** + * Magnetometer calibration reference temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M2_TREF, 25.0f); + +/** + * Magnetometer calibration minimum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M2_TMIN, 0.0f); + +/** + * Magnetometer calibration maximum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M2_TMAX, 100.0f); diff --git a/src/modules/temperature_compensation/temp_comp_params_mag_3.c b/src/modules/temperature_compensation/temp_comp_params_mag_3.c new file mode 100644 index 0000000000..7b44d3f59b --- /dev/null +++ b/src/modules/temperature_compensation/temp_comp_params_mag_3.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2020 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* Magnetometer 3 */ + +/** + * ID of Magnetometer that the calibration is for. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_INT32(TC_M3_ID, 0); + +/** + * Magnetometer offset temperature ^3 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M3_X3_0, 0.0f); + +/** + * Magnetometer offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M3_X3_1, 0.0f); + +/** + * Magnetometer offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M3_X3_2, 0.0f); + +/** + * Magnetometer offset temperature ^2 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M3_X2_0, 0.0f); + +/** + * Magnetometer offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M3_X2_1, 0.0f); + +/** + * Magnetometer offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M3_X2_2, 0.0f); + +/** + * Magnetometer offset temperature ^1 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M3_X1_0, 0.0f); + +/** + * Magnetometer offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M3_X1_1, 0.0f); + +/** + * Magnetometer offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M3_X1_2, 0.0f); + +/** + * Magnetometer offset temperature ^0 polynomial coefficient - X axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M3_X0_0, 0.0f); + +/** + * Magnetometer offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M3_X0_1, 0.0f); + +/** + * Magnetometer offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M3_X0_2, 0.0f); + +/** + * Magnetometer calibration reference temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M3_TREF, 25.0f); + +/** + * Magnetometer calibration minimum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M3_TMIN, 0.0f); + +/** + * Magnetometer calibration maximum temperature. + * + * @group Thermal Compensation + * @category system + */ +PARAM_DEFINE_FLOAT(TC_M3_TMAX, 100.0f); diff --git a/src/modules/temperature_compensation/temperature_calibration/mag.cpp b/src/modules/temperature_compensation/temperature_calibration/mag.cpp new file mode 100644 index 0000000000..2eeba2dc57 --- /dev/null +++ b/src/modules/temperature_compensation/temperature_calibration/mag.cpp @@ -0,0 +1,229 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mag.cpp + * Implementation of the Mag Temperature Calibration for onboard sensors. + * + * @author Siddharth Bharat Purohit + * @author Paul Riseborough + * @author Beat Küng + */ + +#include "mag.h" +#include +#include +#include + +TemperatureCalibrationMag::TemperatureCalibrationMag(float min_temperature_rise, float min_start_temperature, + float max_start_temperature) + : TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature) +{ + // init subscriptions + _num_sensor_instances = orb_group_count(ORB_ID(sensor_mag)); + + if (_num_sensor_instances > SENSOR_COUNT_MAX) { + _num_sensor_instances = SENSOR_COUNT_MAX; + } + + for (unsigned i = 0; i < _num_sensor_instances; i++) { + _sensor_subs[i] = orb_subscribe_multi(ORB_ID(sensor_mag), i); + } +} + +TemperatureCalibrationMag::~TemperatureCalibrationMag() +{ + for (unsigned i = 0; i < _num_sensor_instances; i++) { + orb_unsubscribe(_sensor_subs[i]); + } +} + +int TemperatureCalibrationMag::update_sensor_instance(PerSensorData &data, int sensor_sub) +{ + bool finished = data.hot_soaked; + + bool updated; + orb_check(sensor_sub, &updated); + + if (!updated) { + return finished ? 0 : 1; + } + + sensor_mag_s mag_data; + orb_copy(ORB_ID(sensor_mag), sensor_sub, &mag_data); + + if (finished) { + // if we're done, return, but we need to return after orb_copy because of poll() + return 0; + } + + if (PX4_ISFINITE(mag_data.temperature)) { + data.has_valid_temperature = true; + + } else { + return 0; + } + + data.device_id = mag_data.device_id; + + data.sensor_sample_filt[0] = mag_data.x; + data.sensor_sample_filt[1] = mag_data.y; + data.sensor_sample_filt[2] = mag_data.z; + data.sensor_sample_filt[3] = mag_data.temperature; + + // wait for min start temp to be reached before starting calibration + if (data.sensor_sample_filt[3] < _min_start_temperature) { + return 1; + } + + if (!data.cold_soaked) { + // allow time for sensors and filters to settle + if (hrt_absolute_time() > 10E6) { + // If intial temperature exceeds maximum declare an error condition and exit + if (data.sensor_sample_filt[3] > _max_start_temperature) { + return -TC_ERROR_INITIAL_TEMP_TOO_HIGH; + + } else { + data.cold_soaked = true; + data.low_temp = data.sensor_sample_filt[3]; // Record the low temperature + data.high_temp = data.low_temp; // Initialise the high temperature to the initial temperature + data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise; + return 1; + } + + } else { + return 1; + } + } + + // check if temperature increased + if (data.sensor_sample_filt[3] > data.high_temp) { + data.high_temp = data.sensor_sample_filt[3]; + data.hot_soak_sat = 0; + + } else { + return 1; + } + + //TODO: Detect when temperature has stopped rising for more than TBD seconds + if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) { + data.hot_soaked = true; + } + + if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor + TC_DEBUG("\nMag: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0], + (double)data.sensor_sample_filt[1], + (double)data.sensor_sample_filt[2], (double)data.sensor_sample_filt[3], (double)data.low_temp, (double)data.high_temp, + (double)(data.high_temp - data.low_temp)); + } + + //update linear fit matrices + double relative_temperature = (double)data.sensor_sample_filt[3] - (double)data.ref_temp; + data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]); + data.P[1].update(relative_temperature, (double)data.sensor_sample_filt[1]); + data.P[2].update(relative_temperature, (double)data.sensor_sample_filt[2]); + + return 1; +} + +int TemperatureCalibrationMag::finish() +{ + for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) { + finish_sensor_instance(_data[uorb_index], uorb_index); + } + + int32_t enabled = 1; + int result = param_set_no_notification(param_find("TC_A_ENABLE"), &enabled); + + if (result != PX4_OK) { + PX4_ERR("unable to reset TC_A_ENABLE (%i)", result); + } + + return result; +} + +int TemperatureCalibrationMag::finish_sensor_instance(PerSensorData &data, int sensor_index) +{ + if (!data.has_valid_temperature) { + PX4_WARN("Result Mag %d does not have a valid temperature sensor", sensor_index); + + uint32_t param = 0; + set_parameter("TC_M%d_ID", sensor_index, ¶m); + return 0; + } + + if (!data.hot_soaked || data.tempcal_complete) { + return 0; + } + + double res[3][4] = {}; + data.P[0].fit(res[0]); + res[0][3] = 0.0; // normalise the correction to be zero at the reference temperature + PX4_INFO("Result Mag %d Axis 0: %.20f %.20f %.20f %.20f", sensor_index, (double)res[0][0], (double)res[0][1], + (double)res[0][2], + (double)res[0][3]); + data.P[1].fit(res[1]); + res[1][3] = 0.0; // normalise the correction to be zero at the reference temperature + PX4_INFO("Result Mag %d Axis 1: %.20f %.20f %.20f %.20f", sensor_index, (double)res[1][0], (double)res[1][1], + (double)res[1][2], + (double)res[1][3]); + data.P[2].fit(res[2]); + res[2][3] = 0.0; // normalise the correction to be zero at the reference temperature + PX4_INFO("Result Mag %d Axis 2: %.20f %.20f %.20f %.20f", sensor_index, (double)res[2][0], (double)res[2][1], + (double)res[2][2], + (double)res[2][3]); + data.tempcal_complete = true; + + char str[30]; + float param = 0.0f; + int result = PX4_OK; + + set_parameter("TC_M%d_ID", sensor_index, &data.device_id); + + for (unsigned axis_index = 0; axis_index < 3; axis_index++) { + for (unsigned coef_index = 0; coef_index <= 3; coef_index++) { + sprintf(str, "TC_M%d_X%d_%d", sensor_index, 3 - coef_index, axis_index); + param = (float)res[axis_index][coef_index]; + result = param_set_no_notification(param_find(str), ¶m); + + if (result != PX4_OK) { + PX4_ERR("unable to reset %s", str); + } + } + } + + set_parameter("TC_M%d_TMAX", sensor_index, &data.high_temp); + set_parameter("TC_M%d_TMIN", sensor_index, &data.low_temp); + set_parameter("TC_M%d_TREF", sensor_index, &data.ref_temp); + return 0; +} diff --git a/src/modules/temperature_compensation/temperature_calibration/mag.h b/src/modules/temperature_compensation/temperature_calibration/mag.h new file mode 100644 index 0000000000..3494531054 --- /dev/null +++ b/src/modules/temperature_compensation/temperature_calibration/mag.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "common.h" +#include "polyfit.hpp" + +class TemperatureCalibrationMag : public TemperatureCalibrationCommon<3, 3> +{ +public: + TemperatureCalibrationMag(float min_temperature_rise, float min_start_temperature, float max_start_temperature); + virtual ~TemperatureCalibrationMag(); + + /** + * @see TemperatureCalibrationBase::finish() + */ + int finish(); + +private: + + virtual inline int update_sensor_instance(PerSensorData &data, int sensor_sub); + + inline int finish_sensor_instance(PerSensorData &data, int sensor_index); +}; diff --git a/src/modules/temperature_compensation/temperature_calibration/task.cpp b/src/modules/temperature_compensation/temperature_calibration/task.cpp index 7328784522..396bd81b1c 100644 --- a/src/modules/temperature_compensation/temperature_calibration/task.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/task.cpp @@ -56,6 +56,7 @@ #include "accel.h" #include "baro.h" #include "gyro.h" +#include "mag.h" class TemperatureCalibration; @@ -68,7 +69,12 @@ class TemperatureCalibration { public: - TemperatureCalibration(bool accel, bool baro, bool gyro) : _accel(accel), _baro(baro), _gyro(gyro) {} + TemperatureCalibration(bool accel, bool gyro, bool mag, bool baro) + : _accel(accel) + , _gyro(gyro) + , _mag(mag) + , _baro(baro) {}; + ~TemperatureCalibration() = default; /** @@ -93,8 +99,9 @@ private: int _control_task = -1; // task handle for task const bool _accel; ///< enable accel calibration? - const bool _baro; ///< enable baro calibration? - const bool _gyro; ///< enable gyro calibration? + const bool _gyro; ///< enable gyro calibration? + const bool _mag; ///< enable mag calibration? + const bool _baro; ///< enable baro calibration? }; void TemperatureCalibration::task_main() @@ -163,6 +170,17 @@ void TemperatureCalibration::task_main() } } + if (_mag) { + calibrators[num_calibrators] = new TemperatureCalibrationMag(min_temp_rise, min_start_temp, max_start_temp); + + if (calibrators[num_calibrators]) { + ++num_calibrators; + + } else { + PX4_ERR("alloc failed"); + } + } + hrt_abstime next_progress_output = hrt_absolute_time() + 1e6; // control LED's: blink, then turn solid according to progress @@ -331,11 +349,12 @@ void TemperatureCalibration::publish_led_control(led_control_s &led_control) _led_control_pub.publish(led_control); } -int run_temperature_calibration(bool accel, bool baro, bool gyro) +int run_temperature_calibration(bool accel, bool gyro, bool mag, bool baro) { if (temperature_calibration::instance.load() == nullptr) { - PX4_INFO("Starting temperature calibration task (accel=%i, baro=%i, gyro=%i)", (int)accel, (int)baro, (int)gyro); - temperature_calibration::instance.store(new TemperatureCalibration(accel, baro, gyro)); + PX4_INFO("Starting temperature calibration task (accel=%i, gyro=%i, mag=%i, baro=%i)", (int)accel, (int)gyro, (int)mag, + (int)baro); + temperature_calibration::instance.store(new TemperatureCalibration(accel, gyro, mag, baro)); if (temperature_calibration::instance.load() == nullptr) { PX4_ERR("alloc failed"); diff --git a/src/modules/temperature_compensation/temperature_calibration/temperature_calibration.h b/src/modules/temperature_compensation/temperature_calibration/temperature_calibration.h index d5a7bd2577..8d326dc7cd 100644 --- a/src/modules/temperature_compensation/temperature_calibration/temperature_calibration.h +++ b/src/modules/temperature_compensation/temperature_calibration/temperature_calibration.h @@ -36,4 +36,4 @@ /** start temperature calibration in a new task for one or multiple sensors * @return 0 on success, <0 error otherwise */ -int run_temperature_calibration(bool accel, bool baro, bool gyro); +int run_temperature_calibration(bool accel, bool gyro, bool mag, bool baro);