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->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<QString> 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<QString> 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);

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

@ -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<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();
UAVObjectManager * uavObjectManager = pm->getObject<UAVObjectManager>();
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<UAVObjectManager>();
@ -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<UAVObjectManager>();
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<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
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<double> m_accelerometerX;
QList<double> m_accelerometerY;
QList<double> m_accelerometerZ;
QList<double> m_gyroX;
QList<double> m_gyroY;
QList<double> 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<double> list);
};
#endif // LEVELLINGUTIL_H

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

@ -26,6 +26,7 @@
*/
#include <QMessageBox>
#include <QDebug>
#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();

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

@ -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;

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

@ -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;
}

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 ACTUATOR_MIN = 1000;
static const qint16 ACTUATOR_NEUTRAL = 1000;
static const qint16 ACTUATOR_MAX = 2000;
static const int MIXER_TYPE_DISABLED = 0;