From 3c93224fcc30254a9497e52e0306a1e62ad4f501 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Erik=20Bot=C3=B6?= Date: Wed, 12 Feb 2020 18:23:47 +0100 Subject: [PATCH] Initial import --- .gitignore | 1 + ftms-example.pro | 13 +++ ftmsdevice.cpp | 254 +++++++++++++++++++++++++++++++++++++++++++++++ ftmsdevice.h | 67 +++++++++++++ main.cpp | 19 ++++ 5 files changed, 354 insertions(+) create mode 100644 .gitignore create mode 100644 ftms-example.pro create mode 100644 ftmsdevice.cpp create mode 100644 ftmsdevice.h create mode 100644 main.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ec68b91 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +*pro.user diff --git a/ftms-example.pro b/ftms-example.pro new file mode 100644 index 0000000..867c940 --- /dev/null +++ b/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 diff --git a/ftmsdevice.cpp b/ftmsdevice.cpp new file mode 100644 index 0000000..60cab4c --- /dev/null +++ b/ftmsdevice.cpp @@ -0,0 +1,254 @@ +#include "ftmsdevice.h" + +#include +#include + +#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((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(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); + } +} diff --git a/ftmsdevice.h b/ftmsdevice.h new file mode 100644 index 0000000..7bef719 --- /dev/null +++ b/ftmsdevice.h @@ -0,0 +1,67 @@ +#ifndef FTMSDEVICE_H +#define FTMSDEVICE_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..2072917 --- /dev/null +++ b/main.cpp @@ -0,0 +1,19 @@ +#include +#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(); +}