Initial import

.gitignore

@@ -0,0 +1 @@
+*pro.user

ftms-example.pro

@@ -0,0 +1,13 @@
+TEMPLATE = app
+TARGET = ftms-example
+
+QT = core bluetooth
+CONFIG += c++11
+
+SOURCES += main.cpp \
+    ftmsdevice.cpp
+
+INSTALLS += target
+
+HEADERS += \
+    ftmsdevice.h

ftmsdevice.cpp

@@ -0,0 +1,254 @@
+#include "ftmsdevice.h"
+
+#include <QDataStream>
+#include <QTimer>
+
+#define FTMSDEVICE_FTMS_UUID 0x1826
+#define FTMSDEVICE_INDOOR_BIKE_CHAR_UUID 0x2AD2
+#define FTMSDEVICE_POWER_RANGE_CHAR_UUID 0x2AD8
+#define FTMSDEVICE_RESISTANCE_RANGE_CHAR_UUID 0x2AD6
+#define FTMSDEVICE_FTMS_FEATURE_CHAR_UUID 0x2ACC
+#define FTMSDEVICE_FTMS_CONTROL_POINT_CHAR_UUID 0x2AD9
+#define FTMSDEVICE_FTMS_STATUS_CHAR_UUID 0x2ADA
+
+FTMSDevice::FTMSDevice(QObject *parent) : QObject(parent),
+    m_currentHeartrate(0),
+    m_currentPower(0),
+    m_currentCadence(0),
+    m_currentSpeed(0)
+{
+}
+
+enum FtmsControlPointCommand {
+    FTMS_REQUEST_CONTROL = 0x00,
+    FTMS_RESET,
+    FTMS_SET_TARGET_SPEED,
+    FTMS_SET_TARGET_INCLINATION,
+    FTMS_SET_TARGET_RESISTANCE_LEVEL,
+    FTMS_SET_TARGET_POWER,
+    FTMS_SET_TARGET_HEARTRATE,
+    FTMS_START_RESUME,
+    FTMS_STOP_PAUSE,
+    FTMS_SET_TARGETED_EXP_ENERGY,
+    FTMS_SET_TARGETED_STEPS,
+    FTMS_SET_TARGETED_STRIDES,
+    FTMS_SET_TARGETED_DISTANCE,
+    FTMS_SET_TARGETED_TIME,
+    FTMS_SET_TARGETED_TIME_TWO_HR_ZONES,
+    FTMS_SET_TARGETED_TIME_THREE_HR_ZONES,
+    FTMS_SET_TARGETED_TIME_FIVE_HR_ZONES,
+    FTMS_SET_INDOOR_BIKE_SIMULATION_PARAMS,
+    FTMS_SET_WHEEL_CIRCUMFERENCE,
+    FTMS_SPIN_DOWN_CONTROL,
+    FTMS_SET_TARGETED_CADENCE,
+    FTMS_RESPONSE_CODE = 0x80
+};
+
+enum FtmsResultCode {
+    FTMS_SUCCESS = 0x01,
+    FTMS_NOT_SUPPORTED,
+    FTMS_INVALID_PARAMETER,
+    FTMS_OPERATION_FAILED,
+    FTMS_CONTROL_NOT_PERMITTED
+};
+
+void FTMSDevice::initialize()
+{
+    // Setup up the advertising properly
+    m_advertisingData.setDiscoverability(QLowEnergyAdvertisingData::DiscoverabilityGeneral);
+    m_advertisingData.setIncludePowerLevel(true); // Switch to false?
+    m_advertisingData.setLocalName("M"); // Use short name to save bytes
+    m_advertisingData.setServices(QList<QBluetoothUuid>() << QBluetoothUuid((quint16)FTMSDEVICE_FTMS_UUID));
+
+    // Setup of indoorBike characteristic
+    m_indoorBikeCharData.setUuid(QBluetoothUuid((quint16)FTMSDEVICE_INDOOR_BIKE_CHAR_UUID));
+    m_indoorBikeCharData.setProperties(QLowEnergyCharacteristic::Notify);
+    m_indoorBikeClientConfig = QLowEnergyDescriptorData(QBluetoothUuid::ClientCharacteristicConfiguration,
+                                                        QByteArray(2, 0));
+    m_indoorBikeCharData.addDescriptor(m_indoorBikeClientConfig);
+
+    // Setup of powerRange characteristic
+    m_powerRangeCharData.setUuid(QBluetoothUuid((quint16)FTMSDEVICE_POWER_RANGE_CHAR_UUID));
+    QByteArray powerRangeData;
+    QDataStream powerRangeCharDataDs(&powerRangeData, QIODevice::ReadWrite);
+    qint16 maxPwr = 1400;
+    qint16 minPwr = 0;
+    quint16 stepPwr = 1;
+    powerRangeCharDataDs.setByteOrder(QDataStream::LittleEndian);
+    powerRangeCharDataDs << minPwr << maxPwr << stepPwr;
+    m_powerRangeCharData.setValue(powerRangeData);;
+    m_powerRangeCharData.setProperties(QLowEnergyCharacteristic::Read);
+    m_powerRangeClientConfig = QLowEnergyDescriptorData(QBluetoothUuid::ClientCharacteristicConfiguration,
+                                                QByteArray(2, 0));
+    m_powerRangeCharData.addDescriptor(m_powerRangeClientConfig);
+
+    // Setup of resistanceRange characteristic
+    m_resistanceRangeCharData.setUuid(QBluetoothUuid((quint16)FTMSDEVICE_RESISTANCE_RANGE_CHAR_UUID));
+    QByteArray resistanceRangeData;
+    QDataStream resistanceRangeCharDataDs(&resistanceRangeData, QIODevice::ReadWrite);
+    qint16 maxRes = 70;
+    qint16 minRes = 0;
+    quint16 stepRes = 1;
+    resistanceRangeCharDataDs.setByteOrder(QDataStream::LittleEndian);
+    resistanceRangeCharDataDs << minRes << maxRes << stepRes;
+    m_resistanceRangeCharData.setValue(resistanceRangeData);
+    m_resistanceRangeCharData.setProperties(QLowEnergyCharacteristic::Read);
+    m_resistanceRangeClientConfig = QLowEnergyDescriptorData(QBluetoothUuid::ClientCharacteristicConfiguration,
+                                                QByteArray(2, 0));
+    m_resistanceRangeCharData.addDescriptor(m_resistanceRangeClientConfig);
+
+    // Setup of ftmsFeature characteristic
+    m_ftmsFeatureCharData.setUuid(QBluetoothUuid((quint16)FTMSDEVICE_FTMS_FEATURE_CHAR_UUID));
+    QByteArray ftmsFeatureCharDataRaw;
+    QDataStream ftmsFeatureCharDataDs(&ftmsFeatureCharDataRaw, QIODevice::ReadWrite);
+    ftmsFeatureCharDataDs.setByteOrder(QDataStream::LittleEndian);
+    //                   10987654321098765432109876543210
+    quint32 features = 0b00000000000000000100000010000010;
+    quint32 settings = 0b00000000000000000000000000001100;
+    ftmsFeatureCharDataDs << features << settings;
+    m_ftmsFeatureCharData.setValue(ftmsFeatureCharDataRaw);
+    m_ftmsFeatureCharData.setProperties(QLowEnergyCharacteristic::Read);
+    m_ftmsFeatureClientConfig = QLowEnergyDescriptorData(QBluetoothUuid::ClientCharacteristicConfiguration,
+                                                QByteArray(2, 0));
+    m_ftmsFeatureCharData.addDescriptor(m_ftmsFeatureClientConfig);
+
+    // Setup of m_ftmsControlPoint characteristic
+    m_ftmsControlPointCharData.setUuid(QBluetoothUuid((quint16)0x2AD9));
+    m_ftmsControlPointCharData.setProperties(QLowEnergyCharacteristic::Write | QLowEnergyCharacteristic::Indicate);
+    const QLowEnergyDescriptorData cpClientConfig(QBluetoothUuid::ClientCharacteristicConfiguration,
+                                                QByteArray(2, 0));
+    m_ftmsControlPointCharData.addDescriptor(cpClientConfig);
+
+    // Setup of ftmsStatus characteristic
+    m_ftmsStatusCharData.setUuid(QBluetoothUuid((quint16)FTMSDEVICE_FTMS_STATUS_CHAR_UUID));
+    m_ftmsStatusCharData.setProperties(QLowEnergyCharacteristic::Notify);
+    m_ftmsStatusClientConfig = QLowEnergyDescriptorData(QBluetoothUuid::ClientCharacteristicConfiguration,
+                                                QByteArray(2, 0));
+    m_ftmsStatusCharData.addDescriptor(m_ftmsStatusClientConfig);
+
+    // Final setup (Note that order of chars seems to matter?!)
+    m_ftmsServiceData.setType(QLowEnergyServiceData::ServiceTypePrimary);
+    m_ftmsServiceData.setUuid(QBluetoothUuid((quint16)FTMSDEVICE_FTMS_UUID));
+    m_ftmsServiceData.addCharacteristic(m_indoorBikeCharData);
+    m_ftmsServiceData.addCharacteristic(m_powerRangeCharData);
+    m_ftmsServiceData.addCharacteristic(m_ftmsControlPointCharData);
+    m_ftmsServiceData.addCharacteristic(m_ftmsFeatureCharData);
+    m_ftmsServiceData.addCharacteristic(m_ftmsStatusCharData);
+    m_ftmsServiceData.addCharacteristic(m_resistanceRangeCharData);
+
+    m_controller = QLowEnergyController::createPeripheral();
+    m_ftmsService = m_controller->addService(m_ftmsServiceData);
+
+
+    // Start advertising
+    m_controller->startAdvertising(QLowEnergyAdvertisingParameters(), m_advertisingData,
+                                   m_advertisingData);
+
+    // Set up connection for incoming FTMS CP requests
+    QObject::connect(m_ftmsService, &QLowEnergyService::characteristicChanged, this, &FTMSDevice::onIncomingControlPointCommand);
+
+    connect(&m_updateTimer, &QTimer::timeout, this, &FTMSDevice::sendCurrentValues);
+    m_updateTimer.start(1000);
+
+    connect(m_controller, &QLowEnergyController::disconnected, this, &FTMSDevice::restartAdvertising);
+}
+
+void FTMSDevice::onIncomingControlPointCommand(QLowEnergyCharacteristic c ,QByteArray b)
+{
+    qDebug() << "Write to " << c.uuid();
+    qDebug() << b;
+
+    Q_ASSERT(b.size() >= 1);
+
+    // Prepare for reply
+    QLowEnergyCharacteristic characteristic
+            = m_ftmsService->characteristic(QBluetoothUuid((quint16)FTMSDEVICE_FTMS_CONTROL_POINT_CHAR_UUID));
+    Q_ASSERT(characteristic.isValid());
+    QByteArray reply;
+    QDataStream replyDs(&reply, QIODevice::ReadWrite);
+    replyDs.setByteOrder(QDataStream::LittleEndian);
+
+    // Set up a stream from parsing command
+    QDataStream inData(&b, QIODevice::ReadOnly);
+    inData.setByteOrder(QDataStream::LittleEndian);
+
+    quint8 cmd;
+    inData >> cmd;
+
+    switch (static_cast<FtmsControlPointCommand>(cmd))
+    {
+    case FTMS_REQUEST_CONTROL:
+        qDebug() << "Control requested";
+        replyDs << (quint8)FTMS_RESPONSE_CODE << (quint8)FTMS_REQUEST_CONTROL << (quint8)FTMS_SUCCESS;
+        break;
+    case FTMS_RESET:
+        emit newTargetPower(100);
+        replyDs << (quint8)FTMS_RESPONSE_CODE << (quint8)FTMS_RESET << (quint8)FTMS_SUCCESS ;
+        break;
+    case FTMS_SET_TARGET_RESISTANCE_LEVEL:
+    {
+        qint8 requestedResistanceLevel;
+        inData >> requestedResistanceLevel;
+        emit newTargetKp(requestedResistanceLevel/10.0);
+        replyDs << (quint8)FTMS_RESPONSE_CODE << (quint8)FTMS_SET_TARGET_RESISTANCE_LEVEL << (quint8)FTMS_SUCCESS ;
+    }
+        break;
+    case FTMS_SET_TARGET_POWER:
+    {
+        qint16 targetPower;
+        inData >> targetPower;
+        replyDs << (quint8)FTMS_RESPONSE_CODE << (quint8)FTMS_SET_TARGET_POWER << (quint8)FTMS_SUCCESS;
+        emit newTargetPower(targetPower);
+        qDebug() << "New Target Power: " << targetPower;
+    }
+        break;
+    default:
+        qDebug() << "Unhandled command";
+    }
+
+    if (!reply.isEmpty())
+    {
+        //qDebug() << reply;
+        m_ftmsService->writeCharacteristic(characteristic, reply);
+    }
+};
+
+void FTMSDevice::sendCurrentValues()
+{
+    //               bits 5432109876543210
+    quint16 charFlags = 0b0000000001000100; // insta cadance and power
+
+    QByteArray charData;
+    QDataStream charDataDs(&charData, QIODevice::ReadWrite);
+    charDataDs.setByteOrder(QDataStream::LittleEndian);
+    charDataDs << charFlags << m_currentSpeed << m_currentCadence << m_currentPower;
+
+    QLowEnergyCharacteristic characteristic
+            = m_ftmsService->characteristic(QBluetoothUuid((quint16)FTMSDEVICE_INDOOR_BIKE_CHAR_UUID));
+    Q_ASSERT(characteristic.isValid());
+    m_ftmsService->writeCharacteristic(characteristic, charData); // Potentially causes notification.
+}
+
+
+void FTMSDevice::setCurrentPower(qint16 power)
+{
+    m_currentPower = power;
+}
+void FTMSDevice::setCurrentCadence(quint8 cadence)
+{
+    m_currentCadence = cadence;
+}
+void FTMSDevice::setCurrentHeartRate(quint8 heartRate)
+{
+    m_currentHeartrate = heartRate;
+}
+
+void FTMSDevice::restartAdvertising()
+{
+    m_ftmsService = m_controller->addService(m_ftmsServiceData);
+    if (m_ftmsService)
+    {
+        m_controller->startAdvertising(QLowEnergyAdvertisingParameters(),
+                                       m_advertisingData, m_advertisingData);
+    }
+}

ftmsdevice.h

@@ -0,0 +1,67 @@
+#ifndef FTMSDEVICE_H
+#define FTMSDEVICE_H
+
+#include <QObject>
+#include <QLowEnergyAdvertisingData>
+#include <QLowEnergyController>
+#include <QLowEnergyService>
+#include <QLowEnergyServiceData>
+#include <QLowEnergyCharacteristicData>
+#include <QLowEnergyDescriptorData>
+#include <QLowEnergyAdvertisingParameters>
+#include <QTimer>
+
+class FTMSDevice : public QObject
+{
+    Q_OBJECT
+public:
+    explicit FTMSDevice(QObject *parent = nullptr);
+
+public slots:
+    void setCurrentPower(qint16 power);
+    void setCurrentCadence(quint8 cadence);
+    void setCurrentHeartRate(quint8 heartRate);
+
+    void initialize();
+
+    void sendCurrentValues();
+    void restartAdvertising();
+
+signals:
+    void newTargetPower(quint32 targetPower);
+    void newTargetKp(double kp);
+
+private slots:
+    void onIncomingControlPointCommand(QLowEnergyCharacteristic c, QByteArray b);
+
+private:
+    QLowEnergyService *m_ftmsService;
+    QLowEnergyDescriptorData m_ftmsClientConfig;
+    QLowEnergyServiceData m_ftmsServiceData;
+    QLowEnergyAdvertisingData m_advertisingData;
+
+    QLowEnergyCharacteristicData m_indoorBikeCharData;
+    QLowEnergyCharacteristicData m_powerRangeCharData;
+    QLowEnergyCharacteristicData m_resistanceRangeCharData;
+    QLowEnergyCharacteristicData m_ftmsFeatureCharData;
+    QLowEnergyCharacteristicData m_ftmsControlPointCharData;
+    QLowEnergyCharacteristicData m_ftmsStatusCharData;
+
+    QLowEnergyDescriptorData m_indoorBikeClientConfig;
+    QLowEnergyDescriptorData m_powerRangeClientConfig;
+    QLowEnergyDescriptorData m_resistanceRangeClientConfig;
+    QLowEnergyDescriptorData m_ftmsFeatureClientConfig;
+    QLowEnergyDescriptorData m_ftmsControlPointClientConfig;
+    QLowEnergyDescriptorData m_ftmsStatusClientConfig;
+
+    QLowEnergyController *m_controller;
+
+    quint8 m_currentHeartrate;
+    qint16 m_currentPower;
+    quint16 m_currentCadence;
+    quint16 m_currentSpeed;
+
+    QTimer m_updateTimer;
+};
+
+#endif // FTMSDEVICE_H

main.cpp

@@ -0,0 +1,19 @@
+#include <QtCore/qcoreapplication.h>
+#include "ftmsdevice.h"
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication app(argc, argv);
+
+    FTMSDevice fd;
+    fd.initialize();
+
+    QObject::connect(&fd, &FTMSDevice::newTargetPower, [](quint32 targetPower){
+        qDebug() << "Got new target power:" << targetPower;
+    });
+
+    QObject::connect(&fd, &FTMSDevice::newTargetKp, [](double targetKp){
+        qDebug() << "Got new target kp" << targetKp;
+    });
+    return app.exec();
+}