ground/gcs/src/plugins/config/configrevowidget.cpp

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       ConfigRevoWidget.h
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6  * @addtogroup GCSPlugins GCS Plugins
   7  * @{
   8  * @addtogroup ConfigPlugin Config Plugin
   9  * @{
  10  * @brief The Configuration Gadget used to update settings in the firmware
  11  *****************************************************************************/
  12 /*
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 3 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21  * for more details.
  22  *
  23  * You should have received a copy of the GNU General Public License along
  24  * with this program; if not, write to the Free Software Foundation, Inc.,
  25  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26  */
  27 #include "configrevowidget.h"
  28
  29 #include "ui_revosensors.h"
  30
  31 #include <attitudestate.h>
  32 #include <attitudesettings.h>
  33 #include <revocalibration.h>
  34 #include <accelgyrosettings.h>
  35 #include <homelocation.h>
  36 #include <accelstate.h>
  37 #include <magstate.h>
  38
  39 #include <extensionsystem/pluginmanager.h>
  40 #include <coreplugin/generalsettings.h>
  41
  42 #include "assertions.h"
  43 #include "calibration.h"
  44 #include "calibration/calibrationutils.h"
  45
  46 #include "math.h"
  47 #include <QDebug>
  48 #include <QTimer>
  49 #include <QStringList>
  50 #include <QWidget>
  51 #include <QTextEdit>
  52 #include <QVBoxLayout>
  53 #include <QPushButton>
  54 #include <QMessageBox>
  55 #include <QThread>
  56 #include <QErrorMessage>
  57 #include <QDesktopServices>
  58 #include <QUrl>
  59 #include <iostream>
  60
  61 #include <math.h>
  62
  63 // #define DEBUG
  64
  65 #define MAG_ALARM_THRESHOLD 5
  66
  67 // Uncomment this to enable 6 point calibration on the accels
  68 #define NOISE_SAMPLES       50
  69
  70 class Thread : public QThread {
  71 public:
  72     static void usleep(unsigned long usecs)
  73     {
  74         QThread::usleep(usecs);
  75     }
  76 };
  77
  78 ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
  79     ConfigTaskWidget(parent),
  80     m_ui(new Ui_RevoSensorsWidget()),
  81     isBoardRotationStored(false)
  82 {
  83     m_ui->setupUi(this);
  84     m_ui->tabWidget->setCurrentIndex(0);
  85
  86     addApplySaveButtons(m_ui->revoCalSettingsSaveRAM, m_ui->revoCalSettingsSaveSD);
  87
  88     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
  89     Core::Internal::GeneralSettings *settings = pm->getObject<Core::Internal::GeneralSettings>();
  90     if (!settings->useExpertMode()) {
  91         m_ui->revoCalSettingsSaveRAM->setVisible(false);
  92     }
  93
  94     // Initialization of the visual help
  95     m_ui->calibrationVisualHelp->setScene(new QGraphicsScene(this));
  96     m_ui->calibrationVisualHelp->setRenderHint(QPainter::HighQualityAntialiasing, true);
  97     m_ui->calibrationVisualHelp->setRenderHint(QPainter::SmoothPixmapTransform, true);
  98     m_ui->calibrationVisualHelp->setBackgroundBrush(QBrush(QColor(200, 200, 200)));
  99     displayVisualHelp("empty");
 100
 101     // Must set up the UI (above) before setting up the UAVO mappings or refreshWidgetValues
 102     // will be dealing with some null pointers
 103     addUAVObject("HomeLocation");
 104     addUAVObject("RevoCalibration");
 105     addUAVObject("AttitudeSettings");
 106     addUAVObject("RevoSettings");
 107     addUAVObject("AccelGyroSettings");
 108     addUAVObject("AuxMagSettings");
 109     autoLoadWidgets();
 110
 111     // accel calibration
 112     m_accelCalibrationModel = new OpenPilot::SixPointCalibrationModel(this);
 113     connect(m_ui->accelStart, SIGNAL(clicked()), m_accelCalibrationModel, SLOT(accelStart()));
 114     connect(m_ui->accelSavePos, SIGNAL(clicked()), m_accelCalibrationModel, SLOT(savePositionData()));
 115
 116     connect(m_accelCalibrationModel, SIGNAL(started()), this, SLOT(disableAllCalibrations()));
 117     connect(m_accelCalibrationModel, SIGNAL(stopped()), this, SLOT(enableAllCalibrations()));
 118     connect(m_accelCalibrationModel, SIGNAL(storeAndClearBoardRotation()), this, SLOT(storeAndClearBoardRotation()));
 119     connect(m_accelCalibrationModel, SIGNAL(recallBoardRotation()), this, SLOT(recallBoardRotation()));
 120     connect(m_accelCalibrationModel, SIGNAL(displayInstructions(QString, WizardModel::MessageType)),
 121             this, SLOT(addInstructions(QString, WizardModel::MessageType)));
 122     connect(m_accelCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString)));
 123     connect(m_accelCalibrationModel, SIGNAL(savePositionEnabledChanged(bool)), m_ui->accelSavePos, SLOT(setEnabled(bool)));
 124     connect(m_accelCalibrationModel, SIGNAL(progressChanged(int)), m_ui->accelProgress, SLOT(setValue(int)));
 125     m_ui->accelSavePos->setEnabled(false);
 126
 127     // mag calibration
 128     m_magCalibrationModel = new OpenPilot::SixPointCalibrationModel(this);
 129     connect(m_ui->magStart, SIGNAL(clicked()), m_magCalibrationModel, SLOT(magStart()));
 130     connect(m_ui->magSavePos, SIGNAL(clicked()), m_magCalibrationModel, SLOT(savePositionData()));
 131
 132     connect(m_magCalibrationModel, SIGNAL(started()), this, SLOT(disableAllCalibrations()));
 133     connect(m_magCalibrationModel, SIGNAL(stopped()), this, SLOT(enableAllCalibrations()));
 134     connect(m_magCalibrationModel, SIGNAL(storeAndClearBoardRotation()), this, SLOT(storeAndClearBoardRotation()));
 135     connect(m_magCalibrationModel, SIGNAL(recallBoardRotation()), this, SLOT(recallBoardRotation()));
 136     connect(m_magCalibrationModel, SIGNAL(displayInstructions(QString, WizardModel::MessageType)),
 137             this, SLOT(addInstructions(QString, WizardModel::MessageType)));
 138     connect(m_magCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString)));
 139     connect(m_magCalibrationModel, SIGNAL(savePositionEnabledChanged(bool)), m_ui->magSavePos, SLOT(setEnabled(bool)));
 140     connect(m_magCalibrationModel, SIGNAL(progressChanged(int)), m_ui->magProgress, SLOT(setValue(int)));
 141     m_ui->magSavePos->setEnabled(false);
 142
 143     // board level calibration
 144     m_levelCalibrationModel = new OpenPilot::LevelCalibrationModel(this);
 145     connect(m_ui->boardLevelStart, SIGNAL(clicked()), m_levelCalibrationModel, SLOT(start()));
 146     connect(m_ui->boardLevelSavePos, SIGNAL(clicked()), m_levelCalibrationModel, SLOT(savePosition()));
 147
 148     connect(m_levelCalibrationModel, SIGNAL(started()), this, SLOT(disableAllCalibrations()));
 149     connect(m_levelCalibrationModel, SIGNAL(stopped()), this, SLOT(enableAllCalibrations()));
 150     connect(m_levelCalibrationModel, SIGNAL(displayInstructions(QString, WizardModel::MessageType)),
 151             this, SLOT(addInstructions(QString, WizardModel::MessageType)));
 152     connect(m_levelCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString)));
 153     connect(m_levelCalibrationModel, SIGNAL(savePositionEnabledChanged(bool)), m_ui->boardLevelSavePos, SLOT(setEnabled(bool)));
 154     connect(m_levelCalibrationModel, SIGNAL(progressChanged(int)), m_ui->boardLevelProgress, SLOT(setValue(int)));
 155     m_ui->boardLevelSavePos->setEnabled(false);
 156
 157     // gyro zero calibration
 158     m_gyroBiasCalibrationModel = new OpenPilot::GyroBiasCalibrationModel(this);
 159     connect(m_ui->gyroBiasStart, SIGNAL(clicked()), m_gyroBiasCalibrationModel, SLOT(start()));
 160
 161     connect(m_gyroBiasCalibrationModel, SIGNAL(progressChanged(int)), m_ui->gyroBiasProgress, SLOT(setValue(int)));
 162
 163     connect(m_gyroBiasCalibrationModel, SIGNAL(started()), this, SLOT(disableAllCalibrations()));
 164     connect(m_gyroBiasCalibrationModel, SIGNAL(stopped()), this, SLOT(enableAllCalibrations()));
 165     connect(m_gyroBiasCalibrationModel, SIGNAL(displayInstructions(QString, WizardModel::MessageType)),
 166             this, SLOT(addInstructions(QString, WizardModel::MessageType)));
 167     connect(m_gyroBiasCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString)));
 168
 169     // thermal calibration
 170     m_thermalCalibrationModel = new OpenPilot::ThermalCalibrationModel(this);
 171     connect(m_ui->thermalBiasStart, SIGNAL(clicked()), m_thermalCalibrationModel, SLOT(btnStart()));
 172     connect(m_ui->thermalBiasEnd, SIGNAL(clicked()), m_thermalCalibrationModel, SLOT(btnEnd()));
 173     connect(m_ui->thermalBiasCancel, SIGNAL(clicked()), m_thermalCalibrationModel, SLOT(btnAbort()));
 174
 175     connect(m_thermalCalibrationModel, SIGNAL(startEnabledChanged(bool)), m_ui->thermalBiasStart, SLOT(setEnabled(bool)));
 176     connect(m_thermalCalibrationModel, SIGNAL(endEnabledChanged(bool)), m_ui->thermalBiasEnd, SLOT(setEnabled(bool)));
 177     connect(m_thermalCalibrationModel, SIGNAL(cancelEnabledChanged(bool)), m_ui->thermalBiasCancel, SLOT(setEnabled(bool)));
 178     connect(m_thermalCalibrationModel, SIGNAL(wizardStarted()), this, SLOT(disableAllCalibrations()));
 179     connect(m_thermalCalibrationModel, SIGNAL(wizardStopped()), this, SLOT(enableAllCalibrations()));
 180
 181     connect(m_thermalCalibrationModel, SIGNAL(instructionsAdded(QString, WizardModel::MessageType)),
 182             this, SLOT(addInstructions(QString, WizardModel::MessageType)));
 183     connect(m_thermalCalibrationModel, SIGNAL(temperatureChanged(float)), this, SLOT(displayTemperature(float)));
 184     connect(m_thermalCalibrationModel, SIGNAL(temperatureGradientChanged(float)), this, SLOT(displayTemperatureGradient(float)));
 185     connect(m_thermalCalibrationModel, SIGNAL(temperatureRangeChanged(float)), this, SLOT(displayTemperatureRange(float)));
 186     connect(m_thermalCalibrationModel, SIGNAL(progressChanged(int)), m_ui->thermalBiasProgress, SLOT(setValue(int)));
 187     connect(m_thermalCalibrationModel, SIGNAL(progressMaxChanged(int)), m_ui->thermalBiasProgress, SLOT(setMaximum(int)));
 188     m_thermalCalibrationModel->init();
 189
 190     // home location
 191     connect(m_ui->hlClearButton, SIGNAL(clicked()), this, SLOT(clearHomeLocation()));
 192
 193     addWidgetBinding("RevoSettings", "FusionAlgorithm", m_ui->FusionAlgorithm, 0, 1, true);
 194
 195     addWidgetBinding("AttitudeSettings", "BoardRotation", m_ui->rollRotation, AttitudeSettings::BOARDROTATION_ROLL);
 196     addWidgetBinding("AttitudeSettings", "BoardRotation", m_ui->pitchRotation, AttitudeSettings::BOARDROTATION_PITCH);
 197     addWidgetBinding("AttitudeSettings", "BoardRotation", m_ui->yawRotation, AttitudeSettings::BOARDROTATION_YAW);
 198     addWidgetBinding("AttitudeSettings", "AccelTau", m_ui->accelTau);
 199
 200     addWidgetBinding("AuxMagSettings", "Usage", m_ui->auxMagUsage, 0, 1, true);
 201     addWidgetBinding("AuxMagSettings", "Type", m_ui->auxMagType, 0, 1, true);
 202
 203     addWidgetBinding("RevoSettings", "MagnetometerMaxDeviation", m_ui->maxDeviationWarning, RevoSettings::MAGNETOMETERMAXDEVIATION_WARNING);
 204     addWidgetBinding("RevoSettings", "MagnetometerMaxDeviation", m_ui->maxDeviationError, RevoSettings::MAGNETOMETERMAXDEVIATION_ERROR);
 205
 206     addWidgetBinding("AuxMagSettings", "BoardRotation", m_ui->auxMagRollRotation, AuxMagSettings::BOARDROTATION_ROLL);
 207     addWidgetBinding("AuxMagSettings", "BoardRotation", m_ui->auxMagPitchRotation, AuxMagSettings::BOARDROTATION_PITCH);
 208     addWidgetBinding("AuxMagSettings", "BoardRotation", m_ui->auxMagYawRotation, AuxMagSettings::BOARDROTATION_YAW);
 209
 210     connect(m_ui->tabWidget, SIGNAL(currentChanged(int)), this, SLOT(onBoardAuxMagError()));
 211     connect(MagSensor::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(onBoardAuxMagError()));
 212     connect(MagState::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateMagStatus()));
 213     connect(HomeLocation::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateMagBeVector()));
 214
 215     addWidget(m_ui->internalAuxErrorX);
 216     addWidget(m_ui->internalAuxErrorY);
 217     addWidget(m_ui->internalAuxErrorZ);
 218
 219     displayMagError = false;
 220
 221     // Connect the help button
 222     connect(m_ui->attitudeHelp, SIGNAL(clicked()), this, SLOT(openHelp()));
 223
 224     populateWidgets();
 225     enableAllCalibrations();
 226
 227     updateEnableControls();
 228
 229     forceConnectedState();
 230     refreshWidgetsValues();
 231 }
 232
 233 ConfigRevoWidget::~ConfigRevoWidget()
 234 {
 235     // Do nothing
 236 }
 237
 238 void ConfigRevoWidget::showEvent(QShowEvent *event)
 239 {
 240     Q_UNUSED(event);
 241     updateVisualHelp();
 242 }
 243
 244 void ConfigRevoWidget::resizeEvent(QResizeEvent *event)
 245 {
 246     Q_UNUSED(event);
 247     updateVisualHelp();
 248 }
 249
 250 void ConfigRevoWidget::updateVisualHelp()
 251 {
 252     m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::KeepAspectRatio);
 253 }
 254
 255 void ConfigRevoWidget::storeAndClearBoardRotation()
 256 {
 257     if (!isBoardRotationStored) {
 258         // Store current board rotation
 259         isBoardRotationStored = true;
 260         AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
 261         Q_ASSERT(attitudeSettings);
 262         AttitudeSettings::DataFields data  = attitudeSettings->getData();
 263         storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW]   = data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW];
 264         storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL]  = data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL];
 265         storedBoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH];
 266
 267         // Set board rotation to no rotation
 268         data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW]    = 0;
 269         data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL]   = 0;
 270         data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH]  = 0;
 271         attitudeSettings->setData(data);
 272
 273         // Store current aux mag board rotation
 274         AuxMagSettings *auxMagSettings = AuxMagSettings::GetInstance(getObjectManager());
 275         Q_ASSERT(auxMagSettings);
 276         AuxMagSettings::DataFields auxMagData = auxMagSettings->getData();
 277         auxMagStoredBoardRotation[AuxMagSettings::BOARDROTATION_YAW]   = auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_YAW];
 278         auxMagStoredBoardRotation[AuxMagSettings::BOARDROTATION_ROLL]  = auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_ROLL];
 279         auxMagStoredBoardRotation[AuxMagSettings::BOARDROTATION_PITCH] = auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_PITCH];
 280
 281         // Set aux mag board rotation to no rotation
 282         auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_YAW]    = 0;
 283         auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_ROLL]   = 0;
 284         auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_PITCH]  = 0;
 285         auxMagSettings->setData(auxMagData);
 286     }
 287 }
 288
 289 void ConfigRevoWidget::recallBoardRotation()
 290 {
 291     if (isBoardRotationStored) {
 292         // Recall current board rotation
 293         isBoardRotationStored = false;
 294
 295         // Restore the flight controller board rotation
 296         AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
 297         Q_ASSERT(attitudeSettings);
 298         AttitudeSettings::DataFields data  = attitudeSettings->getData();
 299         data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW]   = storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW];
 300         data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL]  = storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL];
 301         data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = storedBoardRotation[AttitudeSettings::BOARDROTATION_PITCH];
 302         attitudeSettings->setData(data);
 303
 304         // Restore the aux mag board rotation
 305         AuxMagSettings *auxMagSettings = AuxMagSettings::GetInstance(getObjectManager());
 306         Q_ASSERT(auxMagSettings);
 307         AuxMagSettings::DataFields auxMagData = auxMagSettings->getData();
 308         auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_YAW]   = auxMagStoredBoardRotation[AuxMagSettings::BOARDROTATION_YAW];
 309         auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_ROLL]  = auxMagStoredBoardRotation[AuxMagSettings::BOARDROTATION_ROLL];
 310         auxMagData.BoardRotation[AuxMagSettings::BOARDROTATION_PITCH] = auxMagStoredBoardRotation[AuxMagSettings::BOARDROTATION_PITCH];
 311         auxMagSettings->setData(auxMagData);
 312     }
 313 }
 314
 315 /**
 316    Show the selected visual aid
 317  */
 318 void ConfigRevoWidget::displayVisualHelp(QString elementID)
 319 {
 320     m_ui->calibrationVisualHelp->scene()->clear();
 321     QPixmap pixmap = QPixmap(":/configgadget/images/calibration/" + elementID + ".png");
 322     m_ui->calibrationVisualHelp->scene()->addPixmap(pixmap);
 323     m_ui->calibrationVisualHelp->setSceneRect(pixmap.rect());
 324     updateVisualHelp();
 325 }
 326
 327 void ConfigRevoWidget::clearInstructions()
 328 {
 329     m_ui->calibrationInstructions->clear();
 330 }
 331
 332 void ConfigRevoWidget::addInstructions(QString text, WizardModel::MessageType type)
 333 {
 334     QString msg;
 335
 336     switch (type) {
 337     case WizardModel::Debug:
 338 #ifdef DEBUG
 339         msg = QString("<i>%1</i>").arg(text);
 340 #endif
 341         break;
 342     case WizardModel::Info:
 343         msg = QString("%1").arg(text);
 344         break;
 345     case WizardModel::Prompt:
 346         msg = QString("<b><font color='blue'>%1</font>").arg(text);
 347         break;
 348     case WizardModel::Warn:
 349         msg = QString("<b>%1</b>").arg(text);
 350         break;
 351     case WizardModel::Success:
 352         msg = QString("<b><font color='green'>%1</font>").arg(text);
 353         break;
 354     case WizardModel::Failure:
 355         msg = QString("<b><font color='red'>%1</font>").arg(text);
 356         break;
 357     default:
 358         break;
 359     }
 360     if (!msg.isEmpty()) {
 361         m_ui->calibrationInstructions->append(msg);
 362     }
 363 }
 364
 365 static QString format(float v)
 366 {
 367     QString str;
 368
 369     if (!std::isnan(v)) {
 370         // format as ##.##
 371         str = QString("%1").arg(v, 5, 'f', 2, ' ');
 372         str = str.replace(" ", "&nbsp;");
 373     } else {
 374         str = "--.--";
 375     }
 376     // use a fixed width font
 377     QString style("font-family:courier new,monospace;");
 378     return QString("<span style=\"%1\">%2</span>").arg(style).arg(str);
 379 }
 380
 381 void ConfigRevoWidget::displayTemperature(float temperature)
 382 {
 383     m_ui->temperatureLabel->setText(tr("Temperature: %1°C").arg(format(temperature)));
 384 }
 385
 386 void ConfigRevoWidget::displayTemperatureGradient(float temperatureGradient)
 387 {
 388     m_ui->temperatureGradientLabel->setText(tr("Gradient: %1°C/min").arg(format(temperatureGradient)));
 389 }
 390
 391 void ConfigRevoWidget::displayTemperatureRange(float temperatureRange)
 392 {
 393     m_ui->temperatureRangeLabel->setText(tr("Sampled range: %1°C").arg(format(temperatureRange)));
 394 }
 395
 396 /**
 397  * Called by the ConfigTaskWidget parent when RevoCalibration is updated
 398  * to update the UI
 399  */
 400 void ConfigRevoWidget::refreshWidgetsValues(UAVObject *object)
 401 {
 402     ConfigTaskWidget::refreshWidgetsValues(object);
 403
 404     m_ui->isSetCheckBox->setEnabled(false);
 405
 406     HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
 407     Q_ASSERT(homeLocation);
 408     HomeLocation::DataFields homeLocationData = homeLocation->getData();
 409
 410     QString beStr = QString("%1:%2:%3").arg(QString::number(homeLocationData.Be[0]), QString::number(homeLocationData.Be[1]), QString::number(homeLocationData.Be[2]));
 411     m_ui->beBox->setText(beStr);
 412
 413     updateMagBeVector();
 414     onBoardAuxMagError();
 415 }
 416
 417 void ConfigRevoWidget::updateObjectsFromWidgets()
 418 {
 419     ConfigTaskWidget::updateObjectsFromWidgets();
 420
 421     if (m_accelCalibrationModel->dirty()) {
 422         m_accelCalibrationModel->save();
 423     }
 424     if (m_magCalibrationModel->dirty()) {
 425         m_magCalibrationModel->save();
 426     }
 427     if (m_levelCalibrationModel->dirty()) {
 428         m_levelCalibrationModel->save();
 429     }
 430     if (m_gyroBiasCalibrationModel->dirty()) {
 431         m_gyroBiasCalibrationModel->save();
 432     }
 433     if (m_thermalCalibrationModel->dirty()) {
 434         m_thermalCalibrationModel->save();
 435     }
 436 }
 437
 438 void ConfigRevoWidget::clearHomeLocation()
 439 {
 440     HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
 441
 442     Q_ASSERT(homeLocation);
 443     HomeLocation::DataFields homeLocationData;
 444     homeLocationData.Latitude  = 0;
 445     homeLocationData.Longitude = 0;
 446     homeLocationData.Altitude  = 0;
 447     homeLocationData.Be[0]     = 0;
 448     homeLocationData.Be[1]     = 0;
 449     homeLocationData.Be[2]     = 0;
 450     homeLocationData.g_e = 9.81f;
 451     homeLocationData.Set = HomeLocation::SET_FALSE;
 452     homeLocation->setData(homeLocationData);
 453 }
 454
 455 void ConfigRevoWidget::disableAllCalibrations()
 456 {
 457     clearInstructions();
 458
 459     m_ui->accelStart->setEnabled(false);
 460     m_ui->magStart->setEnabled(false);
 461     m_ui->boardLevelStart->setEnabled(false);
 462     m_ui->gyroBiasStart->setEnabled(false);
 463     m_ui->thermalBiasStart->setEnabled(false);
 464 }
 465
 466 void ConfigRevoWidget::enableAllCalibrations()
 467 {
 468     // TODO this logic should not be here and should use a signal instead
 469     // need to check if ConfigTaskWidget has support for this kind of use cases
 470     if (m_accelCalibrationModel->dirty() || m_magCalibrationModel->dirty() || m_levelCalibrationModel->dirty()
 471         || m_gyroBiasCalibrationModel->dirty() || m_thermalCalibrationModel->dirty()) {
 472         widgetsContentsChanged();
 473     }
 474
 475     m_ui->accelStart->setEnabled(true);
 476     m_ui->magStart->setEnabled(true);
 477     m_ui->boardLevelStart->setEnabled(true);
 478     m_ui->gyroBiasStart->setEnabled(true);
 479     m_ui->thermalBiasStart->setEnabled(true);
 480 }
 481
 482 void ConfigRevoWidget::onBoardAuxMagError()
 483 {
 484     MagSensor *magSensor = MagSensor::GetInstance(getObjectManager());
 485
 486     Q_ASSERT(magSensor);
 487     AuxMagSensor *auxMagSensor = AuxMagSensor::GetInstance(getObjectManager());
 488     Q_ASSERT(auxMagSensor);
 489
 490     if (m_ui->tabWidget->currentIndex() != 2) {
 491         // Apply default metadata
 492         if (displayMagError) {
 493             magSensor->setMetadata(magSensor->getDefaultMetadata());
 494             auxMagSensor->setMetadata(auxMagSensor->getDefaultMetadata());
 495             displayMagError = false;
 496         }
 497         return;
 498     }
 499
 500     if (!displayMagError) {
 501         // Apply new rates
 502         UAVObject::Metadata mdata = magSensor->getMetadata();
 503         UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
 504         mdata.flightTelemetryUpdatePeriod = 300;
 505         magSensor->setMetadata(mdata);
 506
 507         mdata = auxMagSensor->getMetadata();
 508         UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
 509         mdata.flightTelemetryUpdatePeriod = 300;
 510         auxMagSensor->setMetadata(mdata);
 511
 512         displayMagError = true;
 513         return;
 514     }
 515
 516     float onboardMag[3];
 517     float auxMag[3];
 518
 519     onboardMag[0] = magSensor->x();
 520     onboardMag[1] = magSensor->y();
 521     onboardMag[2] = magSensor->z();
 522
 523     float normalizedMag[3];
 524     float normalizedAuxMag[3];
 525     float xDiff     = 0.0f;
 526     float yDiff     = 0.0f;
 527     float zDiff     = 0.0f;
 528
 529     // Smooth Mag readings
 530     float alpha     = 0.7f;
 531     float inv_alpha = (1.0f - alpha);
 532     // Onboard mag
 533     onboardMagFiltered[0] = (onboardMagFiltered[0] * alpha) + (onboardMag[0] * inv_alpha);
 534     onboardMagFiltered[1] = (onboardMagFiltered[1] * alpha) + (onboardMag[1] * inv_alpha);
 535     onboardMagFiltered[2] = (onboardMagFiltered[2] * alpha) + (onboardMag[2] * inv_alpha);
 536
 537     // Normalize vector
 538     float magLength = sqrt((onboardMagFiltered[0] * onboardMagFiltered[0]) +
 539                            (onboardMagFiltered[1] * onboardMagFiltered[1]) +
 540                            (onboardMagFiltered[2] * onboardMagFiltered[2]));
 541
 542     normalizedMag[0] = onboardMagFiltered[0] / magLength;
 543     normalizedMag[1] = onboardMagFiltered[1] / magLength;
 544     normalizedMag[2] = onboardMagFiltered[2] / magLength;
 545
 546     if (auxMagSensor->status() > (int)AuxMagSensor::STATUS_NONE) {
 547         auxMag[0] = auxMagSensor->x();
 548         auxMag[1] = auxMagSensor->y();
 549         auxMag[2] = auxMagSensor->z();
 550
 551         auxMagFiltered[0] = (auxMagFiltered[0] * alpha) + (auxMag[0] * inv_alpha);
 552         auxMagFiltered[1] = (auxMagFiltered[1] * alpha) + (auxMag[1] * inv_alpha);
 553         auxMagFiltered[2] = (auxMagFiltered[2] * alpha) + (auxMag[2] * inv_alpha);
 554
 555         // Normalize vector
 556         float auxMagLength = sqrt((auxMagFiltered[0] * auxMagFiltered[0]) +
 557                                   (auxMagFiltered[1] * auxMagFiltered[1]) +
 558                                   (auxMagFiltered[2] * auxMagFiltered[2]));
 559
 560         normalizedAuxMag[0] = auxMagFiltered[0] / auxMagLength;
 561         normalizedAuxMag[1] = auxMagFiltered[1] / auxMagLength;
 562         normalizedAuxMag[2] = auxMagFiltered[2] / auxMagLength;
 563
 564         // Calc diff and scale
 565         xDiff = (normalizedMag[0] - normalizedAuxMag[0]) * 25.0f;
 566         yDiff = (normalizedMag[1] - normalizedAuxMag[1]) * 25.0f;
 567         zDiff = (normalizedMag[2] - normalizedAuxMag[2]) * 25.0f;
 568     } else {
 569         auxMag[0] = auxMag[1] = auxMag[2] = 0.0f;
 570         auxMagFiltered[0] = auxMagFiltered[1] = auxMagFiltered[2] = 0.0f;
 571     }
 572
 573     // Display Mag/AuxMag diff for every axis
 574     m_ui->internalAuxErrorX->setValue(xDiff > 50.0f ? 50.0f : xDiff < -50.0f ? -50.0f : xDiff);
 575     m_ui->internalAuxErrorY->setValue(yDiff > 50.0f ? 50.0f : yDiff < -50.0f ? -50.0f : yDiff);
 576     m_ui->internalAuxErrorZ->setValue(zDiff > 50.0f ? 50.0f : zDiff < -50.0f ? -50.0f : zDiff);
 577
 578     updateMagAlarm(getMagError(onboardMag), (auxMagSensor->status() == (int)AuxMagSensor::STATUS_NONE) ? -1.0f : getMagError(auxMag));
 579 }
 580
 581 void ConfigRevoWidget::updateMagAlarm(float errorMag, float errorAuxMag)
 582 {
 583     RevoSettings *revoSettings = RevoSettings::GetInstance(getObjectManager());
 584
 585     Q_ASSERT(revoSettings);
 586     RevoSettings::DataFields revoSettingsData = revoSettings->getData();
 587
 588     QStringList AlarmColor;
 589     AlarmColor << "grey" << "green" << "orange" << "red";
 590     enum magAlarmState { MAG_NOT_FOUND = 0, MAG_OK = 1, MAG_WARNING = 2, MAG_ERROR = 3 };
 591
 592     QString bgColorMag    = AlarmColor[MAG_OK];
 593     QString bgColorAuxMag = AlarmColor[MAG_OK];
 594
 595     // Onboard Mag
 596     if (errorMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_WARNING]) {
 597         magWarningCount = 0;
 598         magErrorCount   = 0;
 599     }
 600
 601     if (errorMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_ERROR]) {
 602         magErrorCount = 0;
 603         if (magWarningCount > MAG_ALARM_THRESHOLD) {
 604             bgColorMag = AlarmColor[MAG_WARNING];
 605         } else {
 606             magWarningCount++;
 607         }
 608     }
 609
 610     if (magErrorCount > MAG_ALARM_THRESHOLD) {
 611         bgColorMag = AlarmColor[MAG_ERROR];
 612     } else {
 613         magErrorCount++;
 614     }
 615
 616     // Auxiliary Mag
 617     if (errorAuxMag > -1.0f) {
 618         if (errorAuxMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_WARNING]) {
 619             auxMagWarningCount = 0;
 620             auxMagErrorCount   = 0;
 621         }
 622
 623         if (errorAuxMag < revoSettingsData.MagnetometerMaxDeviation[RevoSettings::MAGNETOMETERMAXDEVIATION_ERROR]) {
 624             auxMagErrorCount = 0;
 625             if (auxMagWarningCount > MAG_ALARM_THRESHOLD) {
 626                 bgColorAuxMag = AlarmColor[MAG_WARNING];
 627             } else {
 628                 auxMagWarningCount++;
 629             }
 630         }
 631
 632         if (auxMagErrorCount > MAG_ALARM_THRESHOLD) {
 633             bgColorAuxMag = AlarmColor[MAG_ERROR];
 634         } else {
 635             auxMagErrorCount++;
 636         }
 637         errorAuxMag = ((errorAuxMag * 100.0f) <= 100.0f) ? errorAuxMag * 100.0f : 100.0f;
 638         m_ui->auxMagStatus->setText("AuxMag\n" + QString::number(errorAuxMag, 'f', 1) + "%");
 639     } else {
 640         // Disable aux mag alarm
 641         bgColorAuxMag = AlarmColor[MAG_NOT_FOUND];
 642         m_ui->auxMagStatus->setText("AuxMag\nnot found");
 643     }
 644
 645     errorMag = ((errorMag * 100.0f) <= 100.0f) ? errorMag * 100.0f : 100.0f;
 646     m_ui->onBoardMagStatus->setText("Onboard\n" + QString::number(errorMag, 'f', 1) + "%");
 647     m_ui->onBoardMagStatus->setStyleSheet(
 648         "QLabel { background-color: " + bgColorMag + ";"
 649         "color: rgb(255, 255, 255); border-radius: 5; margin:1px; font:bold; }");
 650     m_ui->auxMagStatus->setStyleSheet(
 651         "QLabel { background-color: " + bgColorAuxMag + ";"
 652         "color: rgb(255, 255, 255); border-radius: 5; margin:1px; font:bold; }");
 653 }
 654
 655 float ConfigRevoWidget::getMagError(float mag[3])
 656 {
 657     float magnitude      = sqrt((mag[0] * mag[0]) + (mag[1] * mag[1]) + (mag[2] * mag[2]));
 658     float magnitudeBe    = sqrt((magBe[0] * magBe[0]) + (magBe[1] * magBe[1]) + (magBe[2] * magBe[2]));
 659     float invMagnitudeBe = 1.0f / magnitudeBe;
 660     // Absolute value of relative error against Be
 661     float error = fabsf(magnitude - magnitudeBe) * invMagnitudeBe;
 662
 663     return error;
 664 }
 665
 666 void ConfigRevoWidget::updateMagBeVector()
 667 {
 668     HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
 669
 670     Q_ASSERT(homeLocation);
 671     HomeLocation::DataFields homeLocationData = homeLocation->getData();
 672
 673     magBe[0] = homeLocationData.Be[0];
 674     magBe[1] = homeLocationData.Be[1];
 675     magBe[2] = homeLocationData.Be[2];
 676 }
 677
 678 void ConfigRevoWidget::updateMagStatus()
 679 {
 680     MagState *magState = MagState::GetInstance(getObjectManager());
 681
 682     Q_ASSERT(magState);
 683
 684     MagState::DataFields magStateData = magState->getData();
 685
 686     if (magStateData.Source == MagState::SOURCE_INVALID) {
 687         m_ui->magStatusSource->setText(tr("Source invalid"));
 688         m_ui->magStatusSource->setToolTip(tr("Currently no attitude estimation algorithm uses magnetometer or there is something wrong"));
 689     } else if (magStateData.Source == MagState::SOURCE_ONBOARD) {
 690         m_ui->magStatusSource->setText(tr("Onboard magnetometer"));
 691         m_ui->magStatusSource->setToolTip("");
 692     } else if (magStateData.Source == MagState::SOURCE_AUX) {
 693         m_ui->magStatusSource->setText(tr("Auxiliary magnetometer"));
 694         m_ui->magStatusSource->setToolTip("");
 695     } else {
 696         m_ui->magStatusSource->setText(tr("Unknown"));
 697         m_ui->magStatusSource->setToolTip("");
 698     }
 699 }
 700
 701 void ConfigRevoWidget::openHelp()
 702 {
 703     QDesktopServices::openUrl(QUrl(QString(WIKI_URL_ROOT) + QString("Revo+Attitude+Configuration"),
 704                                    QUrl::StrictMode));
 705 }