diff --git a/ground/openpilotgcs/src/plugins/config/ccattitude.ui b/ground/openpilotgcs/src/plugins/config/ccattitude.ui new file mode 100644 index 000000000..07717dd93 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/ccattitude.ui @@ -0,0 +1,58 @@ + + + ccattitude + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + 10 + 40 + 281 + 32 + + + + + + + Zero Accel Bias + + + + + + + 0 + + + + + + + + + 10 + 20 + 281 + 16 + + + + Place aircraft flat before computing + + + + + + diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp new file mode 100644 index 000000000..a5ea90734 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -0,0 +1,114 @@ +/** + ****************************************************************************** + * + * @file configccattitudewidget.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief Configure Attitude module on CopterControl + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#include "configccattitudewidget.h" +#include "ui_ccattitude.h" +#include +#include +#include + +ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) : + ConfigTaskWidget(parent), + ui(new Ui_ccattitude) +{ + ui->setupUi(this); + connect(ui->zeroBias,SIGNAL(clicked()),this,SLOT(startAccelCalibration())); +} + +ConfigCCAttitudeWidget::~ConfigCCAttitudeWidget() +{ + delete ui; +} + +void ConfigCCAttitudeWidget::attitudeRawUpdated(UAVObject * obj) { + QMutexLocker locker(&startStop); + UAVDataObject * attitudeRaw = dynamic_cast(obj); + + ui->zeroBiasProgress->setValue((float) updates / NUM_ACCEL_UPDATES * 100); + + if(updates < NUM_ACCEL_UPDATES) { + updates++; + UAVObjectField * field = obj->getField(QString("accels")); + x_accum.append(field->getDouble(0)); + y_accum.append(field->getDouble(1)); + z_accum.append(field->getDouble(2)); + } else { + timer.stop(); + disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(attitudeRawUpdated(UAVObject*))); + disconnect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); + + float x_bias = listMean(x_accum) / ACCEL_SCALE; + float y_bias = listMean(y_accum) / ACCEL_SCALE; + float z_bias = (listMean(z_accum) + 9.81) / ACCEL_SCALE; + + obj->setMetadata(initialMdata); + + UAVDataObject * settings = dynamic_cast(getObjectManager()->getObject(QString("AttitudeSettings"))); + UAVObjectField * field = settings->getField("AccelBias"); + field->setDouble(field->getDouble(0) + x_bias,0); + field->setDouble(field->getDouble(1) + y_bias,1); + field->setDouble(field->getDouble(2) + z_bias,2); + settings->updated(); + } +} + +void ConfigCCAttitudeWidget::timeout() { + QMutexLocker locker(&startStop); + UAVObjectManager * objMngr = getObjectManager(); + UAVDataObject * obj = dynamic_cast(getObjectManager()->getObject(QString("AttitudeRaw"))); + disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(attitudeRawUpdated(UAVObject*))); + disconnect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); + + QErrorMessage errmsg; + errmsg.showMessage("Calibration timed out before receiving required updates"); + errmsg.exec(); +} + +void ConfigCCAttitudeWidget::startAccelCalibration() { + QMutexLocker locker(&startStop); + + qDebug() << "Click"; + updates = 0; + x_accum.clear(); + y_accum.clear(); + z_accum.clear(); + + // Set up to receive updates + UAVDataObject * obj = dynamic_cast(getObjectManager()->getObject(QString("AttitudeRaw"))); + connect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(attitudeRawUpdated(UAVObject*))); + + // Set up timeout timer + timer.start(2000); + connect(&timer,SIGNAL(timeout()),this,SLOT(timeout())); + + // Speed up updates + initialMdata = obj->getMetadata(); + UAVObject::Metadata mdata = initialMdata; + mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; + mdata.flightTelemetryUpdatePeriod = 100; + obj->setMetadata(mdata); + +} diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h new file mode 100644 index 000000000..fc8983aae --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h @@ -0,0 +1,68 @@ +/** + ****************************************************************************** + * + * @file configccattitudewidget.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief Configure the properties of the attitude module in CopterControl + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef CCATTITUDEWIDGET_H +#define CCATTITUDEWIDGET_H + +#include "ui_ccattitude.h" +#include "configtaskwidget.h" +#include "extensionsystem/pluginmanager.h" +#include "uavobjectmanager.h" +#include "uavobject.h" +#include +#include +#include + +class Ui_Widget; + +class ConfigCCAttitudeWidget : public ConfigTaskWidget +{ + Q_OBJECT + +public: + explicit ConfigCCAttitudeWidget(QWidget *parent = 0); + ~ConfigCCAttitudeWidget(); + +private slots: + void attitudeRawUpdated(UAVObject *); + void timeout(); + void startAccelCalibration(); + +private: + QMutex startStop; + Ui_ccattitude *ui; + QTimer timer; + UAVObject::Metadata initialMdata; + + int updates; + + QList x_accum, y_accum, z_accum; + + static const int NUM_ACCEL_UPDATES = 10; + static const float ACCEL_SCALE = 0.004f * 9.81f; +}; + +#endif // CCATTITUDEWIDGET_H