Parse cycling power cadence in CSC sensors

Author: cagnulein

src/devices/bluetooth.cpp

@@ -3262,10 +3262,8 @@ void bluetooth::connectedAndDiscovered() {
 
                     connect(powerSensor, &stagesbike::debug, this, &bluetooth::debug);
                     connect(powerSensor, &bluetoothdevice::powerChanged, this->device(), &bluetoothdevice::powerSensor);
-                    if (cscName.startsWith(QStringLiteral("Disabled"))) {
-                        connect(powerSensor, &bluetoothdevice::cadenceChanged, this->device(),
-                                &bluetoothdevice::cadenceSensor);
-                    }
+                    connect(powerSensor, &bluetoothdevice::cadenceChanged, this->device(),
+                            &bluetoothdevice::cadenceSensor);
                     powerSensor->deviceDiscovered(b);
                 } else if (device() && device()->deviceType() == TREADMILL) {
                     powerSensorRun = new strydrunpowersensor(false, false, true);

src/devices/cscbike/cscbike.cpp

@@ -170,10 +170,6 @@ void cscbike::update() {
     QSettings settings;
     QString heartRateBeltName =
         settings.value(QZSettings::heart_rate_belt_name, QZSettings::default_heart_rate_belt_name).toString();
-    bool externalPowerSensorEnabled =
-        !settings.value(QZSettings::power_sensor_name, QZSettings::default_power_sensor_name)
-             .toString()
-             .startsWith(QStringLiteral("Disabled"));
 
     if (!noVirtualDevice) {
 #ifdef Q_OS_ANDROID
@@ -187,24 +183,21 @@ void cscbike::update() {
         }
     }
 
-    if (!externalPowerSensorEnabled) {
-        bool rogue_echo_bike =
-            settings.value(QZSettings::rogue_echo_bike, QZSettings::default_rogue_echo_bike).toBool();
-
-        if (manualResistancePowerAdjustmentActive && jorotoBike) {
-            m_watt = manualResistanceAdjustedWatts();
-        } else if (manualResistancePowerAdjustmentActive && useCustomResistancePowerTable()) {
-            m_watt = customResistanceAdjustedWatts(currentCadence().value(), manualResistanceTarget);
-        } else if (rogue_echo_bike) {
-            double rpm = currentCadence().value();
-            m_watt = 0.000602337 * pow(rpm, 3.11762) + 32.6404;
+    bool rogue_echo_bike = settings.value(QZSettings::rogue_echo_bike, QZSettings::default_rogue_echo_bike).toBool();
+    
+    if (manualResistancePowerAdjustmentActive && jorotoBike) {
+        m_watt = manualResistanceAdjustedWatts();
+    } else if (manualResistancePowerAdjustmentActive && useCustomResistancePowerTable()) {
+        m_watt = customResistanceAdjustedWatts(currentCadence().value(), manualResistanceTarget);
+    } else if (rogue_echo_bike) {
+        double rpm = currentCadence().value();
+        m_watt = 0.000602337 * pow(rpm, 3.11762) + 32.6404;
+    } else {
+        // When cadence is zero, watts should be zero regardless of HR
+        if (currentCadence().value() == 0) {
+            m_watt = 0;
         } else {
-            // When cadence is zero, watts should be zero regardless of HR
-            if (currentCadence().value() == 0) {
-                m_watt = 0;
-            } else {
-                m_watt = wattFromHR(false);
-            }
+            m_watt = wattFromHR(false);
         }
     }
     emit debug(QStringLiteral("Current Watt: ") + QString::number(m_watt.value()));
@@ -279,15 +272,6 @@ void cscbike::characteristicChanged(const QLowEnergyCharacteristic &characterist
     qDebug() << "characteristicChanged << " << characteristic.uuid() << newValue.toHex(' ') << newValue.length();
     Q_UNUSED(characteristic);
     QSettings settings;
-    bool externalCadenceSensorEnabled =
-        !settings.value(QZSettings::cadence_sensor_name, QZSettings::default_cadence_sensor_name)
-             .toString()
-             .startsWith(QStringLiteral("Disabled"));
-    bool externalPowerSensorEnabled =
-        !settings.value(QZSettings::power_sensor_name, QZSettings::default_power_sensor_name)
-             .toString()
-             .startsWith(QStringLiteral("Disabled"));
-    bool useMachineCadence = !externalCadenceSensorEnabled && !externalPowerSensorEnabled;
     // QString heartRateBeltName = //unused QString
     // settings.value(QZSettings::heart_rate_belt_name, QZSettings::default_heart_rate_belt_name).toString();
 
@@ -309,14 +293,47 @@ void cscbike::characteristicChanged(const QLowEnergyCharacteristic &characterist
         return;
     }
 
-    if (characteristic.uuid() != QBluetoothUuid((quint16)0x2A5B)) {
+    bool cyclingPowerMeasurement = characteristic.uuid() == QBluetoothUuid::CyclingPowerMeasurement;
+    if (characteristic.uuid() != QBluetoothUuid((quint16)0x2A5B) && !cyclingPowerMeasurement) {
         return;
     }
 
     lastPacket = newValue;
 
-    bool CrankPresent = (newValue.at(0) & 0x02) == 0x02;
-    bool WheelPresent = (newValue.at(0) & 0x01) == 0x01;
+    bool CrankPresent = false;
+    bool WheelPresent = false;
+    if (cyclingPowerMeasurement) {
+        if (newValue.length() < 4) {
+            return;
+        }
+
+        uint16_t flags = (((uint16_t)((uint8_t)newValue.at(1)) << 8) | (uint16_t)((uint8_t)newValue.at(0)));
+        CrankPresent = (flags & 0x20) == 0x20;
+        if (!CrankPresent) {
+            return;
+        }
+
+        uint8_t index = 4;
+        if ((flags & 0x01) == 0x01) {
+            index += 1; // pedal power balance
+        }
+        if ((flags & 0x04) == 0x04) {
+            index += 2; // accumulated torque
+        }
+        if ((flags & 0x10) == 0x10) {
+            index += 6; // wheel revolutions and wheel event time
+        }
+        if (newValue.length() < index + 4) {
+            return;
+        }
+
+        _CrankRevs = (((uint16_t)((uint8_t)newValue.at(index + 1)) << 8) | (uint16_t)((uint8_t)newValue.at(index)));
+        _LastCrankEventTime =
+            (((uint16_t)((uint8_t)newValue.at(index + 3)) << 8) | (uint16_t)((uint8_t)newValue.at(index + 2)));
+    } else {
+        CrankPresent = (newValue.at(0) & 0x02) == 0x02;
+        WheelPresent = (newValue.at(0) & 0x01) == 0x01;
+    }
     qDebug() << QStringLiteral("CrankPresent: ") << CrankPresent;
     qDebug() << QStringLiteral("WheelPresent: ") << WheelPresent;
 
@@ -334,14 +351,18 @@ void cscbike::characteristicChanged(const QLowEnergyCharacteristic &characterist
         emit debug(QStringLiteral("Current Wheel Event Time: ") + QString::number(_LastWheelEventTime));
         index += 2;
     }
-    if (CrankPresent) {
+    if (CrankPresent && !cyclingPowerMeasurement) {
         _CrankRevs = (((uint16_t)((uint8_t)newValue.at(index + 1)) << 8) | (uint16_t)((uint8_t)newValue.at(index)));
         emit debug(QStringLiteral("Current Crank Revs: ") + QString::number(_CrankRevs));
         index += 2;
         _LastCrankEventTime =
             (((uint16_t)((uint8_t)newValue.at(index + 1)) << 8) | (uint16_t)((uint8_t)newValue.at(index)));
         emit debug(QStringLiteral("Current Crank Event Time: ") + QString::number(_LastCrankEventTime));
     }
+    if (cyclingPowerMeasurement) {
+        emit debug(QStringLiteral("Current Crank Revs: ") + QString::number(_CrankRevs));
+        emit debug(QStringLiteral("Current Crank Event Time: ") + QString::number(_LastCrankEventTime));
+    }
 
     // CSC Combo Sensor Fallback Logic
     //
@@ -367,12 +388,12 @@ void cscbike::characteristicChanged(const QLowEnergyCharacteristic &characterist
         LastCrankEventTime = _LastCrankEventTime;
     }
 
-    int16_t deltaT = LastCrankEventTime - oldLastCrankEventTime;
+    int32_t deltaT = LastCrankEventTime - oldLastCrankEventTime;
     if (deltaT < 0) {
-        deltaT = LastCrankEventTime + 65535 - oldLastCrankEventTime;
+        deltaT = LastCrankEventTime + 65536 - oldLastCrankEventTime;
     }
 
-    if (useMachineCadence && CrankRevs != oldCrankRevs && deltaT) {
+    if (CrankRevs != oldCrankRevs && deltaT) {
         double cadence = ((CrankRevs - oldCrankRevs) / deltaT) * 1024 * 60;
 
         // Cadence Validation Logic
@@ -393,7 +414,7 @@ void cscbike::characteristicChanged(const QLowEnergyCharacteristic &characterist
         if ((cadence >= 0 && (cadence < 256 || _CrankRevs == 0) && CrankPresent) || (!CrankPresent && WheelPresent))
             Cadence = cadence;
         lastGoodCadence = now;
-    } else if (useMachineCadence && lastGoodCadence.msecsTo(now) > 2000) {
+    } else if (lastGoodCadence.msecsTo(now) > 2000) {
         Cadence = 0;
     }
     emit cadenceChanged(Cadence.value());
@@ -490,13 +511,14 @@ void cscbike::stateChanged(QLowEnergyService::ServiceState state) {
     emit debug(QStringLiteral("BTLE stateChanged ") + QString::fromLocal8Bit(metaEnum.valueToKey(state)));
 
     QBluetoothUuid CyclingSpeedAndCadence(QBluetoothUuid::CyclingSpeedAndCadence);
+    QBluetoothUuid CyclingPower(QBluetoothUuid::CyclingPower);
     QBluetoothUuid Battery(QBluetoothUuid::BatteryService);
     for (QLowEnergyService *s : qAsConst(gattCommunicationChannelService)) {
         qDebug() << QStringLiteral("stateChanged") << s->serviceUuid() << s->state();
 #ifdef Q_OS_WINDOWS
-        qDebug() << "windows workaround, check only CyclingSpeedAndCadence ftms service"
-                 << (s->serviceUuid() == CyclingSpeedAndCadence);
-        if (s->serviceUuid() == CyclingSpeedAndCadence)
+        qDebug() << "windows workaround, check only cycling sensor services"
+                 << (s->serviceUuid() == CyclingSpeedAndCadence || s->serviceUuid() == CyclingPower);
+        if (s->serviceUuid() == CyclingSpeedAndCadence || s->serviceUuid() == CyclingPower)
 #endif
         {
             if (s->state() != QLowEnergyService::ServiceDiscovered && s->state() != QLowEnergyService::InvalidService) {
@@ -511,13 +533,15 @@ void cscbike::stateChanged(QLowEnergyService::ServiceState state) {
     for (QLowEnergyService *s : qAsConst(gattCommunicationChannelService)) {
         if (s->state() == QLowEnergyService::ServiceDiscovered) {
 
-            if(s->serviceUuid() == CyclingSpeedAndCadence) {
-                qDebug() << "CyclingSpeedAndCadence found";
+            if (s->serviceUuid() == CyclingSpeedAndCadence || s->serviceUuid() == CyclingPower) {
+                qDebug() << "Cycling cadence service found" << s->serviceUuid();
                 cadenceService = s;
             }
 
-            if(s->serviceUuid() != CyclingSpeedAndCadence && s->serviceUuid() != Battery) {
-                //  No data from sensors and avatar won’t move in Zwift (even when data showed on first try) (Issue #2178)
+            if (s->serviceUuid() != CyclingSpeedAndCadence && s->serviceUuid() != CyclingPower &&
+                s->serviceUuid() != Battery) {
+                //  No data from sensors and avatar won’t move in Zwift (even when data showed on first try) (Issue
+                //  #2178)
                 qDebug() << "avoid unwaned service";
                 continue;
             }
@@ -536,8 +560,9 @@ void cscbike::stateChanged(QLowEnergyService::ServiceState state) {
 
             auto characteristics_list = s->characteristics();
             for (const QLowEnergyCharacteristic &c : qAsConst(characteristics_list)) {
-                if(c.uuid() == QBluetoothUuid((quint16)0x2A5B)) {
-                    qDebug() << "CyclingSpeedAndCadence char found";
+                if (c.uuid() == QBluetoothUuid((quint16)0x2A5B) ||
+                    c.uuid() == QBluetoothUuid::CyclingPowerMeasurement) {
+                    qDebug() << "Cycling cadence characteristic found" << c.uuid();
                     cadenceChar = c;
                 }
                 qDebug() << QStringLiteral("char uuid") << c.uuid() << QStringLiteral("handle") << c.handle() << QStringLiteral("properties") << c.properties();
@@ -663,9 +688,10 @@ void cscbike::serviceScanDone(void) {
     for (const QBluetoothUuid &s : qAsConst(services_list)) {
 #ifdef Q_OS_WINDOWS
         QBluetoothUuid CyclingSpeedAndCadence(QBluetoothUuid::CyclingSpeedAndCadence);
-        qDebug() << "windows workaround, check only the CyclingSpeedAndCadence service" << s << CyclingSpeedAndCadence
-                 << (s == CyclingSpeedAndCadence);
-        if (s == CyclingSpeedAndCadence)
+        QBluetoothUuid CyclingPower(QBluetoothUuid::CyclingPower);
+        qDebug() << "windows workaround, check only cycling sensor services" << s << CyclingSpeedAndCadence
+                 << CyclingPower << (s == CyclingSpeedAndCadence || s == CyclingPower);
+        if (s == CyclingSpeedAndCadence || s == CyclingPower)
 #endif
         {
             gattCommunicationChannelService.append(m_control->createServiceObject(s));