From df987ab57f746f63d3879fd836baf1b4074b7023 Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Thu, 30 Aug 2012 22:13:44 +0200 Subject: [PATCH] OP-39 Connection diagram changes. Fixed a bug levelling util and changed rate from 100ms to 30ms. --- .../plugins/setupwizard/connectiondiagram.cpp | 7 +- .../src/plugins/setupwizard/levellingutil.cpp | 134 +++++++++++------- .../src/plugins/setupwizard/levellingutil.h | 34 +++-- .../setupwizard/pages/levellingpage.cpp | 4 +- .../plugins/setupwizard/pages/levellingpage.h | 4 +- .../vehicleconfigurationhelper.cpp | 2 +- .../setupwizard/vehicleconfigurationhelper.h | 1 + 7 files changed, 117 insertions(+), 69 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp index 9ca7f5db4..05158a656 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp @@ -70,7 +70,8 @@ void ConnectionDiagram::setupGraphicsScene() m_background->setSharedRenderer(m_renderer); m_background->setElementId("background"); m_background->setVisible(true); - m_background->setFlags(QGraphicsItem::ItemClipsChildrenToShape | QGraphicsItem::ItemClipsToShape); + m_background->setFlags(QGraphicsItem::ItemClipsToShape); + m_background->setZValue(-1); scene->addItem(m_background); QList elementsToShow; @@ -123,7 +124,7 @@ void ConnectionDiagram::setupGraphicsScene() switch (m_configSource->getInputType()) { case VehicleConfigurationSource::INPUT_PWM: - elementsToShow << "pwm" << "receiver" ; + elementsToShow << "receiver" << "pwm" ; break; case VehicleConfigurationSource::INPUT_PPM: elementsToShow << "receiver" << "ppm"; @@ -149,12 +150,14 @@ void ConnectionDiagram::setupGraphicsScene() void ConnectionDiagram::setupGraphicsSceneItems(QGraphicsScene *scene, QList elementsToShow) { + qreal z = 0; foreach(QString elementId, elementsToShow) { if(m_renderer->elementExists(elementId)) { QGraphicsSvgItem* element = new QGraphicsSvgItem(); element->setSharedRenderer(m_renderer); element->setElementId(elementId); element->setVisible(true); + element->setZValue(z++); scene->addItem(element); QMatrix matrix = m_renderer->matrixForElement(elementId); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/levellingutil.cpp b/ground/openpilotgcs/src/plugins/setupwizard/levellingutil.cpp index 0678ad136..23d422adf 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/levellingutil.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/levellingutil.cpp @@ -33,8 +33,16 @@ #include "gyros.h" -LevellingUtil::LevellingUtil(long measurementCount, long measurementPeriod) : QObject(), - m_isMeasuring(false), m_measurementCount(measurementCount), m_measurementPeriod(measurementPeriod) +LevellingUtil::LevellingUtil(long measurementCount, long measurementRate) : QObject(), + m_isMeasuring(false), m_accelMeasurementCount(measurementCount), m_gyroMeasurementCount(measurementCount), + m_accelMeasurementRate(measurementRate), m_gyroMeasurementRate(measurementRate) +{ +} + +LevellingUtil::LevellingUtil(long accelMeasurementCount, long accelMeasurementRate, + long gyroMeasurementCount, long gyroMeasurementRate) : QObject(), + m_isMeasuring(false), m_accelMeasurementCount(accelMeasurementCount), m_gyroMeasurementCount(gyroMeasurementCount), + m_accelMeasurementRate(accelMeasurementRate), m_gyroMeasurementRate(gyroMeasurementRate) { } @@ -45,7 +53,8 @@ void LevellingUtil::start() // Set up timeout timer connect(&m_timeoutTimer, SIGNAL(timeout()), this, SLOT(timeout())); - m_timeoutTimer.start(m_measurementCount * m_measurementPeriod * 2); + //Set timeout to max time of measurement + 10 secs + m_timeoutTimer.start(std::max(m_accelMeasurementCount * m_accelMeasurementRate, m_gyroMeasurementCount * m_gyroMeasurementRate) + 10000); } } @@ -56,14 +65,37 @@ void LevellingUtil::abort() } } -void LevellingUtil::measurementsUpdated(UAVObject *obj) +void LevellingUtil::gyroMeasurementsUpdated(UAVObject *obj) { QMutexLocker locker(&m_measurementMutex); - m_receivedUpdates++; - emit progress(m_receivedUpdates, m_measurementCount); + if(m_receivedGyroUpdates < m_gyroMeasurementCount) { + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager * uavObjectManager = pm->getObject(); + Q_ASSERT(uavObjectManager); - if(m_receivedUpdates < m_measurementCount) { + Gyros * gyros = Gyros::GetInstance(uavObjectManager); + Gyros::DataFields gyrosData = gyros->getData(); + + m_gyroX += gyrosData.x; + m_gyroY += gyrosData.y; + m_gyroZ += gyrosData.z; + + m_receivedGyroUpdates++; + emit progress(m_receivedGyroUpdates + m_receivedAccelUpdates, m_gyroMeasurementCount + m_accelMeasurementCount); + } + else if (m_receivedAccelUpdates >= m_accelMeasurementCount && + m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) { + stopMeasurement(); + emit done(calculateLevellingData()); + } +} + +void LevellingUtil::accelMeasurementsUpdated(UAVObject *obj) +{ + QMutexLocker locker(&m_measurementMutex); + + if(m_receivedAccelUpdates < m_accelMeasurementCount) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager * uavObjectManager = pm->getObject(); Q_ASSERT(uavObjectManager); @@ -71,18 +103,15 @@ void LevellingUtil::measurementsUpdated(UAVObject *obj) 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); + m_accelerometerX += accelsData.x; + m_accelerometerY += accelsData.y; + m_accelerometerZ += 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); + m_receivedAccelUpdates++; + emit progress(m_receivedGyroUpdates + m_receivedAccelUpdates, m_gyroMeasurementCount + m_accelMeasurementCount); } - else if (m_receivedUpdates >= m_measurementCount) { + else if (m_receivedAccelUpdates >= m_accelMeasurementCount && + m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) { stopMeasurement(); emit done(calculateLevellingData()); } @@ -103,13 +132,15 @@ void LevellingUtil::startMeasurement() 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(); + m_receivedAccelUpdates = 0; + m_accelerometerX = 0; + m_accelerometerY = 0; + m_accelerometerZ = 0; + + m_receivedGyroUpdates = 0; + m_gyroX = 0; + m_gyroY = 0; + m_gyroZ = 0; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager * uavObjectManager = pm->getObject(); @@ -120,15 +151,26 @@ void LevellingUtil::startMeasurement() attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE; AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData); - // Set up to receive updates + // Set up to receive updates for accels UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager); - connect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(measurementsUpdated(UAVObject*))); + connect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelMeasurementsUpdated(UAVObject*))); - // Set update period - m_previousMetaData = uavObject->getMetadata(); - UAVObject::Metadata newMetaData = m_previousMetaData; + // Set update period for accels + m_previousAccelMetaData = uavObject->getMetadata(); + UAVObject::Metadata newMetaData = m_previousAccelMetaData; UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC); - newMetaData.flightTelemetryUpdatePeriod = m_measurementPeriod; + newMetaData.flightTelemetryUpdatePeriod = m_accelMeasurementRate; + uavObject->setMetadata(newMetaData); + + // Set up to receive updates from gyros + uavObject = Gyros::GetInstance(uavObjectManager); + connect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(gyroMeasurementsUpdated(UAVObject*))); + + // Set update period for gyros + m_previousGyroMetaData = uavObject->getMetadata(); + newMetaData = m_previousGyroMetaData; + UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC); + newMetaData.flightTelemetryUpdatePeriod = m_gyroMeasurementRate; uavObject->setMetadata(newMetaData); } @@ -144,10 +186,15 @@ void LevellingUtil::stopMeasurement() UAVObjectManager * uavObjectManager = pm->getObject(); Q_ASSERT(uavObjectManager); - // Stop listening for updates + // Stop listening for updates from accels UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager); - disconnect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(measurementsUpdated(UAVObject*))); - uavObject->setMetadata(m_previousMetaData); + disconnect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelMeasurementsUpdated(UAVObject*))); + uavObject->setMetadata(m_previousAccelMetaData); + + // Stop listening for updates from gyros + uavObject = Gyros::GetInstance(uavObjectManager); + disconnect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(gyroMeasurementsUpdated(UAVObject*))); + uavObject->setMetadata(m_previousGyroMetaData); // Enable gyro bias correction again AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData(); @@ -158,22 +205,13 @@ void LevellingUtil::stopMeasurement() 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_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_gyroXBias = listMean(m_gyroX) * 100.0f; - bias.m_gyroYBias = listMean(m_gyroY) * 100.0f; - bias.m_gyroZBias = listMean(m_gyroZ) * 100.0f; + 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; } -double LevellingUtil::listMean(QList list) -{ - double accum = 0; - for(int i = 0; i < list.size(); i++) { - accum += list.at(i); - } - return accum / list.size(); -} - diff --git a/ground/openpilotgcs/src/plugins/setupwizard/levellingutil.h b/ground/openpilotgcs/src/plugins/setupwizard/levellingutil.h index fb8ffcdce..c5cbe8709 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/levellingutil.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/levellingutil.h @@ -39,8 +39,10 @@ class LevellingUtil : public QObject { Q_OBJECT public: - explicit LevellingUtil(long measurementCount, long measurementPeriod); - + explicit LevellingUtil(long measurementCount, long measurementRate); + explicit LevellingUtil(long accelMeasurementCount, long accelMeasurementRate, + long gyroMeasurementCount, long gyroMeasurementRate); + signals: void progress(long current, long total); void done(accelGyroBias measuredBias); @@ -51,7 +53,8 @@ public slots: void abort(); private slots: - void measurementsUpdated(UAVObject * obj); + void gyroMeasurementsUpdated(UAVObject * obj); + void accelMeasurementsUpdated(UAVObject * obj); void timeout(); private: @@ -62,25 +65,28 @@ private: QTimer m_timeoutTimer; bool m_isMeasuring; - long m_receivedUpdates; + long m_receivedAccelUpdates; + long m_receivedGyroUpdates; - long m_measurementCount; - long m_measurementPeriod; + long m_accelMeasurementCount; + long m_gyroMeasurementCount; + long m_accelMeasurementRate; + long m_gyroMeasurementRate; - UAVObject::Metadata m_previousMetaData; + UAVObject::Metadata m_previousGyroMetaData; + UAVObject::Metadata m_previousAccelMetaData; - QList m_accelerometerX; - QList m_accelerometerY; - QList m_accelerometerZ; - QList m_gyroX; - QList m_gyroY; - QList m_gyroZ; + double m_accelerometerX; + double m_accelerometerY; + double m_accelerometerZ; + double m_gyroX; + double m_gyroY; + double m_gyroZ; void stop(); void startMeasurement(); void stopMeasurement(); accelGyroBias calculateLevellingData(); - double listMean(QList list); }; #endif // LEVELLINGUTIL_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.cpp index 34ef253f5..992e7dd50 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.cpp @@ -26,6 +26,7 @@ */ #include +#include #include "levellingpage.h" #include "ui_levellingpage.h" #include "setupwizard.h" @@ -75,8 +76,7 @@ void LevellingPage::performLevelling() if(!m_levellingUtil) { - // Measure every 100ms * 100times = 10s - m_levellingUtil = new LevellingUtil(BIAS_CYCLES, BIAS_PERIOD); + m_levellingUtil = new LevellingUtil(BIAS_CYCLES, BIAS_RATE); } emit completeChanged(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.h index b5f555ecc..447851739 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.h @@ -52,8 +52,8 @@ private slots: void levellingTimeout(QString message); private: - static const int BIAS_CYCLES = 100; - static const int BIAS_PERIOD = 100; + static const int BIAS_CYCLES = 200; + static const int BIAS_RATE = 30; Ui::LevellingPage *ui; LevellingUtil *m_levellingUtil; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 9511c3ef1..0ce6d8bfc 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -192,7 +192,7 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() data.ChannelType[i] = ActuatorSettings::CHANNELTYPE_PWM; data.ChannelAddr[i] = i; data.ChannelMin[i] = ACTUATOR_MIN; - data.ChannelNeutral[i] = ACTUATOR_MIN; + data.ChannelNeutral[i] = ACTUATOR_NEUTRAL; data.ChannelMax[i] = ACTUATOR_MAX; } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h index c54afde73..e09c19cde 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h @@ -69,6 +69,7 @@ private: static const qint16 RAPID_ESC_FREQUENCE = 400; static const qint16 ACTUATOR_MIN = 1000; + static const qint16 ACTUATOR_NEUTRAL = 1000; static const qint16 ACTUATOR_MAX = 2000; static const int MIXER_TYPE_DISABLED = 0;