initial importHEADmaster

1 files changed, 160 insertions, 0 deletions

diff --git a/src/stylus.cpp b/src/stylus.cpp
new file mode 100644
index 0000000..f4fd0a8
--- /dev/null
+++ b/src/stylus.cpp
@@ -0,0 +1,160 @@
+#include <QDataStream>
+#include <gato/gato.h>
+#include "stylus.h"
+
+static const QString stylus_name(QString::fromLatin1("JN104FE9"));
+static const GatoUUID stylus_service(QString::fromLatin1("dcd68980-aadc-11e1-a22a-0002a5d5c51b"));
+static const GatoUUID agg_char_uuid(QString::fromLatin1("00002a5a-0000-1000-8000-00805f9b34fb"));
+
+Stylus::Stylus(QObject *parent) :
+	QObject(parent),
+	_manager(new GatoCentralManager(this)),
+	_peripheral(0),
+	_p(0), _x(0), _y(0), _z(0)
+{
+	connect(_manager, SIGNAL(discoveredPeripheral(GatoPeripheral*,int)),
+			SLOT(handleDiscoveredPeripheral(GatoPeripheral*,int)));
+}
+
+Stylus::~Stylus()
+{
+	if (_peripheral) {
+		disconnect(_peripheral, 0, this, 0);
+		_peripheral->disconnectPeripheral();
+	}
+}
+
+qreal Stylus::pressure() const
+{
+	return _p;
+}
+
+qreal Stylus::x() const
+{
+	return _x;
+}
+
+qreal Stylus::y() const
+{
+	return _y;
+}
+
+qreal Stylus::z() const
+{
+	return _z;
+}
+
+void Stylus::connectToAnyDevice()
+{
+	_manager->scanForPeripherals();
+}
+
+void Stylus::connectDevice(const QString &addr)
+{
+	connectToPeripheral(new GatoPeripheral(GatoAddress(addr), this));
+}
+
+void Stylus::disconnectDevice()
+{
+	if (_peripheral) {
+		_peripheral->disconnectPeripheral();
+		_peripheral->deleteLater();
+		_peripheral = 0;
+	}
+}
+
+void Stylus::connectToPeripheral(GatoPeripheral *peripheral)
+{
+	if (_peripheral) {
+		disconnectDevice();
+	}
+
+	_peripheral = peripheral;
+
+	qDebug() << "Connecting to peripheral" << peripheral->name() << peripheral->address().toString();
+
+	connect(_peripheral, SIGNAL(connected()), SLOT(handleConnected()));
+	connect(_peripheral, SIGNAL(disconnected()), SLOT(handleDisconnected()));
+	connect(_peripheral, SIGNAL(servicesDiscovered()), SLOT(handleServices()));
+	connect(_peripheral, SIGNAL(characteristicsDiscovered(GatoService)), SLOT(handleCharacteristics(GatoService)));
+	connect(_peripheral, SIGNAL(valueUpdated(GatoCharacteristic,QByteArray)), SLOT(handleValueUpdated(GatoCharacteristic,QByteArray)));
+
+	_peripheral->connectPeripheral();
+}
+
+void Stylus::handleDiscoveredPeripheral(GatoPeripheral *peripheral, int rssi)
+{
+	qDebug() << "Found peripheral" << peripheral->address().toString() << peripheral->name() << rssi;
+	if (peripheral->name() == stylus_name) {
+		_manager->stopScan();
+		connectToPeripheral(peripheral);
+	}
+}
+void Stylus::handleConnected()
+{
+	qDebug() << "Connected to" << _peripheral->name();
+	_peripheral->discoverServices();
+}
+
+void Stylus::handleDisconnected()
+{
+	qDebug() << "Peripheral disconnected";
+}
+
+void Stylus::handleServices()
+{
+	qDebug() << "Services found";
+	foreach (const GatoService &service, _peripheral->services()) {
+		qDebug() << service.uuid();
+		if (service.uuid() == stylus_service) {
+			// Found the service we want
+			qDebug() << "Found stylus service";
+			_peripheral->discoverCharacteristics(service);
+		}
+	}
+}
+
+void Stylus::handleCharacteristics(const GatoService &service)
+{
+	foreach (const GatoCharacteristic &c, service.characteristics()) {
+		if (c.uuid() == agg_char_uuid) {
+			qDebug() << c.uuid();
+			_peripheral->setNotification(c, true);
+		}
+	}
+}
+
+void Stylus::handleValueUpdated(const GatoCharacteristic &characteristic, const QByteArray &value)
+{
+	if (characteristic.uuid() == agg_char_uuid) {
+		QDataStream s(value);
+		s.setByteOrder(QDataStream::BigEndian);
+		qint16 p, x, y, z;
+		s >> p >> x >> y >> z;
+		handleReport(p, x, y, z);
+	}
+}
+
+void Stylus::handleReport(int p, int x, int y, int z)
+{
+	qreal newp = p / 7000.0;
+	if (newp != _p) {
+		_p = newp;
+		emit pressureChanged();
+	}
+	qreal newx = x / 268.0;
+	if (newx != _x) {
+		_x = newx;
+		emit xChanged();
+	}
+	qreal newy = y / 268.0;
+	if (newy != _y) {
+		_y = newy;
+		emit yChanged();
+	}
+	qreal newz = z / 268.0;
+	if (newz != _z) {
+		_z = newz;
+		emit zChanged();
+	}
+}