RangeSensor::RangeSensor(const QString &port, const trikKernel::Configurer &configurer, ModuleLoader &moduleLoader
, const trikHal::HardwareAbstractionInterface &hardwareAbstraction)
: mState("Range Sensor on " + port)
{
if (!moduleLoader.load(configurer.attributeByPort(port, "module"))
|| !moduleLoader.load(configurer.attributeByDevice("rangeSensor", "commonModule")))
{
QLOG_ERROR() << "Module loading failed";
mState.fail();
return;
}
mMinValue = ConfigurerHelper::configureInt(configurer, mState, port, "minValue");
mMaxValue = ConfigurerHelper::configureInt(configurer, mState, port, "maxValue");
mSensorWorker.reset(new RangeSensorWorker(configurer.attributeByPort(port, "eventFile"), mState
, hardwareAbstraction));
if (!mState.isFailed()) {
mSensorWorker->moveToThread(&mWorkerThread);
connect(mSensorWorker.data(), SIGNAL(newData(int, int, trikKernel::TimeVal))
, this, SIGNAL(newData(int, int, trikKernel::TimeVal)));
QLOG_INFO() << "Starting RangeSensor worker thread" << &mWorkerThread;
mWorkerThread.start();
}
ServoMotor::ServoMotor(QString const &port, trikKernel::Configurer const &configurer)
: mDutyFile(configurer.attributeByPort(port, "deviceFile"))
, mPeriodFile(configurer.attributeByPort(port, "periodFile"))
, mCurrentDutyPercent(0)
, mInvert(configurer.attributeByPort(port, "invert") == "true")
, mCurrentPower(0)
{
auto configure = [this, &port, &configurer](QString const ¶meterName) {
return ConfigurerHelper::configureInt(configurer, mState, port, parameterName);
};
mPeriod = configure("period");
mMin = configure("min");
mMax = configure("max");
mZero = configure("zero");
mStop = configure("stop");
mMotorType = configurer.attributeByPort(port, "type") == "angular" ? Type::angular : Type::continiousRotation;
if (!mPeriodFile.open(QIODevice::WriteOnly | QIODevice::Truncate | QIODevice::Unbuffered | QIODevice::Text)) {
QLOG_ERROR() << "Can't open motor period file " << mPeriodFile.fileName();
mState.fail();
return;
}
QString const command = QString::number(mPeriod);
mPeriodFile.write(command.toLatin1());
mPeriodFile.close();
mState.ready();
}
I2cCommunicator::I2cCommunicator(const trikKernel::Configurer &configurer)
: mDevicePath(configurer.attributeByDevice("i2c", "path"))
{
bool ok = false;
mDeviceId = configurer.attributeByDevice("i2c", "deviceId").toInt(&ok, 0);
if (!ok) {
QLOG_ERROR() << "Incorrect I2C device id" << configurer.attributeByDevice("i2c", "deviceId");
mState.fail();
return;
}
connect();
}
Led::Led(trikKernel::Configurer const &configurer)
: mRedDeviceFile(configurer.attributeByDevice("led", "red"))
, mGreenDeviceFile(configurer.attributeByDevice("led", "green"))
{
if (!mRedDeviceFile.open(QIODevice::WriteOnly | QIODevice::Truncate | QIODevice::Unbuffered | QIODevice::Text)) {
QLOG_ERROR() << "Can't open red led control file " << mRedDeviceFile.fileName();
mState.fail();
}
if (!mGreenDeviceFile.open(QIODevice::WriteOnly | QIODevice::Truncate | QIODevice::Unbuffered | QIODevice::Text)) {
QLOG_ERROR() << "Can't open green led control file " << mGreenDeviceFile.fileName();
mState.fail();
}
mState.ready();
}
Keys::Keys(trikKernel::Configurer const &configurer)
{
mKeysWorker.reset(new KeysWorker(configurer.attributeByDevice("keys", "deviceFile"), mState));
if (!mState.isFailed()) {
connect(mKeysWorker.data(), SIGNAL(buttonPressed(int, int)), this, SIGNAL(buttonPressed(int, int)));
connect(mKeysWorker.data(), SIGNAL(buttonPressed(int, int)), this, SLOT(changeButtonState(int, int)));
mKeysWorker->moveToThread(&mWorkerThread);
mWorkerThread.start();
mState.ready();
}
Mailbox::Mailbox(const trikKernel::Configurer &configurer)
{
bool ok = false;
const int port = configurer.attributeByDevice("mailbox", "port").toInt(&ok);
if (!ok) {
throw trikKernel::MalformedConfigException("Incorrect mailbox port");
}
init(port);
}
RangeSensor::RangeSensor(QString const &port, trikKernel::Configurer const &configurer, ModuleLoader &moduleLoader)
{
if (!moduleLoader.load(configurer.attributeByPort(port, "module"))
|| !moduleLoader.load(configurer.attributeByDevice("rangeSensor", "commonModule")))
{
QLOG_ERROR() << "Module loading failed";
mState.fail();
return;
}
mSensorWorker.reset(new RangeSensorWorker(configurer.attributeByPort(port, "eventFile"), mState));
if (!mState.isFailed()) {
mSensorWorker->moveToThread(&mWorkerThread);
connect(mSensorWorker.data(), SIGNAL(newData(int, int)), this, SIGNAL(newData(int, int)));
mWorkerThread.start();
}
VectorSensor::VectorSensor(QString const &deviceName, trikKernel::Configurer const &configurer)
{
mVectorSensorWorker.reset(new VectorSensorWorker(configurer.attributeByDevice(deviceName, "deviceFile"), mState));
if (!mState.isFailed()) {
connect(mVectorSensorWorker.data(), SIGNAL(newData(QVector<int>)), this, SIGNAL(newData(QVector<int>)));
mVectorSensorWorker->moveToThread(&mWorkerThread);
mWorkerThread.start();
mState.ready();
}
}
DigitalSensor::DigitalSensor(const QString &port, const trikKernel::Configurer &configurer)
: mDeviceFile(configurer.attributeByPort(port, "deviceFile"))
{
mMin = ConfigurerHelper::configureInt(configurer, mState, port, "min");
mMax = ConfigurerHelper::configureInt(configurer, mState, port, "max");
mState.ready();
// Testing availability of a device.
read();
}
Keys::Keys(const trikKernel::Configurer &configurer, const trikHal::HardwareAbstractionInterface &hardwareAbstraction)
: mState("Keys")
{
mKeysWorker.reset(new KeysWorker(configurer.attributeByDevice("keys", "deviceFile"), mState, hardwareAbstraction));
if (!mState.isFailed()) {
connect(mKeysWorker.data(), SIGNAL(buttonPressed(int, int)), this, SIGNAL(buttonPressed(int, int)));
connect(mKeysWorker.data(), SIGNAL(buttonPressed(int, int)), this, SLOT(changeButtonState(int, int)));
mKeysWorker->moveToThread(&mWorkerThread);
QLOG_INFO() << "Starting Keys worker thread" << &mWorkerThread;
mWorkerThread.start();
mState.ready();
}
VectorSensor::VectorSensor(const QString &deviceName, const trikKernel::Configurer &configurer
, const trikHal::HardwareAbstractionInterface &hardwareAbstraction)
: mState(deviceName)
{
mVectorSensorWorker.reset(new VectorSensorWorker(configurer.attributeByDevice(deviceName, "deviceFile"), mState
, hardwareAbstraction));
if (!mState.isFailed()) {
qRegisterMetaType<trikKernel::TimeVal>("trikUtils::TimeVal");
connect(mVectorSensorWorker.data(), SIGNAL(newData(QVector<int>, trikKernel::TimeVal))
, this, SIGNAL(newData(QVector<int>, trikKernel::TimeVal)));
mVectorSensorWorker->moveToThread(&mWorkerThread);
QLOG_INFO() << "Starting VectorSensor worker thread" << &mWorkerThread;
mWorkerThread.start();
mState.ready();
}
}
GyroSensor::GyroSensor(const QString &deviceName, const trikKernel::Configurer &configurer
, const trikHal::HardwareAbstractionInterface &hardwareAbstraction, VectorSensorInterface *accelerometer)
: mState(deviceName)
, mIsCalibrated(false)
, mQ(QQuaternion(1, 0, 0, 0))
, mGyroCounter(0)
, mLastUpdate(trikKernel::TimeVal(0, 0))
, mAccelerometer(accelerometer)
, mAxesSwapped(false)
{
mVectorSensorWorker.reset(new VectorSensorWorker(configurer.attributeByDevice(deviceName, "deviceFile"), mState
, hardwareAbstraction, mWorkerThread));
mBias.resize(3);
mCalibrationValues.resize(6);
mGyroSum.resize(3);
mResult.resize(7);
mRawData.resize(4);
mAccelerometerSum.resize(3);
mAccelerometerCounter = 0;
mCalibrationTimer.moveToThread(&mWorkerThread);
mCalibrationTimer.setSingleShot(true);
if (!mState.isFailed()) {
qRegisterMetaType<trikKernel::TimeVal>("trikKernel::TimeVal");
connect(mVectorSensorWorker.data(), SIGNAL(newData(QVector<int>,trikKernel::TimeVal))
, this, SLOT(countTilt(QVector<int>,trikKernel::TimeVal)));
connect(&mCalibrationTimer, SIGNAL(timeout()), this, SLOT(countCalibrationParameters()));
QLOG_INFO() << "Starting VectorSensor worker thread" << &mWorkerThread;
mState.ready();
}
}
Gamepad::Gamepad(const trikKernel::Configurer &configurer
, const trikHal::HardwareAbstractionInterface &hardwareAbstraction)
: mUnderlyingFifo(configurer.attributeByDevice("gamepad", "file"), hardwareAbstraction)
{
connect(&mUnderlyingFifo, SIGNAL(newData(QString)), this, SLOT(onNewData(QString)));
}
