diff --git a/ground/openpilotgcs/src/plugins/setupwizard/cccalibrationutil.cpp b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp similarity index 84% rename from ground/openpilotgcs/src/plugins/setupwizard/cccalibrationutil.cpp rename to ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp index 86ffbabca..bfba40313 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/cccalibrationutil.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp @@ -1,11 +1,11 @@ /** ****************************************************************************** * - * @file cccalibrationutil.cpp + * @file biascalibrationutil.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @addtogroup * @{ - * @addtogroup CCCalibrationUtil + * @addtogroup BiasCalibrationUtil * @{ * @brief *****************************************************************************/ @@ -25,28 +25,29 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "cccalibrationutil.h" +#include "biascalibrationutil.h" #include "extensionsystem/pluginmanager.h" #include "uavobjectmanager.h" #include "attitudesettings.h" #include "accels.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_accelMeasurementRate(measurementRate), m_gyroMeasurementRate(measurementRate) { } -CCCalibrationUtil::CCCalibrationUtil(long accelMeasurementCount, long accelMeasurementRate, +BiasCalibrationUtil::BiasCalibrationUtil(long accelMeasurementCount, long accelMeasurementRate, long gyroMeasurementCount, long gyroMeasurementRate) : QObject(), m_isMeasuring(false), m_accelMeasurementCount(accelMeasurementCount), m_gyroMeasurementCount(gyroMeasurementCount), m_accelMeasurementRate(accelMeasurementRate), m_gyroMeasurementRate(gyroMeasurementRate) { } -void CCCalibrationUtil::start() +void BiasCalibrationUtil::start() { if(!m_isMeasuring) { startMeasurement(); @@ -58,17 +59,16 @@ void CCCalibrationUtil::start() } } -void CCCalibrationUtil::abort() +void BiasCalibrationUtil::abort() { if(m_isMeasuring) { stopMeasurement(); } } -void CCCalibrationUtil::gyroMeasurementsUpdated(UAVObject *obj) +void BiasCalibrationUtil::gyroMeasurementsUpdated(UAVObject *obj) { Q_UNUSED(obj); - QMutexLocker locker(&m_measurementMutex); if(m_receivedGyroUpdates < m_gyroMeasurementCount) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); @@ -88,14 +88,12 @@ void CCCalibrationUtil::gyroMeasurementsUpdated(UAVObject *obj) else if (m_receivedAccelUpdates >= m_accelMeasurementCount && m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) { stopMeasurement(); - emit done(calculateLevellingData()); } } -void CCCalibrationUtil::accelMeasurementsUpdated(UAVObject *obj) +void BiasCalibrationUtil::accelMeasurementsUpdated(UAVObject *obj) { Q_UNUSED(obj); - QMutexLocker locker(&m_measurementMutex); if(m_receivedAccelUpdates < m_accelMeasurementCount) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); @@ -115,22 +113,17 @@ void CCCalibrationUtil::accelMeasurementsUpdated(UAVObject *obj) else if (m_receivedAccelUpdates >= m_accelMeasurementCount && m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) { stopMeasurement(); - emit done(calculateLevellingData()); } } -void CCCalibrationUtil::timeout() +void BiasCalibrationUtil::timeout() { - QMutexLocker locker(&m_measurementMutex); - stopMeasurement(); emit timeout(tr("Calibration timed out before receiving required updates.")); } -void CCCalibrationUtil::startMeasurement() +void BiasCalibrationUtil::startMeasurement() { - QMutexLocker locker(&m_measurementMutex); - m_isMeasuring = true; // Reset variables @@ -176,8 +169,10 @@ void CCCalibrationUtil::startMeasurement() uavObject->setMetadata(newMetaData); } -void CCCalibrationUtil::stopMeasurement() +void BiasCalibrationUtil::stopMeasurement() { + qDebug() << "Sampling done, G =" << m_receivedGyroUpdates << "A =" << m_receivedAccelUpdates; + m_isMeasuring = false; //Stop timeout timer @@ -202,18 +197,16 @@ void CCCalibrationUtil::stopMeasurement() AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData(); attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE; AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData); -} -accelGyroBias CCCalibrationUtil::calculateLevellingData() -{ accelGyroBias bias; - bias.m_accelerometerXBias = m_accelerometerX / (double)m_receivedAccelUpdates / ACCELERATION_SCALE; - bias.m_accelerometerYBias = m_accelerometerY / (double)m_receivedAccelUpdates / ACCELERATION_SCALE; - bias.m_accelerometerZBias = (m_accelerometerZ / (double)m_receivedAccelUpdates + G) / ACCELERATION_SCALE; + bias.m_accelerometerXBias = m_accelerometerX / (double)m_receivedAccelUpdates; + bias.m_accelerometerYBias = m_accelerometerY / (double)m_receivedAccelUpdates; + bias.m_accelerometerZBias = m_accelerometerZ / (double)m_receivedAccelUpdates; - bias.m_gyroXBias = m_gyroX / (double)m_receivedGyroUpdates * 100.0; - bias.m_gyroYBias = m_gyroY / (double)m_receivedGyroUpdates * 100.0; - bias.m_gyroZBias = m_gyroZ / (double)m_receivedGyroUpdates * 100.0; - return bias; + bias.m_gyroXBias = m_gyroX / (double)m_receivedGyroUpdates; + bias.m_gyroYBias = m_gyroY / (double)m_receivedGyroUpdates; + bias.m_gyroZBias = m_gyroZ / (double)m_receivedGyroUpdates; + + qDebug() << "Bias calculations finished"; + emit done(bias); } - diff --git a/ground/openpilotgcs/src/plugins/setupwizard/cccalibrationutil.h b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.h similarity index 79% rename from ground/openpilotgcs/src/plugins/setupwizard/cccalibrationutil.h rename to ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.h index 06947e79c..980305eeb 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/cccalibrationutil.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.h @@ -1,11 +1,11 @@ /** ****************************************************************************** * - * @file cccalibrationutil.h + * @file biascalibrationutil.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @addtogroup * @{ - * @addtogroup CCCalibrationUtil + * @addtogroup BiasCalibrationUtil * @{ * @brief *****************************************************************************/ @@ -25,22 +25,21 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef CCCALIBRATIONUTIL_H -#define CCCALIBRATIONUTIL_H +#ifndef BIASCALIBRATIONUTIL_H +#define BIASCALIBRATIONUTIL_H #include #include -#include #include "uavobject.h" #include "vehicleconfigurationsource.h" -class CCCalibrationUtil : public QObject +class BiasCalibrationUtil : public QObject { Q_OBJECT public: - explicit CCCalibrationUtil(long measurementCount, long measurementRate); - explicit CCCalibrationUtil(long accelMeasurementCount, long accelMeasurementRate, + explicit BiasCalibrationUtil(long measurementCount, long measurementRate); + explicit BiasCalibrationUtil(long accelMeasurementCount, long accelMeasurementRate, long gyroMeasurementCount, long gyroMeasurementRate); signals: @@ -58,10 +57,6 @@ private slots: 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; @@ -86,7 +81,6 @@ private: void stop(); void startMeasurement(); void stopMeasurement(); - accelGyroBias calculateLevellingData(); }; -#endif // CCCALIBRATIONUTIL_H +#endif // BIASCALIBRATIONUTIL_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/cccalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.cpp similarity index 81% rename from ground/openpilotgcs/src/plugins/setupwizard/pages/cccalibrationpage.cpp rename to ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.cpp index 54f0b7b62..3fb5a3ca6 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/cccalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.cpp @@ -1,11 +1,11 @@ /** ****************************************************************************** * - * @file cccalibrationpage.cpp + * @file biascalibrationpage.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @addtogroup * @{ - * @addtogroup CCCalibrationPage + * @addtogroup BiasCalibrationPage * @{ * @brief *****************************************************************************/ @@ -27,19 +27,19 @@ #include #include -#include "cccalibrationpage.h" -#include "ui_cccalibrationpage.h" +#include "biascalibrationpage.h" +#include "ui_biascalibrationpage.h" #include "setupwizard.h" -CCCalibrationPage::CCCalibrationPage(SetupWizard *wizard, QWidget *parent) : +BiasCalibrationPage::BiasCalibrationPage(SetupWizard *wizard, QWidget *parent) : AbstractWizardPage(wizard, parent), - ui(new Ui::CCCalibrationPage), m_calibrationUtil(0) + ui(new Ui::BiasCalibrationPage), m_calibrationUtil(0) { ui->setupUi(this); connect(ui->levelButton, SIGNAL(clicked()), this, SLOT(performCalibration())); } -CCCalibrationPage::~CCCalibrationPage() +BiasCalibrationPage::~BiasCalibrationPage() { if(m_calibrationUtil) { delete m_calibrationUtil; @@ -47,17 +47,17 @@ CCCalibrationPage::~CCCalibrationPage() delete ui; } -bool CCCalibrationPage::validatePage() +bool BiasCalibrationPage::validatePage() { return true; } -bool CCCalibrationPage::isComplete() const +bool BiasCalibrationPage::isComplete() const { return ui->levelButton->isEnabled(); } -void CCCalibrationPage::enableButtons(bool enable) +void BiasCalibrationPage::enableButtons(bool enable) { ui->levelButton->setEnabled(enable); getWizard()->button(QWizard::NextButton)->setEnabled(enable); @@ -67,7 +67,7 @@ void CCCalibrationPage::enableButtons(bool enable) QApplication::processEvents(); } -void CCCalibrationPage::performCalibration() +void BiasCalibrationPage::performCalibration() { if(!getWizard()->getConnectionManager()->isConnected()) { QMessageBox msgBox; @@ -85,7 +85,7 @@ void CCCalibrationPage::performCalibration() 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))); @@ -95,7 +95,7 @@ void CCCalibrationPage::performCalibration() m_calibrationUtil->start(); } -void CCCalibrationPage::calibrationProgress(long current, long total) +void BiasCalibrationPage::calibrationProgress(long current, long total) { if(ui->levellinProgressBar->maximum() != (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(); getWizard()->setLevellingBias(bias); emit completeChanged(); } -void CCCalibrationPage::calibrationTimeout(QString message) +void BiasCalibrationPage::calibrationTimeout(QString message) { stopCalibration(); @@ -123,7 +123,7 @@ void CCCalibrationPage::calibrationTimeout(QString message) msgBox.exec(); } -void CCCalibrationPage::stopCalibration() +void BiasCalibrationPage::stopCalibration() { if(m_calibrationUtil) { diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/cccalibrationpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.h similarity index 74% rename from ground/openpilotgcs/src/plugins/setupwizard/pages/cccalibrationpage.h rename to ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.h index bee647da7..f34c48abe 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/cccalibrationpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.h @@ -1,11 +1,11 @@ /** ****************************************************************************** * - * @file cccalibrationpage.h + * @file biascalibrationpage.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @addtogroup * @{ - * @addtogroup CCCalibrationPage + * @addtogroup BiasCalibrationPage * @{ * @brief *****************************************************************************/ @@ -25,23 +25,23 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef CCCALIBRATIONPAGE_H -#define CCCALIBRATIONPAGE_H +#ifndef BIASCALIBRATIONPAGE_H +#define BIASCALIBRATIONPAGE_H #include "abstractwizardpage.h" -#include "cccalibrationutil.h" +#include "biascalibrationutil.h" namespace Ui { -class CCCalibrationPage; +class BiasCalibrationPage; } -class CCCalibrationPage : public AbstractWizardPage +class BiasCalibrationPage : public AbstractWizardPage { Q_OBJECT public: - explicit CCCalibrationPage(SetupWizard *wizard, QWidget *parent = 0); - ~CCCalibrationPage(); + explicit BiasCalibrationPage(SetupWizard *wizard, QWidget *parent = 0); + ~BiasCalibrationPage(); bool validatePage(); bool isComplete() const; @@ -53,13 +53,13 @@ private slots: private: static const int BIAS_CYCLES = 200; - static const int BIAS_RATE = 30; + static const int BIAS_RATE = 50; - Ui::CCCalibrationPage *ui; - CCCalibrationUtil *m_calibrationUtil; + Ui::BiasCalibrationPage *ui; + BiasCalibrationUtil *m_calibrationUtil; void stopCalibration(); void enableButtons(bool enable); }; -#endif // CCCALIBRATIONPAGE_H +#endif // BIASCALIBRATIONPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/cccalibrationpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.ui similarity index 98% rename from ground/openpilotgcs/src/plugins/setupwizard/pages/cccalibrationpage.ui rename to ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.ui index 00757027f..b746072a1 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/cccalibrationpage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.ui @@ -1,7 +1,7 @@ - CCCalibrationPage - + BiasCalibrationPage + 0 diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index d1d747dae..bd237f875 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -36,7 +36,7 @@ #include "pages/surfacepage.h" #include "pages/inputpage.h" #include "pages/outputpage.h" -#include "pages/cccalibrationpage.h" +#include "pages/biascalibrationpage.h" #include "pages/summarypage.h" #include "pages/savepage.h" #include "pages/notyetimplementedpage.h" @@ -118,10 +118,10 @@ int SetupWizard::nextId() const return PAGE_VEHICLES; case PAGE_OUTPUT: return PAGE_SUMMARY; - case PAGE_CC_CALIBRATION: - return PAGE_OUTPUT_CALIBRATION; - case PAGE_REVO_CALIBRATION: + case PAGE_BIAS_CALIBRATION: return PAGE_OUTPUT_CALIBRATION; + //case PAGE_REVO_CALIBRATION: + // return PAGE_OUTPUT_CALIBRATION; case PAGE_OUTPUT_CALIBRATION: return PAGE_SAVE; case PAGE_SUMMARY: @@ -130,9 +130,8 @@ int SetupWizard::nextId() const { case CONTROLLER_CC: case CONTROLLER_CC3D: - return PAGE_CC_CALIBRATION; case CONTROLLER_REVO: - return PAGE_REVO_CALIBRATION; + return PAGE_BIAS_CALIBRATION; default: return PAGE_NOTYETIMPLEMENTED; } @@ -289,8 +288,8 @@ void SetupWizard::createPages() setPage(PAGE_SURFACE, new SurfacePage(this)); setPage(PAGE_INPUT, new InputPage(this)); setPage(PAGE_OUTPUT, new OutputPage(this)); - setPage(PAGE_CC_CALIBRATION, new CCCalibrationPage(this)); - setPage(PAGE_REVO_CALIBRATION, new RevoCalibrationPage(this)); + setPage(PAGE_BIAS_CALIBRATION, new BiasCalibrationPage(this)); + //setPage(PAGE_REVO_CALIBRATION, new RevoCalibrationPage(this)); setPage(PAGE_OUTPUT_CALIBRATION, new OutputCalibrationPage(this)); setPage(PAGE_SUMMARY, new SummaryPage(this)); setPage(PAGE_SAVE, new SavePage(this)); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h index 262cc844b..f0418660c 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h @@ -29,7 +29,7 @@ #define SETUPWIZARD_H #include -#include "cccalibrationutil.h" +#include "biascalibrationutil.h" #include #include #include "vehicleconfigurationsource.h" @@ -90,7 +90,7 @@ private slots: void pageChanged(int currId); private: 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_NOTYETIMPLEMENTED, PAGE_REBOOT, PAGE_END, PAGE_UPDATE}; void createPages(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro index 5e250a5d5..cd864f7c7 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro @@ -33,9 +33,9 @@ HEADERS += setupwizardplugin.h \ pages/rebootpage.h \ pages/savepage.h \ pages/autoupdatepage.h \ - pages/cccalibrationpage.h \ - cccalibrationutil.h \ - pages/revocalibrationpage.h + pages/revocalibrationpage.h \ + biascalibrationutil.h \ + pages/biascalibrationpage.h SOURCES += setupwizardplugin.cpp \ setupwizard.cpp \ @@ -60,9 +60,9 @@ SOURCES += setupwizardplugin.cpp \ pages/rebootpage.cpp \ pages/savepage.cpp \ pages/autoupdatepage.cpp \ - pages/cccalibrationpage.cpp \ - cccalibrationutil.cpp \ - pages/revocalibrationpage.cpp + pages/revocalibrationpage.cpp \ + biascalibrationutil.cpp \ + pages/biascalibrationpage.cpp OTHER_FILES += SetupWizard.pluginspec @@ -84,8 +84,8 @@ FORMS += \ pages/rebootpage.ui \ pages/savepage.ui \ pages/autoupdatepage.ui \ - pages/cccalibrationpage.ui \ - pages/revocalibrationpage.ui + pages/revocalibrationpage.ui \ + pages/biascalibrationpage.ui RESOURCES += \ wizardResources.qrc diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 59d7ab7d9..854a629cc 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -34,6 +34,7 @@ #include "systemsettings.h" #include "manualcontrolsettings.h" #include "stabilizationsettings.h" +#include "revocalibration.h" const qint16 VehicleConfigurationHelper::LEGACY_ESC_FREQUENCE = 50; const qint16 VehicleConfigurationHelper::RAPID_ESC_FREQUENCE = 400; @@ -64,7 +65,11 @@ bool VehicleConfigurationHelper::setupVehicle(bool save) applyVehicleConfiguration(); applyActuatorConfiguration(); applyFlighModeConfiguration(); - applyLevellingConfiguration(); + + if(save) { + applySensorBiasConfiguration(); + } + applyStabilizationConfiguration(); applyManualControlDefaults(); @@ -327,25 +332,63 @@ void VehicleConfigurationHelper::applyFlighModeConfiguration() 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()) { accelGyroBias bias = m_configSource->getCalibrationBias(); + float G = 9.81f; - data.AccelBias[0] += bias.m_accelerometerXBias; - data.AccelBias[1] += bias.m_accelerometerYBias; - data.AccelBias[2] += bias.m_accelerometerZBias; - data.GyroBias[0] = -bias.m_gyroXBias; - data.GyroBias[1] = -bias.m_gyroYBias; - data.GyroBias[2] = -bias.m_gyroZBias; + switch(m_configSource->getControllerType()) { + case VehicleConfigurationSource::CONTROLLER_CC: + case VehicleConfigurationSource::CONTROLLER_CC3D: + { + const float ACCELERATION_SCALE = 0.004f * G; + 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() diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h index b72e40f63..d890db66a 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h @@ -81,7 +81,7 @@ private: void applyVehicleConfiguration(); void applyActuatorConfiguration(); void applyFlighModeConfiguration(); - void applyLevellingConfiguration(); + void applySensorBiasConfiguration(); void applyStabilizationConfiguration(); void applyManualControlDefaults();