diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c
index a735b31b2..f01939c17 100644
--- a/flight/AHRS/ahrs.c
+++ b/flight/AHRS/ahrs.c
@@ -640,9 +640,9 @@ void adc_callback(float * downsampled_data)
accel[2] = (downsampled_data[4] * accel_data.calibration.scale[2]) + accel_data.calibration.bias[2];
// Gyro data is (x,y,z) in second, fifth and seventh byte. Convert to rad/s
- gyro[0] = (downsampled_data[1] * gyro_data.calibration.scale[0]) + gyro_data.calibration.bias[0];
- gyro[1] = (downsampled_data[3] * gyro_data.calibration.scale[1]) + gyro_data.calibration.bias[1];
- gyro[2] = (downsampled_data[5] * gyro_data.calibration.scale[2]) + gyro_data.calibration.bias[2];
+ gyro[0] = ( ( downsampled_data[1] + gyro_data.calibration.tempcompfactor[0] * downsampled_data[6] ) * gyro_data.calibration.scale[0]) + gyro_data.calibration.bias[0];
+ gyro[1] = ( ( downsampled_data[3] + gyro_data.calibration.tempcompfactor[1] * downsampled_data[6] ) * gyro_data.calibration.scale[1]) + gyro_data.calibration.bias[1];
+ gyro[2] = ( ( downsampled_data[5] + gyro_data.calibration.tempcompfactor[2] * downsampled_data[7] ) * gyro_data.calibration.scale[2]) + gyro_data.calibration.bias[2];
#if 0
static float gravity_tracking[3] = {0,0,0};
@@ -856,6 +856,9 @@ void reset_values() {
gyro_data.calibration.variance[0] = 1;
gyro_data.calibration.variance[1] = 1;
gyro_data.calibration.variance[2] = 1;
+ gyro_data.calibration.tempcompfactor[0] = 0;
+ gyro_data.calibration.tempcompfactor[1] = 0;
+ gyro_data.calibration.tempcompfactor[2] = 0;
mag_data.calibration.scale[0] = 1;
mag_data.calibration.scale[1] = 1;
mag_data.calibration.scale[2] = 1;
@@ -947,6 +950,7 @@ void calibration_callback(AhrsObjHandle obj)
gyro_data.calibration.scale[ct] = cal.gyro_scale[ct];
gyro_data.calibration.bias[ct] = cal.gyro_bias[ct];
gyro_data.calibration.variance[ct] = cal.gyro_var[ct];
+ gyro_data.calibration.tempcompfactor[ct] = cal.gyro_tempcompfactor[ct];
mag_data.calibration.bias[ct] = cal.mag_bias[ct];
mag_data.calibration.scale[ct] = cal.mag_scale[ct];
mag_data.calibration.variance[ct] = cal.mag_var[ct];
diff --git a/flight/AHRS/inc/ahrs.h b/flight/AHRS/inc/ahrs.h
index 202588f75..fb1d994d8 100644
--- a/flight/AHRS/inc/ahrs.h
+++ b/flight/AHRS/inc/ahrs.h
@@ -80,6 +80,7 @@ struct gyro_sensor {
float bias[3];
float scale[3];
float variance[3];
+ float tempcompfactor[3];
} calibration;
struct {
uint16_t xy;
diff --git a/flight/OpenPilot/UAVObjects/ahrscalibration.c b/flight/OpenPilot/UAVObjects/ahrscalibration.c
index 6e46cd6e9..42edf7a0a 100644
--- a/flight/OpenPilot/UAVObjects/ahrscalibration.c
+++ b/flight/OpenPilot/UAVObjects/ahrscalibration.c
@@ -99,6 +99,9 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
data.gyro_var[0] = 0.0001;
data.gyro_var[1] = 0.0001;
data.gyro_var[2] = 0.0001;
+ data.gyro_tempcompfactor[0] = 0;
+ data.gyro_tempcompfactor[1] = 0;
+ data.gyro_tempcompfactor[2] = 0;
data.mag_bias[0] = 0;
data.mag_bias[1] = 0;
data.mag_bias[2] = 0;
diff --git a/flight/OpenPilot/UAVObjects/inc/ahrscalibration.h b/flight/OpenPilot/UAVObjects/inc/ahrscalibration.h
index ed5a105ca..933d03c7a 100644
--- a/flight/OpenPilot/UAVObjects/inc/ahrscalibration.h
+++ b/flight/OpenPilot/UAVObjects/inc/ahrscalibration.h
@@ -41,7 +41,7 @@
#define AHRSCALIBRATION_H
// Object constants
-#define AHRSCALIBRATION_OBJID 3761202980U
+#define AHRSCALIBRATION_OBJID 806362034U
#define AHRSCALIBRATION_NAME "AHRSCalibration"
#define AHRSCALIBRATION_METANAME "AHRSCalibrationMeta"
#define AHRSCALIBRATION_ISSINGLEINST 1
@@ -78,6 +78,7 @@ typedef struct {
float gyro_bias[3];
float gyro_scale[3];
float gyro_var[3];
+ float gyro_tempcompfactor[3];
float mag_bias[3];
float mag_scale[3];
float mag_var[3];
@@ -120,6 +121,11 @@ typedef enum { AHRSCALIBRATION_GYRO_SCALE_X=0, AHRSCALIBRATION_GYRO_SCALE_Y=1, A
typedef enum { AHRSCALIBRATION_GYRO_VAR_X=0, AHRSCALIBRATION_GYRO_VAR_Y=1, AHRSCALIBRATION_GYRO_VAR_Z=2 } AHRSCalibrationgyro_varElem;
/* Number of elements for field gyro_var */
#define AHRSCALIBRATION_GYRO_VAR_NUMELEM 3
+// Field gyro_tempcompfactor information
+/* Array element names for field gyro_tempcompfactor */
+typedef enum { AHRSCALIBRATION_GYRO_TEMPCOMPFACTOR_X=0, AHRSCALIBRATION_GYRO_TEMPCOMPFACTOR_Y=1, AHRSCALIBRATION_GYRO_TEMPCOMPFACTOR_Z=2 } AHRSCalibrationgyro_tempcompfactorElem;
+/* Number of elements for field gyro_tempcompfactor */
+#define AHRSCALIBRATION_GYRO_TEMPCOMPFACTOR_NUMELEM 3
// Field mag_bias information
/* Array element names for field mag_bias */
typedef enum { AHRSCALIBRATION_MAG_BIAS_X=0, AHRSCALIBRATION_MAG_BIAS_Y=1, AHRSCALIBRATION_MAG_BIAS_Z=2 } AHRSCalibrationmag_biasElem;
diff --git a/ground/src/plugins/uavobjects/OPLogConvert.m b/ground/src/plugins/uavobjects/OPLogConvert.m
index f61f97c83..1070fb62d 100644
--- a/ground/src/plugins/uavobjects/OPLogConvert.m
+++ b/ground/src/plugins/uavobjects/OPLogConvert.m
@@ -44,6 +44,7 @@ function [] = OPLogConvert()
AHRSCalibration(1).gyro_bias = zeros(1,3);
AHRSCalibration(1).gyro_scale = zeros(1,3);
AHRSCalibration(1).gyro_var = zeros(1,3);
+ AHRSCalibration(1).gyro_tempcompfactor = zeros(1,3);
AHRSCalibration(1).mag_bias = zeros(1,3);
AHRSCalibration(1).mag_scale = zeros(1,3);
AHRSCalibration(1).mag_var = zeros(1,3);
@@ -435,7 +436,7 @@ function [] = OPLogConvert()
case 3054509114
ActuatorSettings(actuatorsettingsIdx) = ReadActuatorSettingsObject(fid, timestamp);
actuatorsettingsIdx = actuatorsettingsIdx + 1;
- case 3761202980
+ case 806362034
AHRSCalibration(ahrscalibrationIdx) = ReadAHRSCalibrationObject(fid, timestamp);
ahrscalibrationIdx = ahrscalibrationIdx + 1;
case 3741078856
@@ -639,6 +640,7 @@ function [AHRSCalibration] = ReadAHRSCalibrationObject(fid, timestamp)
AHRSCalibration.gyro_bias = double(fread(fid, 3, 'float32'));
AHRSCalibration.gyro_scale = double(fread(fid, 3, 'float32'));
AHRSCalibration.gyro_var = double(fread(fid, 3, 'float32'));
+ AHRSCalibration.gyro_tempcompfactor = double(fread(fid, 3, 'float32'));
AHRSCalibration.mag_bias = double(fread(fid, 3, 'float32'));
AHRSCalibration.mag_scale = double(fread(fid, 3, 'float32'));
AHRSCalibration.mag_var = double(fread(fid, 3, 'float32'));
diff --git a/ground/src/plugins/uavobjects/ahrscalibration.cpp b/ground/src/plugins/uavobjects/ahrscalibration.cpp
index 6186c2abe..2a25ec75c 100644
--- a/ground/src/plugins/uavobjects/ahrscalibration.cpp
+++ b/ground/src/plugins/uavobjects/ahrscalibration.cpp
@@ -79,6 +79,11 @@ AHRSCalibration::AHRSCalibration(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTING
gyro_varElemNames.append("Y");
gyro_varElemNames.append("Z");
fields.append( new UAVObjectField(QString("gyro_var"), QString("deg^s/s^2"), UAVObjectField::FLOAT32, gyro_varElemNames, QStringList()) );
+ QStringList gyro_tempcompfactorElemNames;
+ gyro_tempcompfactorElemNames.append("X");
+ gyro_tempcompfactorElemNames.append("Y");
+ gyro_tempcompfactorElemNames.append("Z");
+ fields.append( new UAVObjectField(QString("gyro_tempcompfactor"), QString("raw/raw"), UAVObjectField::FLOAT32, gyro_tempcompfactorElemNames, QStringList()) );
QStringList mag_biasElemNames;
mag_biasElemNames.append("X");
mag_biasElemNames.append("Y");
@@ -154,6 +159,9 @@ void AHRSCalibration::setDefaultFieldValues()
data.gyro_var[0] = 0.0001;
data.gyro_var[1] = 0.0001;
data.gyro_var[2] = 0.0001;
+ data.gyro_tempcompfactor[0] = 0;
+ data.gyro_tempcompfactor[1] = 0;
+ data.gyro_tempcompfactor[2] = 0;
data.mag_bias[0] = 0;
data.mag_bias[1] = 0;
data.mag_bias[2] = 0;
diff --git a/ground/src/plugins/uavobjects/ahrscalibration.h b/ground/src/plugins/uavobjects/ahrscalibration.h
index 7c1ae93f8..e4f0bc827 100644
--- a/ground/src/plugins/uavobjects/ahrscalibration.h
+++ b/ground/src/plugins/uavobjects/ahrscalibration.h
@@ -50,6 +50,7 @@ public:
float gyro_bias[3];
float gyro_scale[3];
float gyro_var[3];
+ float gyro_tempcompfactor[3];
float mag_bias[3];
float mag_scale[3];
float mag_var[3];
@@ -92,6 +93,11 @@ public:
typedef enum { GYRO_VAR_X=0, GYRO_VAR_Y=1, GYRO_VAR_Z=2 } gyro_varElem;
/* Number of elements for field gyro_var */
static const quint32 GYRO_VAR_NUMELEM = 3;
+ // Field gyro_tempcompfactor information
+ /* Array element names for field gyro_tempcompfactor */
+ typedef enum { GYRO_TEMPCOMPFACTOR_X=0, GYRO_TEMPCOMPFACTOR_Y=1, GYRO_TEMPCOMPFACTOR_Z=2 } gyro_tempcompfactorElem;
+ /* Number of elements for field gyro_tempcompfactor */
+ static const quint32 GYRO_TEMPCOMPFACTOR_NUMELEM = 3;
// Field mag_bias information
/* Array element names for field mag_bias */
typedef enum { MAG_BIAS_X=0, MAG_BIAS_Y=1, MAG_BIAS_Z=2 } mag_biasElem;
@@ -112,7 +118,7 @@ public:
// Constants
- static const quint32 OBJID = 3761202980U;
+ static const quint32 OBJID = 806362034U;
static const QString NAME;
static const QString DESCRIPTION;
static const bool ISSINGLEINST = 1;
diff --git a/ground/src/plugins/uavobjects/ahrscalibration.py b/ground/src/plugins/uavobjects/ahrscalibration.py
index bc92ea0f2..e2bccce4a 100644
--- a/ground/src/plugins/uavobjects/ahrscalibration.py
+++ b/ground/src/plugins/uavobjects/ahrscalibration.py
@@ -121,6 +121,18 @@ _fields = [ \
{
}
),
+ uavobject.UAVObjectField(
+ 'gyro_tempcompfactor',
+ 'f',
+ 3,
+ [
+ 'X',
+ 'Y',
+ 'Z',
+ ],
+ {
+ }
+ ),
uavobject.UAVObjectField(
'mag_bias',
'f',
@@ -182,7 +194,7 @@ _fields = [ \
class AHRSCalibration(uavobject.UAVObject):
## Object constants
- OBJID = 3761202980
+ OBJID = 806362034
NAME = "AHRSCalibration"
METANAME = "AHRSCalibrationMeta"
ISSINGLEINST = 1
diff --git a/ground/src/shared/uavobjectdefinition/ahrscalibration.xml b/ground/src/shared/uavobjectdefinition/ahrscalibration.xml
index e6f2af100..33da5342e 100644
--- a/ground/src/shared/uavobjectdefinition/ahrscalibration.xml
+++ b/ground/src/shared/uavobjectdefinition/ahrscalibration.xml
@@ -8,6 +8,7 @@
+
@@ -18,4 +19,4 @@
-
\ No newline at end of file
+