Added Levelling functionality.

ground/openpilotgcs/src/plugins/setupwizard/levellingutil.cpp

@@ -0,0 +1,179 @@
+/**
+ ******************************************************************************
+ *
+ * @file       levellingutil.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @addtogroup
+ * @{
+ * @addtogroup LevellingUtil
+ * @{
+ * @brief
+ *****************************************************************************/
+/*
+ * 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 "levellingutil.h"
+#include "extensionsystem/pluginmanager.h"
+#include "uavobjectmanager.h"
+#include "attitudesettings.h"
+#include "accels.h"
+#include "gyros.h"
+
+
+LevellingUtil::LevellingUtil(long measurementCount, long measurementPeriod) : QObject(),
+    m_isMeasuring(false), m_measurementCount(measurementCount), m_measurementPeriod(measurementPeriod)
+{
+}
+
+void LevellingUtil::start()
+{
+    if(!m_isMeasuring) {
+        startMeasurement();
+
+        // Set up timeout timer
+        connect(&m_timeoutTimer, SIGNAL(timeout()), this, SLOT(timeout()));
+        m_timeoutTimer.start(m_measurementCount * m_measurementPeriod * 2);
+    }
+}
+
+void LevellingUtil::abort()
+{
+    if(m_isMeasuring) {
+        stopMeasurement();
+    }
+}
+
+void LevellingUtil::measurementsUpdated(UAVObject *obj)
+{
+    QMutexLocker locker(&m_measurementMutex);
+
+    m_receivedUpdates++;
+    emit progress(m_receivedUpdates, m_measurementCount);
+
+    if(m_receivedUpdates < m_measurementCount) {
+        ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
+        UAVObjectManager * uavObjectManager = pm->getObject<UAVObjectManager>();
+        Q_ASSERT(uavObjectManager);
+
+        Accels * accels = Accels::GetInstance(uavObjectManager);
+        Accels::DataFields accelsData = accels->getData();
+
+        m_accelerometerX.append(accelsData.x);
+        m_accelerometerY.append(accelsData.y);
+        m_accelerometerZ.append(accelsData.z);
+
+        Gyros * gyros = Gyros::GetInstance(uavObjectManager);
+        Gyros::DataFields gyrosData = gyros->getData();
+
+        m_gyroX.append(gyrosData.x);
+        m_gyroY.append(gyrosData.y);
+        m_gyroZ.append(gyrosData.z);
+    }
+    else if (m_receivedUpdates >= m_measurementCount) {
+        stopMeasurement();
+        emit done(calculateLevellingData());
+    }
+}
+
+void LevellingUtil::timeout()
+{
+    QMutexLocker locker(&m_measurementMutex);
+
+    stopMeasurement();
+    emit timeout(tr("Calibration timed out before receiving required updates."));
+}
+
+void LevellingUtil::startMeasurement()
+{
+    QMutexLocker locker(&m_measurementMutex);
+
+    m_isMeasuring = true;
+
+    // Reset variables
+    m_receivedUpdates = 0;
+    m_accelerometerX.clear();
+    m_accelerometerY.clear();
+    m_accelerometerZ.clear();
+    m_gyroX.clear();
+    m_gyroY.clear();
+    m_gyroZ.clear();
+
+    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
+    UAVObjectManager * uavObjectManager = pm->getObject<UAVObjectManager>();
+    Q_ASSERT(uavObjectManager);
+
+    // Disable gyro bias correction to see raw data
+    AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData();
+    attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
+    AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData);
+
+    // Set up to receive updates
+    UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager);
+    connect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(measurementsUpdated(UAVObject*)));
+
+    // Set update period
+    m_previousMetaData = uavObject->getMetadata();
+    UAVObject::Metadata newMetaData = m_previousMetaData;
+    UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC);
+    newMetaData.flightTelemetryUpdatePeriod = m_measurementPeriod;
+    uavObject->setMetadata(newMetaData);
+}
+
+void LevellingUtil::stopMeasurement()
+{
+    m_isMeasuring = false;
+
+    //Stop timeout timer
+    m_timeoutTimer.stop();
+    disconnect(&m_timeoutTimer, SIGNAL(timeout()), this, SLOT(timeout()));
+
+    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
+    UAVObjectManager * uavObjectManager = pm->getObject<UAVObjectManager>();
+    Q_ASSERT(uavObjectManager);
+
+    // Stop listening for updates
+    UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager);
+    disconnect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(measurementsUpdated(UAVObject*)));
+    uavObject->setMetadata(m_previousMetaData);
+
+    // Enable gyro bias correction again
+    AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData();
+    attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE;
+    AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData);
+}
+
+accelGyroBias LevellingUtil::calculateLevellingData()
+{
+    accelGyroBias bias;
+    bias.m_accelerometerXBias = listMean(m_accelerometerX) / ACCELERATION_SCALE;
+    bias.m_accelerometerYBias = listMean(m_accelerometerY) / ACCELERATION_SCALE;
+    bias.m_accelerometerZBias = (listMean(m_accelerometerZ) + G) / ACCELERATION_SCALE;
+
+    bias.m_gyroXBias = listMean(m_gyroX) * 100.0f;
+    bias.m_gyroYBias = listMean(m_gyroY) * 100.0f;
+    bias.m_gyroZBias = listMean(m_gyroZ) * 100.0f;
+    return bias;
+}
+
+double LevellingUtil::listMean(QList<double> list)
+{
+    double accum = 0;
+    for(int i = 0; i < list.size(); i++) {
+        accum += list.at(i);
+    }
+    return accum / list.size();
+}
+

ground/openpilotgcs/src/plugins/setupwizard/levellingutil.h

@@ -0,0 +1,95 @@
+/**
+ ******************************************************************************
+ *
+ * @file       levellingutil.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @addtogroup
+ * @{
+ * @addtogroup LevellingUtil
+ * @{
+ * @brief
+ *****************************************************************************/
+/*
+ * 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 LEVELLINGUTIL_H
+#define LEVELLINGUTIL_H
+
+#include <QObject>
+#include <QTimer>
+#include <QMutex>
+
+#include "uavobject.h"
+
+struct accelGyroBias {
+    float m_accelerometerXBias;
+    float m_accelerometerYBias;
+    float m_accelerometerZBias;
+
+    float m_gyroXBias;
+    float m_gyroYBias;
+    float m_gyroZBias;
+};
+
+class LevellingUtil : public QObject
+{
+    Q_OBJECT
+public:
+    explicit LevellingUtil(long measurementCount, long measurementPeriod);
+        
+signals:
+    void progress(long current, long total);
+    void done(accelGyroBias measuredBias);
+    void timeout(QString message);
+
+public slots:
+    void start();
+    void abort();
+
+private slots:
+    void measurementsUpdated(UAVObject * obj);
+    void timeout();
+
+private:
+    static const float G = 9.81f;
+    static const float ACCELERATION_SCALE = 0.004f * 9.81f;
+
+    QMutex m_measurementMutex;
+    QTimer m_timeoutTimer;
+
+    bool m_isMeasuring;
+    long m_receivedUpdates;
+
+    long m_measurementCount;
+    long m_measurementPeriod;
+
+    UAVObject::Metadata m_previousMetaData;
+
+    QList<double> m_accelerometerX;
+    QList<double> m_accelerometerY;
+    QList<double> m_accelerometerZ;
+    QList<double> m_gyroX;
+    QList<double> m_gyroY;
+    QList<double> m_gyroZ;
+
+    void stop();
+    void startMeasurement();
+    void stopMeasurement();
+    accelGyroBias calculateLevellingData();
+    double listMean(QList<double> list);
+};
+
+#endif // LEVELLINGUTIL_H

ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp

@@ -38,7 +38,7 @@ ControllerPage::ControllerPage(SetupWizard *wizard, QWidget *parent) :
 {
     ui->setupUi(this);
 
-    m_connectionManager = Core::ICore::instance()->connectionManager();
+    m_connectionManager = getWizard()->getConnectionManager();
     Q_ASSERT(m_connectionManager);
     connect(m_connectionManager, SIGNAL(availableDevicesChanged(QLinkedList<Core::devListItem>)), this, SLOT(devicesChanged(QLinkedList<Core::devListItem>)));
 

ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.cpp

@@ -25,20 +25,24 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
+#include <QMessageBox>
 #include "levellingpage.h"
 #include "ui_levellingpage.h"
 #include "setupwizard.h"
 
 LevellingPage::LevellingPage(SetupWizard *wizard, QWidget *parent) :
-        AbstractWizardPage(wizard, parent),
-
-    ui(new Ui::LevellingPage)
+    AbstractWizardPage(wizard, parent),
+    ui(new Ui::LevellingPage),  m_levellingUtil(0)
 {
     ui->setupUi(this);
+    connect(ui->levelButton, SIGNAL(clicked()), this, SLOT(performLevelling()));
 }
 
 LevellingPage::~LevellingPage()
 {
+    if(m_levellingUtil) {
+        delete m_levellingUtil;
+    }
     delete ui;
 }
 
@@ -46,3 +50,71 @@ bool LevellingPage::validatePage()
 {
     return true;
 }
+
+bool LevellingPage::isComplete()
+{
+    return getWizard()->isLevellingPerformed();
+}
+
+void LevellingPage::performLevelling()
+{
+    if(!getWizard()->getConnectionManager()->isConnected()) {
+        QMessageBox msgBox;
+        msgBox.setText(tr("An OpenPilot controller must be connected to your computer to perform bias calculations.\nPlease connect your OpenPilot controller to continue."));
+        msgBox.setStandardButtons(QMessageBox::Ok);
+        msgBox.setDefaultButton(QMessageBox::Ok);
+        msgBox.exec();
+        return;
+    }
+
+    if(!m_levellingUtil)
+    {
+        ui->levelButton->setEnabled(false);
+
+        // Measure every 100ms * 100times = 10s
+        m_levellingUtil = new LevellingUtil(BIAS_CYCLES, BIAS_PERIOD);
+        connect(m_levellingUtil, SIGNAL(progress(long,long)), this, SLOT(levellingProgress(long,long)));
+        connect(m_levellingUtil, SIGNAL(done(accelGyroBias)), this, SLOT(levellingDone(accelGyroBias)));
+        connect(m_levellingUtil, SIGNAL(timeout(QString)), this, SLOT(levellingTimeout(QString)));
+    }
+    m_levellingUtil->start();
+}
+
+void LevellingPage::levellingProgress(long current, long total)
+{
+    if(!ui->levellinProgressBar->maximum() != (int)total) {
+        ui->levellinProgressBar->setMaximum((int)total);
+    }
+    if(ui->levellinProgressBar->value() != (int)current) {
+        ui->levellinProgressBar->setValue((int)current);
+    }
+}
+
+void LevellingPage::levellingDone(accelGyroBias bias)
+{
+    stopLevelling();
+    getWizard()->setLevellingBias(bias);
+    emit completeChanged();
+}
+
+void LevellingPage::levellingTimeout(QString message)
+{
+    stopLevelling();
+
+    QMessageBox msgBox;
+    msgBox.setText(message);
+    msgBox.setStandardButtons(QMessageBox::Ok);
+    msgBox.setDefaultButton(QMessageBox::Ok);
+    msgBox.exec();
+}
+
+void LevellingPage::stopLevelling()
+{
+    if(m_levellingUtil)
+    {
+        disconnect(m_levellingUtil, SIGNAL(progress(long,long)), this, SLOT(levellingProgress(long,long)));
+        disconnect(m_levellingUtil, SIGNAL(done(accelGyroBias)), this, SLOT(levellingDone(accelGyroBias)));
+        disconnect(m_levellingUtil, SIGNAL(timeout(QString)), this, SLOT(levellingTimeout(QString)));
+        ui->levelButton->setEnabled(true);
+    }
+}

ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.h

@@ -29,6 +29,7 @@
 #define LEVELLINGPAGE_H
 
 #include "abstractwizardpage.h"
+#include "levellingutil.h"
 
 namespace Ui {
 class LevellingPage;
@@ -42,9 +43,22 @@ public:
     explicit LevellingPage(SetupWizard *wizard, QWidget *parent = 0);
     ~LevellingPage();
     bool validatePage();
+    bool isComplete();
+
+private slots:
+    void performLevelling();
+    void levellingProgress(long current, long total);
+    void levellingDone(accelGyroBias bias);
+    void levellingTimeout(QString message);
 
 private:
+    static const int BIAS_CYCLES = 100;
+    static const int BIAS_PERIOD = 100;
+
     Ui::LevellingPage *ui;
+    LevellingUtil *m_levellingUtil;
+
+    void stopLevelling();
 };
 
 #endif // LEVELLINGPAGE_H

ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.ui

@@ -33,7 +33,7 @@ p, li { white-space: pre-wrap; }
 <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:10pt;"></p>
 <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt;">The wizard needs to get information from the controller about in which position the vehicle normally is considered to be level. To be able to successfully perform these measurements you need to place the vehicle on a flat and as level as possible surface. Example of such surfaces could be a table top or the floor. Be careful to assure that the vehicle really is level since this step will affect the accelerometer and gyro bias in the controller software.</span></p>
 <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:10pt;"></p>
-&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;To perform the levelling, please push the Level button and wait for the process to finish.&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;To perform the levelling, please push the Calculate button and wait for the process to finish.&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
    </property>
    <property name="alignment">
     <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
@@ -63,7 +63,7 @@ p, li { white-space: pre-wrap; }
    <property name="alignment">
     <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
    </property>
-   <widget class="QProgressBar" name="progressBar">
+   <widget class="QProgressBar" name="levellinProgressBar">
     <property name="geometry">
      <rect>
       <x>20</x>
@@ -76,17 +76,17 @@ p, li { white-space: pre-wrap; }
      <number>0</number>
     </property>
    </widget>
-   <widget class="QPushButton" name="pushButton">
+   <widget class="QPushButton" name="levelButton">
     <property name="geometry">
      <rect>
-      <x>424</x>
+      <x>414</x>
       <y>40</y>
-      <width>91</width>
+      <width>101</width>
       <height>23</height>
      </rect>
     </property>
     <property name="text">
-     <string>Level...</string>
+     <string>Calculate...</string>
     </property>
    </widget>
   </widget>

ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp

@@ -41,14 +41,12 @@
 #include "pages/flashpage.h"
 #include "pages/notyetimplementedpage.h"
 
-SetupWizard::SetupWizard(QWidget *parent) : QWizard(parent)
+SetupWizard::SetupWizard(QWidget *parent) : QWizard(parent),
+    m_controllerSelectionMode(CONTROLLER_SELECTION_UNKNOWN), m_controllerType(CONTROLLER_UNKNOWN),
+    m_vehicleType(VEHICLE_UNKNOWN), m_inputType(INPUT_UNKNOWN), m_escType(ESC_UNKNOWN),
+    m_levellingPerformed(false), m_connectionManager(0)
 {
     setWindowTitle("OpenPilot Setup Wizard");
-    m_controllerSelectionMode = CONTROLLER_SELECTION_UNKNOWN;
-    m_controllerType = CONTROLLER_UNKNOWN;
-    m_vehicleType = VEHICLE_UNKNOWN;
-    m_escType = ESC_UNKNOWN;
-    m_inputType = INPUT_UNKNOWN;
     createPages();
 }
 
@@ -185,6 +183,16 @@ QString SetupWizard::getSummaryText()
         default:
             summary.append(tr("Unknown"));
     }
+
+    summary.append('\n');
+    summary.append(tr("Accel & Gyro bias calibrated: "));
+    if (isLevellingPerformed()) {
+        summary.append(tr("Yes"));
+    }
+    else {
+        summary.append(tr("No"));
+    }
+
     return summary;
 }
 

ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h

@@ -29,6 +29,9 @@
 #define SETUPWIZARD_H
 
 #include <QWizard>
+#include "levellingutil.h"
+#include <coreplugin/icore.h>
+#include <coreplugin/connectionmanager.h>
 
 class SetupWizard : public QWizard
 {
@@ -61,8 +64,21 @@ public:
     void setESCType(SetupWizard::ESC_TYPE type) { m_escType = type; }
     SetupWizard::ESC_TYPE getESCType() const { return m_escType; }
 
+    void setLevellingBias(accelGyroBias bias) { m_levellingBias = bias; m_levellingPerformed = true; }
+    bool isLevellingPerformed() { return m_levellingPerformed; }
+    accelGyroBias getLevellingBias() const { return m_levellingBias; }
+
+
     QString getSummaryText();
 
+    Core::ConnectionManager* getConnectionManager() {
+        if (!m_connectionManager) {
+            m_connectionManager = Core::ICore::instance()->connectionManager();
+            Q_ASSERT(m_connectionManager);
+        }
+        return m_connectionManager;
+    }
+
 private:
     enum {PAGE_START, PAGE_CONTROLLER, PAGE_VEHICLES, PAGE_MULTI, PAGE_FIXEDWING,
           PAGE_HELI, PAGE_SURFACE, PAGE_INPUT, PAGE_OUTPUT, PAGE_LEVELLING,
@@ -74,6 +90,11 @@ private:
     VEHICLE_TYPE m_vehicleType;
     INPUT_TYPE m_inputType;
     ESC_TYPE m_escType;
+    bool m_levellingPerformed;
+    accelGyroBias m_levellingBias;
+
+    Core::ConnectionManager *m_connectionManager;
+
 };
 
 #endif // SETUPWIZARD_H

ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro

@@ -23,7 +23,8 @@ HEADERS += setupwizardplugin.h \
     pages/inputpage.h \
     pages/summarypage.h \
     pages/flashpage.h \
-    pages/levellingpage.h
+    pages/levellingpage.h \
+    levellingutil.h
 
 SOURCES += setupwizardplugin.cpp \
     setupwizard.cpp \
@@ -41,7 +42,8 @@ SOURCES += setupwizardplugin.cpp \
     pages/inputpage.cpp \
     pages/summarypage.cpp \
     pages/flashpage.cpp \
-    pages/levellingpage.cpp
+    pages/levellingpage.cpp \
+    levellingutil.cpp
 
 OTHER_FILES += SetupWizard.pluginspec