From 4a33008c616683c8aea49454bf2d8133da39bf5d Mon Sep 17 00:00:00 2001 From: Roberto Viola Date: Sat, 5 Oct 2024 07:45:19 +0200 Subject: [PATCH] Renpho smart bike r-q002 n (Issue #2401) (#2409) --- src/CRC16IBM.h | 61 ++++++ src/devices/renphobike/renphobike.cpp | 271 +++++++++++++++++++++++++- src/devices/renphobike/renphobike.h | 9 + src/qdomyos-zwift.pri | 1 + 4 files changed, 334 insertions(+), 8 deletions(-) create mode 100644 src/CRC16IBM.h diff --git a/src/CRC16IBM.h b/src/CRC16IBM.h new file mode 100644 index 000000000..0190fd2fa --- /dev/null +++ b/src/CRC16IBM.h @@ -0,0 +1,61 @@ +#ifndef CRC16IBM_H +#define CRC16IBM_H + +#include +#include + +class CRC16IBM { + public: + + // Function to calculate CRC-16 (XMODEM) checksum + static quint16 calculateCRC(const QByteArray &data) { + quint16 crc = 0xFFFF; // Initial value + + for (char byte : data) { + quint8 index = (crc >> 8) ^ static_cast(byte); + crc = (crc << 8) ^ crc16Table[index]; + } + + return crc; + } + + private: + // Precomputed CRC-16-IBM table + static constexpr quint16 crc16Table[256] = { + 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, + 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, + 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, + 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, + 0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, + 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, + 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, + 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, + 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, + 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, + 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, + 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, + 0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, + 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49, + 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, + 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78, + 0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, + 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067, + 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, + 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, + 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, + 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, + 0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, + 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, + 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, + 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3, + 0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, + 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, + 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, + 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, + 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, + 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0 + }; +}; + + +#endif // CRC16IBM_H diff --git a/src/devices/renphobike/renphobike.cpp b/src/devices/renphobike/renphobike.cpp index 6d0ff8e5c..5e5bf9b4d 100644 --- a/src/devices/renphobike/renphobike.cpp +++ b/src/devices/renphobike/renphobike.cpp @@ -1,4 +1,5 @@ #include "renphobike.h" +#include "CRC16IBM.h"" #include "devices/ftmsbike/ftmsbike.h" #include "virtualdevices/virtualbike.h" #include @@ -60,6 +61,39 @@ void renphobike::writeCharacteristic(uint8_t *data, uint8_t data_len, QString in loop.exec(); } +void renphobike::writeCharacteristicCustom(uint8_t *data, uint8_t data_len, QString info, bool disable_log, + bool wait_for_response) { + QEventLoop loop; + QTimer timeout; + + if (gattCustomService == nullptr) { + qDebug() << QStringLiteral("gattCustomService not found! skip writing..."); + return; + } + + if (wait_for_response) { + connect(gattCustomService, SIGNAL(characteristicChanged(QLowEnergyCharacteristic, QByteArray)), &loop, + SLOT(quit())); + timeout.singleShot(300, &loop, SLOT(quit())); + } else { + connect(gattCustomService, SIGNAL(characteristicWritten(QLowEnergyCharacteristic, QByteArray)), &loop, + SLOT(quit())); + timeout.singleShot(300, &loop, SLOT(quit())); + } + + if (writeBuffer) { + delete writeBuffer; + } + writeBuffer = new QByteArray((const char *)data, data_len); + + gattCustomService->writeCharacteristic(gattWriteCustomCharControlPointId, *writeBuffer); + + if (!disable_log) + debug(" >> " + writeBuffer->toHex(' ') + " // " + info); + + loop.exec(); +} + void renphobike::forcePower(int16_t requestPower) { QSettings settings; double watt_gain = settings.value(QZSettings::watt_gain, QZSettings::default_watt_gain).toDouble(); @@ -75,6 +109,7 @@ void renphobike::forcePower(int16_t requestPower) { void renphobike::forceResistance(resistance_t requestResistance) { // requestPower = powerFromResistanceRequest(requestResistance); + /* uint8_t write[] = {FTMS_SET_TARGET_RESISTANCE_LEVEL, 0x00}; QSettings settings; bool renpho_bike_double_resistance = @@ -87,6 +122,20 @@ void renphobike::forceResistance(resistance_t requestResistance) { write[1] = ((uint8_t)(requestResistance * 2)); writeCharacteristic(write, sizeof(write), QStringLiteral("forceResistance ") + QString::number(requestResistance)); +*/ + uint8_t write[] = {0x00, 0x44, 0x11, 0x0f, 0x84, 0xc0}; + write[2] = requestResistance; + Resistance = requestResistance; + quint16 crc = CRC16IBM::calculateCRC(QByteArray(reinterpret_cast(write), 3)); + + // Log the calculated CRC (for debugging purposes) + qDebug() << "Calculated CRC:" << QString::number(crc, 16); + + // Replace the last two bytes of the write array with the CRC + write[3] = static_cast(crc >> 8); // High byte of CRC + write[4] = static_cast(crc & 0xFF); // Low byte of CRC + + writeCharacteristicCustom(write, sizeof(write), QStringLiteral("forceResistance ") + QString::number(requestResistance)); } void renphobike::update() { @@ -96,13 +145,48 @@ void renphobike::update() { } if (initRequest) { + uint8_t init0[] = {0x00, 0x22, 0x19, 0x2f, 0xc0}; + uint8_t init1[] = {0x00, 0x46, 0x00, 0x6b, 0xf6, 0xc0}; + uint8_t init2[] = {0x00, 0x45, 0x00, 0x3e, 0xa5, 0xc0}; + uint8_t init3[] = {0x00, 0x40, 0x00, 0x02, 0xb9, 0x2f, 0xc0}; + uint8_t init4[] = {0x00, 0x40, 0x00, 0x04, 0xd9, 0xe9, 0xc0}; + uint8_t init5[] = {0x00, 0x40, 0x00, 0x03, 0xa9, 0x0e, 0xc0}; + uint8_t init6[] = {0x00, 0x40, 0x00, 0x05, 0xc9, 0xc8, 0xc0}; + uint8_t init7[] = {0x00, 0x46, 0x00, 0x6b, 0xf6, 0xc0}; + uint8_t init8[] = {0x00, 0x45, 0x01, 0x2e, 0x84, 0xc0}; + uint8_t init9[] = {0x00, 0x40, 0x00, 0x02, 0xb9, 0x2f, 0xc0}; + uint8_t init10[] = {0x00, 0x40, 0x00, 0x04, 0xd9, 0xe9, 0xc0}; + writeCharacteristicCustom(init0, sizeof(init0), "init0", false, true); + QThread::msleep(1000); + writeCharacteristicCustom(init1, sizeof(init1), "init1", false, true); + QThread::msleep(1000); + writeCharacteristicCustom(init2, sizeof(init2), "init2", false, true); + QThread::msleep(1000); + writeCharacteristicCustom(init3, sizeof(init3), "init3", false, true); + QThread::msleep(1000); + writeCharacteristicCustom(init4, sizeof(init4), "init4", false, true); + QThread::msleep(1000); + writeCharacteristicCustom(init5, sizeof(init5), "init5", false, true); + QThread::msleep(1000); + writeCharacteristicCustom(init6, sizeof(init6), "init6", false, true); + QThread::msleep(1000); + writeCharacteristicCustom(init7, sizeof(init7), "init7", false, true); + QThread::msleep(1000); + writeCharacteristicCustom(init8, sizeof(init8), "init8", false, true); + QThread::msleep(1000); + writeCharacteristicCustom(init9, sizeof(init9), "init9", false, true); + QThread::msleep(1000); + writeCharacteristicCustom(init10, sizeof(init10), "init10", false, true); + /* uint8_t write[] = {FTMS_REQUEST_CONTROL}; writeCharacteristic(write, sizeof(write), "requestControl", false, true); write[0] = {FTMS_START_RESUME}; writeCharacteristic(write, sizeof(write), "start simulation", false, true); uint8_t ftms[] = {0x11, 0x00, 0x00, 0xf3, 0x00, 0x28, 0x33}; - writeCharacteristic(ftms, sizeof(ftms), "fake FTMS", false, true); + writeCharacteristic(ftms, sizeof(ftms), "fake FTMS", false, true);*/ + initRequest = false; + } else if (bluetoothDevice.isValid() && m_control->state() == QLowEnergyController::DiscoveredState //&& // gattCommunicationChannelService && @@ -187,6 +271,177 @@ void renphobike::characteristicChanged(const QLowEnergyCharacteristic &character debug(" << " + newValue.toHex(' ')); + if (characteristic.uuid() == QBluetoothUuid::CyclingPowerMeasurement) { + lastPacket = newValue; + + uint16_t flags = (((uint16_t)((uint8_t)newValue.at(1)) << 8) | (uint16_t)((uint8_t)newValue.at(0))); + bool cadence_present = false; + bool wheel_revs = false; + bool crank_rev_present = false; + uint16_t time_division = 1024; + uint8_t index = 4; + + if (newValue.length() > 3) { + m_watt = (((uint16_t)((uint8_t)newValue.at(3)) << 8) | (uint16_t)((uint8_t)newValue.at(2))); + } + + emit powerChanged(m_watt.value()); + emit debug(QStringLiteral("Current watt: ") + QString::number(m_watt.value())); + + if ((flags & 0x1) == 0x01) // Pedal Power Balance Present + { + index += 1; + } + if ((flags & 0x2) == 0x02) // Pedal Power Balance Reference + { + } + if ((flags & 0x4) == 0x04) // Accumulated Torque Present + { + index += 2; + } + if ((flags & 0x8) == 0x08) // Accumulated Torque Source + { + } + + if ((flags & 0x10) == 0x10) // Wheel Revolution Data Present + { + cadence_present = true; + wheel_revs = true; + } + + if ((flags & 0x20) == 0x20) // Crank Revolution Data Present + { + cadence_present = true; + crank_rev_present = true; + } + + if (cadence_present) { + if (wheel_revs && !crank_rev_present) { + time_division = 2048; + CrankRevs = + (((uint32_t)((uint8_t)newValue.at(index + 3)) << 24) | + ((uint32_t)((uint8_t)newValue.at(index + 2)) << 16) | + ((uint32_t)((uint8_t)newValue.at(index + 1)) << 8) | (uint32_t)((uint8_t)newValue.at(index))); + index += 4; + + LastCrankEventTime = + (((uint16_t)((uint8_t)newValue.at(index + 1)) << 8) | (uint16_t)((uint8_t)newValue.at(index))); + + index += 2; // wheel event time + + } else if (wheel_revs && crank_rev_present) { + index += 4; // wheel revs + index += 2; // wheel event time + } + + if (crank_rev_present) { + CrankRevs = + (((uint16_t)((uint8_t)newValue.at(index + 1)) << 8) | (uint16_t)((uint8_t)newValue.at(index))); + index += 2; + + LastCrankEventTime = + (((uint16_t)((uint8_t)newValue.at(index + 1)) << 8) | (uint16_t)((uint8_t)newValue.at(index))); + index += 2; + } + + int16_t deltaT = LastCrankEventTime - oldLastCrankEventTime; + if (deltaT < 0) { + deltaT = LastCrankEventTime + time_division - oldLastCrankEventTime; + } + + if (settings.value(QZSettings::cadence_sensor_name, QZSettings::default_cadence_sensor_name) + .toString() + .startsWith(QStringLiteral("Disabled"))) { + if (CrankRevs != oldCrankRevs && deltaT) { + double cadence = ((CrankRevs - oldCrankRevs) / deltaT) * time_division * 60; + if (!crank_rev_present) + cadence = + cadence / + 2; // I really don't like this, there is no relationship between wheel rev and crank rev + if (cadence >= 0) { + Cadence = cadence; + } + lastGoodCadence = now; + } else if (lastGoodCadence.msecsTo(now) > 2000) { + Cadence = 0; + } + } + + qDebug() << QStringLiteral("Current Cadence: ") << Cadence.value() << CrankRevs << oldCrankRevs << deltaT + << time_division << LastCrankEventTime << oldLastCrankEventTime; + + oldLastCrankEventTime = LastCrankEventTime; + oldCrankRevs = CrankRevs; + + if (!settings.value(QZSettings::speed_power_based, QZSettings::default_speed_power_based).toBool()) { + Speed = Cadence.value() * settings + .value(QZSettings::cadence_sensor_speed_ratio, + QZSettings::default_cadence_sensor_speed_ratio) + .toDouble(); + } else { + Speed = metric::calculateSpeedFromPower( + watts(), Inclination.value(), Speed.value(), + fabs(now.msecsTo(Speed.lastChanged()) / 1000.0), this->speedLimit()); + } + emit debug(QStringLiteral("Current Speed: ") + QString::number(Speed.value())); + + Distance += ((Speed.value() / 3600000.0) * + ((double)lastRefreshCharacteristicChanged.msecsTo(now))); + emit debug(QStringLiteral("Current Distance: ") + QString::number(Distance.value())); + + // if we change this, also change the wattsFromResistance function. We can create a standard function in + // order to have all the costants in one place (I WANT MORE TIME!!!) + double ac = 0.01243107769; + double bc = 1.145964912; + double cc = -23.50977444; + + double ar = 0.1469553975; + double br = -5.841344538; + double cr = 97.62165482; + + double res = + (((sqrt(pow(br, 2.0) - 4.0 * ar * + (cr - (m_watt.value() * 132.0 / + (ac * pow(Cadence.value(), 2.0) + bc * Cadence.value() + cc)))) - + br) / + (2.0 * ar)) * + settings.value(QZSettings::peloton_gain, QZSettings::default_peloton_gain).toDouble()) + + settings.value(QZSettings::peloton_offset, QZSettings::default_peloton_offset).toDouble(); + + if (isnan(res)) { + if (Cadence.value() > 0) { + // let's keep the last good value + } else { + m_pelotonResistance = 0; + } + } else { + m_pelotonResistance = res; + } + + qDebug() << QStringLiteral("Current Peloton Resistance: ") + QString::number(m_pelotonResistance.value()); + + { + if (settings.value(QZSettings::schwinn_bike_resistance, QZSettings::default_schwinn_bike_resistance) + .toBool()) + Resistance = pelotonToBikeResistance(m_pelotonResistance.value()); + else + Resistance = m_pelotonResistance; + emit resistanceRead(Resistance.value()); + qDebug() << QStringLiteral("Current Resistance Calculated: ") + QString::number(Resistance.value()); + } + + if (watts()) + KCal += + ((((0.048 * ((double)watts()) + 1.19) * + settings.value(QZSettings::weight, QZSettings::default_weight).toFloat() * 3.5) / + 200.0) / + (60000.0 / ((double)lastRefreshCharacteristicChanged.msecsTo( + now)))); //(( (0.048* Output in watts +1.19) * body weight + // in kg * 3.5) / 200 ) / 60 + emit debug(QStringLiteral("Current KCal: ") + QString::number(KCal.value())); + } + } + if (characteristic.uuid() != QBluetoothUuid((quint16)0x2AD2)) return; @@ -421,13 +676,6 @@ void renphobike::stateChanged(QLowEnergyService::ServiceState state) { qDebug() << s->serviceUuid() << "connected!"; - // zwift doesn't write the client configuration on services different from these ones - if (s->serviceUuid() != ((QBluetoothUuid)(quint16)0x1826) && - s->serviceUuid() != ((QBluetoothUuid)(quint16)0x1816)) { - qDebug() << QStringLiteral("skipping service") << s->serviceUuid(); - continue; - } - foreach (QLowEnergyCharacteristic c, s->characteristics()) { qDebug() << "char uuid" << c.uuid() << "handle" << c.handle(); foreach (QLowEnergyDescriptor d, c.descriptors()) @@ -471,6 +719,13 @@ void renphobike::stateChanged(QLowEnergyService::ServiceState state) { gattWriteCharControlPointId = c; gattFTMSService = s; } + + QBluetoothUuid _gattWriteCustomCharControlPointId(QStringLiteral("00000004-21a4-11e8-8812-000c2920efff")); + if (c.properties() & QLowEnergyCharacteristic::Write && c.uuid() == _gattWriteCustomCharControlPointId) { + qDebug() << "Custom service and Custom Control Point found"; + gattWriteCustomCharControlPointId = c; + gattCustomService = s; + } } } } diff --git a/src/devices/renphobike/renphobike.h b/src/devices/renphobike/renphobike.h index 99874ba59..50634f335 100644 --- a/src/devices/renphobike/renphobike.h +++ b/src/devices/renphobike/renphobike.h @@ -46,6 +46,8 @@ class renphobike : public bike { double bikeResistanceToPeloton(double resistance); void writeCharacteristic(uint8_t *data, uint8_t data_len, QString info, bool disable_log = false, bool wait_for_response = false); + void writeCharacteristicCustom(uint8_t *data, uint8_t data_len, QString info, bool disable_log = false, + bool wait_for_response = false); void startDiscover(); uint16_t ergModificator(uint16_t powerRequested); uint16_t watts() override; @@ -57,6 +59,9 @@ class renphobike : public bike { QLowEnergyCharacteristic gattWriteCharControlPointId; QLowEnergyService *gattFTMSService = nullptr; + QLowEnergyCharacteristic gattWriteCustomCharControlPointId; + QLowEnergyService *gattCustomService = nullptr; + uint8_t sec1Update = 0; QByteArray lastPacket; QDateTime lastRefreshCharacteristicChanged = QDateTime::currentDateTime(); @@ -73,6 +78,10 @@ class renphobike : public bike { metric wattFromBike; + uint16_t oldLastCrankEventTime = 0; + uint16_t oldCrankRevs = 0; + QDateTime lastGoodCadence = QDateTime::currentDateTime(); + #ifdef Q_OS_IOS lockscreen *h = 0; #endif diff --git a/src/qdomyos-zwift.pri b/src/qdomyos-zwift.pri index 6af02ff4e..95f84d91f 100755 --- a/src/qdomyos-zwift.pri +++ b/src/qdomyos-zwift.pri @@ -301,6 +301,7 @@ else: unix:!android: target.path = /opt/$${TARGET}/bin INCLUDEPATH += fit-sdk/ devices/ HEADERS += \ + $$PWD/CRC16IBM.h \ $$PWD/EventHandler.h \ $$PWD/devices/antbike/antbike.h \ $$PWD/devices/crossrope/crossrope.h \