Skip to content

Commit

Permalink
Merge pull request #23 from lpotter/master
Browse files Browse the repository at this point in the history 
[sensorfw] make use of orientation adaptor for compass heading if
  • Loading branch information
@lpotter
lpotter committed Nov 14, 2013
2 parents 7bd805e + 363a964 commit b1c400f
Show file tree
Hide file tree
Showing 13 changed files with 208 additions and 111 deletions.
177 changes: 99 additions & 78 deletions chains/compasschain/compasschain.cpp
View file
Expand Up @@ -23,6 +23,7 @@
#include <QDebug>

#include "compasschain.h"
#include "orientationfilter.h"
#include "sensormanager.h"
#include "bin.h"
#include "bufferreader.h"
Expand All @@ -33,42 +34,52 @@


CompassChain::CompassChain(const QString& id) :
AbstractChain(id)
AbstractChain(id),
hasOrientationAdaptor(false)
{
qDebug() << Q_FUNC_INFO << id;

SensorManager& sm = SensorManager::instance();

magChain = sm.requestChain("magcalibrationchain");
Q_ASSERT(magChain);
setValid(magChain->isValid());
orientAdaptor = sm.requestDeviceAdaptor("orientationadaptor");

if (orientAdaptor) {
Q_ASSERT(orientAdaptor);
hasOrientationAdaptor = true;
setValid(orientAdaptor->isValid());
if (orientAdaptor->isValid())
orientationdataReader = new BufferReader<TimedXyzData>(1);

// orientAdaptor = sm.requestDeviceAdaptor("orientationadaptor");
// Q_ASSERT(orientAdaptor);
// setValid(orientAdaptor->isValid());
orientationFilter = sm.instantiateFilter("orientationfilter");
Q_ASSERT(orientationFilter);

accelerometerChain = sm.requestChain("accelerometerchain");
Q_ASSERT(accelerometerChain);
setValid(accelerometerChain->isValid());
declinationFilter = sm.instantiateFilter("declinationfilter");
Q_ASSERT(declinationFilter);
//hybris has compass heading in orientation sensor
}

accelerometerReader = new BufferReader<AccelerationData>(1);
if (!hasOrientationAdaptor) {
magChain = sm.requestChain("magcalibrationchain");
Q_ASSERT(magChain);
setValid(magChain->isValid());
accelerometerChain = sm.requestChain("accelerometerchain");
Q_ASSERT(accelerometerChain);
setValid(accelerometerChain->isValid());

magReader = new BufferReader<CalibratedMagneticFieldData>(1);
accelerometerReader = new BufferReader<AccelerationData>(1);

// if (orientAdaptor->isValid())
// orientationdataReader = new BufferReader<TimedXyzData>(1);
magReader = new BufferReader<CalibratedMagneticFieldData>(1);

compassFilter = sm.instantiateFilter("compassfilter");
Q_ASSERT(compassFilter);
compassFilter = sm.instantiateFilter("compassfilter");
Q_ASSERT(compassFilter);

declinationFilter = sm.instantiateFilter("declinationfilter");
Q_ASSERT(declinationFilter);
declinationFilter = sm.instantiateFilter("declinationfilter");
Q_ASSERT(declinationFilter);

downsampleFilter = sm.instantiateFilter("downsamplefilter");
Q_ASSERT(downsampleFilter);
downsampleFilter = sm.instantiateFilter("downsamplefilter");
Q_ASSERT(downsampleFilter);

avgaccFilter = sm.instantiateFilter("avgaccfilter");
Q_ASSERT(avgaccFilter);
avgaccFilter = sm.instantiateFilter("avgaccfilter");
Q_ASSERT(avgaccFilter);
}

trueNorthBuffer = new RingBuffer<CompassData>(1);
nameOutputBuffer("truenorth", trueNorthBuffer); //
Expand All @@ -79,86 +90,96 @@ CompassChain::CompassChain(const QString& id) :
// Create buffers for filter chain
filterBin = new Bin;

// if (orientAdaptor->isValid())
// filterBin->add(orientationdataReader, "orientation");

filterBin->add(magReader, "magnetometer");
filterBin->add(accelerometerReader, "accelerometer");
if (!hasOrientationAdaptor) {
filterBin->add(magReader, "magnetometer");
filterBin->add(accelerometerReader, "accelerometer");
filterBin->add(compassFilter, "compass");
filterBin->add(avgaccFilter, "avgaccelerometer"); //normalize to flat ?
filterBin->add(downsampleFilter, "downsamplefilter");
} else {
////////////////////
filterBin->add(orientationdataReader, "orientation");
filterBin->add(orientationFilter, "orientationfilter");
}

filterBin->add(compassFilter, "compass");
filterBin->add(avgaccFilter, "avgaccelerometer"); //normalize to flat ?
filterBin->add(downsampleFilter, "downsamplefilter");
filterBin->add(declinationFilter, "declinationcorrection");

filterBin->add(trueNorthBuffer, "truenorth");
filterBin->add(magneticNorthBuffer, "magneticnorth");

// if (orientAdaptor->isValid()) { //hybris has compass heading in orientation sensor
// if (!filterBin->join("orientation", "source", "compass", "orientsink"))
// qDebug() << Q_FUNC_INFO << "magnetometer join failed";
// } else {

if (!hasOrientationAdaptor) {
if (!filterBin->join("magnetometer", "source", "compass", "magsink"))
qDebug() << Q_FUNC_INFO << "magnetometer join failed";

if (!filterBin->join("accelerometer", "source", "avgaccelerometer", "sink"))
qDebug() << Q_FUNC_INFO << "accelerometer join failed";


if (!filterBin->join("avgaccelerometer", "source", "downsamplefilter", "sink"))
qDebug() << Q_FUNC_INFO << "avgaccelerometer join failed";

if (!filterBin->join("downsamplefilter", "source", "compass", "accsink"))
qDebug() << Q_FUNC_INFO << "downsamplefilter join failed";
// }
if (!filterBin->join("compass", "magnorthangle", "magneticnorth", "sink"))
qDebug() << Q_FUNC_INFO << "compass1 join failed";

if (!filterBin->join("compass", "magnorthangle", "magneticnorth", "sink"))
qDebug() << Q_FUNC_INFO << "compass1 join failed";
if (!filterBin->join("compass", "magnorthangle", "declinationcorrection", "sink"))
qDebug() << Q_FUNC_INFO << "compass2 join failed";

if (!filterBin->join("compass", "magnorthangle", "declinationcorrection", "sink"))
qDebug() << Q_FUNC_INFO << "compass2 join failed";

if (!filterBin->join("declinationcorrection", "source", "truenorth", "sink"))
qDebug() << Q_FUNC_INFO << "declinationfilter join failed";
} else {
////////////////////
if (!filterBin->join("orientation", "source", "orientationfilter", "orientsink"))
qDebug() << Q_FUNC_INFO << "orientation join failed";

if (!filterBin->join("orientationfilter", "magnorthangle", "magneticnorth", "sink"))
qDebug() << Q_FUNC_INFO << "orientation2 join failed";

if (!filterBin->join("orientationfilter", "magnorthangle", "declinationcorrection", "sink"))
qDebug() << Q_FUNC_INFO << "orientation3 join failed";
}

if (!connectToSource(accelerometerChain, "accelerometer", accelerometerReader))
qDebug() << Q_FUNC_INFO << "accelerometer connect failed";
if (!filterBin->join("declinationcorrection", "source", "truenorth", "sink"))
qDebug() << Q_FUNC_INFO << "declinationfilter join failed";

if (!connectToSource(magChain, "calibratedmagnetometerdata", magReader))
qDebug() << Q_FUNC_INFO << "magnetometer connect failed";
if (!hasOrientationAdaptor) {
if (!connectToSource(accelerometerChain, "accelerometer", accelerometerReader))
qDebug() << Q_FUNC_INFO << "accelerometer connect failed";

// if (orientAdaptor->isValid()) {
// if (!connectToSource(orientAdaptor, "orientation", orientationdataReader))
// qDebug() << Q_FUNC_INFO << "orientation connect failed";
// }
if (!connectToSource(magChain, "calibratedmagnetometerdata", magReader))
qDebug() << Q_FUNC_INFO << "magnetometer connect failed";
} else {
////////////////////
if (!connectToSource(orientAdaptor, "orientation", orientationdataReader))
qDebug() << Q_FUNC_INFO << "orientation connect failed";
}

setDescription("Compass direction"); //compass north in degrees
introduceAvailableDataRange(DataRange(0, 359, 1));
introduceAvailableInterval(DataRange(50,200,0));

setRangeSource(magChain);
addStandbyOverrideSource(magChain);
if (!hasOrientationAdaptor) {
setRangeSource(magChain);
addStandbyOverrideSource(magChain);

addStandbyOverrideSource(accelerometerChain);
setIntervalSource(accelerometerChain);
addStandbyOverrideSource(accelerometerChain);
setIntervalSource(accelerometerChain);
}
}

CompassChain::~CompassChain()
{
SensorManager& sm = SensorManager::instance();

disconnectFromSource(accelerometerChain, "accelerometer", accelerometerReader);

disconnectFromSource(magChain, "magnetometer", magReader);

// disconnectFromSource(orientAdaptor, "orientation", orientationdataReader);
// sm.releaseDeviceAdaptor("orientationadaptor");

delete accelerometerReader;
delete magReader;
// delete orientationdataReader;
if (!hasOrientationAdaptor) {
disconnectFromSource(accelerometerChain, "accelerometer", accelerometerReader);
disconnectFromSource(magChain, "magnetometer", magReader);
delete accelerometerReader;
delete magReader;
delete compassFilter;
} else {
disconnectFromSource(orientAdaptor, "orientation", orientationdataReader);
sm.releaseDeviceAdaptor("orientationadaptor");
delete orientationdataReader;
delete orientationFilter;
}
delete declinationFilter;
delete trueNorthBuffer;
delete magneticNorthBuffer;
Expand All @@ -170,12 +191,12 @@ bool CompassChain::start()
if (AbstractSensorChannel::start()) {
sensordLogD() << "Starting compassChain";
filterBin->start();
// if (orientAdaptor->isValid()) {
// orientAdaptor->startSensor();
// } else {
if (hasOrientationAdaptor) {
orientAdaptor->startSensor();
} else {
accelerometerChain->start();
magChain->start();
// }
}
}
return true;
}
Expand All @@ -184,12 +205,12 @@ bool CompassChain::stop()
{
if (AbstractSensorChannel::stop()) {
sensordLogD() << "Stopping compassChain";
// if (orientAdaptor->isValid()) {
// orientAdaptor->stopSensor();
// } else {
if (hasOrientationAdaptor) {
orientAdaptor->stopSensor();
} else {
accelerometerChain->stop();
magChain->stop();
// }
}
filterBin->stop();
}
return true;
Expand All @@ -206,7 +227,7 @@ void CompassChain::setCompassEnabled(bool enabled)

quint16 CompassChain::declinationValue() const
{
return 0;
return 0; //TODO
}

void CompassChain::resetCalibration()
Expand Down
11 changes: 6 additions & 5 deletions chains/compasschain/compasschain.h
View file
Expand Up @@ -70,23 +70,24 @@ public Q_SLOTS:
AbstractChain *accelerometerChain;
AbstractChain *magChain;

// DeviceAdaptor *accelerometerAdaptor;
BufferReader<AccelerationData> *accelerometerReader;

// DeviceAdaptor *magAdaptor;
BufferReader<CalibratedMagneticFieldData> *magReader;

// DeviceAdaptor *orientAdaptor;
// BufferReader<TimedXyzData> *orientationdataReader;
DeviceAdaptor *orientAdaptor;
BufferReader<TimedXyzData> *orientationdataReader;


FilterBase *compassFilter;
FilterBase *orientationFilter;
FilterBase *declinationFilter;

FilterBase *downsampleFilter;
FilterBase *avgaccFilter;

RingBuffer<CompassData> *trueNorthBuffer;
RingBuffer<CompassData> *magneticNorthBuffer;

bool hasOrientationAdaptor;
};

#endif // COMPASSCHAIN_H
10 changes: 4 additions & 6 deletions chains/compasschain/compasschain.pro
View file
Expand Up @@ -2,16 +2,14 @@ TARGET = compasschain

HEADERS += compasschain.h \
compasschainplugin.h \
compassfilter.h
compassfilter.h \
orientationfilter.h

SOURCES += compasschain.cpp \
compasschainplugin.cpp \
compassfilter.cpp

#HEADERS += ../../filters/avgaccfilter/avgaccfilter.h
#SOURCES += ../../filters/avgaccfilter/avgaccfilter.cpp
compassfilter.cpp \
orientationfilter.cpp

INCLUDEPATH += ../../filters/coordinatealignfilter
#INCLUDEPATH += ../../filters/avgaccfilter

include( ../chain-config.pri )
2 changes: 2 additions & 0 deletions chains/compasschain/compasschainplugin.cpp
View file
Expand Up @@ -21,6 +21,7 @@
#include "compasschainplugin.h"
#include "compasschain.h"
#include "compassfilter.h"
#include "orientationfilter.h"
#include "sensormanager.h"
#include "logging.h"

Expand All @@ -31,6 +32,7 @@ void CompassChainPlugin::Register(class Loader&)

sm.registerChain<CompassChain>("compasschain");
sm.registerFilter<CompassFilter>("compassfilter");
sm.registerFilter<OrientationFilter>("orientationfilter");
}

QStringList CompassChainPlugin::Dependencies() {
Expand Down
14 changes: 0 additions & 14 deletions chains/compasschain/compassfilter.cpp
View file
Expand Up @@ -29,13 +29,10 @@
CompassFilter::CompassFilter() :
magDataSink(this, &CompassFilter::magDataAvailable),
accelSink(this, &CompassFilter::accelDataAvailable),
orientDataSink(this, &CompassFilter::orientDataAvailable),
factor(1)
{
addSink(&magDataSink, "magsink");
addSink(&accelSink, "accsink");
addSink(&orientDataSink, "orientsink");

addSource(&magSource, "magnorthangle");
}

Expand Down Expand Up @@ -122,14 +119,3 @@ int offset = 90;
compassData.level_ = level;
magSource.propagate(1, &compassData);
}

void CompassFilter::orientDataAvailable(unsigned, const TimedXyzData *data)
{
CompassData compassData; //north angle
compassData.timestamp_ = data->timestamp_;
compassData.degrees_ = data->x_;
compassData.level_ = level;
qDebug() << Q_FUNC_INFO << compassData.degrees_ * .001;

// magSource.propagate(1, &compassData);
}
3 changes: 0 additions & 3 deletions chains/compasschain/compassfilter.h
View file
Expand Up @@ -44,13 +44,10 @@ class CompassFilter : public QObject, public FilterBase

Sink<CompassFilter, CalibratedMagneticFieldData> magDataSink;
Sink<CompassFilter, AccelerationData> accelSink;
Sink<CompassFilter, TimedXyzData> orientDataSink;

Source<CompassData> magSource;

void magDataAvailable(unsigned, const CalibratedMagneticFieldData*);
void accelDataAvailable(unsigned, const AccelerationData*);
void orientDataAvailable(unsigned, const TimedXyzData*);

int factor;
CalibratedMagneticFieldData magData;
Expand Down

0 comments on commit b1c400f

Please sign in to comment.