Skip to content

Commit

Permalink
Merge branch 'proximity-sensor' into 'master'
Browse files Browse the repository at this point in the history 
[iioadaptor] Implement IIO proximityadaptor. Fixes MER#2062

See merge request mer-core/sensorfw!46
  • Loading branch information
@mlehtima
mlehtima committed Oct 25, 2019
2 parents 993c6ea + 08e1376 commit 2ee9322
Show file tree
Hide file tree
Showing 3 changed files with 62 additions and 15 deletions.
69 changes: 55 additions & 14 deletions adaptors/iioadaptor/iioadaptor.cpp
View file
Expand Up @@ -53,6 +53,10 @@
#define GRAVITY 9.80665
#define REV_GRAVITY 0.101936799

// Proximity sensor
#define PROXIMITY_DEFAULT_THRESHOLD 250
#define PROXIMITY_NEAR_VALUE 0

/* Conversion of acceleration data to SI units (m/s^2) */
#define CONVERT_A_X(x) ((float(x) / 1000) * (GRAVITY * -1.0))
#define CONVERT_A_Y(x) ((float(x) / 1000) * (GRAVITY * 1.0))
Expand All @@ -74,6 +78,8 @@ IioAdaptor::~IioAdaptor()
delete alsBuffer_;
if (magnetometerBuffer_)
delete magnetometerBuffer_;
if (proximityBuffer_)
delete proximityBuffer_;
}

void IioAdaptor::setup()
Expand All @@ -95,8 +101,7 @@ void IioAdaptor::setup()

iioDevice.sensorType = IioAdaptor::IIO_ACCELEROMETER;
}
}
else if (deviceId.startsWith("gyro")) {
} else if (deviceId.startsWith("gyro")) {
const QString name = "gyroscope";
const QString inputMatch = SensorFrameworkConfig::configuration()->value<QString>(name + "/input_match");
qDebug() << "input_match" << inputMatch;
Expand All @@ -110,8 +115,7 @@ void IioAdaptor::setup()

iioDevice.sensorType = IioAdaptor::IIO_GYROSCOPE;
}
}
else if (deviceId.startsWith("mag")) {
} else if (deviceId.startsWith("mag")) {
const QString name = "magnetometer";
const QString inputMatch = SensorFrameworkConfig::configuration()->value<QString>(name + "/input_match");
qDebug() << "input_match" << inputMatch;
Expand All @@ -125,8 +129,7 @@ void IioAdaptor::setup()

iioDevice.sensorType = IioAdaptor::IIO_MAGNETOMETER;
}
}
else if (deviceId.startsWith("als")) {
} else if (deviceId.startsWith("als")) {
const QString name = "als";
const QString inputMatch = SensorFrameworkConfig::configuration()->value<QString>(name + "/input_match");

Expand All @@ -139,6 +142,21 @@ void IioAdaptor::setup()
setAdaptedSensor(name, desc, alsBuffer_);
iioDevice.sensorType = IioAdaptor::IIO_ALS;
}
} else if (deviceId.startsWith("prox")) {
const QString name = "proximity";
const QString inputMatch = SensorFrameworkConfig::configuration()->value<QString>(name + "/input_match");
qDebug() << name + ":" << "input_match" << inputMatch;

iioDevice.channelTypeName = "proximity";
devNodeNumber = findSensor(inputMatch);
proximityThreshold = SensorFrameworkConfig::configuration()->value<QString>(name + "/threshold", QString(PROXIMITY_DEFAULT_THRESHOLD)).toInt();
if (devNodeNumber!= -1) {
QString desc = "Industrial I/O proximity sensor (" + iioDevice.name +")";
qDebug() << desc;
proximityBuffer_ = new DeviceAdaptorRingBuffer<ProximityData>(1);
setAdaptedSensor(name, desc, proximityBuffer_);
iioDevice.sensorType = IioAdaptor::IIO_PROXIMITY;
}
}

if (devNodeNumber == -1) {
Expand Down Expand Up @@ -197,7 +215,11 @@ int IioAdaptor::findSensor(const QString &sensorName)
QString eventName = QString::fromLatin1(udev_device_get_sysname(dev));
iioDevice.devicePath = QString::fromLatin1(udev_device_get_syspath(dev)) +"/";
iioDevice.index = eventName.right(1).toInt(&ok2);
qDebug() << Q_FUNC_INFO << "syspath" << iioDevice.devicePath;
// Default values
iioDevice.offset = 0.0;
iioDevice.scale = 1.0;
iioDevice.frequency = 1.0;
qDebug() << Q_FUNC_INFO << "Syspath for sensor (" + sensorName + "):" << iioDevice.devicePath;

udev_list_entry_foreach(sysattr, udev_device_get_sysattr_list_entry(dev)) {
const char *name;
Expand All @@ -213,18 +235,18 @@ int IioAdaptor::findSensor(const QString &sensorName)
if (attributeName.contains(QRegularExpression(iioDevice.channelTypeName + ".*scale$"))) {
iioDevice.scale = QString(value).toDouble(&ok);
if (ok) {
// scale = num;
qDebug() << "scale is" << iioDevice.scale;
qDebug() << sensorName + ":" << "Scale is" << iioDevice.scale;
}
} else if (attributeName.contains(QRegularExpression(iioDevice.channelTypeName + ".*offset$"))) {
iioDevice.offset = QString(value).toDouble(&ok);
if (ok)
qDebug() << "offset is" << value;
if (ok) {
qDebug() << sensorName + ":" << "Offset is" << value;
}
} else if (attributeName.endsWith("frequency")) {
iioDevice.frequency = QString(value).toDouble(&ok);
if (ok)
// frequency = num;
qDebug() << "frequency is" << iioDevice.frequency;
if (ok) {
qDebug() << sensorName + ":" << "Frequency is" << iioDevice.frequency;
}
} else if (attributeName.contains(QRegularExpression(iioDevice.channelTypeName + ".*raw$"))) {
qDebug() << "adding to paths:" << iioDevice.devicePath
<< attributeName << iioDevice.index;
Expand Down Expand Up @@ -432,6 +454,18 @@ void IioAdaptor::processSample(int fileId, int fd)
uData = alsBuffer_->nextSlot();
uData->value_ = (result + iioDevice.offset) * iioDevice.scale;
break;
case IioAdaptor::IIO_PROXIMITY:
{
bool near = false;
int proximityValue = (result + iioDevice.offset) * iioDevice.scale;
proximityData = proximityBuffer_->nextSlot();
if (proximityValue >= proximityThreshold) {
near = true;
}
proximityData->withinProximity_ = near;
proximityData->value_ = near ? PROXIMITY_NEAR_VALUE : proximityValue;
}
break;
default:
break;
};
Expand Down Expand Up @@ -492,6 +526,13 @@ void IioAdaptor::processSample(int fileId, int fd)
uData->timestamp_ = Utils::getTimeStamp();
alsBuffer_->commit();
alsBuffer_->wakeUpReaders();
sensordLogT() << "ALS offset=" << iioDevice.offset << "scale=" << iioDevice.scale << "value=" << uData->value_ << "timestamp=" << uData->timestamp_;
break;
case IioAdaptor::IIO_PROXIMITY:
proximityData->timestamp_ = Utils::getTimeStamp();
proximityBuffer_->commit();
proximityBuffer_->wakeUpReaders();
sensordLogT() << "Proximity offset=" << iioDevice.offset << "scale=" << iioDevice.scale << "value=" << proximityData->value_ << "within proximity=" << proximityData->withinProximity_ << "timestamp=" << proximityData->timestamp_;
break;
default:
break;
Expand Down
7 changes: 6 additions & 1 deletion adaptors/iioadaptor/iioadaptor.h
View file
Expand Up @@ -55,7 +55,8 @@ class IioAdaptor : public SysfsAdaptor
IIO_MAGNETOMETER, // magn_3d
IIO_ROTATION, // dev_rotation, quaternion
IIO_ALS, // als
IIO_TILT // incli_3d
IIO_TILT, // incli_3d
IIO_PROXIMITY // proximity als
};

struct iio_device {
Expand Down Expand Up @@ -130,9 +131,12 @@ class IioAdaptor : public SysfsAdaptor
// Device number for the sensor (-1 if not found)
int devNodeNumber;

int proximityThreshold;

DeviceAdaptorRingBuffer<TimedXyzData>* iioXyzBuffer_;
DeviceAdaptorRingBuffer<TimedUnsigned>* alsBuffer_;
DeviceAdaptorRingBuffer<CalibratedMagneticFieldData>* magnetometerBuffer_;
DeviceAdaptorRingBuffer<ProximityData>* proximityBuffer_;

iio_device iioDevice;

Expand All @@ -141,6 +145,7 @@ class IioAdaptor : public SysfsAdaptor
TimedXyzData* timedData;
CalibratedMagneticFieldData *calData;
TimedUnsigned *uData;
ProximityData *proximityData;

private slots:
void setup();
Expand Down
1 change: 1 addition & 0 deletions adaptors/iioadaptor/iioadaptorplugin.cpp
View file
Expand Up @@ -36,6 +36,7 @@ void IioAdaptorPlugin::Register(class Loader&)
sm.registerDeviceAdaptor<IioAdaptor>("gyroscopeadaptor");
sm.registerDeviceAdaptor<IioAdaptor>("magnetometeradaptor");
sm.registerDeviceAdaptor<IioAdaptor>("alsadaptor");
sm.registerDeviceAdaptor<IioAdaptor>("proximityadaptor");
}

#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
Expand Down

0 comments on commit 2ee9322

Please sign in to comment.