rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

OP-118 AHRS: Added 6 point calibration routine to AHRS config widget. To use it, click "Cal Mode", then place the AHRS on each of 6 sides of a cube then click save position. Finally click compute to push the updated scale abd bias terms to the AHRS. Also fixed a bug in UAVObjectField that would not allow you to setDouble for extra fields.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1449 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-08-28 18:48:51 +00:00 committed by peabody124
parent 331dba4870
commit 8128c3bbfa
12 changed files with 237 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
flight/AHRS/ahrs.c
View File

@ -338,16 +338,16 @@ int main()
// format data for INS algo
gyro[0] = gyro_data.filtered.x;
gyro[1] = gyro_data.filtered.y;
gyro[2] = -gyro_data.filtered.z;
gyro[2] = gyro_data.filtered.z;
accel[0] = accel_data.filtered.x,
accel[1] = accel_data.filtered.y,
accel[2] = accel_data.filtered.z,
// Note: The magnetometer driver returns registers X,Y,Z from the chip which are
// (left, backward, up). Remapping to (forward, right, down).
mag[0] = -mag_data.raw.axis[1];
mag[1] = -mag_data.raw.axis[0];
mag[2] = -mag_data.raw.axis[2];
mag[0] = -(mag_data.raw.axis[1] - mag_bias[1]);
mag[1] = -(mag_data.raw.axis[0] - mag_bias[0]);
mag[2] = -(mag_data.raw.axis[2] - mag_bias[2]);
INSPrediction(gyro, accel, 1 / (float) EKF_RATE);

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

@ -93,9 +93,9 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
data.gyro_bias[0] = -1675;
data.gyro_bias[1] = -1675;
data.gyro_bias[2] = -1675;
data.gyro_scale[0] = 0.007;
data.gyro_scale[1] = 0.007;
data.gyro_scale[2] = 0.007;
data.gyro_scale[0] = -0.007;
data.gyro_scale[1] = -0.007;
data.gyro_scale[2] = -0.007;
data.gyro_var[0] = 0.0001;
data.gyro_var[1] = 0.0001;
data.gyro_var[2] = 0.0001;

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

@ -41,7 +41,7 @@
#define AHRSCALIBRATION_H
// Object constants
#define AHRSCALIBRATION_OBJID 3497014596U
#define AHRSCALIBRATION_OBJID 2082766848U
#define AHRSCALIBRATION_NAME "AHRSCalibration"
#define AHRSCALIBRATION_METANAME "AHRSCalibrationMeta"
#define AHRSCALIBRATION_ISSINGLEINST 1
@ -78,7 +78,7 @@ typedef struct {
int16_t gyro_bias[3];
float gyro_scale[3];
float gyro_var[3];
float mag_bias[3];
int16_t mag_bias[3];
float mag_var[3];
} __attribute__((packed)) AHRSCalibrationData;

2
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -25,6 +25,7 @@
65322D2F12283CCD0046CD7C /* NMEA.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = NMEA.c; sourceTree = "<group>"; };
65322D3012283CD60046CD7C /* NMEA.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = NMEA.h; sourceTree = "<group>"; };
65322D3B122841F60046CD7C /* gpstime.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = gpstime.xml; sourceTree = "<group>"; };
65322D77122897210046CD7C /* MagOrAccelSensorCal.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = MagOrAccelSensorCal.c; path = ../../AHRS/MagOrAccelSensorCal.c; sourceTree = SOURCE_ROOT; };
654330231218E9780063F913 /* insgps.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps.c; path = ../../AHRS/insgps.c; sourceTree = SOURCE_ROOT; };
6543304F121980300063F913 /* insgps.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = insgps.h; sourceTree = "<group>"; };
655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = "<group>"; };
@ -6707,6 +6708,7 @@
65B7E6AC120DF1CD000C1123 /* AHRS */ = {
isa = PBXGroup;
children = (
65322D77122897210046CD7C /* MagOrAccelSensorCal.c */,
654330231218E9780063F913 /* insgps.c */,
65B7E6AD120DF1E2000C1123 /* ahrs_fsm.c */,
65B7E6AE120DF1E2000C1123 /* ahrs.c */,

30
ground/src/plugins/config/ahrs.ui
View File

@ -13,7 +13,7 @@
<property name="windowTitle">
<string>Form</string>
</property>
<widget class="QFrame" name="frame">
<widget class="QFrame" name="Position6">
<property name="geometry">
<rect>
<x>10</x>
@ -158,52 +158,52 @@ Takes about 30 seconds max.</string>
<string>Calibration #2</string>
</property>
</widget>
<widget class="QPushButton" name="pushButton_4">
<widget class="QPushButton" name="ahrsSavePosition">
<property name="enabled">
<bool>false</bool>
</property>
<property name="geometry">
<rect>
<x>230</x>
<y>270</y>
<width>93</width>
<x>220</x>
<y>290</y>
<width>111</width>
<height>27</height>
</rect>
</property>
<property name="text">
<string>PushButton</string>
<string>Save Position</string>
</property>
</widget>
<widget class="QPushButton" name="pushButton_5">
<widget class="QPushButton" name="ahrsComputeScaleBias">
<property name="enabled">
<bool>false</bool>
</property>
<property name="geometry">
<rect>
<x>230</x>
<x>220</x>
<y>320</y>
<width>93</width>
<width>111</width>
<height>27</height>
</rect>
</property>
<property name="text">
<string>PushButton</string>
<string>Compute</string>
</property>
</widget>
<widget class="QPushButton" name="pushButton_6">
<widget class="QPushButton" name="ahrsCalibrationMode">
<property name="enabled">
<bool>false</bool>
</property>
<property name="geometry">
<rect>
<x>230</x>
<y>370</y>
<width>93</width>
<x>220</x>
<y>260</y>
<width>111</width>
<height>27</height>
</rect>
</property>
<property name="text">
<string>PushButton</string>
<string>Cal Mode</string>
</property>
</widget>
<widget class="QTextBrowser" name="textBrowser">

178
ground/src/plugins/config/configahrswidget.cpp
View File

@ -163,7 +163,7 @@ ConfigAHRSWidget::ConfigAHRSWidget(QWidget *parent) : ConfigTaskWidget(parent)
mag_z->setPos(startX,startY);
mag_z->setTransform(QTransform::fromScale(1,0),true);
position = 0;
// Fill the dropdown menus:
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings")));
@ -176,7 +176,9 @@ ConfigAHRSWidget::ConfigAHRSWidget(QWidget *parent) : ConfigTaskWidget(parent)
connect(m_ahrs->ahrsSettingsRequest, SIGNAL(clicked()), this, SLOT(ahrsSettingsRequest()));
connect(m_ahrs->ahrsSettingsSaveRAM, SIGNAL(clicked()), this, SLOT(ahrsSettingsSaveRAM()));
connect(m_ahrs->ahrsSettingsSaveSD, SIGNAL(clicked()), this, SLOT(ahrsSettingsSaveSD()));
connect(m_ahrs->ahrsSavePosition, SIGNAL(clicked()), this, SLOT(savePositionData()));
connect(m_ahrs->ahrsComputeScaleBias, SIGNAL(clicked()), this, SLOT(computeScaleBias()));
connect(m_ahrs->ahrsCalibrationMode, SIGNAL(clicked()), this, SLOT(calibrationMode()));
connect(parent, SIGNAL(autopilotConnected()),this, SLOT(ahrsSettingsRequest()));
@ -294,6 +296,175 @@ void ConfigAHRSWidget::saveAHRSCalibration()
}
/**
* Saves the data from the aircraft in one of six positions
*/
void ConfigAHRSWidget::savePositionData()
{
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeRaw")));
UAVObjectField *accel_field = obj->getField(QString("accels_filtered"));
UAVObjectField *mag_field = obj->getField(QString("magnetometers"));
accel_data_x[position] = accel_field->getValue(0).toDouble();
accel_data_y[position] = accel_field->getValue(1).toDouble();
accel_data_z[position] = accel_field->getValue(2).toDouble();
mag_data_x[position] = mag_field->getValue(0).toDouble();
mag_data_y[position] = mag_field->getValue(1).toDouble();
mag_data_z[position] = mag_field->getValue(2).toDouble();
position = (position + 1) % 6;
}
//*****************************************************************
int LinearEquationsSolving(int nDim, double* pfMatr, double* pfVect, double* pfSolution)
{
double fMaxElem;
double fAcc;
int i , j, k, m;
for(k=0; k<(nDim-1); k++) // base row of matrix
{
// search of line with max element
fMaxElem = fabs( pfMatr[k*nDim + k] );
m = k;
for(i=k+1; i<nDim; i++)
{
if(fMaxElem < fabs(pfMatr[i*nDim + k]) )
{
fMaxElem = pfMatr[i*nDim + k];
m = i;
}
}
// permutation of base line (index k) and max element line(index m)
if(m != k)
{
for(i=k; i<nDim; i++)
{
fAcc = pfMatr[k*nDim + i];
pfMatr[k*nDim + i] = pfMatr[m*nDim + i];
pfMatr[m*nDim + i] = fAcc;
}
fAcc = pfVect[k];
pfVect[k] = pfVect[m];
pfVect[m] = fAcc;
}
if( pfMatr[k*nDim + k] == 0.) return 0; // needs improvement !!!
// triangulation of matrix with coefficients
for(j=(k+1); j<nDim; j++) // current row of matrix
{
fAcc = - pfMatr[j*nDim + k] / pfMatr[k*nDim + k];
for(i=k; i<nDim; i++)
{
pfMatr[j*nDim + i] = pfMatr[j*nDim + i] + fAcc*pfMatr[k*nDim + i];
}
pfVect[j] = pfVect[j] + fAcc*pfVect[k]; // free member recalculation
}
}
for(k=(nDim-1); k>=0; k--)
{
pfSolution[k] = pfVect[k];
for(i=(k+1); i<nDim; i++)
{
pfSolution[k] -= (pfMatr[k*nDim + i]*pfSolution[i]);
}
pfSolution[k] = pfSolution[k] / pfMatr[k*nDim + k];
}
return 1;
}
int SixPointInConstFieldCal( double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3] )
{
int i;
double A[5][5];
double f[5], c[5];
double xp, yp, zp, Sx;
// Fill in matrix A -
// write six difference-in-magnitude equations of the form
// Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0
// or in other words
// 2*Sx*bx*(x2-x1)/Sx^2 + Sy^2(y2^2-y1^2)/Sx^2 + 2*Sy*by*(y2-y1)/Sx^2 + Sz^2(z2^2-z1^2)/Sx^2 + 2*Sz*bz*(z2-z1)/Sx^2 = (x1^2-x2^2)
for (i=0;i<5;i++){
A[i][0] = 2.0 * (x[i+1] - x[i]);
A[i][1] = y[i+1]*y[i+1] - y[i]*y[i];
A[i][2] = 2.0 * (y[i+1] - y[i]);
A[i][3] = z[i+1]*z[i+1] - z[i]*z[i];
A[i][4] = 2.0 * (z[i+1] - z[i]);
f[i] = x[i]*x[i] - x[i+1]*x[i+1];
}
// solve for c0=bx/Sx, c1=Sy^2/Sx^2; c2=Sy*by/Sx^2, c3=Sz^2/Sx^2, c4=Sz*bz/Sx^2
if ( !LinearEquationsSolving( 5, (double *)A, f, c) ) return 0;
// use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer
xp = x[0]; yp = y[0]; zp = z[0];
Sx = sqrt(ConstMag*ConstMag / (xp*xp + 2*c[0]*xp + c[0]*c[0] + c[1]*yp*yp + 2*c[2]*yp + c[2]*c[2]/c[1] + c[3]*zp*zp + 2*c[4]*zp + c[4]*c[4]/c[3]));
S[0] = Sx;
b[0] = Sx*c[0];
S[1] = sqrt(c[1]*Sx*Sx);
b[1] = c[2]*Sx*Sx/S[1];
S[2] = sqrt(c[3]*Sx*Sx);
b[2] = c[4]*Sx*Sx/S[2];
return 1;
}
void ConfigAHRSWidget::computeScaleBias()
{
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration")));
UAVObjectField *field;
double S[3], b[3];
SixPointInConstFieldCal( 9.81, accel_data_x, accel_data_y, accel_data_z, S, b);
field = obj->getField(QString("accel_scale"));
field->setDouble(S[0],0);
field->setDouble(S[1],1);
field->setDouble(S[2],2); // Flip the Z-axis scale to be negative
field = obj->getField(QString("accel_bias"));
field->setDouble((int) b[0] / S[0],0);
field->setDouble((int) b[1] / S[1],1);
field->setDouble((int) -b[2] / S[2],2);
SixPointInConstFieldCal( 1, mag_data_x, mag_data_y, mag_data_z, S, b);
field = obj->getField(QString("mag_bias"));
field->setDouble(b[0] / S[0], 0);
field->setDouble(b[1] / S[1], 1);
field->setDouble(b[2] / S[2], 2);
obj->updated();
}
void ConfigAHRSWidget::calibrationMode()
{
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration")));
// set accels to unity gain
UAVObjectField *field = obj->getField(QString("accel_scale"));
field->setDouble(1,0);
field->setDouble(1,1);
field->setDouble(1,2);
field = obj->getField(QString("accel_bias"));
field->setDouble(0,0);
field->setDouble(0,1);
field->setDouble(0,2);
obj->updated();
obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings")));
field = obj->getField(QString("UpdateRaw"));
field->setValue("TRUE");
obj->updated();
}
/**
Draws the sensor variances bargraph
*/
@ -341,6 +512,9 @@ void ConfigAHRSWidget::ahrsSettingsRequest()
m_ahrs->algorithm->setCurrentIndex(m_ahrs->algorithm->findText(field->getValue().toString()));
drawVariancesGraph();
m_ahrs->ahrsCalibStart->setEnabled(true);
m_ahrs->ahrsSavePosition->setEnabled(true);
m_ahrs->ahrsComputeScaleBias->setEnabled(true);
m_ahrs->ahrsCalibrationMode->setEnabled(true);
m_ahrs->calibInstructions->setText(QString("Press \"Start\" above to calibrate."));
}

12
ground/src/plugins/config/configahrswidget.h
View File

@ -66,6 +66,13 @@ private:
const static double maxVarValue;
const static int calibrationDelay;
double accel_data_x[6];
double accel_data_y[6];
double accel_data_z[6];
double mag_data_x[6];
double mag_data_y[6];
double mag_data_z[6];
int position;
private slots:
void launchAHRSCalibration();
void saveAHRSCalibration();
@ -74,9 +81,12 @@ private slots:
void ahrsSettingsRequest();
void ahrsSettingsSaveRAM();
void ahrsSettingsSaveSD();
void savePositionData();
void computeScaleBias();
void calibrationMode();
protected:
void showEvent(QShowEvent *event);
void showEvent(QShowEvent *event);
};

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

@ -82,7 +82,7 @@ AHRSCalibration::AHRSCalibration(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTING
mag_biasElemNames.append("X");
mag_biasElemNames.append("Y");
mag_biasElemNames.append("Z");
fields.append( new UAVObjectField(QString("mag_bias"), QString("mGau"), UAVObjectField::FLOAT32, mag_biasElemNames, QStringList()) );
fields.append( new UAVObjectField(QString("mag_bias"), QString("mGau"), UAVObjectField::INT16, mag_biasElemNames, QStringList()) );
QStringList mag_varElemNames;
mag_varElemNames.append("X");
mag_varElemNames.append("Y");
@ -134,9 +134,9 @@ void AHRSCalibration::setDefaultFieldValues()
data.gyro_bias[0] = -1675;
data.gyro_bias[1] = -1675;
data.gyro_bias[2] = -1675;
data.gyro_scale[0] = 0.007;
data.gyro_scale[1] = 0.007;
data.gyro_scale[2] = 0.007;
data.gyro_scale[0] = -0.007;
data.gyro_scale[1] = -0.007;
data.gyro_scale[2] = -0.007;
data.gyro_var[0] = 0.0001;
data.gyro_var[1] = 0.0001;
data.gyro_var[2] = 0.0001;

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

@ -50,7 +50,7 @@ public:
qint16 gyro_bias[3];
float gyro_scale[3];
float gyro_var[3];
float mag_bias[3];
qint16 mag_bias[3];
float mag_var[3];
} __attribute__((packed)) DataFields;
@ -102,7 +102,7 @@ public:
// Constants
static const quint32 OBJID = 3497014596U;
static const quint32 OBJID = 2082766848U;
static const QString NAME;
static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 1;

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

@ -123,7 +123,7 @@ _fields = [ \
),
uavobject.UAVObjectField(
'mag_bias',
'f',
'h',
3,
[
'X',
@ -150,7 +150,7 @@ _fields = [ \
class AHRSCalibration(uavobject.UAVObject):
## Object constants
OBJID = 3497014596
OBJID = 2082766848
NAME = "AHRSCalibration"
METANAME = "AHRSCalibrationMeta"
ISSINGLEINST = 1

2
ground/src/plugins/uavobjects/uavobjectfield.cpp
View File

@ -545,6 +545,6 @@ double UAVObjectField::getDouble(quint32 index)
void UAVObjectField::setDouble(double value, quint32 index)
{
setValue(QVariant(value));
setValue(QVariant(value), index);
}

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

@ -1,18 +1,18 @@
<xml>
<object name="AHRSCalibration" singleinstance="true" settings="true">
<description>Contains the calibration settings for the @ref AHRSCommsModule</description>
<field name="measure_var" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="accel_bias" units="raw" type="int16" elementnames="X,Y,Z" defaultvalue="-2048"/>
<field name="accel_scale" units="m/s" type="float" elementnames="X,Y,Z" defaultvalue="0.012"/>
<field name="accel_var" units="m^2/s^s" type="float" elementnames="X,Y,Z" defaultvalue="5e-5"/>
<field name="gyro_bias" units="raw" type="int16" elementnames="X,Y,Z" defaultvalue="-1675"/>
<field name="gyro_scale" units="deg/s" type="float" elementnames="X,Y,Z" defaultvalue="0.0070"/>
<field name="gyro_var" units="deg^s/s^2" type="float" elementnames="X,Y,Z" defaultvalue="1e-4"/>
<field name="mag_bias" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="0"/>
<field name="mag_var" units="mGau^s" type="float" elementnames="X,Y,Z" defaultvalue="5e-5"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<object name="AHRSCalibration" singleinstance="true" settings="true">
<description>Contains the calibration settings for the @ref AHRSCommsModule</description>
<field name="measure_var" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="accel_bias" units="raw" type="int16" elementnames="X,Y,Z" defaultvalue="-2048"/>
<field name="accel_scale" units="m/s" type="float" elementnames="X,Y,Z" defaultvalue="0.012"/>
<field name="accel_var" units="m^2/s^s" type="float" elementnames="X,Y,Z" defaultvalue="5e-5"/>
<field name="gyro_bias" units="raw" type="int16" elementnames="X,Y,Z" defaultvalue="-1675"/>
<field name="gyro_scale" units="deg/s" type="float" elementnames="X,Y,Z" defaultvalue="-0.0070"/>
<field name="gyro_var" units="deg^s/s^2" type="float" elementnames="X,Y,Z" defaultvalue="1e-4"/>
<field name="mag_bias" units="mGau" type="int16" elementnames="X,Y,Z" defaultvalue="0"/>
<field name="mag_var" units="mGau^s" type="float" elementnames="X,Y,Z" defaultvalue="5e-5"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="never" period="0"/>
</object>
</object>
</xml>