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 +