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