dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sensors: allow up to 4 accels, gyros, and baros and add configurable rotations for accel & gyro

This commit is contained in:
Daniel Agar 2020-10-08 19:01:44 -04:00 committed by GitHub
parent 28956e0399
commit eecf2e7a1e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
47 changed files with 2881 additions and 1163 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

301
Tools/process_sensor_caldata.py
View File

@ -98,6 +98,10 @@ for d in data:
sensor_gyro_2 = d.data
print('found gyro 2 data')
num_gyros += 1
elif d.multi_id == 3:
sensor_gyro_3 = d.data
print('found gyro 3 data')
num_gyros += 1
# extract accel data
num_accels = 0
@ -115,6 +119,10 @@ for d in data:
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 baro data
num_baros = 0
@ -132,6 +140,10 @@ for d in data:
sensor_baro_2 = d.data
print('found baro 2 data')
num_baros += 1
elif d.multi_id == 3:
sensor_baro_3 = d.data
print('found baro 3 data')
num_baros += 1
# open file to save plots to PDF
from matplotlib.backends.backend_pdf import PdfPages
@ -464,6 +476,99 @@ if num_gyros >= 3 and not math.isnan(sensor_gyro_2['temperature'][0]):
#################################################################################
# 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']
# 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()
#################################################################################
#################################################################################
# define data dictionary of accel 0 thermal correction parameters
accel_0_params = {
'TC_A0_ID':0,
@ -542,7 +647,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(4,figsize=(20,13))
plt.figure(5,figsize=(20,13))
# draw plots
plt.subplot(3,1,1)
@ -653,7 +758,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(5,figsize=(20,13))
plt.figure(6,figsize=(20,13))
# draw plots
plt.subplot(3,1,1)
@ -765,7 +870,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(6,figsize=(20,13))
plt.figure(7,figsize=(20,13))
# draw plots
plt.subplot(3,1,1)
@ -798,6 +903,102 @@ if num_accels >= 3 and not math.isnan(sensor_accel_2['temperature'][0]):
#################################################################################
# define data dictionary of accel 3 thermal correction parameters
accel_3_params = {
'TC_A3_ID':0,
'TC_A3_TMIN':0.0,
'TC_A3_TMAX':0.0,
'TC_A3_TREF':0.0,
'TC_A3_X0_0':0.0,
'TC_A3_X1_0':0.0,
'TC_A3_X2_0':0.0,
'TC_A3_X3_0':0.0,
'TC_A3_X0_1':0.0,
'TC_A3_X1_1':0.0,
'TC_A3_X2_1':0.0,
'TC_A3_X3_1':0.0,
'TC_A3_X0_2':0.0,
'TC_A3_X1_2':0.0,
'TC_A3_X2_2':0.0,
'TC_A3_X3_2':0.0
}
# curve fit the data for accel 2 corrections
if num_accels >= 4 and not math.isnan(sensor_accel_3['temperature'][0]):
accel_3_params['TC_A3_ID'] = int(np.median(sensor_accel_3['device_id']))
# find the min, max and reference temperature
accel_3_params['TC_A3_TMIN'] = np.amin(sensor_accel_3['temperature'])
accel_3_params['TC_A3_TMAX'] = np.amax(sensor_accel_3['temperature'])
accel_3_params['TC_A3_TREF'] = 0.5 * (accel_3_params['TC_A3_TMIN'] + accel_3_params['TC_A3_TMAX'])
temp_rel = sensor_accel_3['temperature'] - accel_3_params['TC_A3_TREF']
temp_rel_resample = np.linspace(accel_3_params['TC_A3_TMIN']-accel_3_params['TC_A3_TREF'], accel_3_params['TC_A3_TMAX']-accel_3_params['TC_A3_TREF'], 100)
temp_resample = temp_rel_resample + accel_3_params['TC_A3_TREF']
# fit X axis
correction_x = sensor_accel_3['x']-np.median(sensor_accel_3['x'])
coef_accel_3_x = np.polyfit(temp_rel,correction_x,3)
accel_3_params['TC_A3_X3_0'] = coef_accel_3_x[0]
accel_3_params['TC_A3_X2_0'] = coef_accel_3_x[1]
accel_3_params['TC_A3_X1_0'] = coef_accel_3_x[2]
accel_3_params['TC_A3_X0_0'] = coef_accel_3_x[3]
fit_coef_accel_3_x = np.poly1d(coef_accel_3_x)
correction_x_resample = fit_coef_accel_3_x(temp_rel_resample)
# fit Y axis
correction_y = sensor_accel_3['y']-np.median(sensor_accel_3['y'])
coef_accel_3_y = np.polyfit(temp_rel,correction_y,3)
accel_3_params['TC_A3_X3_1'] = coef_accel_3_y[0]
accel_3_params['TC_A3_X2_1'] = coef_accel_3_y[1]
accel_3_params['TC_A3_X1_1'] = coef_accel_3_y[2]
accel_3_params['TC_A3_X0_1'] = coef_accel_3_y[3]
fit_coef_accel_3_y = np.poly1d(coef_accel_3_y)
correction_y_resample = fit_coef_accel_3_y(temp_rel_resample)
# fit Z axis
correction_z = sensor_accel_3['z']-np.median(sensor_accel_3['z'])
coef_accel_3_z = np.polyfit(temp_rel,correction_z,3)
accel_3_params['TC_A3_X3_2'] = coef_accel_3_z[0]
accel_3_params['TC_A3_X2_2'] = coef_accel_3_z[1]
accel_3_params['TC_A3_X1_2'] = coef_accel_3_z[2]
accel_3_params['TC_A3_X0_2'] = coef_accel_3_z[3]
fit_coef_accel_3_z = np.poly1d(coef_accel_3_z)
correction_z_resample = fit_coef_accel_3_z(temp_rel_resample)
# accel 3 vs temperature
plt.figure(8,figsize=(20,13))
# draw plots
plt.subplot(3,1,1)
plt.plot(sensor_accel_3['temperature'],correction_x,'b')
plt.plot(temp_resample,correction_x_resample,'r')
plt.title('Accel 3 ({}) Bias vs Temperature'.format(accel_3_params['TC_A3_ID']))
plt.ylabel('X bias (m/s/s)')
plt.xlabel('temperature (degC)')
plt.grid()
# draw plots
plt.subplot(3,1,2)
plt.plot(sensor_accel_3['temperature'],correction_y,'b')
plt.plot(temp_resample,correction_y_resample,'r')
plt.ylabel('Y bias (m/s/s)')
plt.xlabel('temperature (degC)')
plt.grid()
# draw plots
plt.subplot(3,1,3)
plt.plot(sensor_accel_3['temperature'],correction_z,'b')
plt.plot(temp_resample,correction_z_resample,'r')
plt.ylabel('Z bias (m/s/s)')
plt.xlabel('temperature (degC)')
plt.grid()
pp.savefig()
#################################################################################
#################################################################################
# define data dictionary of baro 0 thermal correction parameters
baro_0_params = {
'TC_B0_ID':0,
@ -841,7 +1042,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(7,figsize=(20,13))
plt.figure(9,figsize=(20,13))
# draw plots
plt.plot(sensor_baro_0['temperature'],100*sensor_baro_0['pressure']-100*median_pressure,'b')
@ -897,8 +1098,8 @@ if num_baros >= 2:
fit_coef_baro_1_x = np.poly1d(coef_baro_1_x)
baro_1_x_resample = fit_coef_baro_1_x(temp_rel_resample)
# baro 1 vs temperature
plt.figure(8,figsize=(20,13))
# baro 2 vs temperature
plt.figure(9,figsize=(20,13))
# draw plots
plt.plot(sensor_baro_1['temperature'],100*sensor_baro_1['pressure']-100*median_pressure,'b')
@ -910,7 +1111,7 @@ if num_baros >= 2:
pp.savefig()
# define data dictionary of baro 1 thermal correction parameters
# define data dictionary of baro 2 thermal correction parameters
baro_2_params = {
'TC_B2_ID':0,
'TC_B2_TMIN':0.0,
@ -956,7 +1157,7 @@ if num_baros >= 3:
baro_2_x_resample = fit_coef_baro_2_x(temp_rel_resample)
# baro 2 vs temperature
plt.figure(8,figsize=(20,13))
plt.figure(10,figsize=(20,13))
# draw plots
plt.plot(sensor_baro_2['temperature'],100*sensor_baro_2['pressure']-100*median_pressure,'b')
@ -968,6 +1169,59 @@ if num_baros >= 3:
pp.savefig()
# define data dictionary of baro 3 thermal correction parameters
baro_3_params = {
'TC_B3_ID':0,
'TC_B3_TMIN':0.0,
'TC_B3_TMAX':0.0,
'TC_B3_TREF':0.0,
'TC_B3_X0':0.0,
'TC_B3_X1':0.0,
'TC_B3_X2':0.0,
'TC_B3_X3':0.0,
'TC_B3_X4':0.0,
'TC_B3_X5':0.0,
'TC_B3_SCL':1.0,
}
if num_baros >= 4:
# curve fit the data for baro 2 corrections
baro_3_params['TC_B3_ID'] = int(np.median(sensor_baro_3['device_id']))
# find the min, max and reference temperature
baro_3_params['TC_B3_TMIN'] = np.amin(sensor_baro_3['temperature'])
baro_3_params['TC_B3_TMAX'] = np.amax(sensor_baro_3['temperature'])
baro_3_params['TC_B3_TREF'] = 0.5 * (baro_3_params['TC_B3_TMIN'] + baro_3_params['TC_B3_TMAX'])
temp_rel = sensor_baro_3['temperature'] - baro_3_params['TC_B3_TREF']
temp_rel_resample = np.linspace(baro_3_params['TC_B3_TMIN']-baro_3_params['TC_B3_TREF'], baro_3_params['TC_B3_TMAX']-baro_3_params['TC_B3_TREF'], 100)
temp_resample = temp_rel_resample + baro_3_params['TC_B3_TREF']
# fit data
median_pressure = np.median(sensor_baro_3['pressure'])
coef_baro_3_x = np.polyfit(temp_rel,100*(sensor_baro_3['pressure']-median_pressure),5) # convert from hPa to Pa
baro_3_params['TC_B3_X5'] = coef_baro_3_x[0]
baro_3_params['TC_B3_X4'] = coef_baro_3_x[1]
baro_3_params['TC_B3_X3'] = coef_baro_3_x[2]
baro_3_params['TC_B3_X2'] = coef_baro_3_x[3]
baro_3_params['TC_B3_X1'] = coef_baro_3_x[4]
baro_3_params['TC_B3_X0'] = coef_baro_3_x[5]
fit_coef_baro_3_x = np.poly1d(coef_baro_3_x)
baro_3_x_resample = fit_coef_baro_3_x(temp_rel_resample)
# baro 3 vs temperature
plt.figure(11,figsize=(20,13))
# draw plots
plt.plot(sensor_baro_3['temperature'],100*sensor_baro_3['pressure']-100*median_pressure,'b')
plt.plot(temp_resample,baro_3_x_resample,'r')
plt.title('Baro 3 ({}) Bias vs Temperature'.format(baro_3_params['TC_B3_ID']))
plt.ylabel('Z bias (Pa)')
plt.xlabel('temperature (degC)')
plt.grid()
pp.savefig()
#################################################################################
# close the pdf file
@ -1013,6 +1267,16 @@ for key in key_list_accel:
type = "9"
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(accel_2_params[key])+"\t"+type+"\n")
# accel 3 corrections
key_list_accel = list(accel_3_params.keys())
key_list_accel.sort
for key in key_list_accel:
if key == 'TC_A3_ID':
type = "6"
else:
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
@ -1043,6 +1307,17 @@ for key in key_list_baro:
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()
@ -1073,6 +1348,16 @@ for key in key_list_gyro:
type = "9"
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(gyro_2_params[key])+"\t"+type+"\n")
# gyro 3 corrections
key_list_gyro = list(gyro_3_params.keys())
key_list_gyro.sort()
for key in key_list_gyro:
if key == 'TC_G3_ID':
type = "6"
else:
type = "9"
file.write("1"+"\t"+"1"+"\t"+key+"\t"+str(gyro_3_params[key])+"\t"+type+"\n")
file.close()
print('Correction parameters written to ' + test_results_filename)

9
msg/sensor_correction.msg
View File

@ -6,21 +6,24 @@ 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[3] gyro_device_ids
uint32[4] gyro_device_ids
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[3] accel_device_ids
uint32[4] accel_device_ids
float32[3] accel_offset_0 # accelerometer 0 XYZ offsets in the sensor frame in m/s/s
float32[3] accel_offset_1 # accelerometer 1 XYZ offsets in the sensor frame in m/s/s
float32[3] accel_offset_2 # accelerometer 2 XYZ offsets in the sensor frame in m/s/s
float32[3] accel_offset_3 # accelerometer 3 XYZ offsets in the sensor frame 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[3] baro_device_ids
uint32[4] baro_device_ids
float32 baro_offset_0 # barometric pressure 0 offsets in the sensor frame in m/s/s
float32 baro_offset_1 # barometric pressure 1 offsets in the sensor frame in m/s/s
float32 baro_offset_2 # barometric pressure 2 offsets in the sensor frame in m/s/s
float32 baro_offset_3 # barometric pressure 3 offsets in the sensor frame in m/s/s

48
src/lib/sensor_calibration/Accelerometer.cpp
View File

@ -110,6 +110,9 @@ void Accelerometer::SensorCorrectionsUpdate(bool force)
case 2:
_thermal_offset = Vector3f{corrections.accel_offset_2};
return;
case 3:
_thermal_offset = Vector3f{corrections.accel_offset_2};
return;
}
}
}
@ -120,6 +123,12 @@ void Accelerometer::SensorCorrectionsUpdate(bool force)
}
}
void Accelerometer::set_rotation(Rotation rotation)
{
_rotation_enum = rotation;
_rotation = get_rot_matrix(rotation);
}
void Accelerometer::ParametersUpdate()
{
if (_device_id == 0) {
@ -131,12 +140,30 @@ void Accelerometer::ParametersUpdate()
if (_calibration_index >= 0) {
if (!_external) {
_rotation = GetBoardRotation();
// CAL_ACCx_ROT
int32_t rotation_value = GetCalibrationParam(SensorString(), "ROT", _calibration_index);
if (_external) {
if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) {
PX4_ERR("External %s %d (%d) invalid rotation %d, resetting to rotation none",
SensorString(), _device_id, _calibration_index, rotation_value);
rotation_value = ROTATION_NONE;
SetCalibrationParam(SensorString(), "ROT", _calibration_index, rotation_value);
}
_rotation_enum = static_cast<Rotation>(rotation_value);
_rotation = get_rot_matrix(_rotation_enum);
} else {
// TODO: per sensor external rotation
_rotation.setIdentity();
// internal, CAL_ACCx_ROT -1
if (rotation_value != -1) {
PX4_ERR("Internal %s %d (%d) invalid rotation %d, resetting",
SensorString(), _device_id, _calibration_index, rotation_value);
SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
}
_rotation = GetBoardRotation();
_rotation_enum = ROTATION_NONE;
}
// CAL_ACCx_PRIO
@ -144,7 +171,7 @@ void Accelerometer::ParametersUpdate()
if ((_priority < 0) || (_priority > 100)) {
// reset to default, -1 is the uninitialized parameter value
int32_t new_priority = DEFAULT_PRIORITY;
int32_t new_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
if (_priority != -1) {
PX4_ERR("%s %d (%d) invalid priority %d, resetting to %d", SensorString(), _device_id, _calibration_index, _priority,
@ -185,6 +212,7 @@ void Accelerometer::ParametersUpdate()
void Accelerometer::Reset()
{
_rotation.setIdentity();
_rotation_enum = ROTATION_NONE;
_offset.zero();
_scale = Vector3f{1.f, 1.f, 1.f};
_thermal_offset.zero();
@ -206,12 +234,12 @@ bool Accelerometer::ParametersSave()
success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
success &= SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, _scale);
// if (_external) {
// SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
if (_external) {
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
// } else {
// SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
// }
} else {
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
}
return success;
}

8
src/lib/sensor_calibration/Accelerometer.hpp
View File

@ -45,10 +45,10 @@ namespace calibration
class Accelerometer
{
public:
static constexpr int MAX_SENSOR_COUNT = 3;
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr uint8_t DEFAULT_PRIORITY = 50;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 25;
static constexpr const char *SensorString() { return "ACC"; }
@ -64,6 +64,7 @@ public:
void set_external(bool external = true);
void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
void set_scale(const matrix::Vector3f &scale) { _scale = scale; }
void set_rotation(Rotation rotation);
uint8_t calibration_count() const { return _calibration_count; }
uint32_t device_id() const { return _device_id; }
@ -71,6 +72,7 @@ public:
bool external() const { return _external; }
const int32_t &priority() const { return _priority; }
const matrix::Dcmf &rotation() const { return _rotation; }
const Rotation &rotation_enum() const { return _rotation_enum; }
// apply offsets and scale
// rotate corrected measurements from sensor to body frame
@ -89,6 +91,8 @@ public:
private:
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
Rotation _rotation_enum{ROTATION_NONE};
matrix::Dcmf _rotation;
matrix::Vector3f _offset;
matrix::Vector3f _scale;

48
src/lib/sensor_calibration/Gyroscope.cpp
View File

@ -110,6 +110,9 @@ void Gyroscope::SensorCorrectionsUpdate(bool force)
case 2:
_thermal_offset = Vector3f{corrections.gyro_offset_2};
return;
case 3:
_thermal_offset = Vector3f{corrections.gyro_offset_3};
return;
}
}
}
@ -120,6 +123,12 @@ void Gyroscope::SensorCorrectionsUpdate(bool force)
}
}
void Gyroscope::set_rotation(Rotation rotation)
{
_rotation_enum = rotation;
_rotation = get_rot_matrix(rotation);
}
void Gyroscope::ParametersUpdate()
{
if (_device_id == 0) {
@ -131,12 +140,30 @@ void Gyroscope::ParametersUpdate()
if (_calibration_index >= 0) {
if (!_external) {
_rotation = GetBoardRotation();
// CAL_GYROx_ROT
int32_t rotation_value = GetCalibrationParam(SensorString(), "ROT", _calibration_index);
if (_external) {
if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) {
PX4_ERR("External %s %d (%d) invalid rotation %d, resetting to rotation none",
SensorString(), _device_id, _calibration_index, rotation_value);
rotation_value = ROTATION_NONE;
SetCalibrationParam(SensorString(), "ROT", _calibration_index, rotation_value);
}
_rotation_enum = static_cast<Rotation>(rotation_value);
_rotation = get_rot_matrix(_rotation_enum);
} else {
// TODO: per sensor external rotation
_rotation.setIdentity();
// internal, CAL_GYROx_ROT -1
if (rotation_value != -1) {
PX4_ERR("Internal %s %d (%d) invalid rotation %d, resetting",
SensorString(), _device_id, _calibration_index, rotation_value);
SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
}
_rotation = GetBoardRotation();
_rotation_enum = ROTATION_NONE;
}
// CAL_GYROx_PRIO
@ -144,7 +171,7 @@ void Gyroscope::ParametersUpdate()
if ((_priority < 0) || (_priority > 100)) {
// reset to default, -1 is the uninitialized parameter value
int32_t new_priority = DEFAULT_PRIORITY;
int32_t new_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
if (_priority != -1) {
PX4_ERR("%s %d (%d) invalid priority %d, resetting to %d", SensorString(), _device_id, _calibration_index, _priority,
@ -177,6 +204,7 @@ void Gyroscope::ParametersUpdate()
void Gyroscope::Reset()
{
_rotation.setIdentity();
_rotation_enum = ROTATION_NONE;
_offset.zero();
_thermal_offset.zero();
@ -196,12 +224,12 @@ bool Gyroscope::ParametersSave()
success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
// if (_external) {
// SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
if (_external) {
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
// } else {
// SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
// }
} else {
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
}
return success;
}

8
src/lib/sensor_calibration/Gyroscope.hpp
View File

@ -45,10 +45,10 @@ namespace calibration
class Gyroscope
{
public:
static constexpr int MAX_SENSOR_COUNT = 3;
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr uint8_t DEFAULT_PRIORITY = 50;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 25;
static constexpr const char *SensorString() { return "GYRO"; }
@ -63,6 +63,7 @@ public:
void set_device_id(uint32_t device_id, bool external = false);
void set_external(bool external = true);
void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
void set_rotation(Rotation rotation);
uint8_t calibration_count() const { return _calibration_count; }
uint32_t device_id() const { return _device_id; }
@ -70,6 +71,7 @@ public:
bool external() const { return _external; }
const int32_t &priority() const { return _priority; }
const matrix::Dcmf &rotation() const { return _rotation; }
const Rotation &rotation_enum() const { return _rotation_enum; }
// apply offsets and scale
// rotate corrected measurements from sensor to body frame
@ -88,6 +90,8 @@ public:
private:
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
Rotation _rotation_enum{ROTATION_NONE};
matrix::Dcmf _rotation;
matrix::Vector3f _offset;
matrix::Vector3f _thermal_offset;

6
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
View File

@ -47,13 +47,13 @@
using namespace time_literals;
static constexpr unsigned max_mandatory_gyro_count = 1;
static constexpr unsigned max_optional_gyro_count = 3;
static constexpr unsigned max_optional_gyro_count = 4;
static constexpr unsigned max_mandatory_accel_count = 1;
static constexpr unsigned max_optional_accel_count = 3;
static constexpr unsigned max_optional_accel_count = 4;
static constexpr unsigned max_mandatory_mag_count = 1;
static constexpr unsigned max_optional_mag_count = 4;
static constexpr unsigned max_mandatory_baro_count = 1;
static constexpr unsigned max_optional_baro_count = 1;
static constexpr unsigned max_optional_baro_count = 4;
bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm,

9
src/modules/commander/accelerometer_calibration.cpp
View File

@ -149,7 +149,7 @@ using namespace matrix;
using namespace time_literals;
static constexpr char sensor_name[] {"accel"};
static constexpr unsigned MAX_ACCEL_SENS = 3;
static constexpr unsigned MAX_ACCEL_SENS = 4;
/// Data passed to calibration worker routine
typedef struct {
@ -176,6 +176,7 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS
{ORB_ID(sensor_accel), 0, 0},
{ORB_ID(sensor_accel), 0, 1},
{ORB_ID(sensor_accel), 0, 2},
{ORB_ID(sensor_accel), 0, 3},
};
/* use the first sensor to pace the readout, but do per-sensor counts */
@ -202,6 +203,9 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS
case 2:
offset = Vector3f{sensor_correction.accel_offset_2};
break;
case 3:
offset = Vector3f{sensor_correction.accel_offset_3};
break;
}
}
}
@ -473,6 +477,9 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
case 2:
offset = Vector3f{sensor_correction.accel_offset_2};
break;
case 3:
offset = Vector3f{sensor_correction.accel_offset_3};
break;
}
}
}

6
src/modules/commander/gyro_calibration.cpp
View File

@ -59,7 +59,7 @@
#include <uORB/topics/sensor_gyro.h>
static constexpr char sensor_name[] {"gyro"};
static constexpr unsigned MAX_GYROS = 3;
static constexpr unsigned MAX_GYROS = 4;
using matrix::Vector3f;
@ -101,6 +101,7 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
{ORB_ID(sensor_gyro), 0, 0},
{ORB_ID(sensor_gyro), 0, 1},
{ORB_ID(sensor_gyro), 0, 2},
{ORB_ID(sensor_gyro), 0, 3},
};
memset(&worker_data.last_sample_0_x, 0, sizeof(worker_data.last_sample_0_x));
@ -148,6 +149,9 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
case 2:
offset = Vector3f{sensor_correction.gyro_offset_2};
break;
case 3:
offset = Vector3f{sensor_correction.gyro_offset_3};
break;
}
}
}

59
src/modules/sensors/sensor_params_acc0.c
View File

@ -55,6 +55,65 @@ PARAM_DEFINE_INT32(CAL_ACC0_ID, 0);
*/
PARAM_DEFINE_INT32(CAL_ACC0_PRIO, -1);
/**
* Rotation of accelerometer 0 relative to airframe.
*
* An internal sensor will force a value of -1, so a GCS
* should only attempt to configure the rotation if the value is
* greater than or equal to zero.
*
* @value -1 Internal
* @value 0 No rotation
* @value 1 Yaw 45°
* @value 2 Yaw 90°
* @value 3 Yaw 135°
* @value 4 Yaw 180°
* @value 5 Yaw 225°
* @value 6 Yaw 270°
* @value 7 Yaw 315°
* @value 8 Roll 180°
* @value 9 Roll 180°, Yaw 45°
* @value 10 Roll 180°, Yaw 90°
* @value 11 Roll 180°, Yaw 135°
* @value 12 Pitch 180°
* @value 13 Roll 180°, Yaw 225°
* @value 14 Roll 180°, Yaw 270°
* @value 15 Roll 180°, Yaw 315°
* @value 16 Roll 90°
* @value 17 Roll 90°, Yaw 45°
* @value 18 Roll 90°, Yaw 90°
* @value 19 Roll 90°, Yaw 135°
* @value 20 Roll 270°
* @value 21 Roll 270°, Yaw 45°
* @value 22 Roll 270°, Yaw 90°
* @value 23 Roll 270°, Yaw 135°
* @value 24 Pitch 90°
* @value 25 Pitch 270°
* @value 26 Pitch 180°, Yaw 90°
* @value 27 Pitch 180°, Yaw 270°
* @value 28 Roll 90°, Pitch 90°
* @value 29 Roll 180°, Pitch 90°
* @value 30 Roll 270°, Pitch 90°
* @value 31 Roll 90°, Pitch 180°
* @value 32 Roll 270°, Pitch 180°
* @value 33 Roll 90°, Pitch 270°
* @value 34 Roll 180°, Pitch 270°
* @value 35 Roll 270°, Pitch 270°
* @value 36 Roll 90°, Pitch 180°, Yaw 90°
* @value 37 Roll 90°, Yaw 270°
* @value 38 Roll 90°, Pitch 68°, Yaw 293°
* @value 39 Pitch 315°
* @value 40 Roll 90°, Pitch 315°
* @value 41 Roll 270°, Yaw 180°
*
* @min -1
* @max 41
* @reboot_required true
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_ACC0_ROT, -1);
/**
* Accelerometer X-axis offset
*

59
src/modules/sensors/sensor_params_acc1.c
View File

@ -55,6 +55,65 @@ PARAM_DEFINE_INT32(CAL_ACC1_ID, 0);
*/
PARAM_DEFINE_INT32(CAL_ACC1_PRIO, -1);
/**
* Rotation of accelerometer 1 relative to airframe.
*
* An internal sensor will force a value of -1, so a GCS
* should only attempt to configure the rotation if the value is
* greater than or equal to zero.
*
* @value -1 Internal
* @value 0 No rotation
* @value 1 Yaw 45°
* @value 2 Yaw 90°
* @value 3 Yaw 135°
* @value 4 Yaw 180°
* @value 5 Yaw 225°
* @value 6 Yaw 270°
* @value 7 Yaw 315°
* @value 8 Roll 180°
* @value 9 Roll 180°, Yaw 45°
* @value 10 Roll 180°, Yaw 90°
* @value 11 Roll 180°, Yaw 135°
* @value 12 Pitch 180°
* @value 13 Roll 180°, Yaw 225°
* @value 14 Roll 180°, Yaw 270°
* @value 15 Roll 180°, Yaw 315°
* @value 16 Roll 90°
* @value 17 Roll 90°, Yaw 45°
* @value 18 Roll 90°, Yaw 90°
* @value 19 Roll 90°, Yaw 135°
* @value 20 Roll 270°
* @value 21 Roll 270°, Yaw 45°
* @value 22 Roll 270°, Yaw 90°
* @value 23 Roll 270°, Yaw 135°
* @value 24 Pitch 90°
* @value 25 Pitch 270°
* @value 26 Pitch 180°, Yaw 90°
* @value 27 Pitch 180°, Yaw 270°
* @value 28 Roll 90°, Pitch 90°
* @value 29 Roll 180°, Pitch 90°
* @value 30 Roll 270°, Pitch 90°
* @value 31 Roll 90°, Pitch 180°
* @value 32 Roll 270°, Pitch 180°
* @value 33 Roll 90°, Pitch 270°
* @value 34 Roll 180°, Pitch 270°
* @value 35 Roll 270°, Pitch 270°
* @value 36 Roll 90°, Pitch 180°, Yaw 90°
* @value 37 Roll 90°, Yaw 270°
* @value 38 Roll 90°, Pitch 68°, Yaw 293°
* @value 39 Pitch 315°
* @value 40 Roll 90°, Pitch 315°
* @value 41 Roll 270°, Yaw 180°
*
* @min -1
* @max 41
* @reboot_required true
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_ACC1_ROT, -1);
/**
* Accelerometer X-axis offset
*

59
src/modules/sensors/sensor_params_acc2.c
View File

@ -55,6 +55,65 @@ PARAM_DEFINE_INT32(CAL_ACC2_ID, 0);
*/
PARAM_DEFINE_INT32(CAL_ACC2_PRIO, -1);
/**
* Rotation of accelerometer 2 relative to airframe.
*
* An internal sensor will force a value of -1, so a GCS
* should only attempt to configure the rotation if the value is
* greater than or equal to zero.
*
* @value -1 Internal
* @value 0 No rotation
* @value 1 Yaw 45°
* @value 2 Yaw 90°
* @value 3 Yaw 135°
* @value 4 Yaw 180°
* @value 5 Yaw 225°
* @value 6 Yaw 270°
* @value 7 Yaw 315°
* @value 8 Roll 180°
* @value 9 Roll 180°, Yaw 45°
* @value 10 Roll 180°, Yaw 90°
* @value 11 Roll 180°, Yaw 135°
* @value 12 Pitch 180°
* @value 13 Roll 180°, Yaw 225°
* @value 14 Roll 180°, Yaw 270°
* @value 15 Roll 180°, Yaw 315°
* @value 16 Roll 90°
* @value 17 Roll 90°, Yaw 45°
* @value 18 Roll 90°, Yaw 90°
* @value 19 Roll 90°, Yaw 135°
* @value 20 Roll 270°
* @value 21 Roll 270°, Yaw 45°
* @value 22 Roll 270°, Yaw 90°
* @value 23 Roll 270°, Yaw 135°
* @value 24 Pitch 90°
* @value 25 Pitch 270°
* @value 26 Pitch 180°, Yaw 90°
* @value 27 Pitch 180°, Yaw 270°
* @value 28 Roll 90°, Pitch 90°
* @value 29 Roll 180°, Pitch 90°
* @value 30 Roll 270°, Pitch 90°
* @value 31 Roll 90°, Pitch 180°
* @value 32 Roll 270°, Pitch 180°
* @value 33 Roll 90°, Pitch 270°
* @value 34 Roll 180°, Pitch 270°
* @value 35 Roll 270°, Pitch 270°
* @value 36 Roll 90°, Pitch 180°, Yaw 90°
* @value 37 Roll 90°, Yaw 270°
* @value 38 Roll 90°, Pitch 68°, Yaw 293°
* @value 39 Pitch 315°
* @value 40 Roll 90°, Pitch 315°
* @value 41 Roll 270°, Yaw 180°
*
* @min -1
* @max 41
* @reboot_required true
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_ACC2_ROT, -1);
/**
* Accelerometer X-axis offset
*

163
src/modules/sensors/sensor_params_acc3.c Normal file
View File

@ -0,0 +1,163 @@
/****************************************************************************
*
* Copyright (c) 2012-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.
*
****************************************************************************/
/**
* ID of the Accelerometer that the calibration is for.
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_ACC3_ID, 0);
/**
* Accelerometer 3 priority.
*
* @value -1 Uninitialized
* @value 0 Disabled
* @value 1 Min
* @value 25 Low
* @value 50 Medium (Default)
* @value 75 High
* @value 100 Max
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_ACC3_PRIO, -1);
/**
* Rotation of accelerometer 3 relative to airframe.
*
* An internal sensor will force a value of -1, so a GCS
* should only attempt to configure the rotation if the value is
* greater than or equal to zero.
*
* @value -1 Internal
* @value 0 No rotation
* @value 1 Yaw 45°
* @value 2 Yaw 90°
* @value 3 Yaw 135°
* @value 4 Yaw 180°
* @value 5 Yaw 225°
* @value 6 Yaw 270°
* @value 7 Yaw 315°
* @value 8 Roll 180°
* @value 9 Roll 180°, Yaw 45°
* @value 10 Roll 180°, Yaw 90°
* @value 11 Roll 180°, Yaw 135°
* @value 12 Pitch 180°
* @value 13 Roll 180°, Yaw 225°
* @value 14 Roll 180°, Yaw 270°
* @value 15 Roll 180°, Yaw 315°
* @value 16 Roll 90°
* @value 17 Roll 90°, Yaw 45°
* @value 18 Roll 90°, Yaw 90°
* @value 19 Roll 90°, Yaw 135°
* @value 20 Roll 270°
* @value 21 Roll 270°, Yaw 45°
* @value 22 Roll 270°, Yaw 90°
* @value 23 Roll 270°, Yaw 135°
* @value 24 Pitch 90°
* @value 25 Pitch 270°
* @value 26 Pitch 180°, Yaw 90°
* @value 27 Pitch 180°, Yaw 270°
* @value 28 Roll 90°, Pitch 90°
* @value 29 Roll 180°, Pitch 90°
* @value 30 Roll 270°, Pitch 90°
* @value 31 Roll 90°, Pitch 180°
* @value 32 Roll 270°, Pitch 180°
* @value 33 Roll 90°, Pitch 270°
* @value 34 Roll 180°, Pitch 270°
* @value 35 Roll 270°, Pitch 270°
* @value 36 Roll 90°, Pitch 180°, Yaw 90°
* @value 37 Roll 90°, Yaw 270°
* @value 38 Roll 90°, Pitch 68°, Yaw 293°
* @value 39 Pitch 315°
* @value 40 Roll 90°, Pitch 315°
* @value 41 Roll 270°, Yaw 180°
*
* @min -1
* @max 41
* @reboot_required true
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_ACC3_ROT, -1);
/**
* Accelerometer X-axis offset
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC3_XOFF, 0.0f);
/**
* Accelerometer Y-axis offset
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC3_YOFF, 0.0f);
/**
* Accelerometer Z-axis offset
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC3_ZOFF, 0.0f);
/**
* Accelerometer X-axis scaling factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC3_XSCALE, 1.0f);
/**
* Accelerometer Y-axis scaling factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC3_YSCALE, 1.0f);
/**
* Accelerometer Z-axis scaling factor
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC3_ZSCALE, 1.0f);

59
src/modules/sensors/sensor_params_gyro0.c
View File

@ -55,6 +55,65 @@ PARAM_DEFINE_INT32(CAL_GYRO0_ID, 0);
*/
PARAM_DEFINE_INT32(CAL_GYRO0_PRIO, -1);
/**
* Rotation of gyro 0 relative to airframe.
*
* An internal sensor will force a value of -1, so a GCS
* should only attempt to configure the rotation if the value is
* greater than or equal to zero.
*
* @value -1 Internal
* @value 0 No rotation
* @value 1 Yaw 45°
* @value 2 Yaw 90°
* @value 3 Yaw 135°
* @value 4 Yaw 180°
* @value 5 Yaw 225°
* @value 6 Yaw 270°
* @value 7 Yaw 315°
* @value 8 Roll 180°
* @value 9 Roll 180°, Yaw 45°
* @value 10 Roll 180°, Yaw 90°
* @value 11 Roll 180°, Yaw 135°
* @value 12 Pitch 180°
* @value 13 Roll 180°, Yaw 225°
* @value 14 Roll 180°, Yaw 270°
* @value 15 Roll 180°, Yaw 315°
* @value 16 Roll 90°
* @value 17 Roll 90°, Yaw 45°
* @value 18 Roll 90°, Yaw 90°
* @value 19 Roll 90°, Yaw 135°
* @value 20 Roll 270°
* @value 21 Roll 270°, Yaw 45°
* @value 22 Roll 270°, Yaw 90°
* @value 23 Roll 270°, Yaw 135°
* @value 24 Pitch 90°
* @value 25 Pitch 270°
* @value 26 Pitch 180°, Yaw 90°
* @value 27 Pitch 180°, Yaw 270°
* @value 28 Roll 90°, Pitch 90°
* @value 29 Roll 180°, Pitch 90°
* @value 30 Roll 270°, Pitch 90°
* @value 31 Roll 90°, Pitch 180°
* @value 32 Roll 270°, Pitch 180°
* @value 33 Roll 90°, Pitch 270°
* @value 34 Roll 180°, Pitch 270°
* @value 35 Roll 270°, Pitch 270°
* @value 36 Roll 90°, Pitch 180°, Yaw 90°
* @value 37 Roll 90°, Yaw 270°
* @value 38 Roll 90°, Pitch 68°, Yaw 293°
* @value 39 Pitch 315°
* @value 40 Roll 90°, Pitch 315°
* @value 41 Roll 270°, Yaw 180°
*
* @min -1
* @max 41
* @reboot_required true
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_GYRO0_ROT, -1);
/**
* Gyro X-axis offset
*

59
src/modules/sensors/sensor_params_gyro1.c
View File

@ -55,6 +55,65 @@ PARAM_DEFINE_INT32(CAL_GYRO1_ID, 0);
*/
PARAM_DEFINE_INT32(CAL_GYRO1_PRIO, -1);
/**
* Rotation of gyro 1 relative to airframe.
*
* An internal sensor will force a value of -1, so a GCS
* should only attempt to configure the rotation if the value is
* greater than or equal to zero.
*
* @value -1 Internal
* @value 0 No rotation
* @value 1 Yaw 45°
* @value 2 Yaw 90°
* @value 3 Yaw 135°
* @value 4 Yaw 180°
* @value 5 Yaw 225°
* @value 6 Yaw 270°
* @value 7 Yaw 315°
* @value 8 Roll 180°
* @value 9 Roll 180°, Yaw 45°
* @value 10 Roll 180°, Yaw 90°
* @value 11 Roll 180°, Yaw 135°
* @value 12 Pitch 180°
* @value 13 Roll 180°, Yaw 225°
* @value 14 Roll 180°, Yaw 270°
* @value 15 Roll 180°, Yaw 315°
* @value 16 Roll 90°
* @value 17 Roll 90°, Yaw 45°
* @value 18 Roll 90°, Yaw 90°
* @value 19 Roll 90°, Yaw 135°
* @value 20 Roll 270°
* @value 21 Roll 270°, Yaw 45°
* @value 22 Roll 270°, Yaw 90°
* @value 23 Roll 270°, Yaw 135°
* @value 24 Pitch 90°
* @value 25 Pitch 270°
* @value 26 Pitch 180°, Yaw 90°
* @value 27 Pitch 180°, Yaw 270°
* @value 28 Roll 90°, Pitch 90°
* @value 29 Roll 180°, Pitch 90°
* @value 30 Roll 270°, Pitch 90°
* @value 31 Roll 90°, Pitch 180°
* @value 32 Roll 270°, Pitch 180°
* @value 33 Roll 90°, Pitch 270°
* @value 34 Roll 180°, Pitch 270°
* @value 35 Roll 270°, Pitch 270°
* @value 36 Roll 90°, Pitch 180°, Yaw 90°
* @value 37 Roll 90°, Yaw 270°
* @value 38 Roll 90°, Pitch 68°, Yaw 293°
* @value 39 Pitch 315°
* @value 40 Roll 90°, Pitch 315°
* @value 41 Roll 270°, Yaw 180°
*
* @min -1
* @max 41
* @reboot_required true
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_GYRO1_ROT, -1);
/**
* Gyro X-axis offset
*

59
src/modules/sensors/sensor_params_gyro2.c
View File

@ -55,6 +55,65 @@ PARAM_DEFINE_INT32(CAL_GYRO2_ID, 0);
*/
PARAM_DEFINE_INT32(CAL_GYRO2_PRIO, -1);
/**
* Rotation of gyro 2 relative to airframe.
*
* An internal sensor will force a value of -1, so a GCS
* should only attempt to configure the rotation if the value is
* greater than or equal to zero.
*
* @value -1 Internal
* @value 0 No rotation
* @value 1 Yaw 45°
* @value 2 Yaw 90°
* @value 3 Yaw 135°
* @value 4 Yaw 180°
* @value 5 Yaw 225°
* @value 6 Yaw 270°
* @value 7 Yaw 315°
* @value 8 Roll 180°
* @value 9 Roll 180°, Yaw 45°
* @value 10 Roll 180°, Yaw 90°
* @value 11 Roll 180°, Yaw 135°
* @value 12 Pitch 180°
* @value 13 Roll 180°, Yaw 225°
* @value 14 Roll 180°, Yaw 270°
* @value 15 Roll 180°, Yaw 315°
* @value 16 Roll 90°
* @value 17 Roll 90°, Yaw 45°
* @value 18 Roll 90°, Yaw 90°
* @value 19 Roll 90°, Yaw 135°
* @value 20 Roll 270°
* @value 21 Roll 270°, Yaw 45°
* @value 22 Roll 270°, Yaw 90°
* @value 23 Roll 270°, Yaw 135°
* @value 24 Pitch 90°
* @value 25 Pitch 270°
* @value 26 Pitch 180°, Yaw 90°
* @value 27 Pitch 180°, Yaw 270°
* @value 28 Roll 90°, Pitch 90°
* @value 29 Roll 180°, Pitch 90°
* @value 30 Roll 270°, Pitch 90°
* @value 31 Roll 90°, Pitch 180°
* @value 32 Roll 270°, Pitch 180°
* @value 33 Roll 90°, Pitch 270°
* @value 34 Roll 180°, Pitch 270°
* @value 35 Roll 270°, Pitch 270°
* @value 36 Roll 90°, Pitch 180°, Yaw 90°
* @value 37 Roll 90°, Yaw 270°
* @value 38 Roll 90°, Pitch 68°, Yaw 293°
* @value 39 Pitch 315°
* @value 40 Roll 90°, Pitch 315°
* @value 41 Roll 270°, Yaw 180°
*
* @min -1
* @max 41
* @reboot_required true
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_GYRO2_ROT, -1);
/**
* Gyro X-axis offset
*

139
src/modules/sensors/sensor_params_gyro3.c Normal file
View File

@ -0,0 +1,139 @@
/****************************************************************************
*
* Copyright (c) 2012-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.
*
****************************************************************************/
/**
* ID of the Gyro that the calibration is for.
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_GYRO3_ID, 0);
/**
* Gyro 3 priority.
*
* @value -1 Uninitialized
* @value 0 Disabled
* @value 1 Min
* @value 25 Low
* @value 50 Medium (Default)
* @value 75 High
* @value 100 Max
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_GYRO3_PRIO, -1);
/**
* Rotation of gyro 3 relative to airframe.
*
* An internal sensor will force a value of -1, so a GCS
* should only attempt to configure the rotation if the value is
* greater than or equal to zero.
*
* @value -1 Internal
* @value 0 No rotation
* @value 1 Yaw 45°
* @value 2 Yaw 90°
* @value 3 Yaw 135°
* @value 4 Yaw 180°
* @value 5 Yaw 225°
* @value 6 Yaw 270°
* @value 7 Yaw 315°
* @value 8 Roll 180°
* @value 9 Roll 180°, Yaw 45°
* @value 10 Roll 180°, Yaw 90°
* @value 11 Roll 180°, Yaw 135°
* @value 12 Pitch 180°
* @value 13 Roll 180°, Yaw 225°
* @value 14 Roll 180°, Yaw 270°
* @value 15 Roll 180°, Yaw 315°
* @value 16 Roll 90°
* @value 17 Roll 90°, Yaw 45°
* @value 18 Roll 90°, Yaw 90°
* @value 19 Roll 90°, Yaw 135°
* @value 20 Roll 270°
* @value 21 Roll 270°, Yaw 45°
* @value 22 Roll 270°, Yaw 90°
* @value 23 Roll 270°, Yaw 135°
* @value 24 Pitch 90°
* @value 25 Pitch 270°
* @value 26 Pitch 180°, Yaw 90°
* @value 27 Pitch 180°, Yaw 270°
* @value 28 Roll 90°, Pitch 90°
* @value 29 Roll 180°, Pitch 90°
* @value 30 Roll 270°, Pitch 90°
* @value 31 Roll 90°, Pitch 180°
* @value 32 Roll 270°, Pitch 180°
* @value 33 Roll 90°, Pitch 270°
* @value 34 Roll 180°, Pitch 270°
* @value 35 Roll 270°, Pitch 270°
* @value 36 Roll 90°, Pitch 180°, Yaw 90°
* @value 37 Roll 90°, Yaw 270°
* @value 38 Roll 90°, Pitch 68°, Yaw 293°
* @value 39 Pitch 315°
* @value 40 Roll 90°, Pitch 315°
* @value 41 Roll 270°, Yaw 180°
*
* @min -1
* @max 41
* @reboot_required true
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_GYRO3_ROT, -1);
/**
* Gyro X-axis offset
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO3_XOFF, 0.0f);
/**
* Gyro Y-axis offset
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO3_YOFF, 0.0f);
/**
* Gyro Z-axis offset
*
* @category system
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO3_ZOFF, 0.0f);

4
src/modules/sensors/sensor_params_mag0.c
View File

@ -58,11 +58,11 @@ PARAM_DEFINE_INT32(CAL_MAG0_PRIO, -1);
/**
* Rotation of magnetometer 0 relative to airframe.
*
* An internal magnetometer will force a value of -1, so a GCS
* An internal sensor will force a value of -1, so a GCS
* should only attempt to configure the rotation if the value is
* greater than or equal to zero.
*
* @value -1 Internal mag
* @value -1 Internal
* @value 0 No rotation
* @value 1 Yaw 45°
* @value 2 Yaw 90°

4
src/modules/sensors/sensor_params_mag1.c
View File

@ -58,11 +58,11 @@ PARAM_DEFINE_INT32(CAL_MAG1_PRIO, -1);
/**
* Rotation of magnetometer 1 relative to airframe.
*
* An internal magnetometer will force a value of -1, so a GCS
* An internal sensor will force a value of -1, so a GCS
* should only attempt to configure the rotation if the value is
* greater than or equal to zero.
*
* @value -1 Internal mag
* @value -1 Internal
* @value 0 No rotation
* @value 1 Yaw 45°
* @value 2 Yaw 90°

4
src/modules/sensors/sensor_params_mag2.c
View File

@ -58,11 +58,11 @@ PARAM_DEFINE_INT32(CAL_MAG2_PRIO, -1);
/**
* Rotation of magnetometer 2 relative to airframe.
*
* An internal magnetometer will force a value of -1, so a GCS
* An internal sensor will force a value of -1, so a GCS
* should only attempt to configure the rotation if the value is
* greater than or equal to zero.
*
* @value -1 Internal mag
* @value -1 Internal
* @value 0 No rotation
* @value 1 Yaw 45°
* @value 2 Yaw 90°

4
src/modules/sensors/sensor_params_mag3.c
View File

@ -58,11 +58,11 @@ PARAM_DEFINE_INT32(CAL_MAG3_PRIO, -1);
/**
* Rotation of magnetometer 3 relative to airframe.
*
* An internal magnetometer will force a value of -1, so a GCS
* An internal sensor will force a value of -1, so a GCS
* should only attempt to configure the rotation if the value is
* greater than or equal to zero.
*
* @value -1 Internal mag
* @value -1 Internal
* @value 0 No rotation
* @value 1 Yaw 45°
* @value 2 Yaw 90°

6
src/modules/sensors/sensors.cpp
View File

@ -117,10 +117,11 @@ private:
sensor_combined_s _sensor_combined{};
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[3] {
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[MAX_SENSOR_COUNT] {
{this, ORB_ID(vehicle_imu), 0},
{this, ORB_ID(vehicle_imu), 1},
{this, ORB_ID(vehicle_imu), 2}
{this, ORB_ID(vehicle_imu), 2},
{this, ORB_ID(vehicle_imu), 3}
};
uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)};
@ -175,7 +176,6 @@ private:
VehicleMagnetometer *_vehicle_magnetometer{nullptr};
VehicleGPSPosition *_vehicle_gps_position{nullptr};
static constexpr int MAX_SENSOR_COUNT = 3;
VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {};

2
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
View File

@ -73,7 +73,7 @@ private:
void SensorBiasUpdate(bool force = false);
bool SensorSelectionUpdate(bool force = false);
static constexpr int MAX_SENSOR_COUNT = 3;
static constexpr int MAX_SENSOR_COUNT = 4;
uORB::Publication<vehicle_acceleration_s> _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)};

4
src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
View File

@ -98,6 +98,10 @@ void VehicleAirData::SensorCorrectionsUpdate(bool force)
case 2:
_thermal_offset[baro_index] = corrections.baro_offset_2;
break;
case 3:
_thermal_offset[baro_index] = corrections.baro_offset_3;
break;
}
}
}

5
src/modules/sensors/vehicle_air_data/VehicleAirData.hpp
View File

@ -71,7 +71,7 @@ private:
void ParametersUpdate();
void SensorCorrectionsUpdate(bool force = false);
static constexpr int MAX_SENSOR_COUNT = 3;
static constexpr int MAX_SENSOR_COUNT = 4;
uORB::Publication<vehicle_air_data_s> _vehicle_air_data_pub{ORB_ID(vehicle_air_data)};
@ -81,7 +81,8 @@ private:
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
{this, ORB_ID(sensor_baro), 0},
{this, ORB_ID(sensor_baro), 1},
{this, ORB_ID(sensor_baro), 2}
{this, ORB_ID(sensor_baro), 2},
{this, ORB_ID(sensor_baro), 3},
};
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};

2
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
View File

@ -75,7 +75,7 @@ private:
void SensorBiasUpdate(bool force = false);
bool SensorSelectionUpdate(bool force = false);
static constexpr int MAX_SENSOR_COUNT = 3;
static constexpr int MAX_SENSOR_COUNT = 4;
uORB::Publication<vehicle_angular_acceleration_s> _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)};
uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};

4
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
View File

@ -42,8 +42,6 @@ namespace sensors
using namespace matrix;
using namespace time_literals;
static constexpr int32_t MAG_ROT_VAL_INTERNAL{-1};
static constexpr uint32_t SENSOR_TIMEOUT{300_ms};
VehicleMagnetometer::VehicleMagnetometer() :
@ -52,7 +50,7 @@ VehicleMagnetometer::VehicleMagnetometer() :
{
char str[20] {};
for (int mag_index = 0; mag_index < 4; mag_index++) {
for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
// CAL_MAGx_ID
sprintf(str, "CAL_%s%u_ID", "MAG", mag_index);
param_find(str);

30
src/modules/sensors/voted_sensors_update.cpp
View File

@ -48,7 +48,8 @@ using namespace sensors;
using namespace matrix;
using namespace time_literals;
VotedSensorsUpdate::VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]) :
VotedSensorsUpdate::VotedSensorsUpdate(bool hil_enabled,
uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]) :
ModuleParams(nullptr),
_vehicle_imu_sub(vehicle_imu_sub),
_hil_enabled(hil_enabled)
@ -73,8 +74,8 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw)
void VotedSensorsUpdate::initializeSensors()
{
initSensorClass(_gyro, GYRO_COUNT_MAX);
initSensorClass(_accel, ACCEL_COUNT_MAX);
initSensorClass(_gyro, MAX_SENSOR_COUNT);
initSensorClass(_accel, MAX_SENSOR_COUNT);
}
void VotedSensorsUpdate::parametersUpdate()
@ -82,7 +83,7 @@ void VotedSensorsUpdate::parametersUpdate()
updateParams();
// run through all IMUs
for (uint8_t uorb_index = 0; uorb_index < math::max(ACCEL_COUNT_MAX, GYRO_COUNT_MAX); uorb_index++) {
for (uint8_t uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
uORB::SubscriptionData<vehicle_imu_s> imu{ORB_ID(vehicle_imu), uorb_index};
imu.update();
@ -137,7 +138,7 @@ void VotedSensorsUpdate::parametersUpdate()
void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
{
for (int uorb_index = 0; uorb_index < SENSOR_COUNT_MAX; uorb_index++) {
for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
vehicle_imu_s imu_report;
if ((_accel.priority[uorb_index] > 0) && (_gyro.priority[uorb_index] > 0)
@ -197,16 +198,16 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
// use sensor_selection to find best
if (_sensor_selection_sub.update(&_selection)) {
// reset inconsistency checks against primary
for (int sensor_index = 0; sensor_index < ACCEL_COUNT_MAX; sensor_index++) {
for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
_accel_diff[sensor_index].zero();
}
for (int sensor_index = 0; sensor_index < GYRO_COUNT_MAX; sensor_index++) {
for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
_gyro_diff[sensor_index].zero();
}
}
for (int i = 0; i < SENSOR_COUNT_MAX; i++) {
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if ((_accel_device_id[i] != 0) && (_accel_device_id[i] == _selection.accel_device_id)) {
accel_best_index = i;
}
@ -243,7 +244,7 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
sub.unregisterCallback();
}
for (int i = 0; i < GYRO_COUNT_MAX; i++) {
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
vehicle_imu_s report{};
if (_vehicle_imu_sub[i].copy(&report)) {
@ -359,11 +360,8 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
_selection_changed = false;
}
for (int sensor_index = 0; sensor_index < ACCEL_COUNT_MAX; sensor_index++) {
for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
_accel_diff[sensor_index].zero();
}
for (int sensor_index = 0; sensor_index < GYRO_COUNT_MAX; sensor_index++) {
_gyro_diff[sensor_index].zero();
}
}
@ -376,7 +374,7 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
status.accel_device_id_primary = _selection.accel_device_id;
status.gyro_device_id_primary = _selection.gyro_device_id;
for (int i = 0; i < SENSOR_COUNT_MAX; i++) {
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (_accel_device_id[i] != 0) {
status.accel_device_ids[i] = _accel_device_id[i];
status.accel_inconsistency_m_s_s[i] = _accel_diff[i].norm();
@ -408,7 +406,7 @@ void VotedSensorsUpdate::calcAccelInconsistency()
const Vector3f primary_accel{_last_sensor_data[_accel.last_best_vote].accelerometer_m_s2};
// Check each sensor against the primary
for (int sensor_index = 0; sensor_index < ACCEL_COUNT_MAX; sensor_index++) {
for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
// check that the sensor we are checking against is not the same as the primary
if (_accel.advertised[sensor_index] && (_accel.priority[sensor_index] > 0)
&& (_accel_device_id[sensor_index] != _selection.accel_device_id)) {
@ -424,7 +422,7 @@ void VotedSensorsUpdate::calcGyroInconsistency()
const Vector3f primary_gyro{_last_sensor_data[_gyro.last_best_vote].gyro_rad};
// Check each sensor against the primary
for (int sensor_index = 0; sensor_index < GYRO_COUNT_MAX; sensor_index++) {
for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
// check that the sensor we are checking against is not the same as the primary
if (_gyro.advertised[sensor_index] && (_gyro.priority[sensor_index] > 0)
&& (_gyro_device_id[sensor_index] != _selection.gyro_device_id)) {

37
src/modules/sensors/voted_sensors_update.h
View File

@ -61,10 +61,7 @@
namespace sensors
{
static constexpr uint8_t GYRO_COUNT_MAX = 3;
static constexpr uint8_t ACCEL_COUNT_MAX = 3;
static constexpr uint8_t SENSOR_COUNT_MAX = math::max(GYRO_COUNT_MAX, ACCEL_COUNT_MAX);
static constexpr uint8_t MAX_SENSOR_COUNT = 4;
/**
** class VotedSensorsUpdate
@ -78,7 +75,7 @@ public:
* @param parameters parameter values. These do not have to be initialized when constructing this object.
* Only when calling init(), they have to be initialized.
*/
VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]);
VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]);
/**
* initialize subscriptions etc.
@ -112,24 +109,20 @@ public:
private:
static constexpr uint8_t ACCEL_COUNT_MAX = 3;
static constexpr uint8_t GYRO_COUNT_MAX = 3;
static constexpr uint8_t SENSOR_COUNT_MAX = math::max(ACCEL_COUNT_MAX, GYRO_COUNT_MAX);
static constexpr uint8_t DEFAULT_PRIORITY = 50;
struct SensorData {
SensorData() = delete;
explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}} {}
explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}, {meta, 3}} {}
uORB::Subscription subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */
uORB::Subscription subscription[MAX_SENSOR_COUNT]; /**< raw sensor data subscription */
DataValidatorGroup voter{1};
unsigned int last_failover_count{0};
int32_t priority[SENSOR_COUNT_MAX] {};
int32_t priority_configured[SENSOR_COUNT_MAX] {};
int32_t priority[MAX_SENSOR_COUNT] {};
int32_t priority_configured[MAX_SENSOR_COUNT] {};
uint8_t last_best_vote{0}; /**< index of the latest best vote */
uint8_t subscription_count{0};
bool advertised[SENSOR_COUNT_MAX] {false, false, false};
bool advertised[MAX_SENSOR_COUNT] {false, false, false};
};
void initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max);
@ -167,24 +160,24 @@ private:
uORB::Publication<sensor_selection_s> _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */
uORB::Publication<sensors_status_imu_s> _sensors_status_imu_pub{ORB_ID(sensors_status_imu)};
uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[SENSOR_COUNT_MAX];
uORB::SubscriptionMultiArray<vehicle_imu_status_s, ACCEL_COUNT_MAX> _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status};
uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[MAX_SENSOR_COUNT];
uORB::SubscriptionMultiArray<vehicle_imu_status_s, MAX_SENSOR_COUNT> _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status};
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
sensor_combined_s _last_sensor_data[MAX_SENSOR_COUNT] {}; /**< latest sensor data from all sensors instances */
const bool _hil_enabled{false}; /**< is hardware-in-the-loop mode enabled? */
bool _selection_changed{true}; /**< true when a sensor selection has changed and not been published */
matrix::Vector3f _accel_diff[ACCEL_COUNT_MAX] {}; /**< filtered accel differences between IMU units (m/s/s) */
matrix::Vector3f _gyro_diff[GYRO_COUNT_MAX] {}; /**< filtered gyro differences between IMU uinits (rad/s) */
matrix::Vector3f _accel_diff[MAX_SENSOR_COUNT] {}; /**< filtered accel differences between IMU units (m/s/s) */
matrix::Vector3f _gyro_diff[MAX_SENSOR_COUNT] {}; /**< filtered gyro differences between IMU uinits (rad/s) */
uint32_t _accel_device_id[SENSOR_COUNT_MAX] {}; /**< accel driver device id for each uorb instance */
uint32_t _gyro_device_id[SENSOR_COUNT_MAX] {}; /**< gyro driver device id for each uorb instance */
uint32_t _accel_device_id[MAX_SENSOR_COUNT] {}; /**< accel driver device id for each uorb instance */
uint32_t _gyro_device_id[MAX_SENSOR_COUNT] {}; /**< gyro driver device id for each uorb instance */
uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX] {}; /**< latest full timestamp */
uint64_t _last_accel_timestamp[MAX_SENSOR_COUNT] {}; /**< latest full timestamp */
sensor_selection_s _selection {}; /**< struct containing the sensor selection to be published to the uORB */

16
src/modules/temperature_compensation/TemperatureCompensation.h
View File

@ -49,16 +49,16 @@
namespace temperature_compensation
{
static constexpr uint8_t GYRO_COUNT_MAX = 3;
static constexpr uint8_t ACCEL_COUNT_MAX = 3;
static constexpr uint8_t BARO_COUNT_MAX = 3;
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_assert(GYRO_COUNT_MAX == 3, "GYRO_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)");
static_assert(ACCEL_COUNT_MAX == 3,
"ACCEL_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)");
static_assert(BARO_COUNT_MAX == 3, "BARO_COUNT_MAX must be 3 (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(ACCEL_COUNT_MAX == 4,
"ACCEL_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 = 3;
static constexpr uint8_t SENSOR_COUNT_MAX = 4;
/**
** class TemperatureCompensation

6
src/modules/temperature_compensation/TemperatureCompensationModule.cpp
View File

@ -113,7 +113,7 @@ void TemperatureCompensationModule::parameters_update()
void TemperatureCompensationModule::accelPoll()
{
float *offsets[] = {_corrections.accel_offset_0, _corrections.accel_offset_1, _corrections.accel_offset_2 };
float *offsets[] = {_corrections.accel_offset_0, _corrections.accel_offset_1, _corrections.accel_offset_2, _corrections.accel_offset_3 };
// For each accel instance
for (uint8_t uorb_index = 0; uorb_index < ACCEL_COUNT_MAX; uorb_index++) {
@ -135,7 +135,7 @@ void TemperatureCompensationModule::accelPoll()
void TemperatureCompensationModule::gyroPoll()
{
float *offsets[] = {_corrections.gyro_offset_0, _corrections.gyro_offset_1, _corrections.gyro_offset_2 };
float *offsets[] = {_corrections.gyro_offset_0, _corrections.gyro_offset_1, _corrections.gyro_offset_2, _corrections.gyro_offset_3 };
// For each gyro instance
for (uint8_t uorb_index = 0; uorb_index < GYRO_COUNT_MAX; uorb_index++) {
@ -157,7 +157,7 @@ void TemperatureCompensationModule::gyroPoll()
void TemperatureCompensationModule::baroPoll()
{
float *offsets[] = {&_corrections.baro_offset_0, &_corrections.baro_offset_1, &_corrections.baro_offset_2 };
float *offsets[] = {&_corrections.baro_offset_0, &_corrections.baro_offset_1, &_corrections.baro_offset_2, &_corrections.baro_offset_3 };
// For each baro instance
for (uint8_t uorb_index = 0; uorb_index < BARO_COUNT_MAX; uorb_index++) {

9
src/modules/temperature_compensation/TemperatureCompensationModule.h
View File

@ -105,19 +105,22 @@ private:
uORB::Subscription _accel_subs[ACCEL_COUNT_MAX] {
{ORB_ID(sensor_accel), 0},
{ORB_ID(sensor_accel), 1},
{ORB_ID(sensor_accel), 2}
{ORB_ID(sensor_accel), 2},
{ORB_ID(sensor_accel), 3},
};
uORB::Subscription _gyro_subs[GYRO_COUNT_MAX] {
{ORB_ID(sensor_gyro), 0},
{ORB_ID(sensor_gyro), 1},
{ORB_ID(sensor_gyro), 2}
{ORB_ID(sensor_gyro), 2},
{ORB_ID(sensor_gyro), 3},
};
uORB::Subscription _baro_subs[BARO_COUNT_MAX] {
{ORB_ID(sensor_baro), 0},
{ORB_ID(sensor_baro), 1},
{ORB_ID(sensor_baro), 2}
{ORB_ID(sensor_baro), 2},
{ORB_ID(sensor_baro), 3},
};
uORB::Subscription _params_sub{ORB_ID(parameter_update)};

400
src/modules/temperature_compensation/temp_comp_params_accel.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
* 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
@ -31,14 +31,6 @@
*
****************************************************************************/
/**
* @file temp_comp_params_accel.c
*
* Parameters required for temperature compensation of rate Accelerometers.
*
* @author Paul Riseborough <gncsolns@gmail.com>
*/
/**
* Thermal compensation for accelerometer sensors.
*
@ -47,393 +39,3 @@
* @boolean
*/
PARAM_DEFINE_INT32(TC_A_ENABLE, 0);
/* Accelerometer 0 */
/**
* ID of Accelerometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_A0_ID, 0);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X3_0, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X3_1, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X3_2, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X2_0, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X2_1, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X2_2, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X1_0, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X1_1, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X1_2, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X0_0, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X0_1, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X0_2, 0.0f);
/**
* Accelerometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_TREF, 25.0f);
/**
* Accelerometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_TMIN, 0.0f);
/**
* Accelerometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_TMAX, 100.0f);
/* Accelerometer 1 */
/**
* ID of Accelerometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_A1_ID, 0);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X3_0, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X3_1, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X3_2, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X2_0, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X2_1, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X2_2, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X1_0, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X1_1, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X1_2, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X0_0, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X0_1, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X0_2, 0.0f);
/**
* Accelerometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_TREF, 25.0f);
/**
* Accelerometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_TMIN, 0.0f);
/**
* Accelerometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_TMAX, 100.0f);
/* Accelerometer 2 */
/**
* ID of Accelerometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_A2_ID, 0);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X3_0, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X3_1, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X3_2, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X2_0, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X2_1, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X2_2, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X1_0, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X1_1, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X1_2, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X0_0, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X0_1, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X0_2, 0.0f);
/**
* Accelerometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_TREF, 25.0f);
/**
* Accelerometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_TMIN, 0.0f);
/**
* Accelerometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_TMAX, 100.0f);

162
src/modules/temperature_compensation/temp_comp_params_accel_0.c Normal file
View File

@ -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.
*
****************************************************************************/
/* Accelerometer 0 */
/**
* ID of Accelerometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_A0_ID, 0);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X3_0, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X3_1, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X3_2, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X2_0, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X2_1, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X2_2, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X1_0, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X1_1, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X1_2, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X0_0, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X0_1, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X0_2, 0.0f);
/**
* Accelerometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_TREF, 25.0f);
/**
* Accelerometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_TMIN, 0.0f);
/**
* Accelerometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_TMAX, 100.0f);

162
src/modules/temperature_compensation/temp_comp_params_accel_1.c Normal file
View File

@ -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.
*
****************************************************************************/
/* Accelerometer 1 */
/**
* ID of Accelerometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_A1_ID, 0);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X3_0, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X3_1, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X3_2, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X2_0, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X2_1, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X2_2, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X1_0, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X1_1, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X1_2, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X0_0, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X0_1, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X0_2, 0.0f);
/**
* Accelerometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_TREF, 25.0f);
/**
* Accelerometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_TMIN, 0.0f);
/**
* Accelerometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_TMAX, 100.0f);

162
src/modules/temperature_compensation/temp_comp_params_accel_2.c Normal file
View File

@ -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.
*
****************************************************************************/
/* Accelerometer 2 */
/**
* ID of Accelerometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_A2_ID, 0);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X3_0, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X3_1, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X3_2, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X2_0, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X2_1, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X2_2, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X1_0, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X1_1, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X1_2, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X0_0, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X0_1, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X0_2, 0.0f);
/**
* Accelerometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_TREF, 25.0f);
/**
* Accelerometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_TMIN, 0.0f);
/**
* Accelerometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_TMAX, 100.0f);

162
src/modules/temperature_compensation/temp_comp_params_accel_3.c Normal file
View File

@ -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.
*
****************************************************************************/
/* Accelerometer 3 */
/**
* ID of Accelerometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_A3_ID, 0);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A3_X3_0, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A3_X3_1, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A3_X3_2, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A3_X2_0, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A3_X2_1, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A3_X2_2, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A3_X1_0, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A3_X1_1, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A3_X1_2, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A3_X0_0, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A3_X0_1, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A3_X0_2, 0.0f);
/**
* Accelerometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A3_TREF, 25.0f);
/**
* Accelerometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A3_TMIN, 0.0f);
/**
* Accelerometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A3_TMAX, 100.0f);

256
src/modules/temperature_compensation/temp_comp_params_baro.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
* 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
@ -31,14 +31,6 @@
*
****************************************************************************/
/**
* @file temp_comp_params_baro.c
*
* Parameters required for temperature compensation of barometers.
*
* @author Paul Riseborough <gncsolns@gmail.com>
*/
/**
* Thermal compensation for barometric pressure sensors.
*
@ -47,249 +39,3 @@
* @boolean
*/
PARAM_DEFINE_INT32(TC_B_ENABLE, 0);
/* Barometer 0 */
/**
* ID of Barometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_B0_ID, 0);
/**
* Barometer offset temperature ^5 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X5, 0.0f);
/**
* Barometer offset temperature ^4 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X4, 0.0f);
/**
* Barometer offset temperature ^3 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X3, 0.0f);
/**
* Barometer offset temperature ^2 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X2, 0.0f);
/**
* Barometer offset temperature ^1 polynomial coefficients.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X1, 0.0f);
/**
* Barometer offset temperature ^0 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X0, 0.0f);
/**
* Barometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_TREF, 40.0f);
/**
* Barometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_TMIN, 5.0f);
/**
* Barometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_TMAX, 75.0f);
/* Barometer 1 */
/**
* ID of Barometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_B1_ID, 0);
/**
* Barometer offset temperature ^5 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X5, 0.0f);
/**
* Barometer offset temperature ^4 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X4, 0.0f);
/**
* Barometer offset temperature ^3 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X3, 0.0f);
/**
* Barometer offset temperature ^2 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X2, 0.0f);
/**
* Barometer offset temperature ^1 polynomial coefficients.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X1, 0.0f);
/**
* Barometer offset temperature ^0 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X0, 0.0f);
/**
* Barometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_TREF, 40.0f);
/**
* Barometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_TMIN, 5.0f);
/**
* Barometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_TMAX, 75.0f);
/* Barometer 2 */
/**
* ID of Barometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_B2_ID, 0);
/**
* Barometer offset temperature ^5 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X5, 0.0f);
/**
* Barometer offset temperature ^4 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X4, 0.0f);
/**
* Barometer offset temperature ^3 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X3, 0.0f);
/**
* Barometer offset temperature ^2 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X2, 0.0f);
/**
* Barometer offset temperature ^1 polynomial coefficients.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X1, 0.0f);
/**
* Barometer offset temperature ^0 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X0, 0.0f);
/**
* Barometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_TREF, 40.0f);
/**
* Barometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_TMIN, 5.0f);
/**
* Barometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_TMAX, 75.0f);

114
src/modules/temperature_compensation/temp_comp_params_baro_0.c Normal file
View File

@ -0,0 +1,114 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/* Barometer 0 */
/**
* ID of Barometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_B0_ID, 0);
/**
* Barometer offset temperature ^5 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X5, 0.0f);
/**
* Barometer offset temperature ^4 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X4, 0.0f);
/**
* Barometer offset temperature ^3 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X3, 0.0f);
/**
* Barometer offset temperature ^2 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X2, 0.0f);
/**
* Barometer offset temperature ^1 polynomial coefficients.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X1, 0.0f);
/**
* Barometer offset temperature ^0 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X0, 0.0f);
/**
* Barometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_TREF, 40.0f);
/**
* Barometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_TMIN, 5.0f);
/**
* Barometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_TMAX, 75.0f);

114
src/modules/temperature_compensation/temp_comp_params_baro_1.c Normal file
View File

@ -0,0 +1,114 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/* Barometer 1 */
/**
* ID of Barometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_B1_ID, 0);
/**
* Barometer offset temperature ^5 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X5, 0.0f);
/**
* Barometer offset temperature ^4 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X4, 0.0f);
/**
* Barometer offset temperature ^3 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X3, 0.0f);
/**
* Barometer offset temperature ^2 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X2, 0.0f);
/**
* Barometer offset temperature ^1 polynomial coefficients.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X1, 0.0f);
/**
* Barometer offset temperature ^0 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X0, 0.0f);
/**
* Barometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_TREF, 40.0f);
/**
* Barometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_TMIN, 5.0f);
/**
* Barometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_TMAX, 75.0f);

114
src/modules/temperature_compensation/temp_comp_params_baro_2.c Normal file
View File

@ -0,0 +1,114 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/* Barometer 2 */
/**
* ID of Barometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_B2_ID, 0);
/**
* Barometer offset temperature ^5 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X5, 0.0f);
/**
* Barometer offset temperature ^4 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X4, 0.0f);
/**
* Barometer offset temperature ^3 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X3, 0.0f);
/**
* Barometer offset temperature ^2 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X2, 0.0f);
/**
* Barometer offset temperature ^1 polynomial coefficients.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X1, 0.0f);
/**
* Barometer offset temperature ^0 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X0, 0.0f);
/**
* Barometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_TREF, 40.0f);
/**
* Barometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_TMIN, 5.0f);
/**
* Barometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_TMAX, 75.0f);

114
src/modules/temperature_compensation/temp_comp_params_baro_3.c Normal file
View File

@ -0,0 +1,114 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/* Barometer 3 */
/**
* ID of Barometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_B3_ID, 0);
/**
* Barometer offset temperature ^5 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B3_X5, 0.0f);
/**
* Barometer offset temperature ^4 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B3_X4, 0.0f);
/**
* Barometer offset temperature ^3 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B3_X3, 0.0f);
/**
* Barometer offset temperature ^2 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B3_X2, 0.0f);
/**
* Barometer offset temperature ^1 polynomial coefficients.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B3_X1, 0.0f);
/**
* Barometer offset temperature ^0 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B3_X0, 0.0f);
/**
* Barometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B3_TREF, 40.0f);
/**
* Barometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B3_TMIN, 5.0f);
/**
* Barometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B3_TMAX, 75.0f);

400
src/modules/temperature_compensation/temp_comp_params_gyro.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
* 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
@ -31,14 +31,6 @@
*
****************************************************************************/
/**
* @file temp_comp_params_gyro.c
*
* Parameters required for temperature compensation of rate gyros.
*
* @author Paul Riseborough <gncsolns@gmail.com>
*/
/**
* Thermal compensation for rate gyro sensors.
*
@ -47,393 +39,3 @@
* @boolean
*/
PARAM_DEFINE_INT32(TC_G_ENABLE, 0);
/* Gyro 0 */
/**
* ID of Gyro that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_G0_ID, 0);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X3_0, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X3_1, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X3_2, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X2_0, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X2_1, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X2_2, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X1_0, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X1_1, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X1_2, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X0_0, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X0_1, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X0_2, 0.0f);
/**
* Gyro calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_TREF, 25.0f);
/**
* Gyro calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_TMIN, 0.0f);
/**
* Gyro calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_TMAX, 100.0f);
/* Gyro 1 */
/**
* ID of Gyro that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_G1_ID, 0);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X3_0, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X3_1, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X3_2, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X2_0, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X2_1, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X2_2, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X1_0, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X1_1, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X1_2, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X0_0, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X0_1, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X0_2, 0.0f);
/**
* Gyro calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_TREF, 25.0f);
/**
* Gyro calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_TMIN, 0.0f);
/**
* Gyro calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_TMAX, 100.0f);
/* Gyro 2 */
/**
* ID of Gyro that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_G2_ID, 0);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X3_0, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X3_1, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X3_2, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X2_0, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X2_1, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X2_2, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X1_0, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X1_1, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X1_2, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X0_0, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X0_1, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X0_2, 0.0f);
/**
* Gyro calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_TREF, 25.0f);
/**
* Gyro calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_TMIN, 0.0f);
/**
* Gyro calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_TMAX, 100.0f);

162
src/modules/temperature_compensation/temp_comp_params_gyro_0.c Normal file
View File

@ -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.
*
****************************************************************************/
/* Gyro 0 */
/**
* ID of Gyro that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_G0_ID, 0);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X3_0, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X3_1, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X3_2, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X2_0, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X2_1, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X2_2, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X1_0, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X1_1, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X1_2, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X0_0, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X0_1, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X0_2, 0.0f);
/**
* Gyro calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_TREF, 25.0f);
/**
* Gyro calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_TMIN, 0.0f);
/**
* Gyro calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_TMAX, 100.0f);

162
src/modules/temperature_compensation/temp_comp_params_gyro_1.c Normal file
View File

@ -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.
*
****************************************************************************/
/* Gyro 1 */
/**
* ID of Gyro that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_G1_ID, 0);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X3_0, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X3_1, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X3_2, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X2_0, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X2_1, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X2_2, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X1_0, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X1_1, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X1_2, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X0_0, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X0_1, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X0_2, 0.0f);
/**
* Gyro calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_TREF, 25.0f);
/**
* Gyro calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_TMIN, 0.0f);
/**
* Gyro calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_TMAX, 100.0f);

162
src/modules/temperature_compensation/temp_comp_params_gyro_2.c Normal file
View File

@ -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.
*
****************************************************************************/
/* Gyro 2 */
/**
* ID of Gyro that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_G2_ID, 0);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X3_0, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X3_1, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X3_2, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X2_0, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X2_1, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X2_2, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X1_0, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X1_1, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X1_2, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X0_0, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X0_1, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X0_2, 0.0f);
/**
* Gyro calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_TREF, 25.0f);
/**
* Gyro calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_TMIN, 0.0f);
/**
* Gyro calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_TMAX, 100.0f);

162
src/modules/temperature_compensation/temp_comp_params_gyro_3.c Normal file
View File

@ -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.
*
****************************************************************************/
/* Gyro 3 */
/**
* ID of Gyro that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_G3_ID, 0);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G3_X3_0, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G3_X3_1, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G3_X3_2, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G3_X2_0, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G3_X2_1, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G3_X2_2, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G3_X1_0, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G3_X1_1, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G3_X1_2, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G3_X0_0, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G3_X0_1, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G3_X0_2, 0.0f);
/**
* Gyro calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G3_TREF, 25.0f);
/**
* Gyro calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G3_TMIN, 0.0f);
/**
* Gyro calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G3_TMAX, 100.0f);