Skip to content

Commit

Permalink
[sensorfw] make use of orientation adaptor for compass heading if
Browse files Browse the repository at this point in the history 
available

This makes compass more reliable and accurate for those systems that
have a calibrated orientation sensor that keeps track of magnetic north.

bump version to 0.7.3.29
  • Loading branch information
Lorn Potter committed Nov 13, 2013
1 parent 7bd805e commit 363a964
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 363a964

Please sign in to comment.