mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: added IMU temperature calibration support
this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
This commit is contained in:
Andrew Tridgell
Peter Barker
parent
c4664d8e32
commit
7921e042f1
|
|
@ -552,6 +552,77 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
|||
// @RebootRequired: True
|
||||
AP_GROUPINFO("GYRO_RATE", 42, AP_InertialSensor, _fast_sampling_rate, MPU_FIFO_FASTSAMPLE_DEFAULT),
|
||||
|
||||
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
// @Group: TCAL1_
|
||||
// @Path: AP_InertialSensor_tempcal.cpp
|
||||
AP_SUBGROUPINFO(tcal[0], "TCAL1_", 43, AP_InertialSensor, AP_InertialSensor::TCal),
|
||||
|
||||
#if INS_MAX_INSTANCES > 1
|
||||
// @Group: TCAL2_
|
||||
// @Path: AP_InertialSensor_tempcal.cpp
|
||||
AP_SUBGROUPINFO(tcal[1], "TCAL2_", 44, AP_InertialSensor, AP_InertialSensor::TCal),
|
||||
#endif
|
||||
|
||||
#if INS_MAX_INSTANCES > 2
|
||||
// @Group: TCAL3_
|
||||
// @Path: AP_InertialSensor_tempcal.cpp
|
||||
AP_SUBGROUPINFO(tcal[2], "TCAL3_", 45, AP_InertialSensor, AP_InertialSensor::TCal),
|
||||
#endif
|
||||
|
||||
// @Param: ACC_CALTEMP1
|
||||
// @DisplayName: Calibration temperature for 1st accelerometer
|
||||
// @Description: Temperature that the 1st accelerometer was calibrated at
|
||||
// @User: Advanced
|
||||
// @Units: degC
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("ACC_CALTEMP1", 46, AP_InertialSensor, caltemp_accel[0], -100),
|
||||
|
||||
// @Param: GYR_CALTEMP1
|
||||
// @DisplayName: Calibration temperature for 1st gyroscope
|
||||
// @Description: Temperature that the 1st gyroscope was calibrated at
|
||||
// @User: Advanced
|
||||
// @Units: degC
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("GYR_CALTEMP1", 47, AP_InertialSensor, caltemp_gyro[0], -100),
|
||||
|
||||
#if INS_MAX_INSTANCES > 1
|
||||
// @Param: ACC_CALTEMP2
|
||||
// @DisplayName: Calibration temperature for 2nd accelerometer
|
||||
// @Description: Temperature that the 2nd accelerometer was calibrated at
|
||||
// @User: Advanced
|
||||
// @Units: degC
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("ACC_CALTEMP2", 48, AP_InertialSensor, caltemp_accel[1], -100),
|
||||
|
||||
// @Param: GYR_CALTEMP2
|
||||
// @DisplayName: Calibration temperature for 2nd gyroscope
|
||||
// @Description: Temperature that the 2nd gyroscope was calibrated at
|
||||
// @User: Advanced
|
||||
// @Units: degC
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("GYR_CALTEMP2", 49, AP_InertialSensor, caltemp_gyro[1], -100),
|
||||
#endif
|
||||
|
||||
#if INS_MAX_INSTANCES > 2
|
||||
// @Param: ACC_CALTEMP3
|
||||
// @DisplayName: Calibration temperature for 3rd accelerometer
|
||||
// @Description: Temperature that the 3rd accelerometer was calibrated at
|
||||
// @User: Advanced
|
||||
// @Units: degC
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("ACC_CALTEMP3", 50, AP_InertialSensor, caltemp_accel[2], -100),
|
||||
|
||||
// @Param: GYR_CALTEMP3
|
||||
// @DisplayName: Calibration temperature for 3rd gyroscope
|
||||
// @Description: Temperature that the 3rd gyroscope was calibrated at
|
||||
// @User: Advanced
|
||||
// @Units: degC
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("GYR_CALTEMP3", 51, AP_InertialSensor, caltemp_gyro[2], -100),
|
||||
#endif
|
||||
#endif // HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
|
||||
/*
|
||||
NOTE: parameter indexes have gaps above. When adding new
|
||||
parameters check for conflicts carefully
|
||||
|
|
@ -1122,11 +1193,11 @@ bool AP_InertialSensor::calibrate_trim(float &trim_roll, float &trim_pitch)
|
|||
Vector3f level_sample;
|
||||
|
||||
// exit immediately if calibration is already in progress
|
||||
if (_calibrating) {
|
||||
if (calibrating()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
_calibrating = true;
|
||||
_calibrating_accel = true;
|
||||
|
||||
const uint8_t update_dt_milliseconds = (uint8_t)(1000.0f/get_loop_rate_hz()+0.5f);
|
||||
|
||||
|
|
@ -1158,11 +1229,11 @@ bool AP_InertialSensor::calibrate_trim(float &trim_roll, float &trim_pitch)
|
|||
goto failed;
|
||||
}
|
||||
|
||||
_calibrating = false;
|
||||
_calibrating_accel = false;
|
||||
return true;
|
||||
|
||||
failed:
|
||||
_calibrating = false;
|
||||
_calibrating_accel = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -1231,14 +1302,17 @@ AP_InertialSensor::_init_gyro()
|
|||
Vector3f new_gyro_offset[INS_MAX_INSTANCES];
|
||||
float best_diff[INS_MAX_INSTANCES];
|
||||
bool converged[INS_MAX_INSTANCES];
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
float start_temperature[INS_MAX_INSTANCES];
|
||||
#endif
|
||||
|
||||
// exit immediately if calibration is already in progress
|
||||
if (_calibrating) {
|
||||
if (calibrating()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// record we are calibrating
|
||||
_calibrating = true;
|
||||
_calibrating_gyro = true;
|
||||
|
||||
// flash leds to tell user to keep the IMU still
|
||||
AP_Notify::flags.initialising = true;
|
||||
|
|
@ -1267,6 +1341,16 @@ AP_InertialSensor::_init_gyro()
|
|||
update();
|
||||
}
|
||||
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
// get start temperature. gyro cal usually happens when the board
|
||||
// has just been powered on, so the temperature may be changing
|
||||
// rapidly. We use the average between start and end temperature
|
||||
// as the calibration temperature to minimise errors
|
||||
for (uint8_t k=0; k<num_gyros; k++) {
|
||||
start_temperature[k] = get_temperature(k);
|
||||
}
|
||||
#endif
|
||||
|
||||
// the strategy is to average 50 points over 0.5 seconds, then do it
|
||||
// again and see if the 2nd average is within a small margin of
|
||||
// the first
|
||||
|
|
@ -1351,6 +1435,9 @@ AP_InertialSensor::_init_gyro()
|
|||
} else {
|
||||
_gyro_cal_ok[k] = true;
|
||||
_gyro_offset[k] = new_gyro_offset[k];
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
caltemp_gyro[k] = 0.5 * (get_temperature(k) + start_temperature[k]);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1358,7 +1445,7 @@ AP_InertialSensor::_init_gyro()
|
|||
_board_orientation = saved_orientation;
|
||||
|
||||
// record calibration complete
|
||||
_calibrating = false;
|
||||
_calibrating_gyro = false;
|
||||
|
||||
// stop flashing leds
|
||||
AP_Notify::flags.initialising = false;
|
||||
|
|
@ -1370,10 +1457,16 @@ void AP_InertialSensor::_save_gyro_calibration()
|
|||
for (uint8_t i=0; i<_gyro_count; i++) {
|
||||
_gyro_offset[i].save();
|
||||
_gyro_id[i].save();
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
caltemp_gyro[i].save();
|
||||
#endif
|
||||
}
|
||||
for (uint8_t i=_gyro_count; i<INS_MAX_INSTANCES; i++) {
|
||||
_gyro_offset[i].set_and_save(Vector3f());
|
||||
_gyro_id[i].set_and_save(0);
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
caltemp_gyro[i].set_and_save_ifchanged(-100);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1846,6 +1939,12 @@ bool AP_InertialSensor::is_still()
|
|||
(vibe.z < _still_threshold);
|
||||
}
|
||||
|
||||
// return true if we are in a calibration
|
||||
bool AP_InertialSensor::calibrating() const
|
||||
{
|
||||
return _calibrating_accel || _calibrating_gyro || (_acal && _acal->active());
|
||||
}
|
||||
|
||||
// initialise and register accel calibrator
|
||||
// called during the startup of accel cal
|
||||
void AP_InertialSensor::acal_init()
|
||||
|
|
@ -1908,9 +2007,15 @@ void AP_InertialSensor::_acal_save_calibrations()
|
|||
_accel_scale[i].set_and_save(gain);
|
||||
_accel_id[i].save();
|
||||
_accel_id_ok[i] = true;
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
caltemp_accel[i].set_and_save(get_temperature(i));
|
||||
#endif
|
||||
} else {
|
||||
_accel_offset[i].set_and_save(Vector3f());
|
||||
_accel_scale[i].set_and_save(Vector3f());
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
caltemp_accel[i].set_and_save(-100);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1919,6 +2024,9 @@ void AP_InertialSensor::_acal_save_calibrations()
|
|||
_accel_id[i].set_and_save(0);
|
||||
_accel_offset[i].set_and_save(Vector3f());
|
||||
_accel_scale[i].set_and_save(Vector3f());
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
caltemp_accel[i].set_and_save_ifchanged(-100);
|
||||
#endif
|
||||
}
|
||||
|
||||
Vector3f aligned_sample;
|
||||
|
|
@ -2045,13 +2153,13 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal()
|
|||
Vector3f rotated_gravity(0, 0, -GRAVITY_MSS);
|
||||
|
||||
// exit immediately if calibration is already in progress
|
||||
if (_calibrating) {
|
||||
if (calibrating()) {
|
||||
return MAV_RESULT_TEMPORARILY_REJECTED;
|
||||
}
|
||||
|
||||
EXPECT_DELAY_MS(20000);
|
||||
// record we are calibrating
|
||||
_calibrating = true;
|
||||
_calibrating_accel = true;
|
||||
|
||||
// flash leds to tell user to keep the IMU still
|
||||
AP_Notify::flags.initialising = true;
|
||||
|
|
@ -2159,6 +2267,9 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal()
|
|||
_accel_scale[k].save();
|
||||
_accel_id[k].save();
|
||||
_accel_id_ok[k] = true;
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
caltemp_accel[k].set_and_save(get_temperature(k));
|
||||
#endif
|
||||
}
|
||||
|
||||
// force trim to zero
|
||||
|
|
@ -2173,7 +2284,7 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal()
|
|||
}
|
||||
|
||||
// record calibration complete
|
||||
_calibrating = false;
|
||||
_calibrating_accel = false;
|
||||
|
||||
// throw away any existing samples that may have the wrong
|
||||
// orientation. We do this by throwing samples away for 0.5s,
|
||||
|
|
|
|||
|
|
@ -31,6 +31,11 @@
|
|||
|
||||
#define DEFAULT_IMU_LOG_BAT_MASK 0
|
||||
|
||||
#ifndef HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
#define HAL_INS_TEMPERATURE_CAL_ENABLE !HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024
|
||||
#endif
|
||||
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <AP_AccelCal/AP_AccelCal.h>
|
||||
|
|
@ -99,7 +104,7 @@ public:
|
|||
bool calibrate_trim(float &trim_roll, float &trim_pitch);
|
||||
|
||||
/// calibrating - returns true if the gyros or accels are currently being calibrated
|
||||
bool calibrating() const { return _calibrating; }
|
||||
bool calibrating() const;
|
||||
|
||||
/// Perform cold-start initialisation for just the gyros.
|
||||
///
|
||||
|
|
@ -596,11 +601,12 @@ private:
|
|||
// are we in HIL mode?
|
||||
bool _hil_mode:1;
|
||||
|
||||
// are gyros or accels currently being calibrated
|
||||
bool _calibrating:1;
|
||||
|
||||
bool _backends_detected:1;
|
||||
|
||||
// are gyros or accels currently being calibrated
|
||||
bool _calibrating_accel;
|
||||
bool _calibrating_gyro;
|
||||
|
||||
// the delta time in seconds for the last sample
|
||||
float _delta_time;
|
||||
|
||||
|
|
@ -673,6 +679,31 @@ private:
|
|||
uint32_t _startup_ms;
|
||||
|
||||
uint8_t imu_kill_mask;
|
||||
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
class TCal {
|
||||
public:
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
void correct_accel(float temperature, float cal_temp, Vector3f &accel) const;
|
||||
void correct_gyro(float temperature, float cal_temp, Vector3f &accel) const;
|
||||
private:
|
||||
AP_Int8 enable;
|
||||
AP_Float temp_min;
|
||||
AP_Float temp_max;
|
||||
AP_Vector3f accel_coeff[3];
|
||||
AP_Vector3f gyro_coeff[3];
|
||||
Vector3f accel_tref;
|
||||
Vector3f gyro_tref;
|
||||
|
||||
void correct_sensor(float temperature, float cal_temp, const AP_Vector3f coeff[3], Vector3f &v) const;
|
||||
Vector3f polynomial_eval(float temperature, const AP_Vector3f coeff[3]) const;
|
||||
};
|
||||
TCal tcal[INS_MAX_INSTANCES];
|
||||
|
||||
// temperature that last calibration was run at
|
||||
AP_Float caltemp_accel[INS_MAX_INSTANCES];
|
||||
AP_Float caltemp_gyro[INS_MAX_INSTANCES];
|
||||
#endif
|
||||
};
|
||||
|
||||
namespace AP {
|
||||
|
|
|
|||
|
|
@ -94,15 +94,24 @@ void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vect
|
|||
|
||||
// rotate for sensor orientation
|
||||
accel.rotate(_imu._accel_orientation[instance]);
|
||||
|
||||
// apply offsets
|
||||
accel -= _imu._accel_offset[instance];
|
||||
|
||||
// apply scaling
|
||||
const Vector3f &accel_scale = _imu._accel_scale[instance].get();
|
||||
accel.x *= accel_scale.x;
|
||||
accel.y *= accel_scale.y;
|
||||
accel.z *= accel_scale.z;
|
||||
if (!_imu._calibrating_accel && (_imu._acal == nullptr || !_imu._acal->active())) {
|
||||
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
// apply temperature corrections
|
||||
_imu.tcal[instance].correct_accel(_imu.get_temperature(instance), _imu.caltemp_accel[instance], accel);
|
||||
#endif
|
||||
|
||||
// apply offsets
|
||||
accel -= _imu._accel_offset[instance];
|
||||
|
||||
|
||||
// apply scaling
|
||||
const Vector3f &accel_scale = _imu._accel_scale[instance].get();
|
||||
accel.x *= accel_scale.x;
|
||||
accel.y *= accel_scale.y;
|
||||
accel.z *= accel_scale.z;
|
||||
}
|
||||
|
||||
// rotate to body frame
|
||||
if (_imu._board_orientation == ROTATION_CUSTOM && _imu._custom_rotation) {
|
||||
|
|
@ -117,8 +126,16 @@ void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vecto
|
|||
// rotate for sensor orientation
|
||||
gyro.rotate(_imu._gyro_orientation[instance]);
|
||||
|
||||
// gyro calibration is always assumed to have been done in sensor frame
|
||||
gyro -= _imu._gyro_offset[instance];
|
||||
if (!_imu._calibrating_gyro) {
|
||||
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
// apply temperature corrections
|
||||
_imu.tcal[instance].correct_gyro(_imu.get_temperature(instance), _imu.caltemp_gyro[instance], gyro);
|
||||
#endif
|
||||
|
||||
// gyro calibration is always assumed to have been done in sensor frame
|
||||
gyro -= _imu._gyro_offset[instance];
|
||||
}
|
||||
|
||||
if (_imu._board_orientation == ROTATION_CUSTOM && _imu._custom_rotation) {
|
||||
gyro = *_imu._custom_rotation * gyro;
|
||||
|
|
@ -315,18 +332,10 @@ void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f
|
|||
if (_imu._accel_calibrator != nullptr && _imu._accel_calibrator[instance].get_status() == ACCEL_CAL_COLLECTING_SAMPLE) {
|
||||
Vector3f cal_sample = _imu._delta_velocity[instance];
|
||||
|
||||
//remove rotation
|
||||
// remove rotation. Note that we don't need to remove offsets or scale factor as those
|
||||
// are not applied when calibrating
|
||||
cal_sample.rotate_inverse(_imu._board_orientation);
|
||||
|
||||
// remove scale factors
|
||||
const Vector3f &accel_scale = _imu._accel_scale[instance].get();
|
||||
cal_sample.x /= accel_scale.x;
|
||||
cal_sample.y /= accel_scale.y;
|
||||
cal_sample.z /= accel_scale.z;
|
||||
|
||||
//remove offsets
|
||||
cal_sample += _imu._accel_offset[instance].get() * _imu._delta_velocity_dt[instance] ;
|
||||
|
||||
_imu._accel_calibrator[instance].new_sample(cal_sample, _imu._delta_velocity_dt[instance]);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,219 @@
|
|||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
IMU temperature calibration handling
|
||||
*/
|
||||
|
||||
#include "AP_InertialSensor.h"
|
||||
|
||||
#if HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
|
||||
// temperature calibration parameters, per IMU
|
||||
const AP_Param::GroupInfo AP_InertialSensor::TCal::var_info[] = {
|
||||
|
||||
// @Param: ENABLE
|
||||
// @DisplayName: Enable temperature calibration
|
||||
// @Description: Enable the use of temperature calibration parameters for this IMU
|
||||
// @Values: 0:Disabled,1:Enabled
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO_FLAGS("ENABLE", 1, AP_InertialSensor::TCal, enable, 0, AP_PARAM_FLAG_ENABLE),
|
||||
|
||||
// @Param: TMIN
|
||||
// @DisplayName: Temperature calibration min
|
||||
// @Description: The minimum temperature that the calibration is valid for
|
||||
// @Range: -70 80
|
||||
// @Units: degC
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("TMIN", 2, AP_InertialSensor::TCal, temp_min, 0),
|
||||
|
||||
// @Param: TMAX
|
||||
// @DisplayName: Temperature calibration max
|
||||
// @Description: The maximum temperature that the calibration is valid for
|
||||
// @Range: -70 80
|
||||
// @Units: degC
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
AP_GROUPINFO("TMAX", 3, AP_InertialSensor::TCal, temp_max, 0),
|
||||
|
||||
// @Param: ACC1_X
|
||||
// @DisplayName: Accelerometer 1st order temperature coefficient X axis
|
||||
// @Description: This is the 1st order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: ACC1_Y
|
||||
// @DisplayName: Accelerometer 1st order temperature coefficient Y axis
|
||||
// @Description: This is the 1st order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: ACC1_Z
|
||||
// @DisplayName: Accelerometer 1st order temperature coefficient Z axis
|
||||
// @Description: This is the 1st order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
AP_GROUPINFO("ACC1", 4, AP_InertialSensor::TCal, accel_coeff[0], 0),
|
||||
|
||||
// @Param: ACC2_X
|
||||
// @DisplayName: Accelerometer 2nd order temperature coefficient X axis
|
||||
// @Description: This is the 2nd order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: ACC2_Y
|
||||
// @DisplayName: Accelerometer 2nd order temperature coefficient Y axis
|
||||
// @Description: This is the 2nd order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: ACC2_Z
|
||||
// @DisplayName: Accelerometer 2nd order temperature coefficient Z axis
|
||||
// @Description: This is the 2nd order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
AP_GROUPINFO("ACC2", 5, AP_InertialSensor::TCal, accel_coeff[1], 0),
|
||||
|
||||
// @Param: ACC3_X
|
||||
// @DisplayName: Accelerometer 3rd order temperature coefficient X axis
|
||||
// @Description: This is the 3rd order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: ACC3_Y
|
||||
// @DisplayName: Accelerometer 3rd order temperature coefficient Y axis
|
||||
// @Description: This is the 3rd order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: ACC3_Z
|
||||
// @DisplayName: Accelerometer 3rd order temperature coefficient Z axis
|
||||
// @Description: This is the 3rd order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
AP_GROUPINFO("ACC3", 6, AP_InertialSensor::TCal, accel_coeff[2], 0),
|
||||
|
||||
// @Param: GYR1_X
|
||||
// @DisplayName: Gyroscope 1st order temperature coefficient X axis
|
||||
// @Description: This is the 1st order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: GYR1_Y
|
||||
// @DisplayName: Gyroscope 1st order temperature coefficient Y axis
|
||||
// @Description: This is the 1st order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: GYR1_Z
|
||||
// @DisplayName: Gyroscope 1st order temperature coefficient Z axis
|
||||
// @Description: This is the 1st order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
AP_GROUPINFO("GYR1", 7, AP_InertialSensor::TCal, gyro_coeff[0], 0),
|
||||
|
||||
// @Param: GYR2_X
|
||||
// @DisplayName: Gyroscope 2nd order temperature coefficient X axis
|
||||
// @Description: This is the 2nd order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: GYR2_Y
|
||||
// @DisplayName: Gyroscope 2nd order temperature coefficient Y axis
|
||||
// @Description: This is the 2nd order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: GYR2_Z
|
||||
// @DisplayName: Gyroscope 2nd order temperature coefficient Z axis
|
||||
// @Description: This is the 2nd order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
AP_GROUPINFO("GYR2", 8, AP_InertialSensor::TCal, gyro_coeff[1], 0),
|
||||
|
||||
// @Param: GYR3_X
|
||||
// @DisplayName: Gyroscope 3rd order temperature coefficient X axis
|
||||
// @Description: This is the 3rd order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: GYR3_Y
|
||||
// @DisplayName: Gyroscope 3rd order temperature coefficient Y axis
|
||||
// @Description: This is the 3rd order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
// @Param: GYR3_Z
|
||||
// @DisplayName: Gyroscope 3rd order temperature coefficient Z axis
|
||||
// @Description: This is the 3rd order temperature coefficient from a temperature calibration
|
||||
// @User: Advanced
|
||||
// @Calibration: 1
|
||||
|
||||
AP_GROUPINFO("GYR3", 9, AP_InertialSensor::TCal, gyro_coeff[2], 0),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
||||
/*
|
||||
evaluate a 3rd order polynomial (without the constant term) given a set of coefficients
|
||||
*/
|
||||
Vector3f AP_InertialSensor::TCal::polynomial_eval(float tdiff, const AP_Vector3f coeff[3]) const
|
||||
{
|
||||
// evaluate order 3 polynomial
|
||||
const Vector3f *c = (Vector3f *)&coeff[0];
|
||||
return (c[0] + (c[1] + c[2]*tdiff)*tdiff)*tdiff;
|
||||
}
|
||||
|
||||
/*
|
||||
correct a single sensor for the current temperature
|
||||
*/
|
||||
void AP_InertialSensor::TCal::correct_sensor(float temperature, float cal_temp, const AP_Vector3f coeff[3], Vector3f &v) const
|
||||
{
|
||||
if (!enable) {
|
||||
return;
|
||||
}
|
||||
temperature = constrain_float(temperature, temp_min, temp_max);
|
||||
cal_temp = constrain_float(cal_temp, temp_min, temp_max);
|
||||
|
||||
const float tmid = (temp_max + temp_min)*0.5;
|
||||
|
||||
// get the polynomial correction for the difference between the
|
||||
// current temperature and the temperature we are at now
|
||||
v -= polynomial_eval(temperature-tmid, coeff);
|
||||
|
||||
// we need to add the correction for the temperature
|
||||
// difference between the tmid, which is the reference used for
|
||||
// the calibration process, and the cal_temp, which is the
|
||||
// temperature that the offsets and scale factors was setup for
|
||||
v += polynomial_eval(cal_temp-tmid, coeff);
|
||||
}
|
||||
|
||||
void AP_InertialSensor::TCal::correct_accel(float temperature, float cal_temp, Vector3f &accel) const
|
||||
{
|
||||
correct_sensor(temperature, cal_temp, accel_coeff, accel);
|
||||
}
|
||||
|
||||
void AP_InertialSensor::TCal::correct_gyro(float temperature, float cal_temp, Vector3f &gyro) const
|
||||
{
|
||||
correct_sensor(temperature, cal_temp, gyro_coeff, gyro);
|
||||
}
|
||||
|
||||
#endif // HAL_INS_TEMPERATURE_CAL_ENABLE
|
||||
Loading…
Reference in New Issue