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

OP-39 Connection diagram changes.

Browse Source 
Fixed a bug levelling util and changed rate from 100ms to 30ms.
This commit is contained in:
Fredrik Arvidsson 2012-08-30 22:13:44 +02:00
parent d647e9d6bb
commit df987ab57f
7 changed files with 117 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp
View File

@ -70,7 +70,8 @@ void ConnectionDiagram::setupGraphicsScene()
m_background->setSharedRenderer(m_renderer); m_background->setSharedRenderer(m_renderer);
m_background->setElementId("background"); m_background->setElementId("background");
m_background->setVisible(true); m_background->setVisible(true);
m_background->setFlags(QGraphicsItem::ItemClipsChildrenToShape | QGraphicsItem::ItemClipsToShape); m_background->setFlags(QGraphicsItem::ItemClipsToShape);
m_background->setZValue(-1);
scene->addItem(m_background); scene->addItem(m_background);
QList<QString> elementsToShow; QList<QString> elementsToShow;
@ -123,7 +124,7 @@ void ConnectionDiagram::setupGraphicsScene()
switch (m_configSource->getInputType()) switch (m_configSource->getInputType())
{ {
case VehicleConfigurationSource::INPUT_PWM: case VehicleConfigurationSource::INPUT_PWM:
elementsToShow << "pwm" << "receiver" ; elementsToShow << "receiver" << "pwm" ;
break; break;
case VehicleConfigurationSource::INPUT_PPM: case VehicleConfigurationSource::INPUT_PPM:
elementsToShow << "receiver" << "ppm"; elementsToShow << "receiver" << "ppm";
@ -149,12 +150,14 @@ void ConnectionDiagram::setupGraphicsScene()
void ConnectionDiagram::setupGraphicsSceneItems(QGraphicsScene *scene, QList<QString> elementsToShow) void ConnectionDiagram::setupGraphicsSceneItems(QGraphicsScene *scene, QList<QString> elementsToShow)
{ {
qreal z = 0;
foreach(QString elementId, elementsToShow) { foreach(QString elementId, elementsToShow) {
if(m_renderer->elementExists(elementId)) { if(m_renderer->elementExists(elementId)) {
QGraphicsSvgItem* element = new QGraphicsSvgItem(); QGraphicsSvgItem* element = new QGraphicsSvgItem();
element->setSharedRenderer(m_renderer); element->setSharedRenderer(m_renderer);
element->setElementId(elementId); element->setElementId(elementId);
element->setVisible(true); element->setVisible(true);
element->setZValue(z++);
scene->addItem(element); scene->addItem(element);
QMatrix matrix = m_renderer->matrixForElement(elementId); QMatrix matrix = m_renderer->matrixForElement(elementId);

134
ground/openpilotgcs/src/plugins/setupwizard/levellingutil.cpp
View File

@ -33,8 +33,16 @@
#include "gyros.h" #include "gyros.h"
LevellingUtil::LevellingUtil(long measurementCount, long measurementPeriod) : QObject(), LevellingUtil::LevellingUtil(long measurementCount, long measurementRate) : QObject(),
m_isMeasuring(false), m_measurementCount(measurementCount), m_measurementPeriod(measurementPeriod) 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 // Set up timeout timer
connect(&m_timeoutTimer, SIGNAL(timeout()), this, SLOT(timeout())); 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); QMutexLocker locker(&m_measurementMutex);
m_receivedUpdates++; if(m_receivedGyroUpdates < m_gyroMeasurementCount) {
emit progress(m_receivedUpdates, m_measurementCount); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager * uavObjectManager = pm->getObject<UAVObjectManager>();
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(); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager * uavObjectManager = pm->getObject<UAVObjectManager>(); UAVObjectManager * uavObjectManager = pm->getObject<UAVObjectManager>();
Q_ASSERT(uavObjectManager); Q_ASSERT(uavObjectManager);
@ -71,18 +103,15 @@ void LevellingUtil::measurementsUpdated(UAVObject *obj)
Accels * accels = Accels::GetInstance(uavObjectManager); Accels * accels = Accels::GetInstance(uavObjectManager);
Accels::DataFields accelsData = accels->getData(); Accels::DataFields accelsData = accels->getData();
m_accelerometerX.append(accelsData.x); m_accelerometerX += accelsData.x;
m_accelerometerY.append(accelsData.y); m_accelerometerY += accelsData.y;
m_accelerometerZ.append(accelsData.z); m_accelerometerZ += accelsData.z;
Gyros * gyros = Gyros::GetInstance(uavObjectManager); m_receivedAccelUpdates++;
Gyros::DataFields gyrosData = gyros->getData(); emit progress(m_receivedGyroUpdates + m_receivedAccelUpdates, m_gyroMeasurementCount + m_accelMeasurementCount);
m_gyroX.append(gyrosData.x);
m_gyroY.append(gyrosData.y);
m_gyroZ.append(gyrosData.z);
} }
else if (m_receivedUpdates >= m_measurementCount) { else if (m_receivedAccelUpdates >= m_accelMeasurementCount &&
m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) {
stopMeasurement(); stopMeasurement();
emit done(calculateLevellingData()); emit done(calculateLevellingData());
} }
@ -103,13 +132,15 @@ void LevellingUtil::startMeasurement()
m_isMeasuring = true; m_isMeasuring = true;
// Reset variables // Reset variables
m_receivedUpdates = 0; m_receivedAccelUpdates = 0;
m_accelerometerX.clear(); m_accelerometerX = 0;
m_accelerometerY.clear(); m_accelerometerY = 0;
m_accelerometerZ.clear(); m_accelerometerZ = 0;
m_gyroX.clear();
m_gyroY.clear(); m_receivedGyroUpdates = 0;
m_gyroZ.clear(); m_gyroX = 0;
m_gyroY = 0;
m_gyroZ = 0;
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager * uavObjectManager = pm->getObject<UAVObjectManager>(); UAVObjectManager * uavObjectManager = pm->getObject<UAVObjectManager>();
@ -120,15 +151,26 @@ void LevellingUtil::startMeasurement()
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE; attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData); AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData);
// Set up to receive updates // Set up to receive updates for accels
UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager); 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 // Set update period for accels
m_previousMetaData = uavObject->getMetadata(); m_previousAccelMetaData = uavObject->getMetadata();
UAVObject::Metadata newMetaData = m_previousMetaData; UAVObject::Metadata newMetaData = m_previousAccelMetaData;
UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC); 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); uavObject->setMetadata(newMetaData);
} }
@ -144,10 +186,15 @@ void LevellingUtil::stopMeasurement()
UAVObjectManager * uavObjectManager = pm->getObject<UAVObjectManager>(); UAVObjectManager * uavObjectManager = pm->getObject<UAVObjectManager>();
Q_ASSERT(uavObjectManager); Q_ASSERT(uavObjectManager);
// Stop listening for updates // Stop listening for updates from accels
UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager); UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager);
disconnect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(measurementsUpdated(UAVObject*))); disconnect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelMeasurementsUpdated(UAVObject*)));
uavObject->setMetadata(m_previousMetaData); 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 // Enable gyro bias correction again
AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData(); AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData();
@ -158,22 +205,13 @@ void LevellingUtil::stopMeasurement()
accelGyroBias LevellingUtil::calculateLevellingData() accelGyroBias LevellingUtil::calculateLevellingData()
{ {
accelGyroBias bias; accelGyroBias bias;
bias.m_accelerometerXBias = listMean(m_accelerometerX) / ACCELERATION_SCALE; bias.m_accelerometerXBias = m_accelerometerX / (double)m_receivedAccelUpdates / ACCELERATION_SCALE;
bias.m_accelerometerYBias = listMean(m_accelerometerY) / ACCELERATION_SCALE; bias.m_accelerometerYBias = m_accelerometerY / (double)m_receivedAccelUpdates / ACCELERATION_SCALE;
bias.m_accelerometerZBias = (listMean(m_accelerometerZ) + G) / ACCELERATION_SCALE; bias.m_accelerometerZBias = (m_accelerometerZ / (double)m_receivedAccelUpdates + G) / ACCELERATION_SCALE;
bias.m_gyroXBias = listMean(m_gyroX) * 100.0f; bias.m_gyroXBias = m_gyroX / (double)m_receivedGyroUpdates * 100.0;
bias.m_gyroYBias = listMean(m_gyroY) * 100.0f; bias.m_gyroYBias = m_gyroY / (double)m_receivedGyroUpdates * 100.0;
bias.m_gyroZBias = listMean(m_gyroZ) * 100.0f; bias.m_gyroZBias = m_gyroZ / (double)m_receivedGyroUpdates * 100.0;
return bias; 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();
}

34
ground/openpilotgcs/src/plugins/setupwizard/levellingutil.h
View File

@ -39,8 +39,10 @@ class LevellingUtil : public QObject
{ {
Q_OBJECT Q_OBJECT
public: public:
explicit LevellingUtil(long measurementCount, long measurementPeriod); explicit LevellingUtil(long measurementCount, long measurementRate);
explicit LevellingUtil(long accelMeasurementCount, long accelMeasurementRate,
long gyroMeasurementCount, long gyroMeasurementRate);
signals: signals:
void progress(long current, long total); void progress(long current, long total);
void done(accelGyroBias measuredBias); void done(accelGyroBias measuredBias);
@ -51,7 +53,8 @@ public slots:
void abort(); void abort();
private slots: private slots:
void measurementsUpdated(UAVObject * obj); void gyroMeasurementsUpdated(UAVObject * obj);
void accelMeasurementsUpdated(UAVObject * obj);
void timeout(); void timeout();
private: private:
@ -62,25 +65,28 @@ private:
QTimer m_timeoutTimer; QTimer m_timeoutTimer;
bool m_isMeasuring; bool m_isMeasuring;
long m_receivedUpdates; long m_receivedAccelUpdates;
long m_receivedGyroUpdates;
long m_measurementCount; long m_accelMeasurementCount;
long m_measurementPeriod; long m_gyroMeasurementCount;
long m_accelMeasurementRate;
long m_gyroMeasurementRate;
UAVObject::Metadata m_previousMetaData; UAVObject::Metadata m_previousGyroMetaData;
UAVObject::Metadata m_previousAccelMetaData;
QList<double> m_accelerometerX; double m_accelerometerX;
QList<double> m_accelerometerY; double m_accelerometerY;
QList<double> m_accelerometerZ; double m_accelerometerZ;
QList<double> m_gyroX; double m_gyroX;
QList<double> m_gyroY; double m_gyroY;
QList<double> m_gyroZ; double m_gyroZ;
void stop(); void stop();
void startMeasurement(); void startMeasurement();
void stopMeasurement(); void stopMeasurement();
accelGyroBias calculateLevellingData(); accelGyroBias calculateLevellingData();
double listMean(QList<double> list);
}; };
#endif // LEVELLINGUTIL_H #endif // LEVELLINGUTIL_H

4
ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.cpp
View File

@ -26,6 +26,7 @@
*/ */
#include <QMessageBox> #include <QMessageBox>
#include <QDebug>
#include "levellingpage.h" #include "levellingpage.h"
#include "ui_levellingpage.h" #include "ui_levellingpage.h"
#include "setupwizard.h" #include "setupwizard.h"
@ -75,8 +76,7 @@ void LevellingPage::performLevelling()
if(!m_levellingUtil) if(!m_levellingUtil)
{ {
// Measure every 100ms * 100times = 10s m_levellingUtil = new LevellingUtil(BIAS_CYCLES, BIAS_RATE);
m_levellingUtil = new LevellingUtil(BIAS_CYCLES, BIAS_PERIOD);
} }
emit completeChanged(); emit completeChanged();

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

@ -52,8 +52,8 @@ private slots:
void levellingTimeout(QString message); void levellingTimeout(QString message);
private: private:
static const int BIAS_CYCLES = 100; static const int BIAS_CYCLES = 200;
static const int BIAS_PERIOD = 100; static const int BIAS_RATE = 30;
Ui::LevellingPage *ui; Ui::LevellingPage *ui;
LevellingUtil *m_levellingUtil; LevellingUtil *m_levellingUtil;

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

@ -192,7 +192,7 @@ void VehicleConfigurationHelper::applyActuatorConfiguration()
data.ChannelType[i] = ActuatorSettings::CHANNELTYPE_PWM; data.ChannelType[i] = ActuatorSettings::CHANNELTYPE_PWM;
data.ChannelAddr[i] = i; data.ChannelAddr[i] = i;
data.ChannelMin[i] = ACTUATOR_MIN; data.ChannelMin[i] = ACTUATOR_MIN;
data.ChannelNeutral[i] = ACTUATOR_MIN; data.ChannelNeutral[i] = ACTUATOR_NEUTRAL;
data.ChannelMax[i] = ACTUATOR_MAX; data.ChannelMax[i] = ACTUATOR_MAX;
} }

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

@ -69,6 +69,7 @@ private:
static const qint16 RAPID_ESC_FREQUENCE = 400; static const qint16 RAPID_ESC_FREQUENCE = 400;
static const qint16 ACTUATOR_MIN = 1000; static const qint16 ACTUATOR_MIN = 1000;
static const qint16 ACTUATOR_NEUTRAL = 1000;
static const qint16 ACTUATOR_MAX = 2000; static const qint16 ACTUATOR_MAX = 2000;
static const int MIXER_TYPE_DISABLED = 0; static const int MIXER_TYPE_DISABLED = 0;