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);
}
DeviceAdaptor* SensorManager::requestDeviceAdaptor(const QString& id)
{
sensordLogD() << "Requesting adaptor: " << id;
clearError();
if( id.contains(';') ) // no parameter passing in release
{
setError( SmIdNotRegistered, QString(tr("unknown adaptor id '%1'").arg(id)) );
return false;
}
DeviceAdaptor* da = NULL;
QMap<QString, DeviceAdaptorInstanceEntry>::iterator entryIt = deviceAdaptorInstanceMap_.find(id);
if ( entryIt != deviceAdaptorInstanceMap_.end() )
{
if ( entryIt.value().adaptor_ )
{
Q_ASSERT( entryIt.value().adaptor_ );
da = entryIt.value().adaptor_;
entryIt.value().cnt_++;
sensordLogD() << "Found adaptor '" << id << "'. Ref count: " << entryIt.value().cnt_;
}
else
{
QString type = entryIt.value().type_;
if ( deviceAdaptorFactoryMap_.contains(type) )
{
da = deviceAdaptorFactoryMap_[type](id);
Q_ASSERT( da );
da->init();
ParameterParser::applyPropertyMap(da, entryIt.value().propertyMap_);
bool ok = da->startAdaptor();
if (ok)
{
entryIt.value().adaptor_ = da;
entryIt.value().cnt_++;
sensordLogD() << "Instantiated adaptor '" << id << "'. Valid = " << da->isValid();
}
else
{
setError(SmAdaptorNotStarted, QString(tr("adaptor '%1' can not be started").arg(id)) );
delete da;
da = NULL;
}
}
else
{
setError( SmFactoryNotRegistered, QString(tr("unknown adaptor type '%1'").arg(type)) );
}
}
}
else
{
setError( SmIdNotRegistered, QString(tr("unknown adaptor id '%1'").arg(id)) );
}
return da;
}