From 8128c3bbfa84858fea711ed2e345a5a95b1a3228 Mon Sep 17 00:00:00 2001 From: peabody124 Date: Sat, 28 Aug 2010 18:48:51 +0000 Subject: [PATCH] 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. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1449 ebee16cc-31ac-478f-84a7-5cbb03baadba --- flight/AHRS/ahrs.c | 8 +- flight/OpenPilot/UAVObjects/ahrscalibration.c | 6 +- .../UAVObjects/inc/ahrscalibration.h | 4 +- .../OpenPilotOSX.xcodeproj/project.pbxproj | 2 + ground/src/plugins/config/ahrs.ui | 30 +-- .../src/plugins/config/configahrswidget.cpp | 178 +++++++++++++++++- ground/src/plugins/config/configahrswidget.h | 12 +- .../plugins/uavobjects/ahrscalibration.cpp | 8 +- .../src/plugins/uavobjects/ahrscalibration.h | 4 +- .../src/plugins/uavobjects/ahrscalibration.py | 4 +- .../src/plugins/uavobjects/uavobjectfield.cpp | 2 +- .../uavobjectdefinition/ahrscalibration.xml | 30 +-- 12 files changed, 237 insertions(+), 51 deletions(-) diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index 58f2af80d..2d5146336 100644 --- a/flight/AHRS/ahrs.c +++ b/flight/AHRS/ahrs.c @@ -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); diff --git a/flight/OpenPilot/UAVObjects/ahrscalibration.c b/flight/OpenPilot/UAVObjects/ahrscalibration.c index 5ca39cf38..178c9aadd 100644 --- a/flight/OpenPilot/UAVObjects/ahrscalibration.c +++ b/flight/OpenPilot/UAVObjects/ahrscalibration.c @@ -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; diff --git a/flight/OpenPilot/UAVObjects/inc/ahrscalibration.h b/flight/OpenPilot/UAVObjects/inc/ahrscalibration.h index 454adc7dc..fd0cdab39 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 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; diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index d5fd527d0..be11cc8e5 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -25,6 +25,7 @@ 65322D2F12283CCD0046CD7C /* NMEA.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = NMEA.c; sourceTree = ""; }; 65322D3012283CD60046CD7C /* NMEA.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = NMEA.h; sourceTree = ""; }; 65322D3B122841F60046CD7C /* gpstime.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = gpstime.xml; sourceTree = ""; }; + 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 = ""; }; 655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = ""; }; @@ -6707,6 +6708,7 @@ 65B7E6AC120DF1CD000C1123 /* AHRS */ = { isa = PBXGroup; children = ( + 65322D77122897210046CD7C /* MagOrAccelSensorCal.c */, 654330231218E9780063F913 /* insgps.c */, 65B7E6AD120DF1E2000C1123 /* ahrs_fsm.c */, 65B7E6AE120DF1E2000C1123 /* ahrs.c */, diff --git a/ground/src/plugins/config/ahrs.ui b/ground/src/plugins/config/ahrs.ui index 54fef10ec..17d5f17ae 100644 --- a/ground/src/plugins/config/ahrs.ui +++ b/ground/src/plugins/config/ahrs.ui @@ -13,7 +13,7 @@ Form - + 10 @@ -158,52 +158,52 @@ Takes about 30 seconds max. Calibration #2 - + false - 230 - 270 - 93 + 220 + 290 + 111 27 - PushButton + Save Position - + false - 230 + 220 320 - 93 + 111 27 - PushButton + Compute - + false - 230 - 370 - 93 + 220 + 260 + 111 27 - PushButton + Cal Mode diff --git a/ground/src/plugins/config/configahrswidget.cpp b/ground/src/plugins/config/configahrswidget.cpp index 31e5dbd93..06892daf6 100644 --- a/ground/src/plugins/config/configahrswidget.cpp +++ b/ground/src/plugins/config/configahrswidget.cpp @@ -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(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(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=0; k--) + { + pfSolution[k] = pfVect[k]; + for(i=(k+1); i(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(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(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.")); } diff --git a/ground/src/plugins/config/configahrswidget.h b/ground/src/plugins/config/configahrswidget.h index a65de1eae..67dbda026 100644 --- a/ground/src/plugins/config/configahrswidget.h +++ b/ground/src/plugins/config/configahrswidget.h @@ -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); }; diff --git a/ground/src/plugins/uavobjects/ahrscalibration.cpp b/ground/src/plugins/uavobjects/ahrscalibration.cpp index 522906d0c..000339a52 100644 --- a/ground/src/plugins/uavobjects/ahrscalibration.cpp +++ b/ground/src/plugins/uavobjects/ahrscalibration.cpp @@ -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; diff --git a/ground/src/plugins/uavobjects/ahrscalibration.h b/ground/src/plugins/uavobjects/ahrscalibration.h index 0304fffbd..ef8b740ef 100644 --- a/ground/src/plugins/uavobjects/ahrscalibration.h +++ b/ground/src/plugins/uavobjects/ahrscalibration.h @@ -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; diff --git a/ground/src/plugins/uavobjects/ahrscalibration.py b/ground/src/plugins/uavobjects/ahrscalibration.py index 9fe0f87dd..173dc52ab 100644 --- a/ground/src/plugins/uavobjects/ahrscalibration.py +++ b/ground/src/plugins/uavobjects/ahrscalibration.py @@ -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 diff --git a/ground/src/plugins/uavobjects/uavobjectfield.cpp b/ground/src/plugins/uavobjects/uavobjectfield.cpp index 49c854ca1..85c615f56 100644 --- a/ground/src/plugins/uavobjects/uavobjectfield.cpp +++ b/ground/src/plugins/uavobjects/uavobjectfield.cpp @@ -545,6 +545,6 @@ double UAVObjectField::getDouble(quint32 index) void UAVObjectField::setDouble(double value, quint32 index) { - setValue(QVariant(value)); + setValue(QVariant(value), index); } diff --git a/ground/src/shared/uavobjectdefinition/ahrscalibration.xml b/ground/src/shared/uavobjectdefinition/ahrscalibration.xml index 585524d1a..c8b2066a2 100644 --- a/ground/src/shared/uavobjectdefinition/ahrscalibration.xml +++ b/ground/src/shared/uavobjectdefinition/ahrscalibration.xml @@ -1,18 +1,18 @@ - - Contains the calibration settings for the @ref AHRSCommsModule - - - - - - - - - - - - + + Contains the calibration settings for the @ref AHRSCommsModule + + + + + + + + + + + + - + \ No newline at end of file