CompassChain::CompassChain(const QString& id) :
AbstractChain(id),
hasOrientationAdaptor(false)
{
SensorManager& sm = SensorManager::instance();
orientAdaptor = sm.requestDeviceAdaptor("orientationadaptor");
if (orientAdaptor) {
Q_ASSERT(orientAdaptor);
hasOrientationAdaptor = true;
setValid(orientAdaptor->isValid());
if (orientAdaptor->isValid())
orientationdataReader = new BufferReader<TimedXyzData>(1);
orientationFilter = sm.instantiateFilter("orientationfilter");
Q_ASSERT(orientationFilter);
declinationFilter = sm.instantiateFilter("declinationfilter");
Q_ASSERT(declinationFilter);
//hybris has compass heading in orientation sensor
}
if (!hasOrientationAdaptor) {
magChain = sm.requestChain("magcalibrationchain");
Q_ASSERT(magChain);
setValid(magChain->isValid());
accelerometerChain = sm.requestChain("accelerometerchain");
Q_ASSERT(accelerometerChain);
setValid(accelerometerChain->isValid());
accelerometerReader = new BufferReader<AccelerationData>(1);
magReader = new BufferReader<CalibratedMagneticFieldData>(1);
compassFilter = sm.instantiateFilter("compassfilter");
Q_ASSERT(compassFilter);
declinationFilter = sm.instantiateFilter("declinationfilter");
Q_ASSERT(declinationFilter);
downsampleFilter = sm.instantiateFilter("downsamplefilter");
Q_ASSERT(downsampleFilter);
avgaccFilter = sm.instantiateFilter("avgaccfilter");
Q_ASSERT(avgaccFilter);
}
trueNorthBuffer = new RingBuffer<CompassData>(1);
nameOutputBuffer("truenorth", trueNorthBuffer); //
magneticNorthBuffer = new RingBuffer<CompassData>(1);
nameOutputBuffer("magneticnorth", magneticNorthBuffer); //
// Create buffers for filter chain
filterBin = new Bin;
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(declinationFilter, "declinationcorrection");
filterBin->add(trueNorthBuffer, "truenorth");
filterBin->add(magneticNorthBuffer, "magneticnorth");
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", "declinationcorrection", "sink"))
qDebug() << Q_FUNC_INFO << "compass2 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 (!filterBin->join("declinationcorrection", "source", "truenorth", "sink"))
qDebug() << Q_FUNC_INFO << "declinationfilter join failed";
if (!hasOrientationAdaptor) {
if (!connectToSource(accelerometerChain, "accelerometer", accelerometerReader))
qDebug() << Q_FUNC_INFO << "accelerometer 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));
if (!hasOrientationAdaptor) {
setRangeSource(magChain);
addStandbyOverrideSource(magChain);
addStandbyOverrideSource(accelerometerChain);
setIntervalSource(accelerometerChain);
}
}
MagCalibrationChain::MagCalibrationChain(const QString& id) :
AbstractChain(id)
{
setMatrixFromString("1,0,0,\
0,1,0,\
0,0,1");
SensorManager& sm = SensorManager::instance();
magAdaptor = sm.requestDeviceAdaptor("magnetometeradaptor");
setValid(magAdaptor->isValid());
// Config::configuration()->value<int>("magnetometer/interval_compensation", 16);
// Get the transformation matrix from config file
QString aconvString = Config::configuration()->value<QString>("magnetometer/transformation_matrix", "");
if (aconvString.size() > 0) {
if (!setMatrixFromString(aconvString)) {
sensordLogW() << "Failed to parse 'transformation_matrix' configuration key. Coordinate alignment may be invalid";
}
}
needsCalibration = Config::configuration()->value<bool>("magnetometer/needs_calibration", true);
calibratedMagnetometerData = new RingBuffer<CalibratedMagneticFieldData>(1);
nameOutputBuffer("calibratedmagnetometerdata", calibratedMagnetometerData);
// Create buffers for filter chain
filterBin = new Bin;
//formationsink
magReader = new BufferReader<CalibratedMagneticFieldData>(1);
// Join filterchain buffers
filterBin->add(magReader, "calibratedmagneticfield");
filterBin->add(calibratedMagnetometerData, "calibratedmagnetometerdata"); //calibration
if (sm.getAdaptorTypes().contains("orientationadaptor")) {
DeviceAdaptor *orientAdaptor = sm.requestDeviceAdaptor("orientationadaptor");
if (orientAdaptor && orientAdaptor->isValid()) {
needsCalibration = false;
}
}
magCoordinateAlignFilter_ = sm.instantiateFilter("magcoordinatealignfilter");
Q_ASSERT(magCoordinateAlignFilter_);
filterBin->add(magCoordinateAlignFilter_, "magcoordinatealigner");
if (needsCalibration) {
magCalFilter = sm.instantiateFilter("calibrationfilter");
((MagCoordinateAlignFilter*)magCoordinateAlignFilter_)->setMatrix(TMagMatrix(aconv_));
filterBin->add(magCalFilter, "calibration");
if (!filterBin->join("calibratedmagneticfield", "source", "magcoordinatealigner", "sink"))
qDebug() << Q_FUNC_INFO << "calibratedmagneticfield/magcoordinatealigner join failed";
if (!filterBin->join("magcoordinatealigner", "source", "calibration", "magsink"))
qDebug() << Q_FUNC_INFO << "magcoordinatealigner/calibration join failed";
if (!filterBin->join("calibration", "source", "calibratedmagnetometerdata", "sink"))
qDebug() << Q_FUNC_INFO << "calibration/calibratedmagnetometerdata join failed";
} else {
if (!filterBin->join("calibratedmagneticfield", "source", "magcoordinatealigner", "sink"))
qDebug() << Q_FUNC_INFO << "calibratedmagneticfield/magcoordinatealigner join failed";
if (!filterBin->join("magcoordinatealigner", "source", "calibratedmagnetometerdata", "sink"))
qDebug() << Q_FUNC_INFO << "magcoordinatealigner/calibratedmagnetometerdata join failed";
}
// Join datasources to the chain
connectToSource(magAdaptor, "calibratedmagneticfield", magReader);
setDescription("Calibrated Mag values"); //Magnetometer calibration
setRangeSource(magAdaptor);
addStandbyOverrideSource(magAdaptor);
setIntervalSource(magAdaptor);
}
