From 710f95feeb22d5003418f675da59dd9a683d3a43 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 24 Dec 2011 15:56:16 -0600 Subject: [PATCH] Get the 6pt calibration working again for revo --- .../src/plugins/config/configgadgetwidget.cpp | 1 + .../src/plugins/config/configrevowidget.cpp | 1072 +++++------------ .../src/plugins/config/configrevowidget.h | 23 +- .../src/plugins/config/revosensors.ui | 4 +- .../src/plugins/coreplugin/OpenPilotGCS.xml | 33 +- 5 files changed, 323 insertions(+), 810 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index f3bd7642f..aecf18072 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -36,6 +36,7 @@ #include "configcamerastabilizationwidget.h" #include "config_pro_hw_widget.h" #include "config_cc_hw_widget.h" +#include "configrevowidget.h" #include "defaultattitudewidget.h" #include "defaulthwsettingswidget.h" #include "uavobjectutilmanager.h" diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index 551515c14..13032f7cb 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -43,8 +43,8 @@ #include #include #include -#include +#define GRAVITY 9.81f #include "assertions.h" #include "calibration.h" @@ -65,13 +65,14 @@ public: // ***************** -ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : ConfigTaskWidget(parent) +ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : + ConfigTaskWidget(parent), + collectingData(false), + position(-1), + m_ui(new Ui_RevoSensorsWidget()) { - m_ui = new Ui_RevoWidget(); m_ui->setupUi(this); - collectingData = false; - // Initialization of the Paper plane widget m_ui->sixPointsHelp->setScene(new QGraphicsScene(this)); @@ -82,8 +83,7 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : ConfigTaskWidget(parent) m_ui->sixPointsHelp->scene()->addItem(paperplane); m_ui->sixPointsHelp->setSceneRect(paperplane->boundingRect()); - // Initialization of the AHRS bargraph graph - + // Initialization of the Revo sensor noise bargraph graph m_ui->ahrsBargraph->setScene(new QGraphicsScene(this)); QSvgRenderer *renderer = new QSvgRenderer(); @@ -203,36 +203,18 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : ConfigTaskWidget(parent) mag_z->setPos(startX,startY); mag_z->setTransform(QTransform::fromScale(1,0),true); - position = -1; - - /* // Fill the dropdown menus: - InsSettings * insSettings = InsSettings::GetInstance(getObjectManager()); - Q_ASSERT(insSettings); - UAVObjectField *field = insSettings->getField(QString("Algorithm")); - Q_ASSERT(field); - m_ui->algorithm->addItems(field->getOptions()); */ - - // Register for Home Location state changes - HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager()); - Q_ASSERT(homeLocation); - connect(homeLocation, SIGNAL(objectUpdated(UAVObject*)), this , SLOT(enableHomeLocSave(UAVObject*))); - - // Don't enable multi-point calibration until HomeLocation is set. - HomeLocation::DataFields homeLocationData = homeLocation->getData(); - m_ui->sixPointsStart->setEnabled(homeLocationData.Set == HomeLocation::SET_TRUE); - // Connect the signals connect(m_ui->ahrsCalibStart, SIGNAL(clicked()), this, SLOT(measureNoise())); connect(m_ui->accelBiasStart, SIGNAL(clicked()), this, SLOT(launchAccelBiasCalibration())); - connect(homeLocation, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshValues())); - connect(insSettings, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshValues())); + RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager()); + Q_ASSERT(revoCalibration); + connect(revoCalibration, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshValues())); - connect(m_ui->ahrsSettingsSaveRAM, SIGNAL(clicked()), this, SLOT(ahrsSettingsSaveRAM())); - connect(m_ui->ahrsSettingsSaveSD, SIGNAL(clicked()), this, SLOT(ahrsSettingsSaveSD())); - connect(m_ui->sixPointsStart, SIGNAL(clicked()), this, SLOT(multiPointCalibrationMode())); + connect(m_ui->ahrsSettingsSaveRAM, SIGNAL(clicked()), this, SLOT(SettingsToRam())); + connect(m_ui->ahrsSettingsSaveSD, SIGNAL(clicked()), this, SLOT(SettingsToFlash())); + connect(m_ui->sixPointsStart, SIGNAL(clicked()), this, SLOT(sixPointCalibrationMode())); connect(m_ui->sixPointsSave, SIGNAL(clicked()), this, SLOT(savePositionData())); - connect(m_ui->startDriftCalib, SIGNAL(clicked()),this, SLOT(launchGyroDriftCalibration())); // Leave this timer permanently connected. The timer itself is started and stopped. connect(&progressBarTimer, SIGNAL(timeout()), this, SLOT(incrementProgress())); @@ -287,29 +269,29 @@ void ConfigRevoWidget::launchAccelBiasCalibration() m_ui->accelBiasStart->setEnabled(false); m_ui->accelBiasProgress->setValue(0); - InsSettings * insSettings = InsSettings::GetInstance(getObjectManager()); - Q_ASSERT(insSettings); - InsSettings::DataFields insSettingsData = insSettings->getData(); - insSettingsData.BiasCorrectedRaw = InsSettings::BIASCORRECTEDRAW_FALSE; - insSettings->setData(insSettingsData); - insSettings->updated(); + RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager()); + Q_ASSERT(revoCalibration); + RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); + revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE; + revoCalibration->setData(revoCalibrationData); + revoCalibration->updated(); accel_accum_x.clear(); accel_accum_y.clear(); accel_accum_z.clear(); /* Need to get as many AttitudeRaw updates as possible */ - AttitudeRaw * attitudeRaw = AttitudeRaw::GetInstance(getObjectManager()); - Q_ASSERT(attitudeRaw); - initialMdata = attitudeRaw->getMetadata(); + Accels * accels = Accels::GetInstance(getObjectManager()); + Q_ASSERT(accels); + initialMdata = accels->getMetadata(); UAVObject::Metadata mdata = initialMdata; mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; mdata.flightTelemetryUpdatePeriod = 100; - attitudeRaw->setMetadata(mdata); + accels->setMetadata(mdata); - // Now connect to the attituderaw updates, gather for 100 samples + // Now connect to the accels and mag updates, gather for 100 samples collectingData = true; - connect(attitudeRaw, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelBiasattitudeRawUpdated(UAVObject*))); + connect(accels, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelBiasattitudeRawUpdated(UAVObject*))); } /** @@ -319,211 +301,40 @@ void ConfigRevoWidget::accelBiasattitudeRawUpdated(UAVObject *obj) { Q_UNUSED(obj); - AttitudeRaw * attitudeRaw = AttitudeRaw::GetInstance(getObjectManager()); - Q_ASSERT(attitudeRaw); - AttitudeRaw::DataFields attitudeRawData = attitudeRaw->getData(); + Accels * accels = Accels::GetInstance(getObjectManager()); + Q_ASSERT(accels); + Accels::DataFields accelsData = accels->getData(); // This is necessary to prevent a race condition on disconnect signal and another update if (collectingData == true) { - accel_accum_x.append(attitudeRawData.accels[InsSettings::ACCEL_BIAS_X]); - accel_accum_y.append(attitudeRawData.accels[InsSettings::ACCEL_BIAS_Y]); - accel_accum_z.append(attitudeRawData.accels[InsSettings::ACCEL_BIAS_Z]); + accel_accum_x.append(accelsData.x); + accel_accum_y.append(accelsData.y); + accel_accum_z.append(accelsData.z); } m_ui->accelBiasProgress->setValue(m_ui->accelBiasProgress->value()+1); if(accel_accum_x.size() >= 100 && collectingData == true) { collectingData = false; - disconnect(attitudeRaw,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(accelBiasattitudeRawUpdated(UAVObject*))); + disconnect(accels,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(accelBiasattitudeRawUpdated(UAVObject*))); m_ui->accelBiasStart->setEnabled(true); - InsSettings * insSettings = InsSettings::GetInstance(getObjectManager()); - Q_ASSERT(insSettings); - InsSettings::DataFields insSettingsData = insSettings->getData(); - insSettingsData.BiasCorrectedRaw = InsSettings::BIASCORRECTEDRAW_TRUE; + RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager()); + Q_ASSERT(revoCalibration); + RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); + revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE; - insSettingsData.accel_bias[InsSettings::ACCEL_BIAS_X] -= listMean(accel_accum_x); - insSettingsData.accel_bias[InsSettings::ACCEL_BIAS_Y] -= listMean(accel_accum_y); - insSettingsData.accel_bias[InsSettings::ACCEL_BIAS_Z] -= 9.81 + listMean(accel_accum_z); + revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] -= listMean(accel_accum_x); + revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] -= listMean(accel_accum_y); + revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] -= GRAVITY + listMean(accel_accum_z); - insSettings->setData(insSettingsData); - insSettings->updated(); + revoCalibration->setData(revoCalibrationData); + revoCalibration->updated(); - attitudeRaw->setMetadata(initialMdata); - - saveAHRSCalibration(); + accels->setMetadata(initialMdata); } } - -/** - Starts a Gyro temperature drift calibration. - */ -void ConfigRevoWidget::launchGyroDriftCalibration() -{ - if (!collectingData) { - // m_ui->startDriftCalib->setEnabled(false); - m_ui->startDriftCalib->setText("Stop"); - m_ui->accelBiasStart->setEnabled(false); - m_ui->ahrsCalibStart->setEnabled(false); - m_ui->sixPointsStart->setEnabled(false); - - // Setup the AHRS to give us the right data at the right rate: - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("AHRSSettings"))); - UAVObjectField* field = obj->getField(QString("BiasCorrectedRaw")); - field->setValue("FALSE"); - obj->updated(); - - /* Need to get as many AttitudeRaw updates as possible */ - obj = dynamic_cast(getObjectManager()->getObject(QString("AttitudeRaw"))); - initialMdata = obj->getMetadata(); - UAVObject::Metadata mdata = initialMdata; - mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; - mdata.flightTelemetryUpdatePeriod = 100; - obj->setMetadata(mdata); - - // Now connect to the attituderaw updates until we stop - collectingData = true; - obj = dynamic_cast(getObjectManager()->getObject(QString("BaroAltitude"))); - field = obj->getField(QString("Temperature")); - double temp = field->getValue().toDouble(); - m_ui->gyroTempSlider->setRange(temp*10,temp*10); - m_ui->gyroMaxTemp->setText(QString::number(temp,'g',3)); - m_ui->gyroMinTemp->setText(QString::number(temp,'g',3)); - - connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(driftCalibrationAttitudeRawUpdated(UAVObject*))); - } else { - // Stop all the gathering: - collectingData = false; - m_ui->startDriftCalib->setText("Start"); - m_ui->accelBiasStart->setEnabled(true); - m_ui->ahrsCalibStart->setEnabled(true); - m_ui->sixPointsStart->setEnabled(true); - - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("AttitudeRaw"))); - disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(driftCalibrationAttitudeRawUpdated(UAVObject*))); - - getObjectManager()->getObject(QString("AttitudeRaw"))->setMetadata(initialMdata); - - obj = dynamic_cast(getObjectManager()->getObject(QString("AHRSSettings"))); - UAVObjectField* field = obj->getField(QString("BiasCorrectedRaw")); - field->setValue("TRUE"); - obj->updated(); - - // TODO: Now compute the drift here - computeGyroDrift(); - - } -} - -/** - Updates the gyro drift calibration values in real time - */ -void ConfigRevoWidget::driftCalibrationAttitudeRawUpdated(UAVObject* obj) { - - Q_UNUSED(obj) - // This is necessary to prevent a race condition on disconnect signal and another update - if (collectingData == true) { - /** - First of all, update the temperature user feedback - This is not what we will use for our calculations, but it it easier for the - user to have the real temperature rather than an obscure unit... - */ - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("BaroAltitude"))); - UAVObjectField *tempField = obj->getField(QString("Temperature")); - Q_ASSERT(tempField != 0); - double mbTemp = tempField->getValue().toDouble(); - if (mbTemp*10 < m_ui->gyroTempSlider->minimum()) { - m_ui->gyroTempSlider->setMinimum(mbTemp*10); - m_ui->gyroMinTemp->setText(QString::number(mbTemp,'g',3)); - } else if (mbTemp*10 > m_ui->gyroTempSlider->maximum()) { - m_ui->gyroTempSlider->setMaximum(mbTemp*10); - m_ui->gyroMaxTemp->setText(QString::number(mbTemp,'g',3)); - } - m_ui->gyroTempSlider->setValue(mbTemp*10); - // TODO: - // - Add an indicator to show that we have a significant - // temperature difference in our gathered data (red/yellow/green) - - /** - Now, append gyro values + gyro temp data into our buffers - */ - // TODO: - // - choose a storage type for this data - // - Check it's not getting too big - // - do the actual appending - // - That's it, really... - - - } -} - -/** - Computes gyro drift based on sampled data - */ -void ConfigRevoWidget::computeGyroDrift() { - // TODO - - // TODO: if this is not too computing-intensive, we could consider - // calling this with a timer when data sampling is enabled, to get - // a real-time view of the computed drift convergence and let the - // user stop sampling when it becomes stable enough... - // - // Hint for whoever wants to implement that: - // The formula I use for computing the temperature compensation factor from - // two nicely filtered (downsampled) sample points is as follows: - // - // gyro_tempcompfactor == -(raw_gyro1 - raw_gyro2)/(gyro_temp1 - gyro_temp2) - // - // where raw_gyro1 and raw_gyro2 are gyroscope raw measurement values and - // gyro_temp1 and gyro_temp2 are the measurements from the gyroscope internal - // temperature sensors, each at two measure points T1 and T2 - // note that the X and Y gyroscopes share one temperature sensor while - // Z has its own. - // - // the formula that calculates the AttitudeRav.gyros[X,Y,Z] values is - // currently as follows: - // - // gyro = 180/Pi * ( ( ( raw_gyro + raw_gyro * gyro_tempcompfactor ) * gyro_scale) + gyro_bias ) - // - // so to get gyro_raw do the following: - // 1. set AHRSSettings.BiasCorrectedRaw to FALSE before measuring! (already done right now) - // 2. set AHRSCalibration.gyro_tempcompfactor to 0 before measuring! - // 3. gyro_raw = ( ( gyro * Pi / 180 ) - gyro_bias ) / gyro_scale - // - // a nice trick is to set gyro_bias to 0 and gyro_scale to (Pi / 180) in which case gyro = raw_gyro - // note that Pi/180 is very close to the "real" scale of the AHRS gyros anyway (though with switched signs) - -} - -/** - Launches the INS sensors noise measurements - */ -void ConfigRevoWidget::measureNoise() -{ - if(collectingData) { - QErrorMessage err(this); - err.showMessage("Cannot start noise measurement as data already being gathered"); - err.exec(); - return; - } - m_ui->calibInstructions->setText("Estimating sensor variance..."); - m_ui->ahrsCalibStart->setEnabled(false); - m_ui->calibProgress->setValue(0); - - InsSettings * insSettings = InsSettings::GetInstance(getObjectManager()); - InsSettings::DataFields insSettingsData = insSettings->getData(); - algorithm = insSettingsData.Algorithm; - insSettingsData.Algorithm = InsSettings::ALGORITHM_CALIBRATION; - insSettings->setData(insSettingsData); - insSettings->updated(); - collectingData = true; - - connect(insSettings,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(noiseMeasured())); - m_ui->calibProgress->setRange(0,calibrationDelay*10); - progressBarTimer.start(100); -} - /** Increment progress bar for noise measurements (not really based on feedback) */ @@ -533,9 +344,9 @@ void ConfigRevoWidget::incrementProgress() if (m_ui->calibProgress->value() >= m_ui->calibProgress->maximum()) { progressBarTimer.stop(); - InsSettings * insSettings = InsSettings::GetInstance(getObjectManager()); - Q_ASSERT(insSettings); - disconnect(insSettings, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(noiseMeasured())); + RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager()); + Q_ASSERT(revoCalibration); + disconnect(revoCalibration, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(noiseMeasured())); collectingData = false; QErrorMessage err(this); @@ -544,137 +355,83 @@ void ConfigRevoWidget::incrementProgress() } } -/** - *@brief Callback once calibration is done on the board. Restores the original algorithm. - */ -void ConfigRevoWidget::noiseMeasured() -{ - Q_ASSERT(collectingData); // Let's catch any race conditions - - // Do all the clean stopping stuff - InsSettings * insSettings = InsSettings::GetInstance(getObjectManager()); - Q_ASSERT(insSettings); - disconnect(insSettings, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(noiseMeasured())); - collectingData = false; - progressBarTimer.stop(); - m_ui->calibProgress->setValue(m_ui->calibProgress->maximum()); - - InsSettings::DataFields insSettingsData = insSettings->getData(); - insSettingsData.Algorithm = algorithm; - insSettings->setData(insSettingsData); - insSettings->updated(); - - m_ui->calibInstructions->setText(QString("Calibration complete.")); - m_ui->ahrsCalibStart->setEnabled(true); -} - -/** - Saves the AHRS sensors calibration (to RAM and SD) - */ -void ConfigRevoWidget::saveAHRSCalibration() -{ - InsSettings * insSettings = InsSettings::GetInstance(getObjectManager()); - saveObjectToSD(insSettings); -} - -FORCE_ALIGN_FUNC -void ConfigRevoWidget::attitudeRawUpdated(UAVObject * obj) +void ConfigRevoWidget::sensorsUpdated(UAVObject * obj) { QMutexLocker lock(&attitudeRawUpdateLock); - UAVObjectField *accel_field = obj->getField(QString("accels")); - UAVObjectField *gyro_field = obj->getField(QString("gyros")); - UAVObjectField *mag_field = obj->getField(QString("magnetometers")); - - Q_ASSERT(gyro_field != 0 && accel_field != 0 && mag_field != 0); - + qDebug() << "Data"; // This is necessary to prevent a race condition on disconnect signal and another update if (collectingData == true) { - accel_accum_x.append(accel_field->getValue(0).toDouble()); - accel_accum_y.append(accel_field->getValue(1).toDouble()); - accel_accum_z.append(accel_field->getValue(2).toDouble()); - // Note gyros actually (-y,-x,-z) but since we consistent here no prob - mag_accum_x.append(mag_field->getValue(0).toDouble()); - mag_accum_y.append(mag_field->getValue(1).toDouble()); - mag_accum_z.append(mag_field->getValue(2).toDouble()); - - gyro_accum_x.append(gyro_field->getValue(0).toDouble()); - gyro_accum_y.append(gyro_field->getValue(1).toDouble()); - gyro_accum_z.append(gyro_field->getValue(2).toDouble()); + qDebug() << "Collecting"; + if( obj->getObjID() == Accels::OBJID ) { + qDebug() << "Accels"; + Accels * accels = Accels::GetInstance(getObjectManager()); + Q_ASSERT(accels); + Accels::DataFields accelsData = accels->getData(); + accel_accum_x.append(accelsData.x); + accel_accum_y.append(accelsData.y); + accel_accum_z.append(accelsData.z); + } else if( obj->getObjID() == Magnetometer::OBJID ) { + qDebug() << "Mag"; + Magnetometer * mag = Magnetometer::GetInstance(getObjectManager()); + Q_ASSERT(mag); + Magnetometer::DataFields magData = mag->getData(); + mag_accum_x.append(magData.x); + mag_accum_y.append(magData.y); + mag_accum_z.append(magData.z); + } else { + Q_ASSERT(0); + } } - if(accel_accum_x.size() >= 8 && collectingData == true) { + if(accel_accum_x.size() >= 20 && mag_accum_x.size() >= 20 && collectingData == true) { collectingData = false; - disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(attitudeRawUpdated(UAVObject*))); + + Accels * accels = Accels::GetInstance(getObjectManager()); + Q_ASSERT(accels); + Magnetometer * mag = Magnetometer::GetInstance(getObjectManager()); + Q_ASSERT(mag); + disconnect(accels,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); + disconnect(mag,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(sensorsUpdated(UAVObject*))); + m_ui->sixPointsSave->setEnabled(true); - accel_data[position] << listMean(accel_accum_x), - listMean(accel_accum_y), - listMean(accel_accum_z); + accel_data_x[position] = listMean(accel_accum_x); + accel_data_y[position] = listMean(accel_accum_y); + accel_data_z[position] = listMean(accel_accum_z); + mag_data_x[position] = listMean(mag_accum_x); + mag_data_y[position] = listMean(mag_accum_y); + mag_data_z[position] = listMean(mag_accum_z); - mag_data[position] << listMean(mag_accum_x), - listMean(mag_accum_y), - listMean(mag_accum_z); - - gyro_data[position] << listMean(gyro_accum_x), - listMean(gyro_accum_y), - listMean(gyro_accum_z); - - - std::cout << "observed accel: " << accel_data[position].transpose() - << "\nobserved mag: " << mag_data[position].transpose() - << "\nobserved gyro: " << gyro_data[position].transpose() - << std::endl; - - struct { - const char* instructions; - const char* display; - } instructions[] = { - { "Pitch up 45 deg and click save position...", "plane-horizontal" }, - { "Pitch down 45 deg and click save position...", "plane-horizontal" }, - { "Roll left 45 deg and click save position...", "plane-left" }, - { "Roll right 45 deg and click save position...", "plane-left" }, - - { "Turn left 90 deg to 09:00 position and click save position...", "plane-horizontal" }, - { "Pitch up 45 deg and click save position...", "plane-horizontal" }, - { "Pitch down 45 deg and click save position...", "plane-horizontal" }, - { "Roll left 45 deg and click save position...", "plane-left" }, - { "Roll right 45 deg and click save position...", "plane-left" }, - - { "Turn left 90 deg to 06:00 position and click save position...", "plane-horizontal" }, - { "Pitch up 45 deg and click save position...", "plane-horizontal" }, - { "Pitch down 45 deg and click save position...", "plane-horizontal" }, - { "Roll left 45 deg and click save position...", "plane-left" }, - { "Roll right 45 deg and click save position...", "plane-left" }, - - { "Turn left 90 deg to 03:00 position and click save position...", "plane-horizontal" }, - { "Pitch up 45 deg and click save position...", "plane-horizontal" }, - { "Pitch down 45 deg and click save position...", "plane-horizontal" }, - { "Roll left 45 deg and click save position...", "plane-left" }, - { "Roll right 45 deg and click save position...", "plane-left" }, - - { "Place with nose vertically up and click save position...", "plane-up" }, - { "Place with nose straight down and click save position...", "plane-down" }, - { "Place upside down and click save position...", "plane-flip" }, - }; - - n_positions = sizeof(instructions) / sizeof(instructions[0]); - position = (position + 1) % n_positions; - - if (position != 0 && position < n_positions) { - - m_ui->sixPointCalibInstructions->append(instructions[position-1].instructions); - displayPlane(instructions[position-1].display); + position = (position + 1) % 6; + if(position == 1) { + m_ui->sixPointCalibInstructions->append("Place with left side down and click save position..."); + displayPlane("plane-left"); } - else if(position == 0) { - position = n_positions; + if(position == 2) { + m_ui->sixPointCalibInstructions->append("Place upside down and click save position..."); + displayPlane("plane-flip"); + } + if(position == 3) { + m_ui->sixPointCalibInstructions->append("Place with right side down and click save position..."); + displayPlane("plane-right"); + } + if(position == 4) { + m_ui->sixPointCalibInstructions->append("Place with nose up and click save position..."); + displayPlane("plane-up"); + } + if(position == 5) { + m_ui->sixPointCalibInstructions->append("Place with nose down and click save position..."); + displayPlane("plane-down"); + } + if(position == 0) { computeScaleBias(); m_ui->sixPointsStart->setEnabled(true); m_ui->sixPointsSave->setEnabled(false); - saveAHRSCalibration(); // Saves the result to SD. /* Cleanup original settings */ - getObjectManager()->getObject(QString("AttitudeRaw"))->setMetadata(initialMdata); + accels->setMetadata(initialMdata); + mag->setMetadata(initialMdata); } } } @@ -698,395 +455,221 @@ void ConfigRevoWidget::savePositionData() gyro_accum_z.clear(); collectingData = true; - UAVObject *obj = dynamic_cast(getObjectManager()->getObject(QString("AttitudeRaw"))); - connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(attitudeRawUpdated(UAVObject*))); + + Accels * accels = Accels::GetInstance(getObjectManager()); + Q_ASSERT(accels); + Magnetometer * mag = Magnetometer::GetInstance(getObjectManager()); + Q_ASSERT(mag); + + connect(accels, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(sensorsUpdated(UAVObject*))); + connect(mag, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(sensorsUpdated(UAVObject*))); m_ui->sixPointCalibInstructions->append("Hold..."); } -//***************************************************************** -namespace { - -/* - * Calibrated scale factors should be real values with scale factor less than 10% from nominal - */ -bool checkScaleFactors(const Vector3f& scalars) +int LinearEquationsSolving(int nDim, double* pfMatr, double* pfVect, double* pfSolution) { - return isReal(scalars) && - scalars.cwise().abs().maxCoeff() < 1.10f; + 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); igetDouble(0), - tuple->getDouble(1), - tuple->getDouble(2)).finished(); -} - -/** - * Convert a 3-vector to a 3-tuple - * @param v A 3-vector - * @param tuple[in] Assign the elements of this three-tuple to the elements of v - */ -void -vectorToTuple(UAVObjectField *tuple, const Vector3f& v) -{ - for (int i = 0; i < 3; ++i) { - tuple->setDouble(v(i), i); - } -} - -/** - * Updates the offsets for a calibrated gyro field. - * @param scale[in] Non-null pointer to a 3-element scale factor field. - * @param bias[out] Non-null pointer to a 3-element bias field. - * @param updateBias the source bias matrix. - */ -void -updateBias(UAVObjectField *scale, - UAVObjectField *bias , - const Vector3f& updateBias) -{ - Vector3f scale_factor = (Vector3f() << scale->getDouble(0), - scale->getDouble(1), - scale->getDouble(2)).finished(); - Vector3f old_bias = (Vector3f() << bias->getDouble(0), - bias->getDouble(1), - bias->getDouble(2)).finished(); - - // Convert to radians/second - Vector3f final_bias = -(M_PI)/180.0f * updateBias + old_bias; - - bias->setDouble(final_bias(0), 0); - bias->setDouble(final_bias(1), 1); - bias->setDouble(final_bias(2), 2); -} - -void -updateRotation(UAVObjectField *rotation, const Vector3f& updateRotation) -{ - for (int i = 0; i < 3; ++i) { - rotation->setDouble(updateRotation[i], i); - } -} - -} // !namespace (anon) - - -/** - * Updates the scale factors and offsets for a calibrated vector field. - * @param scale[out] Non-null pointer to a 3-element scale factor field. - * @param bias[out] Non-null pointer to a 3-element bias field. - * @param ortho[out] Optional pointer to a 3-element orthogonal correction field - * @param updateScale the source scale factor matrix. - * @param updateBias the source bias matrix. - * @param oldScale The original sensor scale factor - * @param oldBias The original bias value - * @param oldOrtho Optional. The original orthogonality scale factor value. - * @return true if successful, false otherwise. - */ -bool -ConfigRevoWidget::updateScaleFactors(UAVObjectField *scale, - UAVObjectField *bias , - UAVObjectField *ortho, - const Matrix3f& updateScale, - const Vector3f& updateBias, - const Vector3f& oldScale, - const Vector3f& oldBias, - const Vector3f& oldOrtho) -{ - // Compose a 4x4 affine transformation matrix composed of the scale factor, - // orthogonality correction, and bias. - Matrix4f calibration; - calibration << tupleToVector(scale).asDiagonal(), - tupleToVector(bias), - Vector4f::UnitW().transpose(); - - if (ortho) { - Vector3f orthof = tupleToVector(ortho); - calibration(1, 0) = calibration(0, 1) = orthof(0); - calibration(2, 0) = calibration(0, 2) = orthof(1); - calibration(1, 2) = calibration(2, 1) = orthof(2); - } - - std::cout << "old calibration matrix: \n" << calibration << "\n"; - - Matrix4f update; - update << updateScale, updateBias, Vector4f::UnitW().transpose(); - std::cout << "new calibration matrix update: \n" << update << "\n"; - - calibration = update * calibration; - - if (checkOffsets(updateBias) && checkScaleFactors(updateScale.diagonal())) { - // Apply the new calibration - vectorToTuple(scale, calibration.diagonal().start<3>()); - vectorToTuple(bias, calibration.col(3).start<3>()); - - if (ortho) { - ortho->setDouble(calibration(0, 1), 0); - ortho->setDouble(calibration(0, 2), 1); - ortho->setDouble(calibration(1, 2), 2); - } - return true; - } - else { - // Give the user the calibration data and restore their settings. - std::ostringstream msg; - msg << "Scale factors and/or offsets are out of range.\n"; - msg << "Please see the troubleshooting section of the manual and retry.\n\n" - "The following values were computed:\n"; - msg << qPrintable(scale->getName()) << ": " - << calibration.diagonal().start<3>().transpose() << "\n"; - vectorToTuple(scale, oldScale); - - if (ortho) { - msg << qPrintable(ortho->getName()) << ": " - << calibration(0,1) << ", " << calibration(0,2) << ", " << calibration(1,2) << "\n"; - vectorToTuple(ortho, oldOrtho); - } - - msg << qPrintable(bias->getName()) << ": " - << calibration.col(3).start<3>().transpose() << "\n"; - vectorToTuple(bias, oldBias); - - m_ui->sixPointCalibInstructions->append(msg.str().c_str()); - return false; - } -} - -FORCE_ALIGN_FUNC void ConfigRevoWidget::computeScaleBias() { - // Extract the local magnetic and gravitational field vectors from HomeLocation. - UAVObject *home = dynamic_cast(getObjectManager()->getObject(QString("HomeLocation"))); - Vector3f localMagField; - localMagField << home->getField("Be")->getValue(0).toDouble(), - home->getField("Be")->getValue(1).toDouble(), - home->getField("Be")->getValue(2).toDouble(); + double S[3], b[3]; + RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager()); + Q_ASSERT(revoCalibration); + RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); - float localGravity = home->getField("g_e")->getDouble(); + // Calibration accel + SixPointInConstFieldCal( GRAVITY, accel_data_x, accel_data_y, accel_data_z, S, b); + revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X] = fabs(S[0]); + revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y] = fabs(S[1]); + revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z] = fabs(S[2]); - Vector3f referenceField = Vector3f::UnitZ()*localGravity; - double noise = 0.04; - Vector3f accelBias; - Matrix3f accelScale; - std::cout << "number of samples: " << n_positions << "\n"; - twostep_bias_scale(accelBias, accelScale, accel_data, n_positions, referenceField, noise*noise); - // Twostep computes an offset from the identity scalar, and a negative bias offset - accelScale += Matrix3f::Identity(); - accelBias = -accelBias; - std::cout << "computed accel bias: " << accelBias.transpose() - << "\ncomputed accel scale:\n" << accelScale<< std::endl; + revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] = sign(S[0]) * b[0]; + revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] = sign(S[1]) * b[1]; + revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] = sign(S[2]) * b[2]; - // Apply the computed scale factor and bias to each sample - for (int i = 0; i < n_positions; ++i) { - accel_data[i] = accelScale * accel_data[i] + accelBias; - } + // Calibration mag + SixPointInConstFieldCal( 1000, mag_data_x, mag_data_y, mag_data_z, S, b); + revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] = fabs(S[0]); + revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] = fabs(S[1]); + revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] = fabs(S[2]); - // Magnetometer has excellent orthogonality, so only calibrate the scale factors. - Vector3f magBias; - Vector3f magScale; - noise = 4.0; - twostep_bias_scale(magBias, magScale, mag_data, n_positions, localMagField, noise*noise); - magScale += Vector3f::Ones(); - magBias = -magBias; - std::cout << "computed mag bias: " << magBias.transpose() - << "\ncomputed mag scale:\n" << magScale << std::endl; + revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] = sign(S[0]) * b[0]; + revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = sign(S[1]) * b[1]; + revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = sign(S[2]) * b[2]; - // Apply the computed scale factor and bias to each sample - for (int i = 0; i < n_positions; ++i) { - mag_data[i] = magScale.asDiagonal() * mag_data[i] + magBias; - } + revoCalibration->setData(revoCalibrationData); - // Calibrate gyro bias and acceleration sensitivity - Matrix3f accelSensitivity; - Vector3f gyroBias; - gyroscope_calibration(gyroBias, accelSensitivity, gyro_data, accel_data, n_positions); - std::cout << "gyro bias: " << gyroBias.transpose() - << "\ngyro's acceleration sensitivity:\n" << accelSensitivity << std::endl; - - // Calibrate alignment between the accelerometer and magnetometer, taking the mag as the - // reference. - Vector3f accelRotation; - calibration_misalignment(accelRotation, accel_data, -Vector3f::UnitZ()*localGravity, - mag_data, localMagField, n_positions); - std::cout << "magnetometer rotation vector: " << accelRotation.transpose() << std::endl; - - // Update the calibration scalars with a clear message box - m_ui->sixPointCalibInstructions->clear(); - UAVObject *obj = dynamic_cast(getObjectManager()->getObject(QString("AHRSCalibration"))); - - - bool success = updateScaleFactors(obj->getField(QString("accel_scale")), - obj->getField(QString("accel_bias")), - obj->getField(QString("accel_ortho")), - accelScale, - accelBias, - saved_accel_scale, - saved_accel_bias, - saved_accel_ortho); - - success &= updateScaleFactors(obj->getField(QString("mag_scale")), - obj->getField(QString("mag_bias")), - NULL, - magScale.asDiagonal(), - magBias, - saved_mag_scale, - saved_mag_bias); - - updateBias(obj->getField(QString("gyro_scale")), - obj->getField(QString("gyro_bias")), - gyroBias); - -#if 0 - // TODO: Enable after v1.0 feature freeze is lifted. - updateRotation(obj->getField(QString("accel_rotation")), accelRotation); -#endif - - obj->updated(); - - position = -1; //set to run again - if (success) - m_ui->sixPointCalibInstructions->append("Computed new accel and mag scale and bias."); + position = -1; //set to run again + m_ui->sixPointCalibInstructions->append("Computed accel and mag scale and bias..."); } /** - Multi-point calibration mode + Six point calibration mode */ -FORCE_ALIGN_FUNC -void ConfigRevoWidget::multiPointCalibrationMode() +void ConfigRevoWidget::sixPointCalibrationMode() { - cacheCurrentCalibration(); - resetCalibrationDefaults(); + double S[3], b[3]; + RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager()); + Q_ASSERT(revoCalibration); + RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); - UAVObject* obj = dynamic_cast(getObjectManager()->getObject(QString("AHRSSettings"))); - UAVObjectField *field = obj->getField(QString("BiasCorrectedRaw")); - field->setValue("FALSE"); - obj->updated(); + // Calibration accel + revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X] = 1; + revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y] = 1; + revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z] = 1; + revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] = 0; + revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] = 0; + revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] = 0; + // Calibration mag + revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] = 1; + revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] = 1; + revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] = 1; + revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] = 0; + revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = 0; + revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = 0; - Thread::usleep(100000); + revoCalibration->setData(revoCalibrationData); - gyro_accum_x.clear(); - gyro_accum_y.clear(); - gyro_accum_z.clear(); + usleep(100000); - /* Need to get as many AttitudeRaw updates as possible */ - obj = getObjectManager()->getObject(QString("AttitudeRaw")); - initialMdata = obj->getMetadata(); - UAVObject::Metadata mdata = initialMdata; - mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; - mdata.flightTelemetryUpdatePeriod = 100; - obj->setMetadata(mdata); + gyro_accum_x.clear(); + gyro_accum_y.clear(); + gyro_accum_z.clear(); + accel_accum_x.clear(); + accel_accum_y.clear(); + accel_accum_z.clear(); + mag_accum_x.clear(); + mag_accum_y.clear(); + mag_accum_z.clear(); - /* Show instructions and enable controls */ - m_ui->sixPointCalibInstructions->clear(); - m_ui->sixPointCalibInstructions->append("Stand facing Earth's magnetic N or S. Place the vehicle horizontally facing forward and click save position..."); - displayPlane("plane-horizontal"); - m_ui->sixPointsStart->setEnabled(false); - m_ui->sixPointsSave->setEnabled(true); - position = 0; + /* Need to get as many accel and mag updates as possible */ + Accels * accels = Accels::GetInstance(getObjectManager()); + Q_ASSERT(accels); + Magnetometer * mag = Magnetometer::GetInstance(getObjectManager()); + Q_ASSERT(mag); + initialMdata = accels->getMetadata(); + UAVObject::Metadata mdata = initialMdata; + mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; + mdata.flightTelemetryUpdatePeriod = 100; + accels->setMetadata(mdata); + + mdata = mag->getMetadata(); + mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; + mdata.flightTelemetryUpdatePeriod = 100; + mag->setMetadata(mdata); + + /* Show instructions and enable controls */ + m_ui->sixPointCalibInstructions->clear(); + m_ui->sixPointCalibInstructions->append("Place horizontally and click save position..."); + displayPlane("plane-horizontal"); + m_ui->sixPointsStart->setEnabled(false); + m_ui->sixPointsSave->setEnabled(true); + position = 0; + qDebug() << "Starting"; } -/** - * Read the current calibration scalars and offsets from the target board, and - * save them for later use. In the event of a calibration failure, if the - * calibration method began by resetting calibration values, they may be - * restored later with this information. - */ -void ConfigRevoWidget::cacheCurrentCalibration() -{ - UAVObject *obj = dynamic_cast(getObjectManager()->getObject(QString("AHRSCalibration"))); - struct field_t { - const char* field_name; - Vector3f& cache; - } fields[] = { - { "accel_scale", this->saved_accel_scale }, - { "accel_bias", this->saved_accel_bias }, - { "accel_ortho", this->saved_accel_ortho }, - // TODO: Enable after V1.0 feature freeze is lifted. - // { "accel_rotation", this->saved_accel_rotation }, - { "gyro_bias", this->saved_gyro_bias }, - { "mag_scale", this->saved_mag_scale }, - { "mag_bias", this->saved_mag_bias }, - { NULL, this->saved_mag_bias }, // sentinnel -}; - for (field_t* i = fields; i->field_name != NULL; ++i) { - UAVObjectField* field = obj->getField(QString(i->field_name)); - if (field) { - i->cache = tupleToVector(field); - } - else { - qDebug() << "WARNING: AHRSCalibration field not found: " << i->field_name << "\n"; - } - } -} -/** - * Reset all calibration scalars to their default values. - */ -void ConfigRevoWidget::resetCalibrationDefaults() -{ - UAVObject *obj = dynamic_cast(getObjectManager()->getObject(QString("AHRSCalibration"))); - - // set accels to unity gain - UAVObjectField *field = obj->getField(QString("accel_scale")); - // TODO: Figure out how to load these directly from the saved metadata - // about default values - field->setDouble(0.0359, 0); - field->setDouble(0.0359, 1); - field->setDouble(0.0359, 2); - - field = obj->getField(QString("accel_bias")); - field->setDouble(-73.5, 0); - field->setDouble(-73.5, 1); - field->setDouble(73.5, 2); - - field = obj->getField(QString("accel_ortho")); - for (int i = 0; i < 3; ++i) { - field->setDouble(0, i); - } - - field = obj->getField(QString("gyro_bias")); - UAVObjectField *field2 = obj->getField(QString("gyro_scale")); - field->setDouble(28/-0.017*field2->getDouble(0),0); - field->setDouble(-28/0.017*field2->getDouble(1),1); - field->setDouble(28/-0.017*field2->getDouble(2),2); - - field = obj->getField(QString("mag_scale")); - for (int i = 0; i < 3; ++i) { - field->setDouble(-1, i); - } - - field = obj->getField(QString("mag_bias")); - for (int i = 0; i < 3; ++i) { - field->setDouble(0, i); - } - -#if 0 - // TODO: Enable after v1.0 feature freeze is lifted. - field = obj->getField(QString("accel_rotation")); - for (int i = 0; i < 3; ++i) { - field->setDouble(0, i); - } -#endif - obj->updated(); -} /** Rotate the paper plane @@ -1105,32 +688,32 @@ void ConfigRevoWidget::displayPlane(QString elementID) */ void ConfigRevoWidget::drawVariancesGraph() { - InsSettings * insSettings = InsSettings::GetInstance(getObjectManager()); - Q_ASSERT(insSettings); - InsSettings::DataFields insSettingsData = insSettings->getData(); + RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager()); + Q_ASSERT(revoCalibration); + RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); // The expected range is from 1E-6 to 1E-1 double steps = 6; // 6 bars on the graph - float accel_x_var = -1/steps*(1+steps+log10(insSettingsData.accel_var[InsSettings::ACCEL_VAR_X])); + float accel_x_var = -1/steps*(1+steps+log10(revoCalibrationData.accel_var[RevoCalibration::ACCEL_VAR_X])); accel_x->setTransform(QTransform::fromScale(1,accel_x_var),false); - float accel_y_var = -1/steps*(1+steps+log10(insSettingsData.accel_var[InsSettings::ACCEL_VAR_Y])); + float accel_y_var = -1/steps*(1+steps+log10(revoCalibrationData.accel_var[RevoCalibration::ACCEL_VAR_Y])); accel_y->setTransform(QTransform::fromScale(1,accel_y_var),false); - float accel_z_var = -1/steps*(1+steps+log10(insSettingsData.accel_var[InsSettings::ACCEL_VAR_Z])); + float accel_z_var = -1/steps*(1+steps+log10(revoCalibrationData.accel_var[RevoCalibration::ACCEL_VAR_Z])); accel_z->setTransform(QTransform::fromScale(1,accel_z_var),false); - float gyro_x_var = -1/steps*(1+steps+log10(insSettingsData.gyro_var[InsSettings::GYRO_VAR_X])); + float gyro_x_var = -1/steps*(1+steps+log10(revoCalibrationData.gyro_var[RevoCalibration::GYRO_VAR_X])); gyro_x->setTransform(QTransform::fromScale(1,gyro_x_var),false); - float gyro_y_var = -1/steps*(1+steps+log10(insSettingsData.gyro_var[InsSettings::GYRO_VAR_Y])); + float gyro_y_var = -1/steps*(1+steps+log10(revoCalibrationData.gyro_var[RevoCalibration::GYRO_VAR_Y])); gyro_y->setTransform(QTransform::fromScale(1,gyro_y_var),false); - float gyro_z_var = -1/steps*(1+steps+log10(insSettingsData.gyro_var[InsSettings::GYRO_VAR_Z])); + float gyro_z_var = -1/steps*(1+steps+log10(revoCalibrationData.gyro_var[RevoCalibration::GYRO_VAR_Z])); gyro_z->setTransform(QTransform::fromScale(1,gyro_z_var),false); // Scale by 1e-3 because mag vars are much higher. - float mag_x_var = -1/steps*(1+steps+log10(1e-3*insSettingsData.mag_var[InsSettings::MAG_VAR_X])); + float mag_x_var = -1/steps*(1+steps+log10(1e-3*revoCalibrationData.mag_var[RevoCalibration::MAG_VAR_X])); mag_x->setTransform(QTransform::fromScale(1,mag_x_var),false); - float mag_y_var = -1/steps*(1+steps+log10(1e-3*insSettingsData.mag_var[InsSettings::MAG_VAR_Y])); + float mag_y_var = -1/steps*(1+steps+log10(1e-3*revoCalibrationData.mag_var[RevoCalibration::MAG_VAR_Y])); mag_y->setTransform(QTransform::fromScale(1,mag_y_var),false); - float mag_z_var = -1/steps*(1+steps+log10(1e-3*insSettingsData.mag_var[InsSettings::MAG_VAR_Z])); + float mag_z_var = -1/steps*(1+steps+log10(1e-3*revoCalibrationData.mag_var[RevoCalibration::MAG_VAR_Z])); mag_z->setTransform(QTransform::fromScale(1,mag_z_var),false); } @@ -1139,17 +722,8 @@ void ConfigRevoWidget::drawVariancesGraph() */ void ConfigRevoWidget::refreshValues() { - InsSettings * insSettings = InsSettings::GetInstance(getObjectManager()); - Q_ASSERT(insSettings); - InsSettings::DataFields insSettingsData = insSettings->getData(); - m_ui->algorithm->setCurrentIndex(insSettingsData.Algorithm); drawVariancesGraph(); - HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager()); - Q_ASSERT(homeLocation); - HomeLocation::DataFields homeLocationData = homeLocation->getData(); - m_ui->homeLocationSet->setEnabled(homeLocationData.Set == HomeLocation::SET_TRUE); - m_ui->ahrsCalibStart->setEnabled(true); m_ui->sixPointsStart->setEnabled(true); m_ui->accelBiasStart->setEnabled(true); @@ -1158,60 +732,32 @@ void ConfigRevoWidget::refreshValues() m_ui->calibInstructions->setText(QString("Press \"Start\" above to calibrate.")); } -/** - Enables/disables the Home Location saving button depending on whether the - home location is set-able - */ -void ConfigRevoWidget::enableHomeLocSave(UAVObject * obj) -{ - UAVObjectField *field = obj->getField(QString("Set")); - if (field) { - m_ui->homeLocationSet->setEnabled(field->getValue().toBool()); - m_ui->sixPointsStart->setEnabled(obj->getField("Set")->getValue().toBool()); - } -} - /** Save current settings to RAM */ -void ConfigRevoWidget::ahrsSettingsSaveRAM() +void ConfigRevoWidget::SettingsToRAM() { - InsSettings * insSettings = InsSettings::GetInstance(getObjectManager()); - Q_ASSERT(insSettings); - InsSettings::DataFields insSettingsData = insSettings->getData(); - insSettingsData.Algorithm = m_ui->algorithm->currentIndex(); - insSettings->setData(insSettingsData); - insSettings->updated(); - - HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager()); - Q_ASSERT(homeLocation); - HomeLocation::DataFields homeLocationData = homeLocation->getData(); - homeLocationData.Set = m_ui->homeLocationSet->isChecked() ? - HomeLocation::SET_TRUE : HomeLocation::SET_FALSE; - homeLocation->setData(homeLocationData); - homeLocation->updated(); + RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager()); + Q_ASSERT(revoCalibration); + revoCalibration->updated(); } /** -Save AHRS Settings and home location to SD +Save Revo calibration settings to flash */ -void ConfigRevoWidget::ahrsSettingsSaveSD() +void ConfigRevoWidget::SettingsToFlash() { - ahrsSettingsSaveRAM(); + SettingsToRAM(); - InsSettings * insSettings = InsSettings::GetInstance(getObjectManager()); - Q_ASSERT(insSettings); - saveObjectToSD(insSettings); - - HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager()); - Q_ASSERT(homeLocation); - saveObjectToSD(homeLocation); + RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager()); + Q_ASSERT(revoCalibration); + saveObjectToSD(revoCalibration); } void ConfigRevoWidget::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/INS+Configuration", QUrl::StrictMode) ); + QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/Revo+Configuration", QUrl::StrictMode) ); } /** diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.h b/ground/openpilotgcs/src/plugins/config/configrevowidget.h index d2ab2a23e..c27bb8bcb 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.h +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.h @@ -40,6 +40,8 @@ #include #include +class Ui_Widget; + class ConfigRevoWidget: public ConfigTaskWidget { Q_OBJECT @@ -84,32 +86,27 @@ private: QList mag_accum_x; QList mag_accum_y; QList mag_accum_z; - quint8 algorithm; + + double accel_data_x[6], accel_data_y[6], accel_data_z[6]; + double mag_data_x[6], mag_data_y[6], mag_data_z[6]; UAVObject::Metadata initialMdata; + int position; private slots: - void enableHomeLocSave(UAVObject *obj); - void measureNoise(); - void noiseMeasured(); - void saveRevoCalibration(); void openHelp(); void launchAccelBiasCalibration(); void incrementProgress(); virtual void refreshValues(); //void ahrsSettingsRequest(); - void SettingsSaveRAM(); - void SettingsSaveSD(); + void SettingsToRAM(); + void SettingsToFlash(); void savePositionData(); void computeScaleBias(); - void sixPointCalibrationMode(); // this function no longer exists - void attitudeRawUpdated(UAVObject * obj); + void sixPointCalibrationMode(); + void sensorsUpdated(UAVObject * obj); void accelBiasattitudeRawUpdated(UAVObject*); - void driftCalibrationAttitudeRawUpdated(UAVObject*); - void launchGyroDriftCalibration(); - void resetCalibrationDefaults(); - void cacheCurrentCalibration(); protected: void showEvent(QShowEvent *event); diff --git a/ground/openpilotgcs/src/plugins/config/revosensors.ui b/ground/openpilotgcs/src/plugins/config/revosensors.ui index 88e7479cf..e92d09d7b 100644 --- a/ground/openpilotgcs/src/plugins/config/revosensors.ui +++ b/ground/openpilotgcs/src/plugins/config/revosensors.ui @@ -1,7 +1,7 @@ - RevoSensorWidget - + RevoSensorsWidget + 0 diff --git a/ground/openpilotgcs/src/plugins/coreplugin/OpenPilotGCS.xml b/ground/openpilotgcs/src/plugins/coreplugin/OpenPilotGCS.xml index 444e1c47f..25af37d97 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/OpenPilotGCS.xml +++ b/ground/openpilotgcs/src/plugins/coreplugin/OpenPilotGCS.xml @@ -989,29 +989,6 @@ - - - false - 0.0.0 - - - %%DATAPATH%%dials/deluxe/lineardial-vertical.svg - 0 - 1 - Andale Mono,12,-1,5,75,0,0,0,0,0 - 50 - 0 - 100 - 0 - 100 - 80 - AhrsStatus - CPULoad - false - 80 - 50 - - false @@ -2260,21 +2237,13 @@ 1000 240 - 4289374847 - RunningTime - InsStatus - 0 - 0 - 0 - - 4294945407 FlightTime SystemStats 0 0 0 - + 2 1 800