rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

AHRS: Support for Linear Temperature Compensation Factors for Gyroscope X,Y,Z

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2327 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
corvus 2011-01-04 00:54:31 +00:00 committed by corvus
parent a0bf048090
commit a59d08b7af
9 changed files with 51 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
flight/AHRS/ahrs.c
View File

@ -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]; 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 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[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.scale[1]) + gyro_data.calibration.bias[1]; 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.scale[2]) + gyro_data.calibration.bias[2]; 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 #if 0
static float gravity_tracking[3] = {0,0,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[0] = 1;
gyro_data.calibration.variance[1] = 1; gyro_data.calibration.variance[1] = 1;
gyro_data.calibration.variance[2] = 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[0] = 1;
mag_data.calibration.scale[1] = 1; mag_data.calibration.scale[1] = 1;
mag_data.calibration.scale[2] = 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.scale[ct] = cal.gyro_scale[ct];
gyro_data.calibration.bias[ct] = cal.gyro_bias[ct]; gyro_data.calibration.bias[ct] = cal.gyro_bias[ct];
gyro_data.calibration.variance[ct] = cal.gyro_var[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.bias[ct] = cal.mag_bias[ct];
mag_data.calibration.scale[ct] = cal.mag_scale[ct]; mag_data.calibration.scale[ct] = cal.mag_scale[ct];
mag_data.calibration.variance[ct] = cal.mag_var[ct]; mag_data.calibration.variance[ct] = cal.mag_var[ct];

1
flight/AHRS/inc/ahrs.h
View File

@ -80,6 +80,7 @@ struct gyro_sensor {
float bias[3]; float bias[3];
float scale[3]; float scale[3];
float variance[3]; float variance[3];
float tempcompfactor[3];
} calibration; } calibration;
struct { struct {
uint16_t xy; uint16_t xy;

3
flight/OpenPilot/UAVObjects/ahrscalibration.c
View File

@ -99,6 +99,9 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
data.gyro_var[0] = 0.0001; data.gyro_var[0] = 0.0001;
data.gyro_var[1] = 0.0001; data.gyro_var[1] = 0.0001;
data.gyro_var[2] = 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[0] = 0;
data.mag_bias[1] = 0; data.mag_bias[1] = 0;
data.mag_bias[2] = 0; data.mag_bias[2] = 0;

8
flight/OpenPilot/UAVObjects/inc/ahrscalibration.h
View File

@ -41,7 +41,7 @@
#define AHRSCALIBRATION_H #define AHRSCALIBRATION_H
// Object constants // Object constants
#define AHRSCALIBRATION_OBJID 3761202980U #define AHRSCALIBRATION_OBJID 806362034U
#define AHRSCALIBRATION_NAME "AHRSCalibration" #define AHRSCALIBRATION_NAME "AHRSCalibration"
#define AHRSCALIBRATION_METANAME "AHRSCalibrationMeta" #define AHRSCALIBRATION_METANAME "AHRSCalibrationMeta"
#define AHRSCALIBRATION_ISSINGLEINST 1 #define AHRSCALIBRATION_ISSINGLEINST 1
@ -78,6 +78,7 @@ typedef struct {
float gyro_bias[3]; float gyro_bias[3];
float gyro_scale[3]; float gyro_scale[3];
float gyro_var[3]; float gyro_var[3];
float gyro_tempcompfactor[3];
float mag_bias[3]; float mag_bias[3];
float mag_scale[3]; float mag_scale[3];
float mag_var[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; 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 */ /* Number of elements for field gyro_var */
#define AHRSCALIBRATION_GYRO_VAR_NUMELEM 3 #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 // Field mag_bias information
/* Array element names for field mag_bias */ /* 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; typedef enum { AHRSCALIBRATION_MAG_BIAS_X=0, AHRSCALIBRATION_MAG_BIAS_Y=1, AHRSCALIBRATION_MAG_BIAS_Z=2 } AHRSCalibrationmag_biasElem;

4
ground/src/plugins/uavobjects/OPLogConvert.m
View File

@ -44,6 +44,7 @@ function [] = OPLogConvert()
AHRSCalibration(1).gyro_bias = zeros(1,3); AHRSCalibration(1).gyro_bias = zeros(1,3);
AHRSCalibration(1).gyro_scale = zeros(1,3); AHRSCalibration(1).gyro_scale = zeros(1,3);
AHRSCalibration(1).gyro_var = 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_bias = zeros(1,3);
AHRSCalibration(1).mag_scale = zeros(1,3); AHRSCalibration(1).mag_scale = zeros(1,3);
AHRSCalibration(1).mag_var = zeros(1,3); AHRSCalibration(1).mag_var = zeros(1,3);
@ -435,7 +436,7 @@ function [] = OPLogConvert()
case 3054509114 case 3054509114
ActuatorSettings(actuatorsettingsIdx) = ReadActuatorSettingsObject(fid, timestamp); ActuatorSettings(actuatorsettingsIdx) = ReadActuatorSettingsObject(fid, timestamp);
actuatorsettingsIdx = actuatorsettingsIdx + 1; actuatorsettingsIdx = actuatorsettingsIdx + 1;
case 3761202980 case 806362034
AHRSCalibration(ahrscalibrationIdx) = ReadAHRSCalibrationObject(fid, timestamp); AHRSCalibration(ahrscalibrationIdx) = ReadAHRSCalibrationObject(fid, timestamp);
ahrscalibrationIdx = ahrscalibrationIdx + 1; ahrscalibrationIdx = ahrscalibrationIdx + 1;
case 3741078856 case 3741078856
@ -639,6 +640,7 @@ function [AHRSCalibration] = ReadAHRSCalibrationObject(fid, timestamp)
AHRSCalibration.gyro_bias = double(fread(fid, 3, 'float32')); AHRSCalibration.gyro_bias = double(fread(fid, 3, 'float32'));
AHRSCalibration.gyro_scale = double(fread(fid, 3, 'float32')); AHRSCalibration.gyro_scale = double(fread(fid, 3, 'float32'));
AHRSCalibration.gyro_var = 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_bias = double(fread(fid, 3, 'float32'));
AHRSCalibration.mag_scale = double(fread(fid, 3, 'float32')); AHRSCalibration.mag_scale = double(fread(fid, 3, 'float32'));
AHRSCalibration.mag_var = double(fread(fid, 3, 'float32')); AHRSCalibration.mag_var = double(fread(fid, 3, 'float32'));

8
ground/src/plugins/uavobjects/ahrscalibration.cpp
View File

@ -79,6 +79,11 @@ AHRSCalibration::AHRSCalibration(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTING
gyro_varElemNames.append("Y"); gyro_varElemNames.append("Y");
gyro_varElemNames.append("Z"); gyro_varElemNames.append("Z");
fields.append( new UAVObjectField(QString("gyro_var"), QString("deg^s/s^2"), UAVObjectField::FLOAT32, gyro_varElemNames, QStringList()) ); 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; QStringList mag_biasElemNames;
mag_biasElemNames.append("X"); mag_biasElemNames.append("X");
mag_biasElemNames.append("Y"); mag_biasElemNames.append("Y");
@ -154,6 +159,9 @@ void AHRSCalibration::setDefaultFieldValues()
data.gyro_var[0] = 0.0001; data.gyro_var[0] = 0.0001;
data.gyro_var[1] = 0.0001; data.gyro_var[1] = 0.0001;
data.gyro_var[2] = 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[0] = 0;
data.mag_bias[1] = 0; data.mag_bias[1] = 0;
data.mag_bias[2] = 0; data.mag_bias[2] = 0;

8
ground/src/plugins/uavobjects/ahrscalibration.h
View File

@ -50,6 +50,7 @@ public:
float gyro_bias[3]; float gyro_bias[3];
float gyro_scale[3]; float gyro_scale[3];
float gyro_var[3]; float gyro_var[3];
float gyro_tempcompfactor[3];
float mag_bias[3]; float mag_bias[3];
float mag_scale[3]; float mag_scale[3];
float mag_var[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; typedef enum { GYRO_VAR_X=0, GYRO_VAR_Y=1, GYRO_VAR_Z=2 } gyro_varElem;
/* Number of elements for field gyro_var */ /* Number of elements for field gyro_var */
static const quint32 GYRO_VAR_NUMELEM = 3; 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 // Field mag_bias information
/* Array element names for field mag_bias */ /* Array element names for field mag_bias */
typedef enum { MAG_BIAS_X=0, MAG_BIAS_Y=1, MAG_BIAS_Z=2 } mag_biasElem; typedef enum { MAG_BIAS_X=0, MAG_BIAS_Y=1, MAG_BIAS_Z=2 } mag_biasElem;
@ -112,7 +118,7 @@ public:
// Constants // Constants
static const quint32 OBJID = 3761202980U; static const quint32 OBJID = 806362034U;
static const QString NAME; static const QString NAME;
static const QString DESCRIPTION; static const QString DESCRIPTION;
static const bool ISSINGLEINST = 1; static const bool ISSINGLEINST = 1;

14
ground/src/plugins/uavobjects/ahrscalibration.py
View File

@ -121,6 +121,18 @@ _fields = [ \
{ {
} }
), ),
uavobject.UAVObjectField(
'gyro_tempcompfactor',
'f',
3,
[
'X',
'Y',
'Z',
],
{
}
),
uavobject.UAVObjectField( uavobject.UAVObjectField(
'mag_bias', 'mag_bias',
'f', 'f',
@ -182,7 +194,7 @@ _fields = [ \
class AHRSCalibration(uavobject.UAVObject): class AHRSCalibration(uavobject.UAVObject):
## Object constants ## Object constants
OBJID = 3761202980 OBJID = 806362034
NAME = "AHRSCalibration" NAME = "AHRSCalibration"
METANAME = "AHRSCalibrationMeta" METANAME = "AHRSCalibrationMeta"
ISSINGLEINST = 1 ISSINGLEINST = 1

1
ground/src/shared/uavobjectdefinition/ahrscalibration.xml
View File

@ -8,6 +8,7 @@
<field name="gyro_bias" units="deg/s" type="float" elementnames="X,Y,Z" defaultvalue="23,-23,23"/> <field name="gyro_bias" units="deg/s" type="float" elementnames="X,Y,Z" defaultvalue="23,-23,23"/>
<field name="gyro_scale" units="deg/s" type="float" elementnames="X,Y,Z" defaultvalue="-0.0140,0.014,-0.014"/> <field name="gyro_scale" units="deg/s" type="float" elementnames="X,Y,Z" defaultvalue="-0.0140,0.014,-0.014"/>
<field name="gyro_var" units="deg^s/s^2" type="float" elementnames="X,Y,Z" defaultvalue="1e-4"/> <field name="gyro_var" units="deg^s/s^2" type="float" elementnames="X,Y,Z" defaultvalue="1e-4"/>
<field name="gyro_tempcompfactor" units="raw/raw" type="float" elementnames="X,Y,Z" defaultvalue="0"/>
<field name="mag_bias" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="0"/> <field name="mag_bias" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="0"/>
<field name="mag_scale" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="1"/> <field name="mag_scale" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="1"/>
<field name="mag_var" units="mGau^s" type="float" elementnames="X,Y,Z" defaultvalue="5e-5"/> <field name="mag_var" units="mGau^s" type="float" elementnames="X,Y,Z" defaultvalue="5e-5"/>