diff --git a/Tools/process_sensor_caldata.py b/Tools/process_sensor_caldata.py index d3871e87ebfe..32acf61dd604 100755 --- a/Tools/process_sensor_caldata.py +++ b/Tools/process_sensor_caldata.py @@ -98,6 +98,10 @@ def is_valid_directory(parser, arg): 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 @@ def is_valid_directory(parser, arg): 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 @@ def is_valid_directory(parser, arg): 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 @@ def is_valid_directory(parser, arg): ################################################################################# +# 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 @@ def is_valid_directory(parser, arg): 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 @@ def is_valid_directory(parser, arg): 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 @@ def is_valid_directory(parser, arg): 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 @@ def is_valid_directory(parser, arg): ################################################################################# +# 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 @@ def is_valid_directory(parser, arg): 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 @@ def is_valid_directory(parser, arg): 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 @@ def is_valid_directory(parser, arg): 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 @@ def is_valid_directory(parser, arg): 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 @@ def is_valid_directory(parser, arg): 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 @@ def is_valid_directory(parser, arg): 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 @@ def is_valid_directory(parser, arg): 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 @@ def is_valid_directory(parser, arg): 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) diff --git a/msg/sensor_correction.msg b/msg/sensor_correction.msg index c492c3cf3fa4..9ec20a288e64 100644 --- a/msg/sensor_correction.msg +++ b/msg/sensor_correction.msg @@ -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 diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor_calibration/Accelerometer.cpp index 3b9fe6e11050..15b9c5c70d43 100644 --- a/src/lib/sensor_calibration/Accelerometer.cpp +++ b/src/lib/sensor_calibration/Accelerometer.cpp @@ -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_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; } diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp index eb5ea271644f..5d95101b7053 100644 --- a/src/lib/sensor_calibration/Accelerometer.hpp +++ b/src/lib/sensor_calibration/Accelerometer.hpp @@ -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 @@ class Accelerometer 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 @@ class Accelerometer 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 @@ class Accelerometer private: uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; + Rotation _rotation_enum{ROTATION_NONE}; + matrix::Dcmf _rotation; matrix::Vector3f _offset; matrix::Vector3f _scale; diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp index 804aac488802..acf4fdb6c21f 100644 --- a/src/lib/sensor_calibration/Gyroscope.cpp +++ b/src/lib/sensor_calibration/Gyroscope.cpp @@ -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_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; } diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor_calibration/Gyroscope.hpp index 1180cabed5a2..0827b90bc6cd 100644 --- a/src/lib/sensor_calibration/Gyroscope.hpp +++ b/src/lib/sensor_calibration/Gyroscope.hpp @@ -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 @@ class Gyroscope 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 @@ class Gyroscope 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 @@ class Gyroscope 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; diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index 73f4e22a6d9b..f328f2a8c536 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -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, diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 5d331643976d..e6a14f901571 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -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; } } } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index dd52c1e19cc2..3e7bcd0c7f22 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -59,7 +59,7 @@ #include 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; } } } diff --git a/src/modules/sensors/sensor_params_acc0.c b/src/modules/sensors/sensor_params_acc0.c index f7dae6fbe400..3319545e7299 100644 --- a/src/modules/sensors/sensor_params_acc0.c +++ b/src/modules/sensors/sensor_params_acc0.c @@ -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 * diff --git a/src/modules/sensors/sensor_params_acc1.c b/src/modules/sensors/sensor_params_acc1.c index b814bd55ba06..6d9c3b1fbffd 100644 --- a/src/modules/sensors/sensor_params_acc1.c +++ b/src/modules/sensors/sensor_params_acc1.c @@ -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 * diff --git a/src/modules/sensors/sensor_params_acc2.c b/src/modules/sensors/sensor_params_acc2.c index c298e08cd898..728148a363e5 100644 --- a/src/modules/sensors/sensor_params_acc2.c +++ b/src/modules/sensors/sensor_params_acc2.c @@ -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 * diff --git a/src/modules/sensors/sensor_params_acc3.c b/src/modules/sensors/sensor_params_acc3.c new file mode 100644 index 000000000000..a9d86bd47b90 --- /dev/null +++ b/src/modules/sensors/sensor_params_acc3.c @@ -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); diff --git a/src/modules/sensors/sensor_params_gyro0.c b/src/modules/sensors/sensor_params_gyro0.c index ff3176f9c264..f771722484b4 100644 --- a/src/modules/sensors/sensor_params_gyro0.c +++ b/src/modules/sensors/sensor_params_gyro0.c @@ -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 * diff --git a/src/modules/sensors/sensor_params_gyro1.c b/src/modules/sensors/sensor_params_gyro1.c index 925c5a13fd18..af9c116984bc 100644 --- a/src/modules/sensors/sensor_params_gyro1.c +++ b/src/modules/sensors/sensor_params_gyro1.c @@ -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 * diff --git a/src/modules/sensors/sensor_params_gyro2.c b/src/modules/sensors/sensor_params_gyro2.c index e662822b4c1d..3165cdbd5ff8 100644 --- a/src/modules/sensors/sensor_params_gyro2.c +++ b/src/modules/sensors/sensor_params_gyro2.c @@ -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 * diff --git a/src/modules/sensors/sensor_params_gyro3.c b/src/modules/sensors/sensor_params_gyro3.c new file mode 100644 index 000000000000..d717db954a55 --- /dev/null +++ b/src/modules/sensors/sensor_params_gyro3.c @@ -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); diff --git a/src/modules/sensors/sensor_params_mag0.c b/src/modules/sensors/sensor_params_mag0.c index a32bef122025..74293a8143e1 100644 --- a/src/modules/sensors/sensor_params_mag0.c +++ b/src/modules/sensors/sensor_params_mag0.c @@ -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° diff --git a/src/modules/sensors/sensor_params_mag1.c b/src/modules/sensors/sensor_params_mag1.c index 59a9030bb66a..4c8e364e6958 100644 --- a/src/modules/sensors/sensor_params_mag1.c +++ b/src/modules/sensors/sensor_params_mag1.c @@ -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° diff --git a/src/modules/sensors/sensor_params_mag2.c b/src/modules/sensors/sensor_params_mag2.c index 7b276c760e3b..05e1a049ef59 100644 --- a/src/modules/sensors/sensor_params_mag2.c +++ b/src/modules/sensors/sensor_params_mag2.c @@ -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° diff --git a/src/modules/sensors/sensor_params_mag3.c b/src/modules/sensors/sensor_params_mag3.c index 3494b2abb2dd..8611947470a1 100644 --- a/src/modules/sensors/sensor_params_mag3.c +++ b/src/modules/sensors/sensor_params_mag3.c @@ -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° diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e56b8511aac4..dffda4693272 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -117,10 +117,11 @@ class Sensors : public ModuleBase, public ModuleParams, public px4::Sch 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 @@ class Sensors : public ModuleBase, public ModuleParams, public px4::Sch VehicleMagnetometer *_vehicle_magnetometer{nullptr}; VehicleGPSPosition *_vehicle_gps_position{nullptr}; - static constexpr int MAX_SENSOR_COUNT = 3; VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp index bebe9760bb8d..755ebc7ab18b 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -73,7 +73,7 @@ class VehicleAcceleration : public ModuleParams, public px4::ScheduledWorkItem 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_pub{ORB_ID(vehicle_acceleration)}; diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 8f2bce29e3fd..c712a708eca0 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -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; } } } diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index 731c8501745f..5570a6891fbd 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -71,7 +71,7 @@ class VehicleAirData : public ModuleParams, public px4::ScheduledWorkItem 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_pub{ORB_ID(vehicle_air_data)}; @@ -81,7 +81,8 @@ class VehicleAirData : public ModuleParams, public px4::ScheduledWorkItem 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")}; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 7d9ab944eaa7..1841d252a696 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -75,7 +75,7 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte 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_pub{ORB_ID(vehicle_angular_acceleration)}; uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 838122f0ad6e..120266566a2d 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -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); diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index c2927846171f..69409983a3ec 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -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 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)) { diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index ef068099b13c..59eaf69c5b7e 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -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 @@ class VotedSensorsUpdate : public ModuleParams * @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 @@ class VotedSensorsUpdate : public ModuleParams 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 @@ class VotedSensorsUpdate : public ModuleParams uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */ uORB::Publication _sensors_status_imu_pub{ORB_ID(sensors_status_imu)}; - uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[SENSOR_COUNT_MAX]; - uORB::SubscriptionMultiArray _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status}; + uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[MAX_SENSOR_COUNT]; + uORB::SubscriptionMultiArray _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 */ diff --git a/src/modules/temperature_compensation/TemperatureCompensation.h b/src/modules/temperature_compensation/TemperatureCompensation.h index 6ac6955b145e..051bc7349637 100644 --- a/src/modules/temperature_compensation/TemperatureCompensation.h +++ b/src/modules/temperature_compensation/TemperatureCompensation.h @@ -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 diff --git a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp index 5e10f3725825..97030b75db37 100644 --- a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp +++ b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp @@ -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++) { diff --git a/src/modules/temperature_compensation/TemperatureCompensationModule.h b/src/modules/temperature_compensation/TemperatureCompensationModule.h index 67fa8cb453a5..282befe60cf9 100644 --- a/src/modules/temperature_compensation/TemperatureCompensationModule.h +++ b/src/modules/temperature_compensation/TemperatureCompensationModule.h @@ -105,19 +105,22 @@ class TemperatureCompensationModule : public ModuleBase - */ - /** * 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); diff --git a/src/modules/temperature_compensation/temp_comp_params_accel_0.c b/src/modules/temperature_compensation/temp_comp_params_accel_0.c new file mode 100644 index 000000000000..1b4f03471e91 --- /dev/null +++ b/src/modules/temperature_compensation/temp_comp_params_accel_0.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* 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); diff --git a/src/modules/temperature_compensation/temp_comp_params_accel_1.c b/src/modules/temperature_compensation/temp_comp_params_accel_1.c new file mode 100644 index 000000000000..ed3d0b0a3964 --- /dev/null +++ b/src/modules/temperature_compensation/temp_comp_params_accel_1.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* 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); diff --git a/src/modules/temperature_compensation/temp_comp_params_accel_2.c b/src/modules/temperature_compensation/temp_comp_params_accel_2.c new file mode 100644 index 000000000000..0155a2ab546f --- /dev/null +++ b/src/modules/temperature_compensation/temp_comp_params_accel_2.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* 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); diff --git a/src/modules/temperature_compensation/temp_comp_params_accel_3.c b/src/modules/temperature_compensation/temp_comp_params_accel_3.c new file mode 100644 index 000000000000..f3d35cbedc2b --- /dev/null +++ b/src/modules/temperature_compensation/temp_comp_params_accel_3.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* 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); diff --git a/src/modules/temperature_compensation/temp_comp_params_baro.c b/src/modules/temperature_compensation/temp_comp_params_baro.c index 651c3ab9f31e..1e5a1add2210 100644 --- a/src/modules/temperature_compensation/temp_comp_params_baro.c +++ b/src/modules/temperature_compensation/temp_comp_params_baro.c @@ -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 - */ - /** * 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); diff --git a/src/modules/temperature_compensation/temp_comp_params_baro_0.c b/src/modules/temperature_compensation/temp_comp_params_baro_0.c new file mode 100644 index 000000000000..1fef48220e1e --- /dev/null +++ b/src/modules/temperature_compensation/temp_comp_params_baro_0.c @@ -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); diff --git a/src/modules/temperature_compensation/temp_comp_params_baro_1.c b/src/modules/temperature_compensation/temp_comp_params_baro_1.c new file mode 100644 index 000000000000..2f0fcda4925e --- /dev/null +++ b/src/modules/temperature_compensation/temp_comp_params_baro_1.c @@ -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); diff --git a/src/modules/temperature_compensation/temp_comp_params_baro_2.c b/src/modules/temperature_compensation/temp_comp_params_baro_2.c new file mode 100644 index 000000000000..b32d9ef535f9 --- /dev/null +++ b/src/modules/temperature_compensation/temp_comp_params_baro_2.c @@ -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); diff --git a/src/modules/temperature_compensation/temp_comp_params_baro_3.c b/src/modules/temperature_compensation/temp_comp_params_baro_3.c new file mode 100644 index 000000000000..a514e783f7e9 --- /dev/null +++ b/src/modules/temperature_compensation/temp_comp_params_baro_3.c @@ -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); diff --git a/src/modules/temperature_compensation/temp_comp_params_gyro.c b/src/modules/temperature_compensation/temp_comp_params_gyro.c index 7bd9ebf0ffca..cd321dd84cb1 100644 --- a/src/modules/temperature_compensation/temp_comp_params_gyro.c +++ b/src/modules/temperature_compensation/temp_comp_params_gyro.c @@ -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 - */ - /** * 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); diff --git a/src/modules/temperature_compensation/temp_comp_params_gyro_0.c b/src/modules/temperature_compensation/temp_comp_params_gyro_0.c new file mode 100644 index 000000000000..7f05b14054a1 --- /dev/null +++ b/src/modules/temperature_compensation/temp_comp_params_gyro_0.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* 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); diff --git a/src/modules/temperature_compensation/temp_comp_params_gyro_1.c b/src/modules/temperature_compensation/temp_comp_params_gyro_1.c new file mode 100644 index 000000000000..b86dea223ab6 --- /dev/null +++ b/src/modules/temperature_compensation/temp_comp_params_gyro_1.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* 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); diff --git a/src/modules/temperature_compensation/temp_comp_params_gyro_2.c b/src/modules/temperature_compensation/temp_comp_params_gyro_2.c new file mode 100644 index 000000000000..ed203776422e --- /dev/null +++ b/src/modules/temperature_compensation/temp_comp_params_gyro_2.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* 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); diff --git a/src/modules/temperature_compensation/temp_comp_params_gyro_3.c b/src/modules/temperature_compensation/temp_comp_params_gyro_3.c new file mode 100644 index 000000000000..a0ed93a4cd73 --- /dev/null +++ b/src/modules/temperature_compensation/temp_comp_params_gyro_3.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* 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);