/**
// http://www.apache.org/licenses/LICENSE-2.0
@file iioadaptor.cpp
@brief IioAdaptor based on SysfsAdaptor
Copyright (C) 2009-2010 Nokia Corporation
Copyright (C) 2012 Tuomas Kulve
Copyright (C) 2012 Srdjan Markovic
Copyright (C) 2016 Canonical
@author Tuomas Kulve
@author Lorn Potter
Sensord is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License
version 2.1 as published by the Free Software Foundation.
Sensord is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with Sensord. If not, see .
*/
#include
#include
#include
#include
#include
#include
#include
#include "iioadaptor.h"
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include "datatypes/orientationdata.h"
#define GRAVITY 9.80665
#define REV_GRAVITY 0.101936799
// Proximity sensor
#define PROXIMITY_DEFAULT_THRESHOLD 250
#define PROXIMITY_NEAR_VALUE 0
#define PROXIMITY_FAR_VALUE 100
/* 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))
#define CONVERT_A_Z(x) ((float(x) / 1000) * (GRAVITY * 1.0))
IioAdaptor::IioAdaptor(const QString &id) :
SysfsAdaptor(id, SysfsAdaptor::IntervalMode, true),
deviceId(id)
{
sensordLogD() << "Creating IioAdaptor with id: " << id;
setup();
}
IioAdaptor::~IioAdaptor()
{
if (iioXyzBuffer_)
delete iioXyzBuffer_;
if (alsBuffer_)
delete alsBuffer_;
if (magnetometerBuffer_)
delete magnetometerBuffer_;
if (proximityBuffer_)
delete proximityBuffer_;
}
void IioAdaptor::setup()
{
qDebug() << Q_FUNC_INFO << deviceId;
if (deviceId.startsWith("accel")) {
const QString name = "accelerometer";
const QString inputMatch = SensorFrameworkConfig::configuration()->value(name + "/input_match");
qDebug() << "input_match" << inputMatch;
iioDevice.channelTypeName = "accel";
devNodeNumber = findSensor(inputMatch);
if (devNodeNumber!= -1) {
const QString desc = "Industrial I/O accelerometer (" + iioDevice.name +")";
qDebug() << Q_FUNC_INFO << "Accelerometer found";
iioXyzBuffer_ = new DeviceAdaptorRingBuffer(1);
setAdaptedSensor(name, desc, iioXyzBuffer_);
iioDevice.sensorType = IioAdaptor::IIO_ACCELEROMETER;
}
} else if (deviceId.startsWith("gyro")) {
const QString name = "gyroscope";
const QString inputMatch = SensorFrameworkConfig::configuration()->value(name + "/input_match");
qDebug() << "input_match" << inputMatch;
iioDevice.channelTypeName = "anglvel";
devNodeNumber = findSensor(inputMatch);
if (devNodeNumber!= -1) {
const QString desc = "Industrial I/O gyroscope (" + iioDevice.name +")";
iioXyzBuffer_ = new DeviceAdaptorRingBuffer(1);
setAdaptedSensor(name, desc, iioXyzBuffer_);
iioDevice.sensorType = IioAdaptor::IIO_GYROSCOPE;
}
} else if (deviceId.startsWith("mag")) {
const QString name = "magnetometer";
const QString inputMatch = SensorFrameworkConfig::configuration()->value(name + "/input_match");
qDebug() << "input_match" << inputMatch;
iioDevice.channelTypeName = "magn";
devNodeNumber = findSensor(inputMatch);
if (devNodeNumber!= -1) {
const QString desc = "Industrial I/O magnetometer (" + iioDevice.name +")";
magnetometerBuffer_ = new DeviceAdaptorRingBuffer(1);
setAdaptedSensor(name, desc, magnetometerBuffer_);
iioDevice.sensorType = IioAdaptor::IIO_MAGNETOMETER;
}
} else if (deviceId.startsWith("als")) {
const QString name = "als";
const QString inputMatch = SensorFrameworkConfig::configuration()->value(name + "/input_match");
iioDevice.channelTypeName = "illuminance";
devNodeNumber = findSensor(inputMatch);
if (devNodeNumber!= -1) {
QString desc = "Industrial I/O light sensor (" + iioDevice.name +")";
qDebug() << desc;
alsBuffer_ = new DeviceAdaptorRingBuffer(1);
setAdaptedSensor(name, desc, alsBuffer_);
iioDevice.sensorType = IioAdaptor::IIO_ALS;
}
} else if (deviceId.startsWith("prox")) {
const QString name = "proximity";
const QString inputMatch = SensorFrameworkConfig::configuration()->value(name + "/input_match");
qDebug() << name + ":" << "input_match" << inputMatch;
iioDevice.channelTypeName = "proximity";
devNodeNumber = findSensor(inputMatch);
proximityThreshold = SensorFrameworkConfig::configuration()->value(name + "/threshold", QString(PROXIMITY_DEFAULT_THRESHOLD)).toInt();
if (devNodeNumber!= -1) {
QString desc = "Industrial I/O proximity sensor (" + iioDevice.name +")";
qDebug() << desc;
proximityBuffer_ = new DeviceAdaptorRingBuffer(1);
setAdaptedSensor(name, desc, proximityBuffer_);
iioDevice.sensorType = IioAdaptor::IIO_PROXIMITY;
}
}
if (devNodeNumber == -1) {
qDebug() << Q_FUNC_INFO << "sensor is invalid";
// setValid(false);
return;
}
if (mode() != SysfsAdaptor::IntervalMode) {
scanElementsEnable(devNodeNumber,1);
scanElementsEnable(devNodeNumber,0);
}
/* Override the scaling factor if asked */
bool ok;
double scale_override = SensorFrameworkConfig::configuration()->value(iioDevice.name + "/scale").toDouble(&ok);
if (ok) {
sensordLogD() << "Overriding scale to" << scale_override;
iioDevice.scale = scale_override;
}
introduceAvailableDataRange(DataRange(0, 65535, 1));
introduceAvailableInterval(DataRange(0, 586, 0));
setDefaultInterval(10);
}
int IioAdaptor::findSensor(const QString &sensorName)
{
udev_list_entry *devices;
udev_list_entry *dev_list_entry;
udev_device *dev = 0;
struct udev *udevice = 0;
struct udev_enumerate *enumerate = 0;
if (!udevice)
udevice = udev_new();
enumerate = udev_enumerate_new(udevice);
udev_enumerate_add_match_subsystem(enumerate, "iio");
udev_enumerate_scan_devices(enumerate);
devices = udev_enumerate_get_list_entry(enumerate);
bool ok2;
udev_list_entry_foreach(dev_list_entry, devices) {
const char *path;
path = udev_list_entry_get_name(dev_list_entry);
dev = udev_device_new_from_syspath(udevice, path);
if (qstrcmp(udev_device_get_subsystem(dev), "iio") == 0) {
iioDevice.name = QString::fromLatin1(udev_device_get_sysattr_value(dev,"name"));
if (iioDevice.name == sensorName) {
struct udev_list_entry *sysattr;
int j = 0;
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);
// 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;
const char *value;
bool ok;
name = udev_list_entry_get_name(sysattr);
value = udev_device_get_sysattr_value(dev, name);
if (value == NULL)
continue;
qDebug() << "attr" << name << value;
QString attributeName(name);
if (attributeName.contains(QRegularExpression(iioDevice.channelTypeName + ".*scale$"))) {
iioDevice.scale = QString(value).toDouble(&ok);
if (ok) {
qDebug() << sensorName + ":" << "Scale is" << iioDevice.scale;
}
} else if (attributeName.contains(QRegularExpression(iioDevice.channelTypeName + ".*offset$"))) {
iioDevice.offset = QString(value).toDouble(&ok);
if (ok) {
qDebug() << sensorName + ":" << "Offset is" << value;
}
} else if (attributeName.endsWith("frequency")) {
iioDevice.frequency = QString(value).toDouble(&ok);
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;
addPath(iioDevice.devicePath + attributeName, j);
j++;
}
}
iioDevice.channels = j;
// in_rot_from_north_magnetic_tilt_comp_raw ?
// type
break;
}
}
}
if (dev)
udev_device_unref(dev);
udev_enumerate_unref(enumerate);
if (ok2)
return iioDevice.index;
else
return -1;
}
/*
* als
* accel_3d
* gyro_3d
* magn_3d
* incli_3d
* dev_rotation
*
* */
bool IioAdaptor::deviceEnable(int device, int enable)
{
qDebug() << Q_FUNC_INFO <<"device"<< device <<"enable" << enable;
qDebug() << "devicePath" << iioDevice.devicePath << iioDevice.name;
qDebug() << "dev_accl_" << devNodeNumber;
qDebug() << "scale" << (double)iioDevice.scale
<< "offset" << iioDevice.offset
<< "frequency" << iioDevice.frequency;
if (devNodeNumber == -1)
return false;
QString pathEnable = iioDevice.devicePath + "buffer/enable";
QString pathLength = iioDevice.devicePath + "buffer/length";
qDebug() << pathEnable << pathLength;
if (enable == 1) {
// FIXME: should enable sensors for this device? Assuming enabled already
scanElementsEnable(device, enable);
sysfsWriteInt(pathLength, IIO_BUFFER_LEN);
sysfsWriteInt(pathEnable, enable);
} else {
sysfsWriteInt(pathEnable, enable);
scanElementsEnable(device, enable);
// FIXME: should disable sensors for this device?
}
return true;
}
bool IioAdaptor::sysfsWriteInt(QString filename, int val)
{
QFile file(filename);
if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
sensordLogW() << "Failed to open " << filename;
return false;
}
QTextStream out(&file);
out << val << "\n";
file.close();
return true;
}
QString IioAdaptor::sysfsReadString(QString filename)
{
QFile file(filename);
if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
sensordLogW() << "Failed to open " << filename;
return QString();
}
QTextStream in(&file);
QString line = in.readLine();
if (line.endsWith("\n")) {
line.chop(1);
}
file.close();
return line;
}
int IioAdaptor::sysfsReadInt(QString filename)
{
QString string = sysfsReadString(filename);
bool ok;
int value = string.toInt(&ok);
if (!ok) {
sensordLogW() << "Failed to parse '" << string << "' to int from file " << filename;
}
return value;
}
// Return the number of channels
int IioAdaptor::scanElementsEnable(int device, int enable)
{
Q_UNUSED(device);
QString elementsPath = iioDevice.devicePath + "scan_elements";
QDir dir(elementsPath);
if (!dir.exists()) {
sensordLogW() << "Directory " << elementsPath << " doesn't exist";
return 0;
}
// Find all the *_en file and write 0/1 to it
QStringList filters;
filters << ("*" + iioDevice.channelTypeName + "*_en");
dir.setNameFilters(filters);
QFileInfoList list = dir.entryInfoList();
for (int i = 0; i < list.size(); ++i) {
QFileInfo fileInfo = list.at(i);
if (enable) {
QString base = fileInfo.filePath();
// Remove the _en
base.chop(3);
int index = sysfsReadInt(base + "_index");
int bytes = deviceChannelParseBytes(base + "_type");
iioDevice.channel_bytes[index] = bytes;
}
sysfsWriteInt(fileInfo.filePath(), enable);
}
return list.size();
}
int IioAdaptor::deviceChannelParseBytes(QString filename)
{
QString type = sysfsReadString(filename);
if (type.compare("le:s16/16>>0") == 0) {
return 2;
} else if (type.compare("le:s32/32>>0") == 0) {
return 4;
} else if (type.compare("le:s64/64>>0") == 0) {
return 8;
} else {
sensordLogW() << "ERROR: invalid type from file " << filename << ": " << type;
}
return 0;
}
void IioAdaptor::processSample(int fileId, int fd)
{
char buf[IIO_BUFFER_LEN];
int readBytes = 0;
qreal result = 0;
int channel = fileId%IIO_MAX_DEVICE_CHANNELS;
int device = (fileId - channel)/IIO_MAX_DEVICE_CHANNELS;
if (device == 0) {
readBytes = read(fd, buf, sizeof(buf));
if (readBytes <= 0) {
sensordLogW() << "read():" << strerror(errno);
return;
}
result = strtol(buf, NULL, 10);
if (result == 0)
return;
switch(channel) {
case 0: {
switch (iioDevice.sensorType) {
case IioAdaptor::IIO_ACCELEROMETER:
case IioAdaptor::IIO_GYROSCOPE:
timedData = iioXyzBuffer_->nextSlot();
timedData->x_= -(result + iioDevice.offset) * iioDevice.scale * 1000 * REV_GRAVITY;
break;
case IioAdaptor::IIO_MAGNETOMETER:
calData = magnetometerBuffer_->nextSlot();
calData->rx_ = (result + iioDevice.offset) * iioDevice.scale;
break;
case IioAdaptor::IIO_ALS:
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();
// IIO proximity sensors are inverted in comparison to Hybris proximity sensors
if (proximityValue >= proximityThreshold) {
near = true;
}
proximityData->withinProximity_ = near;
proximityData->value_ = near ? PROXIMITY_NEAR_VALUE : PROXIMITY_FAR_VALUE;
}
break;
default:
break;
};
}
break;
case 1: {
switch (iioDevice.sensorType) {
case IioAdaptor::IIO_ACCELEROMETER:
case IioAdaptor::IIO_GYROSCOPE:
timedData = iioXyzBuffer_->nextSlot();
timedData->y_= -(result + iioDevice.offset) * iioDevice.scale * 1000 * REV_GRAVITY;
break;
case IioAdaptor::IIO_MAGNETOMETER:
calData = magnetometerBuffer_->nextSlot();
result = (result * iioDevice.scale);
calData->y_ = result;
break;
default:
break;
};
}
break;
case 2: {
switch (iioDevice.sensorType) {
case IioAdaptor::IIO_ACCELEROMETER:
case IioAdaptor::IIO_GYROSCOPE:
timedData = iioXyzBuffer_->nextSlot();
timedData->z_ = -(result + iioDevice.offset) * iioDevice.scale * 1000 * REV_GRAVITY;
break;
case IioAdaptor::IIO_MAGNETOMETER:
calData = magnetometerBuffer_->nextSlot();
result = ((result + iioDevice.offset) * iioDevice.scale) * 100;
calData->rz_ = result;
break;
default:
break;
};
}
break;
};
if (channel == iioDevice.channels - 1) {
switch (iioDevice.sensorType) {
case IioAdaptor::IIO_ACCELEROMETER:
case IioAdaptor::IIO_GYROSCOPE:
timedData->timestamp_ = Utils::getTimeStamp();
iioXyzBuffer_->commit();
iioXyzBuffer_->wakeUpReaders();
break;
case IioAdaptor::IIO_MAGNETOMETER:
calData->timestamp_ = Utils::getTimeStamp();
magnetometerBuffer_->commit();
magnetometerBuffer_->wakeUpReaders();
break;
case IioAdaptor::IIO_ALS:
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;
};
}
}
}
bool IioAdaptor::setInterval(const unsigned int value, const int sessionId)
{
if (mode() == SysfsAdaptor::IntervalMode)
return SysfsAdaptor::setInterval(value, sessionId);
sensordLogD() << "Ignoring setInterval for " << value;
return true;
}
//unsigned int IioAdaptor::interval() const
//{
// int value = 100;
// sensordLogD() << "Returning dummy value in interval(): " << value;
// return value;
//}
bool IioAdaptor::startSensor()
{
if (devNodeNumber == -1)
return false;
qDebug() << Q_FUNC_INFO;
if (mode() != SysfsAdaptor::IntervalMode)
deviceEnable(devNodeNumber, true);
return SysfsAdaptor::startSensor();
}
void IioAdaptor::stopSensor()
{
if (devNodeNumber == -1)
return;
qDebug() << Q_FUNC_INFO;
if (mode() != SysfsAdaptor::IntervalMode)
deviceEnable(devNodeNumber, false);
SysfsAdaptor::stopSensor();
}