rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Merge branch 'thread/OP-816_Setup_Wizard_Revo_Support' into next

Browse Source
This commit is contained in:
Fredrik Arvidsson 2013-04-30 06:51:19 +02:00
commit 1eb04a378f
10 changed files with 138 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
ground/openpilotgcs/src/plugins/setupwizard/cccalibrationutil.cpp → ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp
View File

@ -1,11 +1,11 @@
/** /**
****************************************************************************** ******************************************************************************
* *
* @file cccalibrationutil.cpp * @file biascalibrationutil.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @addtogroup * @addtogroup
* @{ * @{
* @addtogroup CCCalibrationUtil * @addtogroup BiasCalibrationUtil
* @{ * @{
* @brief * @brief
*****************************************************************************/ *****************************************************************************/
@ -25,28 +25,29 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#include "cccalibrationutil.h" #include "biascalibrationutil.h"
#include "extensionsystem/pluginmanager.h" #include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h" #include "uavobjectmanager.h"
#include "attitudesettings.h" #include "attitudesettings.h"
#include "accels.h" #include "accels.h"
#include "gyros.h" #include "gyros.h"
#include "qdebug.h"
CCCalibrationUtil::CCCalibrationUtil(long measurementCount, long measurementRate) : QObject(), BiasCalibrationUtil::BiasCalibrationUtil(long measurementCount, long measurementRate) : QObject(),
m_isMeasuring(false), m_accelMeasurementCount(measurementCount), m_gyroMeasurementCount(measurementCount), m_isMeasuring(false), m_accelMeasurementCount(measurementCount), m_gyroMeasurementCount(measurementCount),
m_accelMeasurementRate(measurementRate), m_gyroMeasurementRate(measurementRate) m_accelMeasurementRate(measurementRate), m_gyroMeasurementRate(measurementRate)
{ {
} }
CCCalibrationUtil::CCCalibrationUtil(long accelMeasurementCount, long accelMeasurementRate, BiasCalibrationUtil::BiasCalibrationUtil(long accelMeasurementCount, long accelMeasurementRate,
long gyroMeasurementCount, long gyroMeasurementRate) : QObject(), long gyroMeasurementCount, long gyroMeasurementRate) : QObject(),
m_isMeasuring(false), m_accelMeasurementCount(accelMeasurementCount), m_gyroMeasurementCount(gyroMeasurementCount), m_isMeasuring(false), m_accelMeasurementCount(accelMeasurementCount), m_gyroMeasurementCount(gyroMeasurementCount),
m_accelMeasurementRate(accelMeasurementRate), m_gyroMeasurementRate(gyroMeasurementRate) m_accelMeasurementRate(accelMeasurementRate), m_gyroMeasurementRate(gyroMeasurementRate)
{ {
} }
void CCCalibrationUtil::start() void BiasCalibrationUtil::start()
{ {
if(!m_isMeasuring) { if(!m_isMeasuring) {
startMeasurement(); startMeasurement();
@ -58,17 +59,16 @@ void CCCalibrationUtil::start()
} }
} }
void CCCalibrationUtil::abort() void BiasCalibrationUtil::abort()
{ {
if(m_isMeasuring) { if(m_isMeasuring) {
stopMeasurement(); stopMeasurement();
} }
} }
void CCCalibrationUtil::gyroMeasurementsUpdated(UAVObject *obj) void BiasCalibrationUtil::gyroMeasurementsUpdated(UAVObject *obj)
{ {
Q_UNUSED(obj); Q_UNUSED(obj);
QMutexLocker locker(&m_measurementMutex);
if(m_receivedGyroUpdates < m_gyroMeasurementCount) { if(m_receivedGyroUpdates < m_gyroMeasurementCount) {
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
@ -88,14 +88,12 @@ void CCCalibrationUtil::gyroMeasurementsUpdated(UAVObject *obj)
else if (m_receivedAccelUpdates >= m_accelMeasurementCount && else if (m_receivedAccelUpdates >= m_accelMeasurementCount &&
m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) { m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) {
stopMeasurement(); stopMeasurement();
emit done(calculateLevellingData());
} }
} }
void CCCalibrationUtil::accelMeasurementsUpdated(UAVObject *obj) void BiasCalibrationUtil::accelMeasurementsUpdated(UAVObject *obj)
{ {
Q_UNUSED(obj); Q_UNUSED(obj);
QMutexLocker locker(&m_measurementMutex);
if(m_receivedAccelUpdates < m_accelMeasurementCount) { if(m_receivedAccelUpdates < m_accelMeasurementCount) {
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
@ -115,22 +113,17 @@ void CCCalibrationUtil::accelMeasurementsUpdated(UAVObject *obj)
else if (m_receivedAccelUpdates >= m_accelMeasurementCount && else if (m_receivedAccelUpdates >= m_accelMeasurementCount &&
m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) { m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) {
stopMeasurement(); stopMeasurement();
emit done(calculateLevellingData());
} }
} }
void CCCalibrationUtil::timeout() void BiasCalibrationUtil::timeout()
{ {
QMutexLocker locker(&m_measurementMutex);
stopMeasurement(); stopMeasurement();
emit timeout(tr("Calibration timed out before receiving required updates.")); emit timeout(tr("Calibration timed out before receiving required updates."));
} }
void CCCalibrationUtil::startMeasurement() void BiasCalibrationUtil::startMeasurement()
{ {
QMutexLocker locker(&m_measurementMutex);
m_isMeasuring = true; m_isMeasuring = true;
// Reset variables // Reset variables
@ -176,8 +169,10 @@ void CCCalibrationUtil::startMeasurement()
uavObject->setMetadata(newMetaData); uavObject->setMetadata(newMetaData);
} }
void CCCalibrationUtil::stopMeasurement() void BiasCalibrationUtil::stopMeasurement()
{ {
qDebug() << "Sampling done, G =" << m_receivedGyroUpdates << "A =" << m_receivedAccelUpdates;
m_isMeasuring = false; m_isMeasuring = false;
//Stop timeout timer //Stop timeout timer
@ -202,18 +197,16 @@ void CCCalibrationUtil::stopMeasurement()
AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData(); AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData();
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE; attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE;
AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData); AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData);
}
accelGyroBias CCCalibrationUtil::calculateLevellingData()
{
accelGyroBias bias; accelGyroBias bias;
bias.m_accelerometerXBias = m_accelerometerX / (double)m_receivedAccelUpdates / ACCELERATION_SCALE; bias.m_accelerometerXBias = m_accelerometerX / (double)m_receivedAccelUpdates;
bias.m_accelerometerYBias = m_accelerometerY / (double)m_receivedAccelUpdates / ACCELERATION_SCALE; bias.m_accelerometerYBias = m_accelerometerY / (double)m_receivedAccelUpdates;
bias.m_accelerometerZBias = (m_accelerometerZ / (double)m_receivedAccelUpdates + G) / ACCELERATION_SCALE; bias.m_accelerometerZBias = m_accelerometerZ / (double)m_receivedAccelUpdates;
bias.m_gyroXBias = m_gyroX / (double)m_receivedGyroUpdates * 100.0; bias.m_gyroXBias = m_gyroX / (double)m_receivedGyroUpdates;
bias.m_gyroYBias = m_gyroY / (double)m_receivedGyroUpdates * 100.0; bias.m_gyroYBias = m_gyroY / (double)m_receivedGyroUpdates;
bias.m_gyroZBias = m_gyroZ / (double)m_receivedGyroUpdates * 100.0; bias.m_gyroZBias = m_gyroZ / (double)m_receivedGyroUpdates;
return bias;
qDebug() << "Bias calculations finished";
emit done(bias);
} }

22
ground/openpilotgcs/src/plugins/setupwizard/cccalibrationutil.h → ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.h
View File

@ -1,11 +1,11 @@
/** /**
****************************************************************************** ******************************************************************************
* *
* @file cccalibrationutil.h * @file biascalibrationutil.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @addtogroup * @addtogroup
* @{ * @{
* @addtogroup CCCalibrationUtil * @addtogroup BiasCalibrationUtil
* @{ * @{
* @brief * @brief
*****************************************************************************/ *****************************************************************************/
@ -25,22 +25,21 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#ifndef CCCALIBRATIONUTIL_H #ifndef BIASCALIBRATIONUTIL_H
#define CCCALIBRATIONUTIL_H #define BIASCALIBRATIONUTIL_H
#include <QObject> #include <QObject>
#include <QTimer> #include <QTimer>
#include <QMutex>
#include "uavobject.h" #include "uavobject.h"
#include "vehicleconfigurationsource.h" #include "vehicleconfigurationsource.h"
class CCCalibrationUtil : public QObject class BiasCalibrationUtil : public QObject
{ {
Q_OBJECT Q_OBJECT
public: public:
explicit CCCalibrationUtil(long measurementCount, long measurementRate); explicit BiasCalibrationUtil(long measurementCount, long measurementRate);
explicit CCCalibrationUtil(long accelMeasurementCount, long accelMeasurementRate, explicit BiasCalibrationUtil(long accelMeasurementCount, long accelMeasurementRate,
long gyroMeasurementCount, long gyroMeasurementRate); long gyroMeasurementCount, long gyroMeasurementRate);
signals: signals:
@ -58,10 +57,6 @@ private slots:
void timeout(); void timeout();
private: private:
static const float G = 9.81f;
static const float ACCELERATION_SCALE = 0.004f * 9.81f;
QMutex m_measurementMutex;
QTimer m_timeoutTimer; QTimer m_timeoutTimer;
bool m_isMeasuring; bool m_isMeasuring;
@ -86,7 +81,6 @@ private:
void stop(); void stop();
void startMeasurement(); void startMeasurement();
void stopMeasurement(); void stopMeasurement();
accelGyroBias calculateLevellingData();
}; };
#endif // CCCALIBRATIONUTIL_H #endif // BIASCALIBRATIONUTIL_H

32
ground/openpilotgcs/src/plugins/setupwizard/pages/cccalibrationpage.cpp → ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.cpp
View File

@ -1,11 +1,11 @@
/** /**
****************************************************************************** ******************************************************************************
* *
* @file cccalibrationpage.cpp * @file biascalibrationpage.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @addtogroup * @addtogroup
* @{ * @{
* @addtogroup CCCalibrationPage * @addtogroup BiasCalibrationPage
* @{ * @{
* @brief * @brief
*****************************************************************************/ *****************************************************************************/
@ -27,19 +27,19 @@
#include <QMessageBox> #include <QMessageBox>
#include <QDebug> #include <QDebug>
#include "cccalibrationpage.h" #include "biascalibrationpage.h"
#include "ui_cccalibrationpage.h" #include "ui_biascalibrationpage.h"
#include "setupwizard.h" #include "setupwizard.h"
CCCalibrationPage::CCCalibrationPage(SetupWizard *wizard, QWidget *parent) : BiasCalibrationPage::BiasCalibrationPage(SetupWizard *wizard, QWidget *parent) :
AbstractWizardPage(wizard, parent), AbstractWizardPage(wizard, parent),
ui(new Ui::CCCalibrationPage), m_calibrationUtil(0) ui(new Ui::BiasCalibrationPage), m_calibrationUtil(0)
{ {
ui->setupUi(this); ui->setupUi(this);
connect(ui->levelButton, SIGNAL(clicked()), this, SLOT(performCalibration())); connect(ui->levelButton, SIGNAL(clicked()), this, SLOT(performCalibration()));
} }
CCCalibrationPage::~CCCalibrationPage() BiasCalibrationPage::~BiasCalibrationPage()
{ {
if(m_calibrationUtil) { if(m_calibrationUtil) {
delete m_calibrationUtil; delete m_calibrationUtil;
@ -47,17 +47,17 @@ CCCalibrationPage::~CCCalibrationPage()
delete ui; delete ui;
} }
bool CCCalibrationPage::validatePage() bool BiasCalibrationPage::validatePage()
{ {
return true; return true;
} }
bool CCCalibrationPage::isComplete() const bool BiasCalibrationPage::isComplete() const
{ {
return ui->levelButton->isEnabled(); return ui->levelButton->isEnabled();
} }
void CCCalibrationPage::enableButtons(bool enable) void BiasCalibrationPage::enableButtons(bool enable)
{ {
ui->levelButton->setEnabled(enable); ui->levelButton->setEnabled(enable);
getWizard()->button(QWizard::NextButton)->setEnabled(enable); getWizard()->button(QWizard::NextButton)->setEnabled(enable);
@ -67,7 +67,7 @@ void CCCalibrationPage::enableButtons(bool enable)
QApplication::processEvents(); QApplication::processEvents();
} }
void CCCalibrationPage::performCalibration() void BiasCalibrationPage::performCalibration()
{ {
if(!getWizard()->getConnectionManager()->isConnected()) { if(!getWizard()->getConnectionManager()->isConnected()) {
QMessageBox msgBox; QMessageBox msgBox;
@ -85,7 +85,7 @@ void CCCalibrationPage::performCalibration()
if(!m_calibrationUtil) if(!m_calibrationUtil)
{ {
m_calibrationUtil = new CCCalibrationUtil(BIAS_CYCLES, BIAS_RATE); m_calibrationUtil = new BiasCalibrationUtil(BIAS_CYCLES, BIAS_RATE);
} }
connect(m_calibrationUtil, SIGNAL(progress(long,long)), this, SLOT(calibrationProgress(long,long))); connect(m_calibrationUtil, SIGNAL(progress(long,long)), this, SLOT(calibrationProgress(long,long)));
@ -95,7 +95,7 @@ void CCCalibrationPage::performCalibration()
m_calibrationUtil->start(); m_calibrationUtil->start();
} }
void CCCalibrationPage::calibrationProgress(long current, long total) void BiasCalibrationPage::calibrationProgress(long current, long total)
{ {
if(ui->levellinProgressBar->maximum() != (int)total) { if(ui->levellinProgressBar->maximum() != (int)total) {
ui->levellinProgressBar->setMaximum((int)total); ui->levellinProgressBar->setMaximum((int)total);
@ -105,14 +105,14 @@ void CCCalibrationPage::calibrationProgress(long current, long total)
} }
} }
void CCCalibrationPage::calibrationDone(accelGyroBias bias) void BiasCalibrationPage::calibrationDone(accelGyroBias bias)
{ {
stopCalibration(); stopCalibration();
getWizard()->setLevellingBias(bias); getWizard()->setLevellingBias(bias);
emit completeChanged(); emit completeChanged();
} }
void CCCalibrationPage::calibrationTimeout(QString message) void BiasCalibrationPage::calibrationTimeout(QString message)
{ {
stopCalibration(); stopCalibration();
@ -123,7 +123,7 @@ void CCCalibrationPage::calibrationTimeout(QString message)
msgBox.exec(); msgBox.exec();
} }
void CCCalibrationPage::stopCalibration() void BiasCalibrationPage::stopCalibration()
{ {
if(m_calibrationUtil) if(m_calibrationUtil)
{ {

26
ground/openpilotgcs/src/plugins/setupwizard/pages/cccalibrationpage.h → ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.h
View File

@ -1,11 +1,11 @@
/** /**
****************************************************************************** ******************************************************************************
* *
* @file cccalibrationpage.h * @file biascalibrationpage.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @addtogroup * @addtogroup
* @{ * @{
* @addtogroup CCCalibrationPage * @addtogroup BiasCalibrationPage
* @{ * @{
* @brief * @brief
*****************************************************************************/ *****************************************************************************/
@ -25,23 +25,23 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#ifndef CCCALIBRATIONPAGE_H #ifndef BIASCALIBRATIONPAGE_H
#define CCCALIBRATIONPAGE_H #define BIASCALIBRATIONPAGE_H
#include "abstractwizardpage.h" #include "abstractwizardpage.h"
#include "cccalibrationutil.h" #include "biascalibrationutil.h"
namespace Ui { namespace Ui {
class CCCalibrationPage; class BiasCalibrationPage;
} }
class CCCalibrationPage : public AbstractWizardPage class BiasCalibrationPage : public AbstractWizardPage
{ {
Q_OBJECT Q_OBJECT
public: public:
explicit CCCalibrationPage(SetupWizard *wizard, QWidget *parent = 0); explicit BiasCalibrationPage(SetupWizard *wizard, QWidget *parent = 0);
~CCCalibrationPage(); ~BiasCalibrationPage();
bool validatePage(); bool validatePage();
bool isComplete() const; bool isComplete() const;
@ -53,13 +53,13 @@ private slots:
private: private:
static const int BIAS_CYCLES = 200; static const int BIAS_CYCLES = 200;
static const int BIAS_RATE = 30; static const int BIAS_RATE = 50;
Ui::CCCalibrationPage *ui; Ui::BiasCalibrationPage *ui;
CCCalibrationUtil *m_calibrationUtil; BiasCalibrationUtil *m_calibrationUtil;
void stopCalibration(); void stopCalibration();
void enableButtons(bool enable); void enableButtons(bool enable);
}; };
#endif // CCCALIBRATIONPAGE_H #endif // BIASCALIBRATIONPAGE_H

4
ground/openpilotgcs/src/plugins/setupwizard/pages/cccalibrationpage.ui → ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.ui
View File

@ -1,7 +1,7 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0"> <ui version="4.0">
<class>CCCalibrationPage</class> <class>BiasCalibrationPage</class>
<widget class="QWizardPage" name="CCCalibrationPage"> <widget class="QWizardPage" name="BiasCalibrationPage">
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>

15
ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp
View File

@ -36,7 +36,7 @@
#include "pages/surfacepage.h" #include "pages/surfacepage.h"
#include "pages/inputpage.h" #include "pages/inputpage.h"
#include "pages/outputpage.h" #include "pages/outputpage.h"
#include "pages/cccalibrationpage.h" #include "pages/biascalibrationpage.h"
#include "pages/summarypage.h" #include "pages/summarypage.h"
#include "pages/savepage.h" #include "pages/savepage.h"
#include "pages/notyetimplementedpage.h" #include "pages/notyetimplementedpage.h"
@ -118,10 +118,10 @@ int SetupWizard::nextId() const
return PAGE_VEHICLES; return PAGE_VEHICLES;
case PAGE_OUTPUT: case PAGE_OUTPUT:
return PAGE_SUMMARY; return PAGE_SUMMARY;
case PAGE_CC_CALIBRATION: case PAGE_BIAS_CALIBRATION:
return PAGE_OUTPUT_CALIBRATION;
case PAGE_REVO_CALIBRATION:
return PAGE_OUTPUT_CALIBRATION; return PAGE_OUTPUT_CALIBRATION;
//case PAGE_REVO_CALIBRATION:
// return PAGE_OUTPUT_CALIBRATION;
case PAGE_OUTPUT_CALIBRATION: case PAGE_OUTPUT_CALIBRATION:
return PAGE_SAVE; return PAGE_SAVE;
case PAGE_SUMMARY: case PAGE_SUMMARY:
@ -130,9 +130,8 @@ int SetupWizard::nextId() const
{ {
case CONTROLLER_CC: case CONTROLLER_CC:
case CONTROLLER_CC3D: case CONTROLLER_CC3D:
return PAGE_CC_CALIBRATION;
case CONTROLLER_REVO: case CONTROLLER_REVO:
return PAGE_REVO_CALIBRATION; return PAGE_BIAS_CALIBRATION;
default: default:
return PAGE_NOTYETIMPLEMENTED; return PAGE_NOTYETIMPLEMENTED;
} }
@ -289,8 +288,8 @@ void SetupWizard::createPages()
setPage(PAGE_SURFACE, new SurfacePage(this)); setPage(PAGE_SURFACE, new SurfacePage(this));
setPage(PAGE_INPUT, new InputPage(this)); setPage(PAGE_INPUT, new InputPage(this));
setPage(PAGE_OUTPUT, new OutputPage(this)); setPage(PAGE_OUTPUT, new OutputPage(this));
setPage(PAGE_CC_CALIBRATION, new CCCalibrationPage(this)); setPage(PAGE_BIAS_CALIBRATION, new BiasCalibrationPage(this));
setPage(PAGE_REVO_CALIBRATION, new RevoCalibrationPage(this)); //setPage(PAGE_REVO_CALIBRATION, new RevoCalibrationPage(this));
setPage(PAGE_OUTPUT_CALIBRATION, new OutputCalibrationPage(this)); setPage(PAGE_OUTPUT_CALIBRATION, new OutputCalibrationPage(this));
setPage(PAGE_SUMMARY, new SummaryPage(this)); setPage(PAGE_SUMMARY, new SummaryPage(this));
setPage(PAGE_SAVE, new SavePage(this)); setPage(PAGE_SAVE, new SavePage(this));

4
ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h
View File

@ -29,7 +29,7 @@
#define SETUPWIZARD_H #define SETUPWIZARD_H
#include <QWizard> #include <QWizard>
#include "cccalibrationutil.h" #include "biascalibrationutil.h"
#include <coreplugin/icore.h> #include <coreplugin/icore.h>
#include <coreplugin/connectionmanager.h> #include <coreplugin/connectionmanager.h>
#include "vehicleconfigurationsource.h" #include "vehicleconfigurationsource.h"
@ -90,7 +90,7 @@ private slots:
void pageChanged(int currId); void pageChanged(int currId);
private: private:
enum {PAGE_START, PAGE_CONTROLLER, PAGE_VEHICLES, PAGE_MULTI, PAGE_FIXEDWING, enum {PAGE_START, PAGE_CONTROLLER, PAGE_VEHICLES, PAGE_MULTI, PAGE_FIXEDWING,
PAGE_HELI, PAGE_SURFACE, PAGE_INPUT, PAGE_OUTPUT, PAGE_CC_CALIBRATION, PAGE_HELI, PAGE_SURFACE, PAGE_INPUT, PAGE_OUTPUT, PAGE_BIAS_CALIBRATION,
PAGE_REVO_CALIBRATION, PAGE_OUTPUT_CALIBRATION, PAGE_SAVE, PAGE_SUMMARY, PAGE_REVO_CALIBRATION, PAGE_OUTPUT_CALIBRATION, PAGE_SAVE, PAGE_SUMMARY,
PAGE_NOTYETIMPLEMENTED, PAGE_REBOOT, PAGE_END, PAGE_UPDATE}; PAGE_NOTYETIMPLEMENTED, PAGE_REBOOT, PAGE_END, PAGE_UPDATE};
void createPages(); void createPages();

16
ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro
View File

@ -33,9 +33,9 @@ HEADERS += setupwizardplugin.h \
pages/rebootpage.h \ pages/rebootpage.h \
pages/savepage.h \ pages/savepage.h \
pages/autoupdatepage.h \ pages/autoupdatepage.h \
pages/cccalibrationpage.h \ pages/revocalibrationpage.h \
cccalibrationutil.h \ biascalibrationutil.h \
pages/revocalibrationpage.h pages/biascalibrationpage.h
SOURCES += setupwizardplugin.cpp \ SOURCES += setupwizardplugin.cpp \
setupwizard.cpp \ setupwizard.cpp \
@ -60,9 +60,9 @@ SOURCES += setupwizardplugin.cpp \
pages/rebootpage.cpp \ pages/rebootpage.cpp \
pages/savepage.cpp \ pages/savepage.cpp \
pages/autoupdatepage.cpp \ pages/autoupdatepage.cpp \
pages/cccalibrationpage.cpp \ pages/revocalibrationpage.cpp \
cccalibrationutil.cpp \ biascalibrationutil.cpp \
pages/revocalibrationpage.cpp pages/biascalibrationpage.cpp
OTHER_FILES += SetupWizard.pluginspec OTHER_FILES += SetupWizard.pluginspec
@ -84,8 +84,8 @@ FORMS += \
pages/rebootpage.ui \ pages/rebootpage.ui \
pages/savepage.ui \ pages/savepage.ui \
pages/autoupdatepage.ui \ pages/autoupdatepage.ui \
pages/cccalibrationpage.ui \ pages/revocalibrationpage.ui \
pages/revocalibrationpage.ui pages/biascalibrationpage.ui
RESOURCES += \ RESOURCES += \
wizardResources.qrc wizardResources.qrc

71
ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp
View File

@ -34,6 +34,7 @@
#include "systemsettings.h" #include "systemsettings.h"
#include "manualcontrolsettings.h" #include "manualcontrolsettings.h"
#include "stabilizationsettings.h" #include "stabilizationsettings.h"
#include "revocalibration.h"
const qint16 VehicleConfigurationHelper::LEGACY_ESC_FREQUENCE = 50; const qint16 VehicleConfigurationHelper::LEGACY_ESC_FREQUENCE = 50;
const qint16 VehicleConfigurationHelper::RAPID_ESC_FREQUENCE = 400; const qint16 VehicleConfigurationHelper::RAPID_ESC_FREQUENCE = 400;
@ -64,7 +65,11 @@ bool VehicleConfigurationHelper::setupVehicle(bool save)
applyVehicleConfiguration(); applyVehicleConfiguration();
applyActuatorConfiguration(); applyActuatorConfiguration();
applyFlighModeConfiguration(); applyFlighModeConfiguration();
applyLevellingConfiguration();
if(save) {
applySensorBiasConfiguration();
}
applyStabilizationConfiguration(); applyStabilizationConfiguration();
applyManualControlDefaults(); applyManualControlDefaults();
@ -327,25 +332,63 @@ void VehicleConfigurationHelper::applyFlighModeConfiguration()
addModifiedObject(controlSettings, tr("Writing flight mode settings")); addModifiedObject(controlSettings, tr("Writing flight mode settings"));
} }
void VehicleConfigurationHelper::applyLevellingConfiguration() void VehicleConfigurationHelper::applySensorBiasConfiguration()
{ {
AttitudeSettings* attitudeSettings = AttitudeSettings::GetInstance(m_uavoManager);
Q_ASSERT(attitudeSettings);
AttitudeSettings::DataFields data = attitudeSettings->getData();
if(m_configSource->isCalibrationPerformed()) if(m_configSource->isCalibrationPerformed())
{ {
accelGyroBias bias = m_configSource->getCalibrationBias(); accelGyroBias bias = m_configSource->getCalibrationBias();
float G = 9.81f;
data.AccelBias[0] += bias.m_accelerometerXBias; switch(m_configSource->getControllerType()) {
data.AccelBias[1] += bias.m_accelerometerYBias; case VehicleConfigurationSource::CONTROLLER_CC:
data.AccelBias[2] += bias.m_accelerometerZBias; case VehicleConfigurationSource::CONTROLLER_CC3D:
data.GyroBias[0] = -bias.m_gyroXBias; {
data.GyroBias[1] = -bias.m_gyroYBias; const float ACCELERATION_SCALE = 0.004f * G;
data.GyroBias[2] = -bias.m_gyroZBias; const float GYRO_SCALE = 100.0f;
AttitudeSettings* copterControlCalibration = AttitudeSettings::GetInstance(m_uavoManager);
Q_ASSERT(copterControlCalibration);
AttitudeSettings::DataFields data = copterControlCalibration->getData();
data.AccelTau = DEFAULT_ENABLED_ACCEL_TAU;
data.AccelBias[AttitudeSettings::ACCELBIAS_X] += bias.m_accelerometerXBias / ACCELERATION_SCALE;
data.AccelBias[AttitudeSettings::ACCELBIAS_Y] += bias.m_accelerometerYBias / ACCELERATION_SCALE;
data.AccelBias[AttitudeSettings::ACCELBIAS_Z] += (bias.m_accelerometerZBias + G) / ACCELERATION_SCALE;
data.GyroBias[AttitudeSettings::GYROBIAS_X] = -(bias.m_gyroXBias * GYRO_SCALE);
data.GyroBias[AttitudeSettings::GYROBIAS_Y] = -(bias.m_gyroYBias * GYRO_SCALE);
data.GyroBias[AttitudeSettings::GYROBIAS_Z] = -(bias.m_gyroZBias * GYRO_SCALE);
copterControlCalibration->setData(data);
addModifiedObject(copterControlCalibration, tr("Writing gyro and accelerometer bias settings"));
break;
}
case VehicleConfigurationSource::CONTROLLER_REVO:
{
RevoCalibration* revolutionCalibration = RevoCalibration::GetInstance(m_uavoManager);
Q_ASSERT(revolutionCalibration);
RevoCalibration::DataFields data = revolutionCalibration->getData();
data.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE;
data.accel_bias[RevoCalibration::ACCEL_BIAS_X] += bias.m_accelerometerXBias;
data.accel_bias[RevoCalibration::ACCEL_BIAS_Y] += bias.m_accelerometerYBias;
data.accel_bias[RevoCalibration::ACCEL_BIAS_Z] += bias.m_accelerometerZBias + G;
data.gyro_bias[RevoCalibration::GYRO_BIAS_X] = bias.m_gyroXBias;
data.gyro_bias[RevoCalibration::GYRO_BIAS_Y] = bias.m_gyroYBias;
data.gyro_bias[RevoCalibration::GYRO_BIAS_Z] = bias.m_gyroZBias;
revolutionCalibration->setData(data);
addModifiedObject(revolutionCalibration, tr("Writing gyro and accelerometer bias settings"));
break;
}
default:
//Something went terribly wrong.
break;
}
} }
data.AccelTau = DEFAULT_ENABLED_ACCEL_TAU;
attitudeSettings->setData(data);
addModifiedObject(attitudeSettings, tr("Writing gyro and accelerometer bias settings"));
} }
void VehicleConfigurationHelper::applyStabilizationConfiguration() void VehicleConfigurationHelper::applyStabilizationConfiguration()

2
ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h
View File

@ -81,7 +81,7 @@ private:
void applyVehicleConfiguration(); void applyVehicleConfiguration();
void applyActuatorConfiguration(); void applyActuatorConfiguration();
void applyFlighModeConfiguration(); void applyFlighModeConfiguration();
void applyLevellingConfiguration(); void applySensorBiasConfiguration();
void applyStabilizationConfiguration(); void applyStabilizationConfiguration();
void applyManualControlDefaults(); void applyManualControlDefaults();