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

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