bool SessionData::write(const void* source, int size)
{
long since = sinceLastWrite();
int allocSize = bufferSize * size + sizeof(unsigned int);
if(!buffer)
buffer = new char[allocSize];
else if(size != this->size)
{
socket->waitForBytesWritten();
delete[] buffer;
buffer = new char[allocSize];
}
this->size = size;
if(bufferSize <= 1)
{
memcpy(buffer + sizeof(unsigned int), source, size);
if(!downsampling || (downsampling && since >= interval))
{
sensordLogT() << "[SocketHandler]: writing, since > interval or downsampling disabled";
gettimeofday(&lastWrite, 0);
return write(buffer, size, 1);
}
}
else
{
memcpy(buffer + sizeof(unsigned int) + size * count, source, size);
++count;
if(bufferSize == count)
{
sensordLogT() << "[SocketHandler]: writing, bufferSize == count";
return delayedWrite();
}
}
if(!timer.isActive())
{
if(bufferSize > 1 && bufferInterval)
{
sensordLogT() << "[SocketHandler]: delayed write by " << bufferInterval << "ms";
timer.start(bufferInterval);
}
else if(!bufferSize && (interval - since) > 0)
{
sensordLogT() << "[SocketHandler]: delayed write by " << (interval - since) << "ms";
timer.start(interval - since);
}
}
else
{
sensordLogT() << "[SocketHandler]: timer already running";
}
return true;
}
void OemtabletMagnetometerAdaptor::processSample(int pathId, int fd)
{
int x, y, z;
if (pathId != devId) {
sensordLogW() << "pathId != devId";
return;
}
lseek(fd, 0, SEEK_SET);
if (read(fd, buf, sizeof(buf)) <= 0) {
sensordLogW() << "read():" << strerror(errno);
return;
}
sensordLogT() << "Magnetometer output value: " << buf;
sscanf(buf, "(%d,%d,%d)", &x, &y, &z);
TimedXyzData* pos = magnetBuffer_->nextSlot();
pos->x_ = x;
pos->y_ = y;
pos->z_ = z;
pos->timestamp_ = Utils::getTimeStamp();
magnetBuffer_->commit();
magnetBuffer_->wakeUpReaders();
}
void MagnetometerAdaptor::processSample(int pathId, int fd)
{
Q_UNUSED(pathId);
struct ak8974_data mag_data;
unsigned int bytesRead = read(fd, &mag_data, sizeof(mag_data));
if (bytesRead < sizeof(mag_data)) {
sensordLogW() << "read " << bytesRead << " bytes out of expected " << sizeof(mag_data) << " bytes. Previous error: " << strerror(errno);
//return;
}
if (!mag_data.valid) {
// Can't trust this, printed for curiosity
sensordLogD() << "Invalid sample received from magnetometer";
}
sensordLogT() << "Magnetometer reading: " << mag_data.x << ", " << mag_data.y << ", " << mag_data.z;
CalibratedMagneticFieldData* sample = magnetometerBuffer_->nextSlot();
sample->timestamp_ = Utils::getTimeStamp();
sample->x_ = mag_data.x;
sample->y_ = mag_data.y;
sample->z_ = mag_data.z;
magnetometerBuffer_->commit();
magnetometerBuffer_->wakeUpReaders();
}
void ALSAdaptorSysfs::processSample(int pathId, int fd)
{
Q_UNUSED(pathId);
char asciidata[6];
__u16 idata = 0;
int bytesRead = read(fd, &asciidata, sizeof(asciidata));
asciidata[sizeof(asciidata)-1] = '\0';
idata = atoi(asciidata);
if (bytesRead <= 0) {
sensordLogW() << "read(): " << strerror(errno);
return;
}
sensordLogT() << "Ambient light value: " << idata;
TimedUnsigned* lux = alsBuffer_->nextSlot();
lux->value_ = idata;
lux->timestamp_ = Utils::getTimeStamp();
alsBuffer_->commit();
alsBuffer_->wakeUpReaders();
}
bool SocketHandler::write(int id, const void* source, int size)
{
QMap<int, SessionData*>::iterator it = m_idMap.find(id);
if (it == m_idMap.end())
{
sensordLogD() << "[SocketHandler]: Trying to write to nonexistent session (normal, no panic).";
return false;
}
sensordLogT() << "[SocketHandler]: Writing to session " << id;
return (*it)->write(source, size);
}
void AccelerometerAdaptor::commitOutput(struct input_event *ev)
{
AccelerationData* d = accelerometerBuffer_->nextSlot();
d->timestamp_ = Utils::getTimeStamp(&(ev->time));
d->x_ = orientationValue_.x_;
d->y_ = orientationValue_.y_;
d->z_ = orientationValue_.z_;
sensordLogT() << "Accelerometer reading: " << d->x_ << ", " << d->y_ << ", " << d->z_;
accelerometerBuffer_->commit();
accelerometerBuffer_->wakeUpReaders();
}
bool SessionData::write(void* source, int size, unsigned int count)
{
if(socket && count)
{
sensordLogT() << "[SocketHandler]: writing " << count << " fragments to socket with payload (bytes): " << size;
memcpy(source, &count, sizeof(unsigned int));
int written = socket->write((const char*)source, size * count + sizeof(unsigned int));
if(written < 0)
{
sensordLogW() << "[SocketHandler]: failed to write payload to the socket: " << socket->errorString();
return false;
}
return true;
}
return false;
}
void SocketHandler::newConnection()
{
sensordLogT() << "[SocketHandler]: New connection received.";
while (m_server->hasPendingConnections()) {
QLocalSocket* socket = m_server->nextPendingConnection();
connect(socket, SIGNAL(readyRead()), this, SLOT(socketReadable()));
connect(socket, SIGNAL(disconnected()), this, SLOT(socketDisconnected()));
connect(socket, SIGNAL(error(QLocalSocket::LocalSocketError)), this, SLOT(socketError(QLocalSocket::LocalSocketError)));
// Initialize socket
socket->write("\n", 1);
socket->waitForBytesWritten();
}
}
void SessionData::setBufferSize(unsigned int size)
{
if(size != bufferSize)
{
if(timer.isActive())
timer.stop();
socket->waitForBytesWritten();
delete[] buffer;
buffer = 0;
count = 0;
bufferSize = size;
if(bufferSize < 1)
bufferSize = 1;
sensordLogT() << "[SocketHandler]: new buffersize: " << bufferSize;
}
}
void DeclinationFilter::correct(unsigned, const CompassData* data)
{
CompassData newOrientation(*data);
if(newOrientation.timestamp_ - lastUpdate_ > updateInterval_)
{
loadSettings();
lastUpdate_ = newOrientation.timestamp_;
}
newOrientation.correctedDegrees_ = newOrientation.degrees_;
if(declinationCorrection_)
{
newOrientation.correctedDegrees_ += declinationCorrection_;
newOrientation.correctedDegrees_ %= 360;
sensordLogT() << "DeclinationFilter corrected degree " << newOrientation.degrees_ << " => " << newOrientation.correctedDegrees_ << ". Level: " << newOrientation.level_;
}
orientation_ = newOrientation;
source_.propagate(1, &orientation_);
}
void AvgAccFilter::interpret(unsigned, const TimedXyzData *data)
{
avgAccdata.x_ = data->x_ * filterFactor + avgAccdata.x_ * (1.0 - filterFactor);
avgAccdata.y_ = data->y_ * filterFactor + avgAccdata.y_ * (1.0 - filterFactor);
avgAccdata.z_ = data->z_ * filterFactor + avgAccdata.z_ * (1.0 - filterFactor);
TimedXyzData filteredData(data->timestamp_,
avgAccdata.x_,
avgAccdata.y_,
avgAccdata.z_);
sensordLogT() << "averaged: "
<< filteredData.x_
<< ", "
<< filteredData.y_
<< ", " << filteredData.z_;
source_.propagate(1, &filteredData);
}
void GyroscopeAdaptor::processSample(int pathId, int fd)
{
Q_UNUSED(pathId);
short x, y, z;
char buf[32];
if (read(fd, buf, sizeof(buf)) <= 0) {
sensordLogW() << "read():" << strerror(errno);
return;
}
sensordLogT() << "gyroscope output value: " << buf;
sscanf(buf, "%hd %hd %hd\n", &x, &y, &z);
TimedXyzData* pos = gyroscopeBuffer_->nextSlot();
pos->x_ = x;
pos->y_ = y;
pos->z_ = z;
gyroscopeBuffer_->wakeUpReaders();
}
void ALSAdaptorAscii::processSample(int pathId, int fd) {
Q_UNUSED(pathId);
if (read(fd, buf, sizeof(buf)) <= 0) {
sensordLogW() << "read():" << strerror(errno);
return;
}
buf[sizeof(buf)-1] = '\0';
sensordLogT() << "Ambient light value: " << buf;
__u16 idata = atoi(buf);
TimedUnsigned* lux = alsBuffer_->nextSlot();
lux->value_ = idata;
lux->timestamp_ = Utils::getTimeStamp();
alsBuffer_->commit();
alsBuffer_->wakeUpReaders();
}
ALSAdaptorAscii::ALSAdaptorAscii(const QString& id) : SysfsAdaptor(id, SysfsAdaptor::IntervalMode)
{
memset(buf, 0x0, 16);
alsBuffer_ = new DeviceAdaptorRingBuffer<TimedUnsigned>(1);
setAdaptedSensor("als", "Internal ambient light sensor lux values", alsBuffer_);
setDescription("Ambient light");
// Get range from a file, if the path is found in configuration
QString rangeFilePath_ = Config::configuration()->value("als/range_file_path",QVariant("")).toString();
if (rangeFilePath_ != "") {
QFile sysFile(rangeFilePath_);
if (!(sysFile.open(QIODevice::ReadOnly))) {
sensordLogW() << "Unable to config ALS range from sysfs";
} else {
sysFile.readLine(buf, sizeof(buf));
int range = QString(buf).toInt();
introduceAvailableDataRange(DataRange(0, range, 1));
sensordLogT() << "Ambient light range: " << range;
}
}
}
void IioAdaptor::processSample(int fileId, int fd)
{
char buf[256];
int readBytes;
int buf_i = 0;
int channel = fileId%IIO_MAX_DEVICE_CHANNELS;
int device = (fileId-channel)/IIO_MAX_DEVICE_CHANNELS;
int values[devices_[device].channels];
readBytes = read(fd, buf, sizeof(buf));
if (readBytes <= 0) {
sensordLogW() << "read():" << strerror(errno);
return;
}
sensordLogT() << "Read " << readBytes << " bytes";
while (buf_i < readBytes) {
int bytes = devices_[device].channel_bytes[channel];
switch(bytes) {
case 1:
values[channel] = buf[buf_i];
break;
case 2:
values[channel] = (buf[buf_i] << 8) | (buf[buf_i] << 0);
break;
case 4:
values[channel] = (buf[buf_i] << 24) | (buf[buf_i] << 16) | (buf[buf_i] << 8) | (buf[buf_i] << 0);
break;
#if 0
// Needs 64bit
case 8:
values[channel] =
(buf[buf_i] << 56) | (buf[buf_i] << 48) | (buf[buf_i] << 40) | (buf[buf_i] << 32) |
(buf[buf_i] << 24) | (buf[buf_i] << 16) | (buf[buf_i] << 8) | (buf[buf_i] << 0);
break;
#endif
}
buf_i += bytes;
channel++;
}
// FIXME: shouldn't hardcode
if (device == 0) {
TimedXyzData* accl = iioAcclBuffer_->nextSlot();
accl->x_ = values[7];
accl->y_ = values[8];
accl->z_ = values[9];
iioAcclBuffer_->wakeUpReaders();
TimedXyzData* gyro = iioGyroBuffer_->nextSlot();
gyro->x_ = values[4];
gyro->y_ = values[5];
gyro->z_ = values[6];
iioGyroBuffer_->wakeUpReaders();
}
if (device == 1) {
TimedXyzData* magn = iioMagnBuffer_->nextSlot();
magn->x_ = values[0];
magn->y_ = values[1];
magn->z_ = values[2];
iioMagnBuffer_->wakeUpReaders();
}
}
void ProximityAdaptor::processSample(int pathId, int fd)
{
Q_UNUSED(pathId);
int ret = 0;
unsigned rawdata = 0;
if (deviceType_ == RM680)
{
bh1770glc_ps ps_data;
int bytesRead = read(fd, &ps_data, sizeof(ps_data));
if (bytesRead > 0) {
sensordLogT() << "Proximity Values: " << ps_data.led1 << ", " << ps_data.led2 << ", " << ps_data.led3;
} else {
sensordLogW() << "read(): " << strerror(errno);
return;
}
if ( ps_data.led1 > threshold_ ) {
ret = 1;
}
rawdata = ps_data.led1;
}
else if(deviceType_ == RM696)
{
apds990x_data ps_data;
int bytesRead = read(fd, &ps_data, sizeof(ps_data));
if (bytesRead > 0) {
sensordLogT() << "Proximity Values: " << ps_data.ps << ", " << ps_data.ps_raw << ", " << ps_data.status;
} else {
sensordLogW() << "read(): " << strerror(errno);
return;
}
if ( ps_data.ps > threshold_ ) {
ret = 1;
}
rawdata = ps_data.ps;
}
else if(deviceType_ == NCDK)
{
char buffer[100];
memset(buffer, 0, sizeof(buffer));
int bytesRead = read(fd, &buffer, sizeof(buffer));
if (bytesRead <= 0) {
sensordLogW() << "read(): " << strerror(errno);
return;
}
sscanf(buffer, "%d", &rawdata);
if ( (signed)rawdata > threshold_ ) {
ret = 1;
}
sensordLogT() << "Proximity value: " << rawdata;
}
else
{
sensordLogW() << "Not known device type: " << deviceType_;
return;
}
ProximityData* proximityData = proximityBuffer_->nextSlot();
proximityData->timestamp_ = Utils::getTimeStamp();
proximityData->withinProximity_ = ret;
proximityData->value_ = rawdata;
proximityBuffer_->commit();
proximityBuffer_->wakeUpReaders();
}
