Skip to content

Commit

Permalink
Browse files Browse the repository at this point in the history
[sensorfw] get compass working Fixes MER#822
* change magcalibration chain expected data type to
CalibratedMagneticFieldData

* add MagCoordinateAlignFilter to remap mag data points when needed

* invalidate the hybris adaptor if no sensor is dynamically found.

* if orientationadaptor (calibrated compass) is valid use it, otherwise
use sensorfw for calibration and compass routines.

This helps for hybris based systems, when virtual sensors are not
present. It keeps the default hybris configuration file usable.
  • Loading branch information
lpotter committed Apr 13, 2015
1 parent 302e85a commit 43b9d55
Show file tree
Hide file tree
Showing 47 changed files with 733 additions and 275 deletions.
13 changes: 7 additions & 6 deletions adaptors/hybrisaccelerometer/hybrisaccelerometeradaptor.cpp
Expand Up @@ -23,6 +23,8 @@
#include "datatypes/utils.h"
#include <hardware/sensors.h>

#define GRAVITY_RECIPROCAL_THOUSANDS 101.971621298

HybrisAccelerometerAdaptor::HybrisAccelerometerAdaptor(const QString& id) :
HybrisAdaptor(id,SENSOR_TYPE_ACCELEROMETER)
{
Expand Down Expand Up @@ -55,15 +57,14 @@ void HybrisAccelerometerAdaptor::stopSensor()

void HybrisAccelerometerAdaptor::processSample(const sensors_event_t& data)
{

AccelerationData *d = buffer->nextSlot();
d->timestamp_ = quint64(data.timestamp * .001);
// sensorfw wants milli-G'
d->x_ = -(data.data[0] / 9.80665 * 1000);
d->y_ = -(data.data[1] / 9.80665 * 1000);
d->z_ = -(data.data[2] / 9.80665 * 1000);
// qt's sensorfw plugin expects G == 9.81286, but it should be
//9.80665

d->x_ = data.acceleration.x * GRAVITY_RECIPROCAL_THOUSANDS;
d->y_ = data.acceleration.y * GRAVITY_RECIPROCAL_THOUSANDS;
d->z_ = data.acceleration.z * GRAVITY_RECIPROCAL_THOUSANDS;

buffer->commit();
buffer->wakeUpReaders();
}
Expand Down
9 changes: 5 additions & 4 deletions adaptors/hybrisgyroscopeadaptor/hybrisgyroscopeadaptor.cpp
Expand Up @@ -24,6 +24,8 @@
#include <hardware/sensors.h>
#include <math.h>

#define RADIANS_TO_DEGREESECONDS 57295.7795
#define RADIANS_TO_DEGREES 57.2957795

HybrisGyroscopeAdaptor::HybrisGyroscopeAdaptor(const QString& id) :
HybrisAdaptor(id,SENSOR_TYPE_GYROSCOPE)
Expand Down Expand Up @@ -61,10 +63,9 @@ void HybrisGyroscopeAdaptor::processSample(const sensors_event_t& data)

TimedXyzData *d = buffer->nextSlot();
d->timestamp_ = quint64(data.timestamp * .001);
d->x_ = (data.acceleration.x) * 57295.7795;
d->y_ = (data.acceleration.y) * 57295.7795;
d->z_ = (data.acceleration.z) * 57295.7795;

d->x_ = (data.gyro.x) * RADIANS_TO_DEGREES * 1000;
d->y_ = (data.gyro.y) * RADIANS_TO_DEGREES * 1000;
d->z_ = (data.gyro.z) * RADIANS_TO_DEGREES * 1000;
buffer->commit();
buffer->wakeUpReaders();
}
Expand Down
35 changes: 28 additions & 7 deletions adaptors/hybrismagnetometeradaptor/hybrismagnetometeradaptor.cpp
Expand Up @@ -27,12 +27,12 @@
HybrisMagnetometerAdaptor::HybrisMagnetometerAdaptor(const QString& id) :
HybrisAdaptor(id,SENSOR_TYPE_MAGNETIC_FIELD)
{
buffer = new DeviceAdaptorRingBuffer<TimedXyzData>(1);
buffer = new DeviceAdaptorRingBuffer<CalibratedMagneticFieldData>(1);
setAdaptedSensor("magnetometer", "Internal magnetometer coordinates", buffer);

setDescription("Hybris magnetometer");
//setStandbyOverride(false);
// setDefaultInterval(50);
setDefaultInterval(50);
}

HybrisMagnetometerAdaptor::~HybrisMagnetometerAdaptor()
Expand All @@ -56,13 +56,34 @@ void HybrisMagnetometerAdaptor::stopSensor()

void HybrisMagnetometerAdaptor::processSample(const sensors_event_t& data)
{
TimedXyzData *d = buffer->nextSlot();
CalibratedMagneticFieldData *d = buffer->nextSlot();
d->timestamp_ = quint64(data.timestamp * .001);
//uT to nT
d->x_ = (data.acceleration.x * 1000);
d->y_ = (data.acceleration.y * 1000);
d->z_ = (data.acceleration.z * 1000);
//uT
d->x_ = (data.magnetic.x * 1000);
d->y_ = (data.magnetic.y * 1000);
d->z_ = (data.magnetic.z * 1000);
d->level_= data.magnetic.status;
#ifdef SENSORS_DEVICE_API_VERSION_1_1
d->rx_ = data.uncalibrated_magnetic.x_uncalib * 1000;
d->ry_ = data.uncalibrated_magnetic.y_uncalib * 1000;
d->rz_ = data.uncalibrated_magnetic.z_uncalib * 1000;
qDebug() << Q_FUNC_INFO
<< "uncalibrated"
<< data.uncalibrated_magnetic.x_uncalib * 1000
<< data.uncalibrated_magnetic.y_uncalib * 1000
<< data.uncalibrated_magnetic.z_uncalib * 1000;
qDebug() << Q_FUNC_INFO
<< "bias"
<< data.uncalibrated_magnetic.x_bias * 1000
<< data.uncalibrated_magnetic.y_bias * 1000
<< data.uncalibrated_magnetic.z_bias * 1000;
#else
d->rx_ = data.magnetic.x * 1000;
d->ry_ = data.magnetic.y * 1000;
d->rz_ = data.magnetic.z * 1000;

#endif
qDebug() << Q_FUNC_INFO << d->x_ << d->y_ << d->z_ << data.magnetic.status;
buffer->commit();
buffer->wakeUpReaders();
}
Expand Down
Expand Up @@ -55,7 +55,7 @@ class HybrisMagnetometerAdaptor : public HybrisAdaptor
void init();

private:
DeviceAdaptorRingBuffer<TimedXyzData>* buffer;
DeviceAdaptorRingBuffer<CalibratedMagneticFieldData>* buffer;
int sensorType;

};
Expand Down
14 changes: 4 additions & 10 deletions adaptors/hybrisorientationadaptor/hybrisorientationadaptor.cpp
Expand Up @@ -44,7 +44,7 @@ HybrisOrientationAdaptor::HybrisOrientationAdaptor(const QString& id) :
HybrisAdaptor(id,SENSOR_TYPE_ORIENTATION)
{
buffer = new DeviceAdaptorRingBuffer<CompassData>(1);
setAdaptedSensor("orientation", "Internal orientation coordinates", buffer);
setAdaptedSensor("hybrisorientation", "Internal orientation coordinates", buffer);

setDescription("Hybris orientation");
// setDefaultInterval(50);
Expand Down Expand Up @@ -74,15 +74,9 @@ void HybrisOrientationAdaptor::processSample(const sensors_event_t& data)
{
CompassData *d = buffer->nextSlot();
d->timestamp_ = quint64(data.timestamp * .001);
d->degrees_ = data.data[0] * 1000; //azimuth
switch (data.orientation.status) {
case SENSOR_STATUS_UNRELIABLE:
d->level_ = 0;
break;
default:
d->level_ = data.orientation.status;
break;
};
d->degrees_ = data.orientation.azimuth; //azimuth
d->rawDegrees_ = d->degrees_;
d->level_ = data.orientation.status;

buffer->commit();
buffer->wakeUpReaders();
Expand Down
Expand Up @@ -43,7 +43,7 @@ MagnetometerAdaptorAscii::MagnetometerAdaptorAscii(const QString& id) :
SysfsAdaptor(id, SysfsAdaptor::IntervalMode)
{
memset(buf, 0x0, 32);
magnetBuffer_ = new DeviceAdaptorRingBuffer<TimedXyzData>(1);
magnetBuffer_ = new DeviceAdaptorRingBuffer<CalibratedMagneticFieldData>(1);
setAdaptedSensor("magnetometer", "ak8974 ascii", magnetBuffer_);
}

Expand All @@ -65,7 +65,7 @@ void MagnetometerAdaptorAscii::processSample(int, int fd)

sscanf(buf, "%hx:%hx:%hx\n", &x, &y, &z);

TimedXyzData* pos = magnetBuffer_->nextSlot();
CalibratedMagneticFieldData* pos = magnetBuffer_->nextSlot();
pos->x_ = (short)x;
pos->y_ = (short)y;
pos->z_ = (short)z;
Expand Down
Expand Up @@ -35,6 +35,7 @@
#include "sysfsadaptor.h"
#include "deviceadaptorringbuffer.h"
#include "datatypes/timedunsigned.h"
#include "datatypes/orientationdata.h"

class MagnetometerAdaptorAscii : public SysfsAdaptor
{
Expand All @@ -53,7 +54,7 @@ class MagnetometerAdaptorAscii : public SysfsAdaptor
void processSample(int pathId, int fd);
char buf[32];

DeviceAdaptorRingBuffer<TimedXyzData>* magnetBuffer_;
DeviceAdaptorRingBuffer<CalibratedMagneticFieldData>* magnetBuffer_;
};

#endif
Expand Up @@ -39,7 +39,7 @@
MagAdaptorEvdev::MagAdaptorEvdev(const QString& id) :
InputDevAdaptor(id, 1)
{
magnetometerBuffer_ = new DeviceAdaptorRingBuffer<TimedXyzData>(1);
magnetometerBuffer_ = new DeviceAdaptorRingBuffer<CalibratedMagneticFieldData>(1);
setAdaptedSensor("magnetometer", "Internal magnetometer coordinates", magnetometerBuffer_);
setDescription("Input device magnetometer adaptor");
powerStatePath_ = Config::configuration()->value("magnetometer/powerstate_path").toByteArray();
Expand Down Expand Up @@ -82,7 +82,7 @@ void MagAdaptorEvdev::interpretSync(int src, struct input_event *ev)

void MagAdaptorEvdev::commitOutput(struct input_event *ev)
{
TimedXyzData* magData = magnetometerBuffer_->nextSlot();
CalibratedMagneticFieldData* magData = magnetometerBuffer_->nextSlot();
magData->x_ = magValue_.x_;
magData->y_ = magValue_.y_;
magData->z_ = magValue_.z_;
Expand Down
4 changes: 2 additions & 2 deletions adaptors/magnetometeradaptor-evdev/magnetometerevdevadaptor.h
Expand Up @@ -68,13 +68,13 @@ class MagAdaptorEvdev : public InputDevAdaptor
virtual unsigned int evaluateIntervalRequests(int& sessionId) const;

private:
DeviceAdaptorRingBuffer<TimedXyzData>* magnetometerBuffer_;
DeviceAdaptorRingBuffer<CalibratedMagneticFieldData>* magnetometerBuffer_;

void interpretEvent(int src, struct input_event *ev);
void commitOutput(struct input_event *ev);
void interpretSync(int src, struct input_event *ev);
QByteArray powerStatePath_;
TimedXyzData magValue_;
CalibratedMagneticFieldData magValue_;
};

#endif
Expand Up @@ -37,7 +37,7 @@ MagnetometerAdaptorNCDK::MagnetometerAdaptorNCDK(const QString& id) :
intervalCompensation_ = Config::configuration()->value<int>("magnetometer/interval_compensation", 0);
powerStateFilePath_ = Config::configuration()->value<QByteArray>("magnetometer/path_power_state", "");
sensAdjFilePath_ = Config::configuration()->value<QByteArray>("magnetometer/path_sens_adjust", "");
magnetometerBuffer_ = new DeviceAdaptorRingBuffer<TimedXyzData>(128);
magnetometerBuffer_ = new DeviceAdaptorRingBuffer<CalibratedMagneticFieldData>(128);
setAdaptedSensor("magnetometer", "Internal magnetometer coordinates", magnetometerBuffer_);
setDescription("Magnetometer adaptor (ak8975) for NCDK");

Expand Down Expand Up @@ -84,7 +84,7 @@ void MagnetometerAdaptorNCDK::processSample(int pathId, int fd)

sensordLogT() << "Magnetometer Reading: " << x << ", " << y << ", " << z;

TimedXyzData* sample = magnetometerBuffer_->nextSlot();
CalibratedMagneticFieldData* sample = magnetometerBuffer_->nextSlot();

sample->timestamp_ = Utils::getTimeStamp();
sample->x_ = x;
Expand Down Expand Up @@ -159,7 +159,7 @@ bool MagnetometerAdaptorNCDK::setInterval(const unsigned int value, const int se
{
if(intervalCompensation_)
{
return SysfsAdaptor::setInterval(value > intervalCompensation_ ? value - intervalCompensation_ : 0, sessionId);
return SysfsAdaptor::setInterval((int)value > intervalCompensation_ ? value - intervalCompensation_ : 0, sessionId);
}
return SysfsAdaptor::setInterval(value, sessionId);
}
Expand Down
4 changes: 2 additions & 2 deletions adaptors/magnetometeradaptor-ncdk/magnetometeradaptor-ncdk.h
Expand Up @@ -29,7 +29,7 @@
#include "deviceadaptorringbuffer.h"
#include "datatypes/genericdata.h"
#include <QString>

#include "datatypes/orientationdata.h"

class MagnetometerAdaptorNCDK : public SysfsAdaptor
{
Expand Down Expand Up @@ -76,7 +76,7 @@ class MagnetometerAdaptorNCDK : public SysfsAdaptor

int x_adj, y_adj, z_adj;
bool powerState_;
DeviceAdaptorRingBuffer<TimedXyzData>* magnetometerBuffer_;
DeviceAdaptorRingBuffer<CalibratedMagneticFieldData>* magnetometerBuffer_;

bool setPowerState(bool value) const;
void getSensitivityAdjustment(int &x, int &y, int &z) const;
Expand Down
4 changes: 2 additions & 2 deletions adaptors/magnetometeradaptor/magnetometeradaptor.cpp
Expand Up @@ -47,7 +47,7 @@ MagnetometerAdaptor::MagnetometerAdaptor(const QString& id) :
SysfsAdaptor(id, SysfsAdaptor::IntervalMode, false)
{
intervalCompensation_ = Config::configuration()->value<int>("magnetometer/interval_compensation", 0);
magnetometerBuffer_ = new DeviceAdaptorRingBuffer<TimedXyzData>(1);
magnetometerBuffer_ = new DeviceAdaptorRingBuffer<CalibratedMagneticFieldData>(1);
setAdaptedSensor("magnetometer", "Internal magnetometer coordinates", magnetometerBuffer_);
overflowLimit_ = Config::configuration()->value<int>("magnetometer/overflow_limit", 8000);
setDescription("Input device Magnetometer adaptor (ak897x)");
Expand Down Expand Up @@ -78,7 +78,7 @@ void MagnetometerAdaptor::processSample(int pathId, int fd)

sensordLogT() << "Magnetometer reading: " << mag_data.x << ", " << mag_data.y << ", " << mag_data.z;

TimedXyzData* sample = magnetometerBuffer_->nextSlot();
CalibratedMagneticFieldData* sample = magnetometerBuffer_->nextSlot();

sample->timestamp_ = Utils::getTimeStamp();
sample->x_ = mag_data.x;
Expand Down
3 changes: 2 additions & 1 deletion adaptors/magnetometeradaptor/magnetometeradaptor.h
Expand Up @@ -32,6 +32,7 @@
#include "deviceadaptorringbuffer.h"
#include "datatypes/genericdata.h"
#include <QString>
#include "datatypes/orientationdata.h"

/**
* @brief Adaptor for internal magnetometer.
Expand Down Expand Up @@ -90,7 +91,7 @@ class MagnetometerAdaptor : public SysfsAdaptor
*/
int overflowLimit() const;

DeviceAdaptorRingBuffer<TimedXyzData>* magnetometerBuffer_;
DeviceAdaptorRingBuffer<CalibratedMagneticFieldData>* magnetometerBuffer_;
int intervalCompensation_;
int overflowLimit_;
};
Expand Down
7 changes: 5 additions & 2 deletions chains/accelerometerchain/accelerometerchain.cpp
Expand Up @@ -75,8 +75,11 @@ AccelerometerChain::AccelerometerChain(const QString& id) :
filterBin_->add(outputBuffer_, "buffer");

// Join filterchain buffers
filterBin_->join("accelerometer", "source", "acccoordinatealigner", "sink");
filterBin_->join("acccoordinatealigner", "source", "buffer", "sink");
if (!filterBin_->join("accelerometer", "source", "acccoordinatealigner", "sink"))
qDebug() << Q_FUNC_INFO << "accelerometer/acccoordinatealigner join failed";

if (!filterBin_->join("acccoordinatealigner", "source", "buffer", "sink"))
qDebug() << Q_FUNC_INFO << "acccoordinatealigner/buffer join failed";

// Join datasources to the chain
connectToSource(accelerometerAdaptor_, "accelerometer", accelerometerReader_);
Expand Down

0 comments on commit 43b9d55

Please sign in to comment.