ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       configccattitudewidget.cpp
   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 Configure Attitude module on CopterControl
  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 "configccattitudewidget.h"
  28 #include "ui_ccattitude.h"
  29 #include "utils/coordinateconversions.h"
  30 #include "attitudesettings.h"
  31 #include <QMutexLocker>
  32 #include <QMessageBox>
  33 #include <QDebug>
  34 #include <QDesktopServices>
  35 #include <QUrl>
  36 #include "accelstate.h"
  37 #include "accelgyrosettings.h"
  38 #include "gyrostate.h"
  39 #include <extensionsystem/pluginmanager.h>
  40 #include <coreplugin/generalsettings.h>
  41 #include <calibration/calibrationutils.h>
  42 ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) :
  43     ConfigTaskWidget(parent),
  44     ui(new Ui_ccattitude)
  45 {
  46     ui->setupUi(this);
  47     connect(ui->zeroBias, SIGNAL(clicked()), this, SLOT(startAccelCalibration()));
  48
  49     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
  50     Core::Internal::GeneralSettings *settings = pm->getObject<Core::Internal::GeneralSettings>();
  51     if (!settings->useExpertMode()) {
  52         ui->applyButton->setVisible(false);
  53     }
  54
  55     addApplySaveButtons(ui->applyButton, ui->saveButton);
  56     addUAVObject("AttitudeSettings");
  57     addUAVObject("AccelGyroSettings");
  58
  59     // Connect the help button
  60     connect(ui->ccAttitudeHelp, SIGNAL(clicked()), this, SLOT(openHelp()));
  61
  62     addWidgetBinding("AttitudeSettings", "ZeroDuringArming", ui->zeroGyroBiasOnArming);
  63     addWidgetBinding("AttitudeSettings", "AccelTau", ui->accelTauSpinbox);
  64
  65     addWidgetBinding("AttitudeSettings", "BoardRotation", ui->rollBias, AttitudeSettings::BOARDROTATION_ROLL);
  66     addWidgetBinding("AttitudeSettings", "BoardRotation", ui->pitchBias, AttitudeSettings::BOARDROTATION_PITCH);
  67     addWidgetBinding("AttitudeSettings", "BoardRotation", ui->yawBias, AttitudeSettings::BOARDROTATION_YAW);
  68     addWidget(ui->zeroBias);
  69     populateWidgets();
  70     refreshWidgetsValues();
  71     forceConnectedState();
  72 }
  73
  74 ConfigCCAttitudeWidget::~ConfigCCAttitudeWidget()
  75 {
  76     delete ui;
  77 }
  78
  79 void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject *obj)
  80 {
  81     if (!timer.isActive()) {
  82         // ignore updates that come in after the timer has expired
  83         return;
  84     }
  85
  86     AccelState *accelState = AccelState::GetInstance(getObjectManager());
  87     GyroState *gyroState   = GyroState::GetInstance(getObjectManager());
  88
  89     // Accumulate samples until we have _at least_ NUM_SENSOR_UPDATES samples
  90     // for both gyros and accels.
  91     // Note that, at present, we stash the samples and then compute the bias
  92     // at the end, even though the mean could be accumulated as we go.
  93     // In future, a better algorithm could be used.
  94     if (obj->getObjID() == AccelState::OBJID) {
  95         accelUpdates++;
  96         AccelState::DataFields accelStateData = accelState->getData();
  97         x_accum.append(accelStateData.x);
  98         y_accum.append(accelStateData.y);
  99         z_accum.append(accelStateData.z);
 100     } else if (obj->getObjID() == GyroState::OBJID) {
 101         gyroUpdates++;
 102         GyroState::DataFields gyroStateData = gyroState->getData();
 103         x_gyro_accum.append(gyroStateData.x);
 104         y_gyro_accum.append(gyroStateData.y);
 105         z_gyro_accum.append(gyroStateData.z);
 106     }
 107
 108     // update the progress indicator
 109     ui->zeroBiasProgress->setValue((float)qMin(accelUpdates, gyroUpdates) / NUM_SENSOR_UPDATES * 100);
 110
 111     // If we have enough samples, then stop sampling and compute the biases
 112     if (accelUpdates >= NUM_SENSOR_UPDATES && gyroUpdates >= NUM_SENSOR_UPDATES) {
 113         timer.stop();
 114         disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *)));
 115         disconnect(&timer, SIGNAL(timeout()), this, SLOT(timeout()));
 116
 117         float x_bias = OpenPilot::CalibrationUtils::listMean(x_accum);
 118         float y_bias = OpenPilot::CalibrationUtils::listMean(y_accum);
 119         float z_bias = OpenPilot::CalibrationUtils::listMean(z_accum) + 9.81;
 120
 121         float x_gyro_bias = OpenPilot::CalibrationUtils::listMean(x_gyro_accum);
 122         float y_gyro_bias = OpenPilot::CalibrationUtils::listMean(y_gyro_accum);
 123         float z_gyro_bias = OpenPilot::CalibrationUtils::listMean(z_gyro_accum);
 124         accelState->setMetadata(initialAccelStateMdata);
 125         gyroState->setMetadata(initialGyroStateMdata);
 126
 127         AccelGyroSettings::DataFields accelGyroSettingsData = AccelGyroSettings::GetInstance(getObjectManager())->getData();
 128         AttitudeSettings::DataFields attitudeSettingsData   = AttitudeSettings::GetInstance(getObjectManager())->getData();
 129         // We offset the gyro bias by current bias to help precision
 130         accelGyroSettingsData.accel_bias[0] += x_bias;
 131         accelGyroSettingsData.accel_bias[1] += y_bias;
 132         accelGyroSettingsData.accel_bias[2] += z_bias;
 133         accelGyroSettingsData.gyro_bias[0]   = -x_gyro_bias;
 134         accelGyroSettingsData.gyro_bias[1]   = -y_gyro_bias;
 135         accelGyroSettingsData.gyro_bias[2]   = -z_gyro_bias;
 136         attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE;
 137         AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData);
 138         AccelGyroSettings::GetInstance(getObjectManager())->setData(accelGyroSettingsData);
 139         this->setDirty(true);
 140
 141         // reenable controls
 142         enableControls(true);
 143     }
 144 }
 145
 146 void ConfigCCAttitudeWidget::timeout()
 147 {
 148     UAVDataObject *obj = AccelState::GetInstance(getObjectManager());
 149
 150     disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *)));
 151     disconnect(&timer, SIGNAL(timeout()), this, SLOT(timeout()));
 152
 153     AccelState *accelState = AccelState::GetInstance(getObjectManager());
 154     GyroState *gyroState   = GyroState::GetInstance(getObjectManager());
 155     accelState->setMetadata(initialAccelStateMdata);
 156     gyroState->setMetadata(initialGyroStateMdata);
 157
 158     QMessageBox msgBox;
 159     msgBox.setText(tr("Calibration timed out before receiving required updates."));
 160     msgBox.setStandardButtons(QMessageBox::Ok);
 161     msgBox.setDefaultButton(QMessageBox::Ok);
 162     msgBox.exec();
 163
 164     // reset progress indicator
 165     ui->zeroBiasProgress->setValue(0);
 166     // reenable controls
 167     enableControls(true);
 168 }
 169
 170 void ConfigCCAttitudeWidget::startAccelCalibration()
 171 {
 172     // disable controls during sampling
 173     enableControls(false);
 174
 175     accelUpdates = 0;
 176     gyroUpdates  = 0;
 177     x_accum.clear();
 178     y_accum.clear();
 179     z_accum.clear();
 180     x_gyro_accum.clear();
 181     y_gyro_accum.clear();
 182     z_gyro_accum.clear();
 183
 184     // Disable gyro bias correction to see raw data
 185     AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(getObjectManager())->getData();
 186     attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
 187     AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData);
 188
 189     // Set up to receive updates
 190     UAVDataObject *accelState = AccelState::GetInstance(getObjectManager());
 191     UAVDataObject *gyroState  = GyroState::GetInstance(getObjectManager());
 192     connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *)));
 193     connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *)));
 194
 195     // Speed up updates
 196     initialAccelStateMdata = accelState->getMetadata();
 197     UAVObject::Metadata accelStateMdata = initialAccelStateMdata;
 198     UAVObject::SetFlightTelemetryUpdateMode(accelStateMdata, UAVObject::UPDATEMODE_PERIODIC);
 199     accelStateMdata.flightTelemetryUpdatePeriod = 30; // ms
 200     accelState->setMetadata(accelStateMdata);
 201
 202     initialGyroStateMdata = gyroState->getMetadata();
 203     UAVObject::Metadata gyroStateMdata = initialGyroStateMdata;
 204     UAVObject::SetFlightTelemetryUpdateMode(gyroStateMdata, UAVObject::UPDATEMODE_PERIODIC);
 205     gyroStateMdata.flightTelemetryUpdatePeriod = 30; // ms
 206     gyroState->setMetadata(gyroStateMdata);
 207
 208     // Set up timeout timer
 209     timer.setSingleShot(true);
 210     timer.start(5000 + (NUM_SENSOR_UPDATES * qMax(accelStateMdata.flightTelemetryUpdatePeriod,
 211                                                   gyroStateMdata.flightTelemetryUpdatePeriod)));
 212     connect(&timer, SIGNAL(timeout()), this, SLOT(timeout()));
 213 }
 214
 215 void ConfigCCAttitudeWidget::openHelp()
 216 {
 217     QDesktopServices::openUrl(QUrl(tr("http://wiki.openpilot.org/x/44Cf"), QUrl::StrictMode));
 218 }
 219
 220 void ConfigCCAttitudeWidget::setAccelFiltering(bool active)
 221 {
 222     Q_UNUSED(active);
 223     setDirty(true);
 224 }
 225
 226 void ConfigCCAttitudeWidget::enableControls(bool enable)
 227 {
 228     ui->zeroBias->setEnabled(enable);
 229     ui->zeroGyroBiasOnArming->setEnabled(enable);
 230     ui->accelTauSpinbox->setEnabled(enable);
 231     ConfigTaskWidget::enableControls(enable);
 232 }
 233
 234 void ConfigCCAttitudeWidget::updateObjectsFromWidgets()
 235 {
 236     ConfigTaskWidget::updateObjectsFromWidgets();
 237
 238     ui->zeroBiasProgress->setValue(0);
 239 }