From 65f56803bb54299699b3e8c1259b90d9e72f1238 Mon Sep 17 00:00:00 2001 From: Anthony Doud Date: Sat, 27 Dec 2025 16:01:54 -0600 Subject: [PATCH 01/12] initial --- include/BLE_KickrBikeService.h | 104 ++++++ lib/SS2K/include/Constants.h | 27 +- src/BLE_KickrBikeService.cpp | 573 +++++++++++++++++++++++++++++++++ src/BLE_Server.cpp | 7 +- 4 files changed, 702 insertions(+), 9 deletions(-) create mode 100644 include/BLE_KickrBikeService.h create mode 100644 src/BLE_KickrBikeService.cpp diff --git a/include/BLE_KickrBikeService.h b/include/BLE_KickrBikeService.h new file mode 100644 index 00000000..6b99fea3 --- /dev/null +++ b/include/BLE_KickrBikeService.h @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2020 Anthony Doud & Joel Baranick + * All rights reserved + * + * SPDX-License-Identifier: GPL-2.0-only + */ + +#pragma once + +#include +#include "BLE_Common.h" + +// Forward declaration for custom callback +class KickrBikeCharacteristicCallbacks; + +// Gear system configuration for virtual shifting +#define KICKR_BIKE_NUM_GEARS 24 +#define KICKR_BIKE_DEFAULT_GEAR 11 // Middle gear (0-indexed, so gear 12 in 1-indexed) + +class BLE_KickrBikeService { + public: + BLE_KickrBikeService(); + void setupService(NimBLEServer *pServer, MyCharacteristicCallbacks *chrCallbacks); + void update(); + + // Gear management + void shiftUp(); + void shiftDown(); + int getCurrentGear() const { return currentGear; } + double getCurrentGearRatio() const; + + // Function to check shifter position and modify incline accordingly + void updateGearFromShifterPosition(); + + // RideOn handshake handling + void processWrite(const std::string& value); + void sendRideOnResponse(); + void sendKeepAlive(); + void sendRideData(); + + // Opcode message handlers + void handleGetRequest(const uint8_t* data, size_t length); + void handleSetRequest(const uint8_t* data, size_t length); + void handleReset(); + void handleSetLogLevel(const uint8_t* data, size_t length); + void handleVendorMessage(const uint8_t* data, size_t length); + void sendGetResponse(uint16_t objectId, const uint8_t* data, size_t length); + void sendStatusResponse(uint8_t status); + + // Gradient/resistance control (independent of FTMS) + void setBaseGradient(double gradientPercent); + double getBaseGradient() const { return baseGradient; } + double getEffectiveGradient() const; + void applyGradientToTrainer(); + + // Power control for ERG mode + void setTargetPower(int watts); + int getTargetPower() const { return targetPower; } + + // Enable/disable the service + void enable() { isEnabled = true; } + void disable() { isEnabled = false; } + bool isServiceEnabled() const { return isEnabled; } + + private: + BLEService *pKickrBikeService; + BLECharacteristic *syncRxCharacteristic; // Write characteristic for commands + BLECharacteristic *asyncTxCharacteristic; // Notify characteristic for events + BLECharacteristic *syncTxCharacteristic; // Notify characteristic for responses + BLECharacteristic *debugCharacteristic; // Optional debug characteristic + BLECharacteristic *unknown6Characteristic; // Optional unknown characteristic + + // Gear system state + int currentGear; + int lastShifterPosition; + + // Gradient and resistance state (independent of FTMS) + double baseGradient; // Base gradient set by Zwift (%) + double effectiveGradient; // Gradient after gear ratio applied (%) + int targetPower; // Target power for ERG mode (watts) + + // Service state + bool isHandshakeComplete; + bool isEnabled; // Whether this service should control the trainer + unsigned long lastKeepAliveTime; + unsigned long lastGradientUpdateTime; + unsigned long lastRideDataTime; + + // Gear ratio table (24 gears) + static const double gearRatios[KICKR_BIKE_NUM_GEARS]; + + // Helper methods + void applyGearChange(); + double calculateEffectiveGrade(double baseGrade, double gearRatio); + bool isRideOnMessage(const std::string& data); + void updateTrainerPosition(); +}; + +// Custom callback class for KickrBike Sync RX characteristic +class KickrBikeCharacteristicCallbacks : public NimBLECharacteristicCallbacks { + void onWrite(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo) override; +}; + +extern BLE_KickrBikeService kickrBikeService; diff --git a/lib/SS2K/include/Constants.h b/lib/SS2K/include/Constants.h index f5a66edb..66e016f5 100644 --- a/lib/SS2K/include/Constants.h +++ b/lib/SS2K/include/Constants.h @@ -19,14 +19,14 @@ // Device Information Service #define DEVICE_INFORMATION_SERVICE_UUID NimBLEUUID((uint16_t)0x180A) -#define MANUFACTURER_NAME_UUID NimBLEUUID((uint16_t)0x2A29) -#define MODEL_NUMBER_UUID NimBLEUUID((uint16_t)0x2A24) -#define SERIAL_NUMBER_UUID NimBLEUUID((uint16_t)0x2A25) -#define HARDWARE_REVISION_UUID NimBLEUUID((uint16_t)0x2A27) -#define FIRMWARE_REVISION_UUID NimBLEUUID((uint16_t)0x2A26) -#define SOFTWARE_REVISION_UUID NimBLEUUID((uint16_t)0x2A28) -#define SYSTEM_ID_UUID NimBLEUUID((uint16_t)0x2A23) -#define PNP_ID_UUID NimBLEUUID((uint16_t)0x2A50) +#define MANUFACTURER_NAME_UUID NimBLEUUID((uint16_t)0x2A29) +#define MODEL_NUMBER_UUID NimBLEUUID((uint16_t)0x2A24) +#define SERIAL_NUMBER_UUID NimBLEUUID((uint16_t)0x2A25) +#define HARDWARE_REVISION_UUID NimBLEUUID((uint16_t)0x2A27) +#define FIRMWARE_REVISION_UUID NimBLEUUID((uint16_t)0x2A26) +#define SOFTWARE_REVISION_UUID NimBLEUUID((uint16_t)0x2A28) +#define SYSTEM_ID_UUID NimBLEUUID((uint16_t)0x2A23) +#define PNP_ID_UUID NimBLEUUID((uint16_t)0x2A50) // Heart Service #define HEARTSERVICE_UUID NimBLEUUID((uint16_t)0x180D) @@ -91,6 +91,17 @@ #define PELOTON_REQ_POS 1 #define PELOTON_CHECKSUM_POS 2 +// Zwift Ride / KICKR BIKE Service +// This service emulates the Wahoo KICKR BIKE protocol for virtual shifting +#define ZWIFT_RIDE_SERVICE_UUID NimBLEUUID("0000FC82-0000-1000-8000-00805F9B34FB") +#define ZWIFT_CUSTOM_SERVICE_UUID NimBLEUUID("00000001-19CA-4651-86E5-FA29DCDD09D1") +#define ZWIFT_SYNC_RX_UUID NimBLEUUID("00000003-19CA-4651-86E5-FA29DCDD09D1") // Write characteristic +#define ZWIFT_ASYNC_TX_UUID NimBLEUUID("00000002-19CA-4651-86E5-FA29DCDD09D1") // Notify characteristic +#define ZWIFT_SYNC_TX_UUID NimBLEUUID("00000004-19CA-4651-86E5-FA29DCDD09D1") // Notify characteristic +#define ZWIFT_DEBUG_CHARACTERISTIC_UUID NimBLEUUID("00000005-19CA-4651-86E5-FA29DCDD09D1") +#define ZWIFT_UNKNOWN_6_CHARACTERISTIC_UUID NimBLEUUID("00000006-19CA-4651-86E5-FA29DCDD09D1") +#define ZWIFT_RIDE_CUSTOM_SERVICE_UUID_SHORT NimBLEUUID("FC82") + // BLE HID #define APPEARANCE_HID_GENERIC_UUID NimBLEUUID((uint16_t)0x3C0) #define APPEARANCE_HID_KEYBOARD_UUID NimBLEUUID((uint16_t)0x3C1) diff --git a/src/BLE_KickrBikeService.cpp b/src/BLE_KickrBikeService.cpp new file mode 100644 index 00000000..245902fd --- /dev/null +++ b/src/BLE_KickrBikeService.cpp @@ -0,0 +1,573 @@ +/* + * Copyright (C) 2020 Anthony Doud & Joel Baranick + * All rights reserved + * + * SPDX-License-Identifier: GPL-2.0-only + */ + +#include "BLE_KickrBikeService.h" +#include "DirConManager.h" +#include "Main.h" +#include +#include +#include + +namespace { +inline void appendVarint(std::vector& buffer, uint32_t value) { + while (value >= 0x80) { + buffer.push_back(static_cast(value | 0x80)); + value >>= 7; + } + buffer.push_back(static_cast(value)); +} + +inline void appendVarintField(std::vector& buffer, uint8_t fieldNumber, uint32_t value) { + uint8_t key = static_cast((fieldNumber << 3) | 0x00); + buffer.push_back(key); + appendVarint(buffer, value); +} + +inline int32_t decodeZigZag32(uint32_t value) { + return static_cast((value >> 1) ^ static_cast(-static_cast(value & 0x01))); +} +} // namespace + +// Gear ratio table: 24 gears from easiest (0.50) to hardest (1.65) +// These ratios are multiplied with the base gradient to simulate gear changes +const double BLE_KickrBikeService::gearRatios[KICKR_BIKE_NUM_GEARS] = { + 0.50, 0.55, 0.60, 0.65, 0.70, 0.75, 0.80, 0.85, // Gears 1-8 (easy) + 0.90, 0.95, 1.00, 1.05, 1.10, 1.15, 1.20, 1.25, // Gears 9-16 (medium) + 1.30, 1.35, 1.40, 1.45, 1.50, 1.55, 1.60, 1.65 // Gears 17-24 (hard) +}; + +BLE_KickrBikeService::BLE_KickrBikeService() + : pKickrBikeService(nullptr), + syncRxCharacteristic(nullptr), + asyncTxCharacteristic(nullptr), + syncTxCharacteristic(nullptr), + debugCharacteristic(nullptr), + unknown6Characteristic(nullptr), + currentGear(KICKR_BIKE_DEFAULT_GEAR), + lastShifterPosition(-1), + baseGradient(0.0), + effectiveGradient(0.0), + targetPower(0), + isHandshakeComplete(false), + isEnabled(false), + lastKeepAliveTime(0), + lastGradientUpdateTime(0), + lastRideDataTime(0) {} + +void BLE_KickrBikeService::setupService(NimBLEServer *pServer, MyCharacteristicCallbacks *chrCallbacks) { + // Create the Zwift Ride service (KICKR BIKE protocol) + pKickrBikeService = spinBLEServer.pServer->createService(ZWIFT_CUSTOM_SERVICE_UUID); + + // Create the three characteristics according to KICKR BIKE specification: + // 1. Sync RX - Write characteristic for receiving commands from Zwift + syncRxCharacteristic = pKickrBikeService->createCharacteristic( + ZWIFT_SYNC_RX_UUID, + NIMBLE_PROPERTY::WRITE_NR); + + // 2. Async TX - Notify characteristic for asynchronous events (button presses, battery) + asyncTxCharacteristic = pKickrBikeService->createCharacteristic( + ZWIFT_ASYNC_TX_UUID, + NIMBLE_PROPERTY::NOTIFY); + + // 3. Sync TX - Notify characteristic for synchronous responses + syncTxCharacteristic = pKickrBikeService->createCharacteristic( + ZWIFT_SYNC_TX_UUID, + NIMBLE_PROPERTY::INDICATE); + + // Optional: Debug characteristic for logging/debug info + debugCharacteristic = pKickrBikeService->createCharacteristic( + ZWIFT_DEBUG_CHARACTERISTIC_UUID, + NIMBLE_PROPERTY::NOTIFY | NIMBLE_PROPERTY::READ); + + // Optional: Unknown characteristic 6 + unknown6Characteristic = pKickrBikeService->createCharacteristic( + ZWIFT_UNKNOWN_6_CHARACTERISTIC_UUID, + NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY | NIMBLE_PROPERTY::INDICATE); + + // Set custom callback for Sync RX to handle RideOn handshake + static KickrBikeCharacteristicCallbacks kickrBikeCallbacks; + syncRxCharacteristic->setCallbacks(&kickrBikeCallbacks); + + // Start the service + pKickrBikeService->start(); + + // Add service UUID to DirCon MDNS (for discovery) + DirConManager::addBleServiceUuid(ZWIFT_RIDE_CUSTOM_SERVICE_UUID_SHORT); + + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE Service initialized with %d gears", KICKR_BIKE_NUM_GEARS); +} + +void BLE_KickrBikeService::update() { + // Send periodic keep-alive messages if handshake is complete + if (isHandshakeComplete) { + unsigned long currentTime = millis(); + // Send keep-alive every 5 seconds + if (currentTime - lastKeepAliveTime >= 5000) { + sendKeepAlive(); + lastKeepAliveTime = currentTime; + } + + if (currentTime - lastRideDataTime >= 1000) { + sendRideData(); + lastRideDataTime = currentTime; + } + } +} + +void BLE_KickrBikeService::shiftUp() { + if (currentGear < KICKR_BIKE_NUM_GEARS - 1) { + currentGear++; + applyGearChange(); + SS2K_LOG(BLE_SERVER_LOG_TAG, "Shifted UP to gear %d (ratio: %.2f)", + currentGear + 1, getCurrentGearRatio()); + } else { + SS2K_LOG(BLE_SERVER_LOG_TAG, "Already in highest gear"); + } +} + +void BLE_KickrBikeService::shiftDown() { + if (currentGear > 0) { + currentGear--; + applyGearChange(); + SS2K_LOG(BLE_SERVER_LOG_TAG, "Shifted DOWN to gear %d (ratio: %.2f)", + currentGear + 1, getCurrentGearRatio()); + } else { + SS2K_LOG(BLE_SERVER_LOG_TAG, "Already in lowest gear"); + } +} + +double BLE_KickrBikeService::getCurrentGearRatio() const { + if (currentGear >= 0 && currentGear < KICKR_BIKE_NUM_GEARS) { + return gearRatios[currentGear]; + } + return 1.0; // Default to neutral ratio +} + +void BLE_KickrBikeService::applyGearChange() { + // Recalculate effective gradient with new gear + effectiveGradient = calculateEffectiveGrade(baseGradient, getCurrentGearRatio()); + + // Apply to trainer if this service is enabled + if (isEnabled) { + applyGradientToTrainer(); + } + + // Optionally notify clients about gear change via async TX characteristic + // This could be used to send gear status to connected apps + uint8_t gearStatus[2] = { + static_cast(currentGear + 1), // 1-indexed gear number + static_cast((getCurrentGearRatio() * 100)) // Ratio as percentage + }; + + asyncTxCharacteristic->setValue(gearStatus, sizeof(gearStatus)); + asyncTxCharacteristic->notify(); +} + +void BLE_KickrBikeService::setBaseGradient(double gradientPercent) { + baseGradient = gradientPercent; + effectiveGradient = calculateEffectiveGrade(baseGradient, getCurrentGearRatio()); + + // Apply to trainer if enabled + if (isEnabled) { + applyGradientToTrainer(); + } + + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Base gradient set to %.2f%%", baseGradient); +} + +double BLE_KickrBikeService::getEffectiveGradient() const { + return effectiveGradient; +} + +void BLE_KickrBikeService::applyGradientToTrainer() { + // Only update if enough time has passed (100ms debounce) + unsigned long currentTime = millis(); + if (currentTime - lastGradientUpdateTime < 100) { + return; + } + lastGradientUpdateTime = currentTime; + + // Clamp to valid trainer limits (-20% to +20%) + double clampedGradient = effectiveGradient; + if (clampedGradient < -20.0) clampedGradient = -20.0; + if (clampedGradient > 20.0) clampedGradient = 20.0; + + // Convert to 0.01% units for rtConfig + int gradientUnits = static_cast(clampedGradient * 100); + + // Update the target incline directly + rtConfig->setTargetIncline(gradientUnits); + + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Applied gradient %.2f%% (gear %d, ratio %.2f)", + clampedGradient, currentGear + 1, getCurrentGearRatio()); +} + +void BLE_KickrBikeService::setTargetPower(int watts) { + targetPower = watts; + + // In ERG mode, the power is fixed and gears affect the "feel" + // This is handled by the trainer's power control logic + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Target power set to %d watts", targetPower); +} + +void BLE_KickrBikeService::updateTrainerPosition() { + // This method updates the physical trainer position based on effective gradient + // Only applies if the service is enabled and controlling the trainer + if (!isEnabled) { + return; + } + + applyGradientToTrainer(); +} + +double BLE_KickrBikeService::calculateEffectiveGrade(double baseGrade, double gearRatio) { + // Calculate effective grade by multiplying base grade with gear ratio + // This simulates the feeling of shifting gears: + // - Lower gear (ratio < 1.0) makes hills feel easier + // - Higher gear (ratio > 1.0) makes hills feel harder + return baseGrade * gearRatio; +} + +void BLE_KickrBikeService::updateGearFromShifterPosition() { + // Get current shifter position + int currentShifterPosition = rtConfig->getShifterPosition(); + + // Check if shifter position has changed + if (lastShifterPosition == -1) { + // First run, just store the position + lastShifterPosition = currentShifterPosition; + return; + } + + if (currentShifterPosition == lastShifterPosition) { + // No change, nothing to do + return; + } + + // Determine direction of shift + if (currentShifterPosition > lastShifterPosition) { + // Shifter moved up - shift to harder gear + shiftUp(); + } else { + // Shifter moved down - shift to easier gear + shiftDown(); + } + + // Update last position + lastShifterPosition = currentShifterPosition; +} + +bool BLE_KickrBikeService::isRideOnMessage(const std::string& data) { + // RideOn handshake prefix = 0x52 0x69 0x64 0x65 0x4f 0x6e + if (data.length() < 6) { + return false; + } + + static const uint8_t rideOnPrefix[6] = {0x52, 0x69, 0x64, 0x65, 0x4f, 0x6e}; + for (size_t i = 0; i < 6; ++i) { + if ((uint8_t)data[i] != rideOnPrefix[i]) { + return false; + } + } + + if (data.length() == 6) { + return true; + } + + // Some Zwift clients append signature bytes (0x01 or 0x02, followed by 0x03) + if (data.length() == 8) { + uint8_t signatureMsb = (uint8_t)data[6]; + uint8_t signatureLsb = (uint8_t)data[7]; + if (signatureLsb == 0x03 && (signatureMsb == 0x01 || signatureMsb == 0x02)) { + return true; + } + } + + return false; +} + +void KickrBikeCharacteristicCallbacks::onWrite(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo) { + std::string rxValue = pCharacteristic->getValue(); + kickrBikeService.processWrite(rxValue); +} + +void BLE_KickrBikeService::processWrite(const std::string& value) { + if (value.empty()) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Received empty write"); + return; + } + + // Check if this is the RideOn handshake (no opcode, just raw bytes) + if (isRideOnMessage(value)) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Received RideOn handshake"); + sendRideOnResponse(); + isHandshakeComplete = true; + lastKeepAliveTime = millis(); + return; + } + + // Process opcode-based messages + uint8_t opcode = (uint8_t)value[0]; + const uint8_t* messageData = (const uint8_t*)value.data() + 1; + size_t messageLength = value.length() - 1; + + switch (opcode) { + case 0x04: // SET - Update trainer state + handleSetRequest(messageData, messageLength); + break; + + case 0x08: // GET - Request data object + handleGetRequest(messageData, messageLength); + break; + + case 0x22: // RESET - Reset device + handleReset(); + break; + + case 0x41: // LOG_LEVEL_SET - Set log level + handleSetLogLevel(messageData, messageLength); + break; + + case 0x32: // VENDOR_MESSAGE - Vendor-specific message + handleVendorMessage(messageData, messageLength); + break; + + case 0x07: // CONTROLLER_NOTIFICATION - Button events (shouldn't be written to us, we send these) + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Unexpected CONTROLLER_NOTIFICATION write"); + break; + + case 0x19: // BATTERY_NOTIF - Battery updates (shouldn't be written to us, we send these) + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Unexpected BATTERY_NOTIF write"); + break; + + default: + // Log full unknown message for debugging + String hexDump; + for (size_t i = 0; i < value.length(); ++i) { + char buf[4]; + snprintf(buf, sizeof(buf), "%02X ", (uint8_t)value[i]); + hexDump += buf; + } + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Received unknown message: %s", hexDump.c_str()); + break; + } +} + +void BLE_KickrBikeService::sendRideOnResponse() { + // Respond with "RideOn" + signature bytes (0x01 0x03) + uint8_t response[8] = { + 0x52, 0x69, 0x64, 0x65, 0x4f, 0x6e, // "RideOn" + 0x01, 0x03 // Signature + }; + + syncTxCharacteristic->setValue(response, sizeof(response)); + syncTxCharacteristic->notify(); + + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sent RideOn response"); +} + +void BLE_KickrBikeService::sendKeepAlive() { + // Keep-alive message to maintain connection with Zwift + // This is a protobuf-encoded message that tells Zwift we're still alive + // The exact format comes from the BikeControl reference implementation + uint8_t keepAliveData[] = { + 0xB7, 0x01, 0x00, 0x00, 0x20, 0x41, 0x20, 0x1C, + 0x00, 0x18, 0x00, 0x04, 0x00, 0x1B, 0x4F, 0x00, + 0xB7, 0x01, 0x00, 0x00, 0x20, 0x79, 0x8E, 0xC5, + 0xBD, 0xEF, 0xCB, 0xE4, 0x56, 0x34, 0x18, 0x26, + 0x9E, 0x49, 0x26, 0xFB, 0xE1 + }; + + syncTxCharacteristic->setValue(keepAliveData, sizeof(keepAliveData)); + syncTxCharacteristic->notify(); + + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sent keep-alive"); +} + +void BLE_KickrBikeService::sendRideData() { + if (!isHandshakeComplete) { + return; + } + + int power = std::max(0, rtConfig->watts.getValue()); + int cadence = std::max(0, static_cast(rtConfig->cad.getValue())); + double speedKmh = rtConfig->getSimulatedSpeed() > 0 ? rtConfig->getSimulatedSpeed() : spinBLEServer.calculateSpeed(); + if (speedKmh < 0) { + speedKmh = 0; + } + uint32_t speedX100 = static_cast(speedKmh * 100.0 + 0.5); + int heartRate = std::max(0, rtConfig->hr.getValue()); + + std::vector payload; + payload.reserve(20); + payload.push_back(0x03); + appendVarintField(payload, 1, static_cast(power)); + appendVarintField(payload, 2, static_cast(cadence)); + appendVarintField(payload, 3, speedX100); + appendVarintField(payload, 4, static_cast(heartRate)); + appendVarintField(payload, 5, 0); + appendVarintField(payload, 6, 0); + + asyncTxCharacteristic->setValue(payload.data(), payload.size()); + asyncTxCharacteristic->notify(); +} + +// Opcode message handlers + +void BLE_KickrBikeService::handleGetRequest(const uint8_t* data, size_t length) { + // GET request - Zwift is requesting a data object + // The data should contain an object ID (protobuf encoded) + + if (length < 1) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: GET request with no data"); + sendStatusResponse(0x02); // Error status + return; + } + + // For now, we'll parse a simple object ID from the first bytes + // In a full implementation, this would be protobuf decoded + uint16_t objectId = 0; + if (length >= 2) { + objectId = ((uint16_t)data[1] << 8) | data[0]; + } else { + objectId = data[0]; + } + + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: GET request for object ID 0x%04X", objectId); + + // Respond with empty data for now (full implementation would return actual object data) + sendGetResponse(objectId, nullptr, 0); +} + +void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) { + if (length < 3) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET request too short (%d)", length); + sendStatusResponse(0x02); + return; + } + + // Zwift sends gradient updates as: 0x22 0x10 + if (data[0] == 0x22 && data[1] >= 2) { + uint8_t payloadLen = data[1]; + size_t payloadEnd = 2 + payloadLen; + if (payloadEnd <= length && data[2] == 0x10) { + size_t index = 3; + uint32_t rawValue = 0; + uint8_t shift = 0; + + while (index < payloadEnd) { + uint8_t byte = data[index++]; + rawValue |= (static_cast(byte & 0x7F) << shift); + if ((byte & 0x80) == 0) { + break; + } + shift += 7; + if (shift > 28) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET gradient varint overflow"); + sendStatusResponse(0x02); + return; + } + } + + int32_t signedValue = decodeZigZag32(rawValue); + double gradientPercent = static_cast(signedValue) / 100.0; + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET gradient to %.2f%% (raw %ld)", gradientPercent, static_cast(signedValue)); + setBaseGradient(gradientPercent); + sendStatusResponse(0x00); + return; + } + } + + // Unknown SET payload; acknowledge to keep protocol flowing but log for future decoding. + String hexDump; + for (size_t i = 0; i < length; ++i) { + char buf[4]; + snprintf(buf, sizeof(buf), "%02X ", data[i]); + hexDump += buf; + } + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Unhandled SET payload: %s", hexDump.c_str()); + sendStatusResponse(0x00); +} + +void BLE_KickrBikeService::handleReset() { + // RESET command - Reset the device to default state + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: RESET command received"); + + // Reset to default gear + currentGear = KICKR_BIKE_DEFAULT_GEAR; + baseGradient = 0.0; + effectiveGradient = 0.0; + targetPower = 0; + + // Apply reset state to trainer + if (isEnabled) { + applyGradientToTrainer(); + } + + // Send success status + sendStatusResponse(0x00); // Success +} + +void BLE_KickrBikeService::handleSetLogLevel(const uint8_t* data, size_t length) { + // LOG_LEVEL_SET - Set logging level + if (length < 1) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET_LOG_LEVEL with no data"); + return; + } + + uint8_t logLevel = data[0]; + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET_LOG_LEVEL to %d", logLevel); + + // For now, just acknowledge - full implementation would adjust logging + sendStatusResponse(0x00); // Success +} + +void BLE_KickrBikeService::handleVendorMessage(const uint8_t* data, size_t length) { + // VENDOR_MESSAGE - Vendor-specific message + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: VENDOR_MESSAGE received (%d bytes)", length); + + // Log the message content for debugging + if (length > 0) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Vendor message first byte: 0x%02X", data[0]); + } + + // Send success status + sendStatusResponse(0x00); +} + +void BLE_KickrBikeService::sendGetResponse(uint16_t objectId, const uint8_t* data, size_t length) { + // Send GET_RESPONSE (opcode 0x3C) with the requested object data + std::vector response; + response.push_back(0x3C); // GET_RESPONSE opcode + + // Add object ID (little-endian) + response.push_back(objectId & 0xFF); + response.push_back((objectId >> 8) & 0xFF); + + // Add data if provided + if (data && length > 0) { + response.insert(response.end(), data, data + length); + } + + syncTxCharacteristic->setValue(response.data(), response.size()); + syncTxCharacteristic->notify(); + + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sent GET_RESPONSE for object 0x%04X", objectId); +} + +void BLE_KickrBikeService::sendStatusResponse(uint8_t status) { + // Send STATUS_RESPONSE (opcode 0x12) with status code + uint8_t response[2] = { + 0x12, // STATUS_RESPONSE opcode + status // Status code (0x00 = success, others = error) + }; + + syncTxCharacteristic->setValue(response, sizeof(response)); + syncTxCharacteristic->notify(); + + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sent STATUS_RESPONSE (status: 0x%02X)", status); +} diff --git a/src/BLE_Server.cpp b/src/BLE_Server.cpp index cee59e9e..1a8d1fc2 100644 --- a/src/BLE_Server.cpp +++ b/src/BLE_Server.cpp @@ -19,6 +19,7 @@ #include "BLE_Fitness_Machine_Service.h" #include "BLE_Custom_Characteristic.h" #include "BLE_Device_Information_Service.h" +#include "BLE_KickrBikeService.h" // BLE Server Settings SpinBLEServer spinBLEServer; @@ -31,6 +32,7 @@ BLE_Heart_Service heartService; BLE_Fitness_Machine_Service fitnessMachineService; BLE_ss2kCustomCharacteristic ss2kCustomCharacteristic; BLE_Device_Information_Service deviceInformationService; +BLE_KickrBikeService kickrBikeService; // BLE_Wattbike_Service wattbikeService; // BLE_SB20_Service sb20Service; @@ -52,13 +54,15 @@ void startBLEServer() { cyclingPowerService.setupService(spinBLEServer.pServer, &chrCallbacks); heartService.setupService(spinBLEServer.pServer, &chrCallbacks); fitnessMachineService.setupService(spinBLEServer.pServer, &chrCallbacks); + kickrBikeService.setupService(spinBLEServer.pServer, &chrCallbacks); ss2kCustomCharacteristic.setupService(spinBLEServer.pServer); deviceInformationService.setupService(spinBLEServer.pServer); //add all service UUIDs to advertisement vector oServiceUUIDs.push_back(CSCSERVICE_UUID); oServiceUUIDs.push_back(CYCLINGPOWERSERVICE_UUID); oServiceUUIDs.push_back(HEARTSERVICE_UUID); - oServiceUUIDs.push_back(FITNESSMACHINESERVICE_UUID); + //oServiceUUIDs.push_back(FITNESSMACHINESERVICE_UUID); + oServiceUUIDs.push_back(ZWIFT_RIDE_CUSTOM_SERVICE_UUID_SHORT); oAdvertisementData.setFlags(0x06); // General Discoverable, BR/EDR Not Supported oAdvertisementData.setCompleteServices16(oServiceUUIDs); pAdvertising->setAdvertisementData(oAdvertisementData); @@ -85,6 +89,7 @@ void SpinBLEServer::update() { cyclingPowerService.update(); cyclingSpeedCadenceService.update(); fitnessMachineService.update(); + kickrBikeService.update(); // wattbikeService.parseNemit(); // Changed from update() to parseNemit() // sb20Service.notify(); } From 03788b63db8ac47b95d4c3b1f9f4e34c6779b6b0 Mon Sep 17 00:00:00 2001 From: Anthony Doud Date: Sat, 27 Dec 2025 17:15:28 -0600 Subject: [PATCH 02/12] a little better --- include/BLE_Common.h | 2 +- include/BLE_KickrBikeService.h | 1 + src/BLE_Cycling_Power_Service.cpp | 7 +-- src/BLE_Cycling_Speed_Cadence.cpp | 8 ++-- src/BLE_Fitness_Machine_Service.cpp | 35 +++----------- src/BLE_Heart_Service.cpp | 6 +-- src/BLE_KickrBikeService.cpp | 73 ++++++++++++++++++++++++----- src/BLE_Server.cpp | 12 ++++- 8 files changed, 86 insertions(+), 58 deletions(-) diff --git a/include/BLE_Common.h b/include/BLE_Common.h index 0a881942..3d12d169 100644 --- a/include/BLE_Common.h +++ b/include/BLE_Common.h @@ -94,7 +94,7 @@ class SpinBLEServer { public: int spinDownFlag = 0; NimBLEServer* pServer = nullptr; - void notifyShift(); + void notifyBleAndDircon(NimBLECharacteristic* pCharacteristic,const uint8_t* pData, int length); double calculateSpeed(); void update(); int connectedClientCount(); diff --git a/include/BLE_KickrBikeService.h b/include/BLE_KickrBikeService.h index 6b99fea3..cc19b999 100644 --- a/include/BLE_KickrBikeService.h +++ b/include/BLE_KickrBikeService.h @@ -41,6 +41,7 @@ class BLE_KickrBikeService { // Opcode message handlers void handleGetRequest(const uint8_t* data, size_t length); void handleSetRequest(const uint8_t* data, size_t length); + void handleInfoRequest(const uint8_t* data, size_t length); void handleReset(); void handleSetLogLevel(const uint8_t* data, size_t length); void handleVendorMessage(const uint8_t* data, size_t length); diff --git a/src/BLE_Cycling_Power_Service.cpp b/src/BLE_Cycling_Power_Service.cpp index fa6b3914..9f3a3e37 100644 --- a/src/BLE_Cycling_Power_Service.cpp +++ b/src/BLE_Cycling_Power_Service.cpp @@ -56,12 +56,7 @@ void BLE_Cycling_Power_Service::update() { auto byteArray = cpm.toByteArray(); // Notify the cycling power measurement characteristic - // Need to set the value before notifying so that read works correctly. - cyclingPowerMeasurementCharacteristic->setValue(&byteArray[0], byteArray.size()); - cyclingPowerMeasurementCharacteristic->notify(); - - // Also notify DirCon TCP clients - DirConManager::notifyCharacteristic(NimBLEUUID(CYCLINGPOWERSERVICE_UUID), cyclingPowerMeasurementCharacteristic->getUUID(), &byteArray[0], byteArray.size()); + spinBLEServer.notifyBleAndDircon(cyclingPowerMeasurementCharacteristic, &byteArray[0], byteArray.size()); const int kLogBufCapacity = 150; char logBuf[kLogBufCapacity]; diff --git a/src/BLE_Cycling_Speed_Cadence.cpp b/src/BLE_Cycling_Speed_Cadence.cpp index 92e9e2cc..85b1e157 100644 --- a/src/BLE_Cycling_Speed_Cadence.cpp +++ b/src/BLE_Cycling_Speed_Cadence.cpp @@ -10,7 +10,7 @@ BLE_Cycling_Speed_Cadence::BLE_Cycling_Speed_Cadence() : pCyclingSpeedCadenceService(nullptr), cscMeasurement(nullptr), cscFeature(nullptr) {} -void BLE_Cycling_Speed_Cadence::setupService(NimBLEServer *pServer, MyCharacteristicCallbacks *chrCallbacks) { +void BLE_Cycling_Speed_Cadence::setupService(NimBLEServer* pServer, MyCharacteristicCallbacks* chrCallbacks) { pCyclingSpeedCadenceService = pServer->createService(CSCSERVICE_UUID); cscMeasurement = pCyclingSpeedCadenceService->createCharacteristic(CSCMEASUREMENT_UUID, NIMBLE_PROPERTY::NOTIFY); cscFeature = pCyclingSpeedCadenceService->createCharacteristic(CSCFEATURE_UUID, NIMBLE_PROPERTY::READ); @@ -37,7 +37,7 @@ void BLE_Cycling_Speed_Cadence::update() { CscMeasurement csc; // Clear all flags initially - *(reinterpret_cast(&(csc.flags))) = 0; + *(reinterpret_cast(&(csc.flags))) = 0; // Set flags based on data presence csc.flags.wheelRevolutionDataPresent = 1; // Wheel Revolution Data Present @@ -52,9 +52,7 @@ void BLE_Cycling_Speed_Cadence::update() { auto byteArray = csc.toByteArray(); // Notify the cycling power measurement characteristic - // Need to set the value before notifying so that read works correctly. - cscMeasurement->setValue(&byteArray[0], byteArray.size()); - cscMeasurement->notify(); + spinBLEServer.notifyBleAndDircon(cscMeasurement, &byteArray[0], byteArray.size()); const int kLogBufCapacity = 150; char logBuf[kLogBufCapacity]; diff --git a/src/BLE_Fitness_Machine_Service.cpp b/src/BLE_Fitness_Machine_Service.cpp index 58a68742..3b027e54 100644 --- a/src/BLE_Fitness_Machine_Service.cpp +++ b/src/BLE_Fitness_Machine_Service.cpp @@ -125,12 +125,7 @@ void BLE_Fitness_Machine_Service::update() { } // Notify the cycling power measurement characteristic - // Need to set the value before notifying so that read works correctly. - fitnessMachineIndoorBikeData->setValue(ftmsIndoorBikeData.data(), ftmsIndoorBikeData.size()); - fitnessMachineIndoorBikeData->notify(); - - // Also notify DirCon TCP clients about Indoor Bike Data - DirConManager::notifyCharacteristic(NimBLEUUID(FITNESSMACHINESERVICE_UUID), fitnessMachineIndoorBikeData->getUUID(), ftmsIndoorBikeData.data(), ftmsIndoorBikeData.size()); + spinBLEServer.notifyBleAndDircon(fitnessMachineIndoorBikeData, ftmsIndoorBikeData.data(), ftmsIndoorBikeData.size()); const int kLogBufCapacity = 200; // Data(30), Sep(data/2), Arrow(3), CharId(37), Sep(3), CharId(37), Sep(3), Name(10), Prefix(2), HR(7), SEP(1), CD(10), SEP(1), PW(8), // SEP(1), SD(7), Suffix(2), Nul(1), rounded up @@ -343,36 +338,20 @@ void BLE_Fitness_Machine_Service::processFTMSWrite() { ftmsTrainingStatus[1] = FitnessMachineTrainingStatus::Other; // 0x00; } // not checking for subscription because a write request would have triggered this - fitnessMachineControlPoint->setValue(returnValue.data(), returnValue.size()); - fitnessMachineControlPoint->notify(); + spinBLEServer.notifyBleAndDircon(fitnessMachineControlPoint, returnValue.data(), returnValue.size()); if (fitnessMachineTrainingStatus->getValue() != ftmsTrainingStatus) { - fitnessMachineTrainingStatus->setValue(ftmsTrainingStatus); - fitnessMachineTrainingStatus->notify(); - // Also notify DirCon TCP clients - DirConManager::notifyCharacteristic(NimBLEUUID(FITNESSMACHINESERVICE_UUID), fitnessMachineTrainingStatus->getUUID(), ftmsTrainingStatus.data(), ftmsTrainingStatus.size()); - } + spinBLEServer.notifyBleAndDircon(fitnessMachineTrainingStatus, ftmsTrainingStatus.data(), ftmsTrainingStatus.size()); + } if (fitnessMachineStatusCharacteristic->getValue() != ftmsStatus) { - fitnessMachineStatusCharacteristic->setValue(ftmsStatus); - fitnessMachineStatusCharacteristic->notify(); - // Also notify DirCon TCP clients - DirConManager::notifyCharacteristic(NimBLEUUID(FITNESSMACHINESERVICE_UUID), fitnessMachineStatusCharacteristic->getUUID(), ftmsStatus.data(), ftmsStatus.size()); - } - - // Also notify DirCon TCP clients - DirConManager::notifyCharacteristic(NimBLEUUID(FITNESSMACHINESERVICE_UUID), fitnessMachineControlPoint->getUUID(), returnValue.data(), returnValue.size()); + spinBLEServer.notifyBleAndDircon(fitnessMachineStatusCharacteristic, ftmsStatus.data(), ftmsStatus.size()); + } } } bool BLE_Fitness_Machine_Service::spinDown(uint8_t response) { uint8_t spinStatus[2] = {FitnessMachineStatus::SpinDownStatus, response}; - // Set the value of the characteristic - fitnessMachineStatusCharacteristic->setValue(spinStatus, sizeof(spinStatus)); - // Notify the connected client - fitnessMachineStatusCharacteristic->notify(); + spinBLEServer.notifyBleAndDircon(fitnessMachineStatusCharacteristic, spinStatus, sizeof(spinStatus)); SS2K_LOG(FMTS_SERVER_LOG_TAG, "Sent SpinDown Status: 0x%02X", response); - // Also notify DirCon TCP clients about the status change - DirConManager::notifyCharacteristic(NimBLEUUID(FITNESSMACHINESERVICE_UUID), fitnessMachineStatusCharacteristic->getUUID(), spinStatus, sizeof(spinStatus)); - return true; } diff --git a/src/BLE_Heart_Service.cpp b/src/BLE_Heart_Service.cpp index d8c5732b..44126962 100644 --- a/src/BLE_Heart_Service.cpp +++ b/src/BLE_Heart_Service.cpp @@ -39,11 +39,7 @@ void BLE_Heart_Service::update() { byte heartRateMeasurement[2] = {0x00, (byte)rtConfig->hr.getValue()}; // Notify the cycling power measurement characteristic - // Need to set the value before notifying so that read works correctly. - heartRateMeasurementCharacteristic->setValue(heartRateMeasurement, 2); - heartRateMeasurementCharacteristic->notify(); - DirConManager::notifyCharacteristic(NimBLEUUID(HEARTSERVICE_UUID), heartRateMeasurementCharacteristic->getUUID(), heartRateMeasurement, 2); - + spinBLEServer.notifyBleAndDircon(heartRateMeasurementCharacteristic, heartRateMeasurement, 2); const int kLogBufCapacity = 125; // Data(10), Sep(data/2), Arrow(3), CharId(37), Sep(3), CharId(37), Sep(3), Name(8), Prefix(2), HR(7), Suffix(2), Nul(1), rounded up char logBuf[kLogBufCapacity]; const size_t heartRateMeasurementLength = sizeof(heartRateMeasurement) / sizeof(heartRateMeasurement[0]); diff --git a/src/BLE_KickrBikeService.cpp b/src/BLE_KickrBikeService.cpp index 245902fd..84714565 100644 --- a/src/BLE_KickrBikeService.cpp +++ b/src/BLE_KickrBikeService.cpp @@ -8,6 +8,7 @@ #include "BLE_KickrBikeService.h" #include "DirConManager.h" #include "Main.h" +#include "BLE_Common.h" #include #include #include @@ -102,6 +103,8 @@ void BLE_KickrBikeService::setupService(NimBLEServer *pServer, MyCharacteristicC } void BLE_KickrBikeService::update() { + updateGearFromShifterPosition(); + // Send periodic keep-alive messages if handshake is complete if (isHandshakeComplete) { unsigned long currentTime = millis(); @@ -162,9 +165,10 @@ void BLE_KickrBikeService::applyGearChange() { static_cast(currentGear + 1), // 1-indexed gear number static_cast((getCurrentGearRatio() * 100)) // Ratio as percentage }; + // log the full TX of our gear status + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Gear status sent: Gear %d, Ratio %.2f", currentGear + 1, getCurrentGearRatio()); - asyncTxCharacteristic->setValue(gearStatus, sizeof(gearStatus)); - asyncTxCharacteristic->notify(); + spinBLEServer.notifyBleAndDircon(asyncTxCharacteristic, gearStatus, sizeof(gearStatus)); } void BLE_KickrBikeService::setBaseGradient(double gradientPercent) { @@ -316,6 +320,10 @@ void BLE_KickrBikeService::processWrite(const std::string& value) { size_t messageLength = value.length() - 1; switch (opcode) { + case 0x00: // INFO_REQUEST - device information query + handleInfoRequest(messageData, messageLength); + break; + case 0x04: // SET - Update trainer state handleSetRequest(messageData, messageLength); break; @@ -364,8 +372,7 @@ void BLE_KickrBikeService::sendRideOnResponse() { 0x01, 0x03 // Signature }; - syncTxCharacteristic->setValue(response, sizeof(response)); - syncTxCharacteristic->notify(); + spinBLEServer.notifyBleAndDircon(syncTxCharacteristic, response, sizeof(response)); SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sent RideOn response"); } @@ -382,8 +389,7 @@ void BLE_KickrBikeService::sendKeepAlive() { 0x9E, 0x49, 0x26, 0xFB, 0xE1 }; - syncTxCharacteristic->setValue(keepAliveData, sizeof(keepAliveData)); - syncTxCharacteristic->notify(); + spinBLEServer.notifyBleAndDircon(syncTxCharacteristic, keepAliveData, sizeof(keepAliveData)); SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sent keep-alive"); } @@ -412,8 +418,8 @@ void BLE_KickrBikeService::sendRideData() { appendVarintField(payload, 5, 0); appendVarintField(payload, 6, 0); - asyncTxCharacteristic->setValue(payload.data(), payload.size()); - asyncTxCharacteristic->notify(); + spinBLEServer.notifyBleAndDircon(asyncTxCharacteristic, payload.data(), payload.size()); + } // Opcode message handlers @@ -493,6 +499,51 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) sendStatusResponse(0x00); } +void BLE_KickrBikeService::handleInfoRequest(const uint8_t* data, size_t length) { + if (length == 0) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: INFO request with no data"); + sendStatusResponse(0x02); + return; + } + + uint32_t requestId = 0; + bool parsed = false; + + if (data[0] == 0x08 && length >= 2) { + size_t index = 1; + uint8_t shift = 0; + while (index < length) { + uint8_t byte = data[index++]; + requestId |= (static_cast(byte & 0x7F) << shift); + if ((byte & 0x80) == 0) { + parsed = true; + break; + } + shift += 7; + if (shift > 28) { + break; + } + } + } + + if (!parsed) { + String hexDump; + for (size_t i = 0; i < length; ++i) { + char buf[4]; + snprintf(buf, sizeof(buf), "%02X ", data[i]); + hexDump += buf; + } + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: INFO request unparsed payload: %s", hexDump.c_str()); + sendStatusResponse(0x02); + return; + } + + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: INFO request for id %lu", static_cast(requestId)); + + // We don't yet build the protobuf reply for these queries, but acknowledging keeps the protocol flowing. + sendStatusResponse(0x00); +} + void BLE_KickrBikeService::handleReset() { // RESET command - Reset the device to default state SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: RESET command received"); @@ -553,8 +604,7 @@ void BLE_KickrBikeService::sendGetResponse(uint16_t objectId, const uint8_t* dat response.insert(response.end(), data, data + length); } - syncTxCharacteristic->setValue(response.data(), response.size()); - syncTxCharacteristic->notify(); + spinBLEServer.notifyBleAndDircon(syncTxCharacteristic, response.data(), response.size()); SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sent GET_RESPONSE for object 0x%04X", objectId); } @@ -566,8 +616,7 @@ void BLE_KickrBikeService::sendStatusResponse(uint8_t status) { status // Status code (0x00 = success, others = error) }; - syncTxCharacteristic->setValue(response, sizeof(response)); - syncTxCharacteristic->notify(); + spinBLEServer.notifyBleAndDircon(syncTxCharacteristic, response, sizeof(response)); SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sent STATUS_RESPONSE (status: 0x%02X)", status); } diff --git a/src/BLE_Server.cpp b/src/BLE_Server.cpp index 1a8d1fc2..96fa761a 100644 --- a/src/BLE_Server.cpp +++ b/src/BLE_Server.cpp @@ -13,6 +13,7 @@ #include #include #include +#include "DirConManager.h" #include "BLE_Cycling_Speed_Cadence.h" #include "BLE_Cycling_Power_Service.h" #include "BLE_Heart_Service.h" @@ -61,7 +62,7 @@ void startBLEServer() { oServiceUUIDs.push_back(CSCSERVICE_UUID); oServiceUUIDs.push_back(CYCLINGPOWERSERVICE_UUID); oServiceUUIDs.push_back(HEARTSERVICE_UUID); - //oServiceUUIDs.push_back(FITNESSMACHINESERVICE_UUID); + oServiceUUIDs.push_back(FITNESSMACHINESERVICE_UUID); oServiceUUIDs.push_back(ZWIFT_RIDE_CUSTOM_SERVICE_UUID_SHORT); oAdvertisementData.setFlags(0x06); // General Discoverable, BR/EDR Not Supported oAdvertisementData.setCompleteServices16(oServiceUUIDs); @@ -94,6 +95,15 @@ void SpinBLEServer::update() { // sb20Service.notify(); } +void SpinBLEServer::notifyBleAndDircon(NimBLECharacteristic* pCharacteristic, const uint8_t* pData, int length) { + // Notify connected BLE clients + pCharacteristic->setValue(pData, length); + pCharacteristic->notify(); + + // Also notify DirCon clients if applicable + DirConManager::notifyCharacteristic(pCharacteristic->getService()->getUUID(), pCharacteristic->getUUID(), const_cast(pData), length); +} + double SpinBLEServer::calculateSpeed() { // Constants for the formula: adjusted for calibration const double dragCoefficient = 1.95; From 78be36f3a3351d34c7799489678f9b5801b11f61 Mon Sep 17 00:00:00 2001 From: Anthony Doud Date: Sat, 27 Dec 2025 19:49:40 -0600 Subject: [PATCH 03/12] almost there --- include/BLE_KickrBikeService.h | 3 +- src/BLE_Cycling_Power_Service.cpp | 2 +- src/BLE_Cycling_Speed_Cadence.cpp | 2 +- src/BLE_Heart_Service.cpp | 2 +- src/BLE_KickrBikeService.cpp | 190 +++++++++++++++++++++++++----- src/BLE_Server.cpp | 3 +- 6 files changed, 167 insertions(+), 35 deletions(-) diff --git a/include/BLE_KickrBikeService.h b/include/BLE_KickrBikeService.h index cc19b999..53ef5b82 100644 --- a/include/BLE_KickrBikeService.h +++ b/include/BLE_KickrBikeService.h @@ -53,6 +53,8 @@ class BLE_KickrBikeService { double getBaseGradient() const { return baseGradient; } double getEffectiveGradient() const; void applyGradientToTrainer(); + void applyGearChange(); + void applyGearChange(bool fromZwift); // Power control for ERG mode void setTargetPower(int watts); @@ -91,7 +93,6 @@ class BLE_KickrBikeService { static const double gearRatios[KICKR_BIKE_NUM_GEARS]; // Helper methods - void applyGearChange(); double calculateEffectiveGrade(double baseGrade, double gearRatio); bool isRideOnMessage(const std::string& data); void updateTrainerPosition(); diff --git a/src/BLE_Cycling_Power_Service.cpp b/src/BLE_Cycling_Power_Service.cpp index 9f3a3e37..1634ae03 100644 --- a/src/BLE_Cycling_Power_Service.cpp +++ b/src/BLE_Cycling_Power_Service.cpp @@ -30,7 +30,7 @@ void BLE_Cycling_Power_Service::setupService(NimBLEServer *pServer, MyCharacteri pPowerMonitor->start(); // Add service UUID to DirCon MDNS - DirConManager::addBleServiceUuid(pPowerMonitor->getUUID()); + //DirConManager::addBleServiceUuid(pPowerMonitor->getUUID()); } void BLE_Cycling_Power_Service::update() { diff --git a/src/BLE_Cycling_Speed_Cadence.cpp b/src/BLE_Cycling_Speed_Cadence.cpp index 85b1e157..66c9838f 100644 --- a/src/BLE_Cycling_Speed_Cadence.cpp +++ b/src/BLE_Cycling_Speed_Cadence.cpp @@ -26,7 +26,7 @@ void BLE_Cycling_Speed_Cadence::setupService(NimBLEServer* pServer, MyCharacteri pCyclingSpeedCadenceService->start(); // Add service UUID to DirCon MDNS - DirConManager::addBleServiceUuid(pCyclingSpeedCadenceService->getUUID()); + //DirConManager::addBleServiceUuid(pCyclingSpeedCadenceService->getUUID()); } void BLE_Cycling_Speed_Cadence::update() { diff --git a/src/BLE_Heart_Service.cpp b/src/BLE_Heart_Service.cpp index 44126962..3b7e252e 100644 --- a/src/BLE_Heart_Service.cpp +++ b/src/BLE_Heart_Service.cpp @@ -19,7 +19,7 @@ void BLE_Heart_Service::setupService(NimBLEServer *pServer, MyCharacteristicCall heartRateMeasurementCharacteristic->setCallbacks(chrCallbacks); pHeartService->start(); // Add service UUID to DirCon MDNS - DirConManager::addBleServiceUuid(pHeartService->getUUID()); + //DirConManager::addBleServiceUuid(pHeartService->getUUID()); } void BLE_Heart_Service::deinit() { diff --git a/src/BLE_KickrBikeService.cpp b/src/BLE_KickrBikeService.cpp index 84714565..d9dc885e 100644 --- a/src/BLE_KickrBikeService.cpp +++ b/src/BLE_KickrBikeService.cpp @@ -11,6 +11,7 @@ #include "BLE_Common.h" #include #include +#include #include namespace { @@ -31,6 +32,98 @@ inline void appendVarintField(std::vector& buffer, uint8_t fieldNumber, inline int32_t decodeZigZag32(uint32_t value) { return static_cast((value >> 1) ^ static_cast(-static_cast(value & 0x01))); } + +inline bool decodeVarint32(const uint8_t* data, size_t endIndex, size_t& index, uint32_t& result) { + result = 0; + uint32_t shift = 0; + while (index < endIndex) { + uint8_t byte = data[index++]; + result |= static_cast(byte & 0x7F) << shift; + if ((byte & 0x80) == 0) { + return true; + } + shift += 7; + if (shift >= 32) { + return false; // Overflow + } + } + return false; // Incomplete varint +} + +// Zwift Play opcodes/tokens derived from qdomyos-zwift reverse engineering. +constexpr uint8_t ZWIFT_OPCODE_GEAR_EVENT = 0x03; +constexpr uint8_t ZWIFT_OPCODE_GEAR_RESPONSE = 0x3C; +constexpr size_t ZWIFT_CHAINRING_COUNT = 2; +constexpr size_t ZWIFT_GEARS_PER_RING = + KICKR_BIKE_NUM_GEARS >= ZWIFT_CHAINRING_COUNT ? (KICKR_BIKE_NUM_GEARS / ZWIFT_CHAINRING_COUNT) : KICKR_BIKE_NUM_GEARS; + +struct GearProtoFields { + uint16_t token = 0; + uint8_t frontIndex = 0; + uint8_t rearIndex = 0; + uint8_t gearIndex = 0; +}; + +// Actual Zwift gear tokens captured from real Zwift communication. +// Gears 1-24 from easiest to hardest. +constexpr std::array zwiftGearTokens = { + 7500, 8700, 9900, 11100, 12300, 13800, 15300, 16800, + 18600, 20400, 22200, 24000, 26099, 28200, 30300, 32400, + 34900, 37400, 39900, 42399, 45400, 48400, 51400, 54899}; + +uint16_t gearTokenFromIndex(int gearIndex) { + if (gearIndex < 0 || gearIndex >= static_cast(zwiftGearTokens.size())) { + return 0; + } + return zwiftGearTokens[gearIndex]; +} + +inline int gearNumberFromInboundToken(uint32_t token) { + for (size_t i = 0; i < zwiftGearTokens.size(); ++i) { + if (zwiftGearTokens[i] == static_cast(token)) { + return static_cast(i) + 1; // Return 1-based gear number + } + } + return -1; // Unknown token +} + +GearProtoFields buildGearProtoFields(int gearIndex) { + GearProtoFields fields; + if (gearIndex < 0) { + return fields; + } + fields.token = gearTokenFromIndex(gearIndex); + if (fields.token == 0) { + return fields; + } + fields.gearIndex = static_cast(gearIndex + 1); + const size_t perRing = ZWIFT_GEARS_PER_RING == 0 ? 1 : ZWIFT_GEARS_PER_RING; + const size_t frontIdx = static_cast(gearIndex) / perRing; + const size_t rearIdx = static_cast(gearIndex) % perRing; + fields.frontIndex = static_cast(frontIdx + 1); + fields.rearIndex = static_cast(rearIdx + 1); + return fields; +} + +uint16_t lastReportedGearToken = 0; + +void emitGearFrame(NimBLECharacteristic* characteristic, const GearProtoFields& fields, uint8_t opcode) { + if (!characteristic || fields.token == 0) { + return; + } + + std::vector payload; + payload.reserve(16); + payload.push_back(opcode); + appendVarintField(payload, 1, fields.token); + appendVarintField(payload, 2, fields.frontIndex); + appendVarintField(payload, 3, fields.rearIndex); + appendVarintField(payload, 4, fields.gearIndex); + + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sending gear event opcode 0x%02X, token %u, size %d", opcode, fields.token, payload.size()); + + spinBLEServer.notifyBleAndDircon(characteristic, payload.data(), payload.size()); +} } // namespace // Gear ratio table: 24 gears from easiest (0.50) to hardest (1.65) @@ -97,7 +190,7 @@ void BLE_KickrBikeService::setupService(NimBLEServer *pServer, MyCharacteristicC pKickrBikeService->start(); // Add service UUID to DirCon MDNS (for discovery) - DirConManager::addBleServiceUuid(ZWIFT_RIDE_CUSTOM_SERVICE_UUID_SHORT); + // DirConManager::addBleServiceUuid(pKickrBikeService->getUUID()); SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE Service initialized with %d gears", KICKR_BIKE_NUM_GEARS); } @@ -151,6 +244,10 @@ double BLE_KickrBikeService::getCurrentGearRatio() const { } void BLE_KickrBikeService::applyGearChange() { + applyGearChange(false); +} + +void BLE_KickrBikeService::applyGearChange(bool fromZwift) { // Recalculate effective gradient with new gear effectiveGradient = calculateEffectiveGrade(baseGradient, getCurrentGearRatio()); @@ -158,17 +255,23 @@ void BLE_KickrBikeService::applyGearChange() { if (isEnabled) { applyGradientToTrainer(); } - - // Optionally notify clients about gear change via async TX characteristic - // This could be used to send gear status to connected apps - uint8_t gearStatus[2] = { - static_cast(currentGear + 1), // 1-indexed gear number - static_cast((getCurrentGearRatio() * 100)) // Ratio as percentage - }; - // log the full TX of our gear status - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Gear status sent: Gear %d, Ratio %.2f", currentGear + 1, getCurrentGearRatio()); - - spinBLEServer.notifyBleAndDircon(asyncTxCharacteristic, gearStatus, sizeof(gearStatus)); + + const GearProtoFields gearFields = buildGearProtoFields(currentGear); + if (gearFields.token != 0) { + // Only send gear event notification if we initiated the change (not Zwift) + if (!fromZwift) { + emitGearFrame(asyncTxCharacteristic, gearFields, ZWIFT_OPCODE_GEAR_EVENT); + } + lastReportedGearToken = gearFields.token; + const double ratio = getCurrentGearRatio(); + SS2K_LOG(BLE_SERVER_LOG_TAG, + "KICKR BIKE: Zwift Play gear token 0x%03X -> gear %u (front %u, rear %u, ratio %.2f)", + gearFields.token, + gearFields.gearIndex, + gearFields.frontIndex, + gearFields.rearIndex, + ratio); + } } void BLE_KickrBikeService::setBaseGradient(double gradientPercent) { @@ -266,12 +369,12 @@ void BLE_KickrBikeService::updateGearFromShifterPosition() { } bool BLE_KickrBikeService::isRideOnMessage(const std::string& data) { - // RideOn handshake prefix = 0x52 0x69 0x64 0x65 0x4f 0x6e + // RideOn handshake prefix = 0x52 0x69 0x64 0x65 0x4F 0x6E if (data.length() < 6) { return false; } - static const uint8_t rideOnPrefix[6] = {0x52, 0x69, 0x64, 0x65, 0x4f, 0x6e}; + static const uint8_t rideOnPrefix[6] = {0x52, 0x69, 0x64, 0x65, 0x4F, 0x6E}; for (size_t i = 0; i < 6; ++i) { if ((uint8_t)data[i] != rideOnPrefix[i]) { return false; @@ -368,7 +471,7 @@ void BLE_KickrBikeService::processWrite(const std::string& value) { void BLE_KickrBikeService::sendRideOnResponse() { // Respond with "RideOn" + signature bytes (0x01 0x03) uint8_t response[8] = { - 0x52, 0x69, 0x64, 0x65, 0x4f, 0x6e, // "RideOn" + 0x52, 0x69, 0x64, 0x65, 0x4F, 0x6E, // "RideOn" 0x01, 0x03 // Signature }; @@ -456,27 +559,54 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) return; } - // Zwift sends gradient updates as: 0x22 0x10 + // Zwift gear select (from some controllers) arrives as: + // 2A 10 + if (data[0] == 0x2A && data[1] >= 2) { + const uint8_t payloadLen = data[1]; + const size_t payloadEnd = 2 + payloadLen; + if (payloadEnd <= length && data[2] == 0x10) { + size_t index = 3; + uint32_t token = 0; + + if (!decodeVarint32(data, payloadEnd, index, token)) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET gear token varint overflow"); + sendStatusResponse(0x02); + return; + } + + const int gearNumber = gearNumberFromInboundToken(token); + if (gearNumber > 0) { + // Sync internal + external representation immediately (avoid incremental shifting logic). + currentGear = std::clamp(gearNumber - 1, 0, KICKR_BIKE_NUM_GEARS - 1); + lastShifterPosition = gearNumber; + rtConfig->setShifterPosition(gearNumber); + applyGearChange(true); // fromZwift = true, don't echo back + + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET gear token %lu -> gear %d", static_cast(token), gearNumber); + sendStatusResponse(0x00); + return; + } + + SS2K_LOG(BLE_SERVER_LOG_TAG, + "KICKR BIKE: SET unknown gear token %lu (add to zwiftInboundGearTokensObserved)", + static_cast(token)); + sendStatusResponse(0x00); + return; + } + } + + // Zwift sends gradient updates as: 0x22 0x10 if (data[0] == 0x22 && data[1] >= 2) { uint8_t payloadLen = data[1]; size_t payloadEnd = 2 + payloadLen; if (payloadEnd <= length && data[2] == 0x10) { size_t index = 3; uint32_t rawValue = 0; - uint8_t shift = 0; - - while (index < payloadEnd) { - uint8_t byte = data[index++]; - rawValue |= (static_cast(byte & 0x7F) << shift); - if ((byte & 0x80) == 0) { - break; - } - shift += 7; - if (shift > 28) { - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET gradient varint overflow"); - sendStatusResponse(0x02); - return; - } + + if (!decodeVarint32(data, payloadEnd, index, rawValue)) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET gradient varint overflow"); + sendStatusResponse(0x02); + return; } int32_t signedValue = decodeZigZag32(rawValue); diff --git a/src/BLE_Server.cpp b/src/BLE_Server.cpp index 96fa761a..e8141932 100644 --- a/src/BLE_Server.cpp +++ b/src/BLE_Server.cpp @@ -63,7 +63,8 @@ void startBLEServer() { oServiceUUIDs.push_back(CYCLINGPOWERSERVICE_UUID); oServiceUUIDs.push_back(HEARTSERVICE_UUID); oServiceUUIDs.push_back(FITNESSMACHINESERVICE_UUID); - oServiceUUIDs.push_back(ZWIFT_RIDE_CUSTOM_SERVICE_UUID_SHORT); + //oServiceUUIDs.push_back(ZWIFT_RIDE_SERVICE_UUID); + //oServiceUUIDs.push_back(ZWIFT_RIDE_CUSTOM_SERVICE_UUID_SHORT); oAdvertisementData.setFlags(0x06); // General Discoverable, BR/EDR Not Supported oAdvertisementData.setCompleteServices16(oServiceUUIDs); pAdvertising->setAdvertisementData(oAdvertisementData); From 9b46a3049084432be5d4866fddd300f510bba8d5 Mon Sep 17 00:00:00 2001 From: Anthony Doud Date: Sat, 27 Dec 2025 23:18:52 -0600 Subject: [PATCH 04/12] wip --- lib/SS2K/include/Constants.h | 2 +- src/BLE_Cycling_Power_Service.cpp | 2 +- src/BLE_Cycling_Speed_Cadence.cpp | 2 +- src/BLE_Fitness_Machine_Service.cpp | 2 +- src/BLE_Heart_Service.cpp | 2 +- src/BLE_KickrBikeService.cpp | 103 +++++++++++++++++++++++----- src/BLE_Server.cpp | 3 +- src/DirConManager.cpp | 38 ++-------- 8 files changed, 98 insertions(+), 56 deletions(-) diff --git a/lib/SS2K/include/Constants.h b/lib/SS2K/include/Constants.h index 66e016f5..c78dc41f 100644 --- a/lib/SS2K/include/Constants.h +++ b/lib/SS2K/include/Constants.h @@ -100,7 +100,7 @@ #define ZWIFT_SYNC_TX_UUID NimBLEUUID("00000004-19CA-4651-86E5-FA29DCDD09D1") // Notify characteristic #define ZWIFT_DEBUG_CHARACTERISTIC_UUID NimBLEUUID("00000005-19CA-4651-86E5-FA29DCDD09D1") #define ZWIFT_UNKNOWN_6_CHARACTERISTIC_UUID NimBLEUUID("00000006-19CA-4651-86E5-FA29DCDD09D1") -#define ZWIFT_RIDE_CUSTOM_SERVICE_UUID_SHORT NimBLEUUID("FC82") +#define ZWIFT_RIDE_CUSTOM_SERVICE_UUID_SHORT NimBLEUUID((uint16_t)0xFC82) // BLE HID #define APPEARANCE_HID_GENERIC_UUID NimBLEUUID((uint16_t)0x3C0) diff --git a/src/BLE_Cycling_Power_Service.cpp b/src/BLE_Cycling_Power_Service.cpp index 1634ae03..9f3a3e37 100644 --- a/src/BLE_Cycling_Power_Service.cpp +++ b/src/BLE_Cycling_Power_Service.cpp @@ -30,7 +30,7 @@ void BLE_Cycling_Power_Service::setupService(NimBLEServer *pServer, MyCharacteri pPowerMonitor->start(); // Add service UUID to DirCon MDNS - //DirConManager::addBleServiceUuid(pPowerMonitor->getUUID()); + DirConManager::addBleServiceUuid(pPowerMonitor->getUUID()); } void BLE_Cycling_Power_Service::update() { diff --git a/src/BLE_Cycling_Speed_Cadence.cpp b/src/BLE_Cycling_Speed_Cadence.cpp index 66c9838f..85b1e157 100644 --- a/src/BLE_Cycling_Speed_Cadence.cpp +++ b/src/BLE_Cycling_Speed_Cadence.cpp @@ -26,7 +26,7 @@ void BLE_Cycling_Speed_Cadence::setupService(NimBLEServer* pServer, MyCharacteri pCyclingSpeedCadenceService->start(); // Add service UUID to DirCon MDNS - //DirConManager::addBleServiceUuid(pCyclingSpeedCadenceService->getUUID()); + DirConManager::addBleServiceUuid(pCyclingSpeedCadenceService->getUUID()); } void BLE_Cycling_Speed_Cadence::update() { diff --git a/src/BLE_Fitness_Machine_Service.cpp b/src/BLE_Fitness_Machine_Service.cpp index 3b027e54..65e4c833 100644 --- a/src/BLE_Fitness_Machine_Service.cpp +++ b/src/BLE_Fitness_Machine_Service.cpp @@ -57,7 +57,7 @@ void BLE_Fitness_Machine_Service::setupService(NimBLEServer *pServer, MyCharacte pFitnessMachineService->start(); // Add service UUID to DirCon MDNS - DirConManager::addBleServiceUuid(pFitnessMachineService->getUUID()); + // DirConManager::addBleServiceUuid(pFitnessMachineService->getUUID()); } void BLE_Fitness_Machine_Service::update() { diff --git a/src/BLE_Heart_Service.cpp b/src/BLE_Heart_Service.cpp index 3b7e252e..44126962 100644 --- a/src/BLE_Heart_Service.cpp +++ b/src/BLE_Heart_Service.cpp @@ -19,7 +19,7 @@ void BLE_Heart_Service::setupService(NimBLEServer *pServer, MyCharacteristicCall heartRateMeasurementCharacteristic->setCallbacks(chrCallbacks); pHeartService->start(); // Add service UUID to DirCon MDNS - //DirConManager::addBleServiceUuid(pHeartService->getUUID()); + DirConManager::addBleServiceUuid(pHeartService->getUUID()); } void BLE_Heart_Service::deinit() { diff --git a/src/BLE_KickrBikeService.cpp b/src/BLE_KickrBikeService.cpp index d9dc885e..70ddfa86 100644 --- a/src/BLE_KickrBikeService.cpp +++ b/src/BLE_KickrBikeService.cpp @@ -190,7 +190,7 @@ void BLE_KickrBikeService::setupService(NimBLEServer *pServer, MyCharacteristicC pKickrBikeService->start(); // Add service UUID to DirCon MDNS (for discovery) - // DirConManager::addBleServiceUuid(pKickrBikeService->getUUID()); + DirConManager::addBleServiceUuid(ZWIFT_RIDE_CUSTOM_SERVICE_UUID_SHORT); SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE Service initialized with %d gears", KICKR_BIKE_NUM_GEARS); } @@ -414,6 +414,14 @@ void BLE_KickrBikeService::processWrite(const std::string& value) { sendRideOnResponse(); isHandshakeComplete = true; lastKeepAliveTime = millis(); + + // Send initial gear state to the app + const GearProtoFields gearFields = buildGearProtoFields(currentGear); + if (gearFields.token != 0) { + emitGearFrame(asyncTxCharacteristic, gearFields, ZWIFT_OPCODE_GEAR_EVENT); + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sent initial gear state: gear %d", currentGear + 1); + } + return; } @@ -513,16 +521,15 @@ void BLE_KickrBikeService::sendRideData() { std::vector payload; payload.reserve(20); - payload.push_back(0x03); + payload.push_back(0x03); // Opcode for HubRidingData appendVarintField(payload, 1, static_cast(power)); appendVarintField(payload, 2, static_cast(cadence)); appendVarintField(payload, 3, speedX100); appendVarintField(payload, 4, static_cast(heartRate)); - appendVarintField(payload, 5, 0); - appendVarintField(payload, 6, 0); + appendVarintField(payload, 5, 0); // Unknown field (possibly flags or state) + appendVarintField(payload, 6, static_cast(currentGear + 1)); // Current gear (1-based) spinBLEServer.notifyBleAndDircon(asyncTxCharacteristic, payload.data(), payload.size()); - } // Opcode message handlers @@ -595,27 +602,87 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) } } - // Zwift sends gradient updates as: 0x22 0x10 + // Zwift/Rouvy send simulation parameters as: + // 0x22 [0x08 ] [0x10 ] [0x18 ] [0x20 ] if (data[0] == 0x22 && data[1] >= 2) { uint8_t payloadLen = data[1]; size_t payloadEnd = 2 + payloadLen; - if (payloadEnd <= length && data[2] == 0x10) { - size_t index = 3; - uint32_t rawValue = 0; + if (payloadEnd > length) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET simulation params payload truncated"); + sendStatusResponse(0x02); + return; + } - if (!decodeVarint32(data, payloadEnd, index, rawValue)) { - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET gradient varint overflow"); - sendStatusResponse(0x02); - return; + size_t index = 2; + uint32_t powerTarget = 0; + uint32_t gradeRaw = 0; + uint32_t windSpeed = 0; + uint32_t rollingResistance = 0; + bool hasPower = false; + bool hasGrade = false; + + // Parse all fields in the simulation message + while (index < payloadEnd) { + uint8_t fieldTag = data[index++]; + + switch (fieldTag) { + case 0x08: // Field 1: Power target (watts) + if (!decodeVarint32(data, payloadEnd, index, powerTarget)) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET power varint overflow"); + sendStatusResponse(0x02); + return; + } + hasPower = true; + break; + + case 0x10: // Field 2: Grade (zigzag encoded, value * 100) + if (!decodeVarint32(data, payloadEnd, index, gradeRaw)) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET grade varint overflow"); + sendStatusResponse(0x02); + return; + } + hasGrade = true; + break; + + case 0x18: // Field 3: Wind speed (m/s * 100, zigzag?) + if (!decodeVarint32(data, payloadEnd, index, windSpeed)) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET wind varint overflow"); + sendStatusResponse(0x02); + return; + } + break; + + case 0x20: // Field 4: Rolling resistance coefficient + if (!decodeVarint32(data, payloadEnd, index, rollingResistance)) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET rolling resistance varint overflow"); + sendStatusResponse(0x02); + return; + } + break; + + default: + // Unknown field, skip it + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET unknown simulation field tag 0x%02X", fieldTag); + break; } + } - int32_t signedValue = decodeZigZag32(rawValue); - double gradientPercent = static_cast(signedValue) / 100.0; - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET gradient to %.2f%% (raw %ld)", gradientPercent, static_cast(signedValue)); + // Apply grade if present + if (hasGrade) { + int32_t signedGrade = decodeZigZag32(gradeRaw); + double gradientPercent = static_cast(signedGrade) / 100.0; + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET gradient to %.2f%%", gradientPercent); setBaseGradient(gradientPercent); - sendStatusResponse(0x00); - return; } + + // Log other parameters for debugging + if (hasPower) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET power target %lu W (ignored in SIM mode)", + static_cast(powerTarget)); + } + + sendStatusResponse(0x00); + return; } // Unknown SET payload; acknowledge to keep protocol flowing but log for future decoding. diff --git a/src/BLE_Server.cpp b/src/BLE_Server.cpp index e8141932..051e24fa 100644 --- a/src/BLE_Server.cpp +++ b/src/BLE_Server.cpp @@ -54,8 +54,9 @@ void startBLEServer() { cyclingSpeedCadenceService.setupService(spinBLEServer.pServer, &chrCallbacks); cyclingPowerService.setupService(spinBLEServer.pServer, &chrCallbacks); heartService.setupService(spinBLEServer.pServer, &chrCallbacks); - fitnessMachineService.setupService(spinBLEServer.pServer, &chrCallbacks); + // Initialize KICKR BIKE before FTMS so it gets priority in DirCon discovery kickrBikeService.setupService(spinBLEServer.pServer, &chrCallbacks); + fitnessMachineService.setupService(spinBLEServer.pServer, &chrCallbacks); ss2kCustomCharacteristic.setupService(spinBLEServer.pServer); deviceInformationService.setupService(spinBLEServer.pServer); //add all service UUIDs to advertisement vector diff --git a/src/DirConManager.cpp b/src/DirConManager.cpp index 71d002b0..90ea66fc 100644 --- a/src/DirConManager.cpp +++ b/src/DirConManager.cpp @@ -570,18 +570,11 @@ std::vector DirConManager::getAvailableServices() { if (!servicesInitialized) { cachedServices.clear(); - // Add each service with descriptive name for better debugging - NimBLEUUID cyclingPowerUuid = NimBLEUUID(CYCLINGPOWERSERVICE_UUID); - cachedServices.push_back(cyclingPowerUuid); - - NimBLEUUID cscUuid = NimBLEUUID(CSCSERVICE_UUID); - cachedServices.push_back(cscUuid); - - NimBLEUUID heartUuid = NimBLEUUID(HEARTSERVICE_UUID); - cachedServices.push_back(heartUuid); - - NimBLEUUID ftmsUuid = NimBLEUUID(FITNESSMACHINESERVICE_UUID); - cachedServices.push_back(ftmsUuid); + cachedServices.push_back(NimBLEUUID(ZWIFT_RIDE_SERVICE_UUID)); + cachedServices.push_back(NimBLEUUID(CYCLINGPOWERSERVICE_UUID)); + cachedServices.push_back(NimBLEUUID(CSCSERVICE_UUID)); + cachedServices.push_back(NimBLEUUID(HEARTSERVICE_UUID)); + //cachedServices.push_back(NimBLEUUID(FITNESSMACHINESERVICE_UUID)); // Log summary SS2K_LOG(DIRCON_LOG_TAG, "Initialized service discovery with %d services", cachedServices.size()); @@ -604,26 +597,7 @@ std::vector DirConManager::getCharacteristics(const NimBL characteristics.push_back(const_cast(characteristic)); } } - // Find service-specific characteristics based on known UUIDs - /*if (serviceUuid.equals(CYCLINGPOWERSERVICE_UUID)) { - characteristics.push_back(service->getCharacteristic(CYCLINGPOWERMEASUREMENT_UUID)); - characteristics.push_back(service->getCharacteristic(CYCLINGPOWERFEATURE_UUID)); - characteristics.push_back(service->getCharacteristic(SENSORLOCATION_UUID)); - } else if (serviceUuid.equals(CSCSERVICE_UUID)) { - characteristics.push_back(service->getCharacteristic(CSCMEASUREMENT_UUID)); - } else if (serviceUuid.equals(HEARTSERVICE_UUID)) { - characteristics.push_back(service->getCharacteristic(HEARTCHARACTERISTIC_UUID)); - } else if (serviceUuid.equals(FITNESSMACHINESERVICE_UUID)) { - characteristics.push_back(service->getCharacteristic(FITNESSMACHINEINDOORBIKEDATA_UUID)); - characteristics.push_back(service->getCharacteristic(FITNESSMACHINEFEATURE_UUID)); - characteristics.push_back(service->getCharacteristic(FITNESSMACHINECONTROLPOINT_UUID)); - characteristics.push_back(service->getCharacteristic(FITNESSMACHINESTATUS_UUID)); - } else if (serviceUuid.equals(DEVICE_INFORMATION_SERVICE_UUID)) { - // Add device info characteristics if needed - } else if (serviceUuid.equals(WATTBIKE_SERVICE_UUID)) { - // Add wattbike service characteristics - } -*/ + // Filter out null characteristics auto it = std::remove_if(characteristics.begin(), characteristics.end(), [](NimBLECharacteristic* c) { return c == nullptr; }); characteristics.erase(it, characteristics.end()); From ffdf20d72b6797c94df4356502491f39226485ff Mon Sep 17 00:00:00 2001 From: Anthony Doud Date: Sun, 28 Dec 2025 14:15:17 -0600 Subject: [PATCH 05/12] sync or async? --- src/BLE_KickrBikeService.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/BLE_KickrBikeService.cpp b/src/BLE_KickrBikeService.cpp index 70ddfa86..3e44674e 100644 --- a/src/BLE_KickrBikeService.cpp +++ b/src/BLE_KickrBikeService.cpp @@ -258,9 +258,10 @@ void BLE_KickrBikeService::applyGearChange(bool fromZwift) { const GearProtoFields gearFields = buildGearProtoFields(currentGear); if (gearFields.token != 0) { - // Only send gear event notification if we initiated the change (not Zwift) + // Only send gear response notification if we initiated the change (not Zwift) + // Use GEAR_RESPONSE opcode (0x3C) like real KICKR BIKE, not GEAR_EVENT (0x03) if (!fromZwift) { - emitGearFrame(asyncTxCharacteristic, gearFields, ZWIFT_OPCODE_GEAR_EVENT); + emitGearFrame(syncTxCharacteristic, gearFields, ZWIFT_OPCODE_GEAR_RESPONSE); } lastReportedGearToken = gearFields.token; const double ratio = getCurrentGearRatio(); @@ -418,7 +419,7 @@ void BLE_KickrBikeService::processWrite(const std::string& value) { // Send initial gear state to the app const GearProtoFields gearFields = buildGearProtoFields(currentGear); if (gearFields.token != 0) { - emitGearFrame(asyncTxCharacteristic, gearFields, ZWIFT_OPCODE_GEAR_EVENT); + emitGearFrame(syncTxCharacteristic, gearFields, ZWIFT_OPCODE_GEAR_EVENT); SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sent initial gear state: gear %d", currentGear + 1); } From ae53db69d04b5b06ce3ca93ab1a85a3d2c8b28c0 Mon Sep 17 00:00:00 2001 From: Anthony Doud Date: Tue, 30 Dec 2025 16:51:16 -0800 Subject: [PATCH 06/12] - More ERG tweaks for Marc Roy. --- CHANGELOG.md | 3 ++ include/ERG_Mode.h | 4 +-- src/ERG_Mode.cpp | 72 ++++++++++++++++++++------------------ src/PowerTable_Helpers.cpp | 3 +- src/SensorCollector.cpp | 14 ++++---- 5 files changed, 51 insertions(+), 45 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 27122624..05822c45 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,6 +11,9 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed - Removed >0 watts requirement to compute ERG. +- Added proper rounding from float to int for power and cadence. +- More ERG tweaks for Marc Roy. +- If homed, we throw out negative PowerTable returns. ### Hardware diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h index aece7a91..c01c5650 100644 --- a/include/ERG_Mode.h +++ b/include/ERG_Mode.h @@ -35,10 +35,10 @@ class ErgMode { bool _userIsSpinning(int cadence, float incline); // calculate incline if setpoint (from Zwift) changes - void _setPointChangeState(int newCadence, Measurement& newWatts); + int32_t _setPointChangeState(int newCadence, Measurement& newWatts); // calculate incline if setpoint is unchanged - void _inSetpointState(int newCadence, Measurement& newWatts); + int32_t _inSetpointState(int newCadence, Measurement& newWatts); // update localvalues + incline, creates a log void _updateValues(int newCadence, Measurement& newWatts, float newIncline); diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp index cdc7974e..b7c663a0 100644 --- a/src/ERG_Mode.cpp +++ b/src/ERG_Mode.cpp @@ -121,6 +121,7 @@ void ErgMode::runERG() { void ErgMode::computeErg() { Measurement newWatts = rtConfig->watts; int newCadence = rtConfig->cad.getValue(); + int32_t result = RETURN_ERROR; bool isUserSpinning = this->_userIsSpinning(newCadence, ss2k->getCurrentPosition()); if (!isUserSpinning) { @@ -141,29 +142,29 @@ void ErgMode::computeErg() { } #ifdef ERG_MODE_USE_POWER_TABLE -// SetPoint changed -#ifdef ERG_MODE_USE_PID if (abs(this->setPoint - newWatts.getTarget()) > ERG_MODE_PID_WINDOW && rtConfig->getHomed()) { -#endif - _setPointChangeState(newCadence, newWatts); - return; -#ifdef ERG_MODE_USE_PID + result = _setPointChangeState(newCadence, newWatts); } #endif -#endif - #ifdef ERG_MODE_USE_PID // Setpoint unchanged - _inSetpointState(newCadence, newWatts); + if (result == INT32_MIN) { + result = _inSetpointState(newCadence, newWatts); + } #endif + _updateValues(newCadence, newWatts, result); } -void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) { - // It's better to undershoot increasing watts and overshoot decreasing watts, so lets set the lookup target to the nearest side of ERG_MODE_PID_WINDOW - int adjustedTarget = newWatts.getTarget() >= newWatts.getValue() ? newWatts.getTarget() - ERG_MODE_PID_WINDOW : newWatts.getTarget() + ERG_MODE_PID_WINDOW; - +int32_t ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) { + // It's better to undershoot increasing watts and overshoot decreasing watts, so lets set the lookup target to the nearest side of POWERTABLE_WATT_INCREMENT + int adjustedTarget = newWatts.getTarget() >= newWatts.getValue() ? newWatts.getTarget() - POWERTABLE_WATT_INCREMENT : newWatts.getTarget() + POWERTABLE_WATT_INCREMENT; int32_t tableResult = powerTable->lookup(adjustedTarget, newCadence); + // A lot of times this likes to undershoot going from High to low. Lets fix it. + if (adjustedTarget < newWatts.getValue() && adjustedTarget < 200) { + adjustedTarget = (adjustedTarget + ss2k->getCurrentPosition()) / 2; + } + // Test current watts against the table result. If We're already lower or higher than target, flag the result as a return error. if (tableResult != RETURN_ERROR) { if (rtConfig->watts.getValue() > adjustedTarget && tableResult > ss2k->getCurrentPosition()) { @@ -176,26 +177,32 @@ void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) { } } + // Sanity check - with homing enabled, we should never have a negative result. If we do, something went wrong. + if (rtConfig->getHomed() && tableResult < 0) { + SS2K_LOG(ERG_MODE_LOG_TAG, "PowerTable returned negative result with homing enabled. Using PID"); + tableResult = RETURN_ERROR; + } + // Handle return errors if (tableResult == RETURN_ERROR) { SS2K_LOG(ERG_MODE_LOG_TAG, "Lookup Error. Using PID"); - _inSetpointState(newCadence, newWatts); - return; - } - - SS2K_LOG(ERG_MODE_LOG_TAG, "SetPoint changed:%dw PowerTable Result: %d", adjustedTarget, tableResult); - _updateValues(newCadence, newWatts, tableResult); - - if (rtConfig->getTargetIncline() != ss2k->getCurrentPosition()) { // add some time to wait while the knob moves to target position. - isDelayed = true; - int timeToAdd = abs(ss2k->getCurrentPosition() - rtConfig->getTargetIncline()); - if (timeToAdd > 3000) { // 3 seconds - SS2K_LOG(ERG_MODE_LOG_TAG, "Capping ERG seek time to 3 seconds"); - timeToAdd = 3000; + tableResult = _inSetpointState(newCadence, newWatts); + } else { + SS2K_LOG(ERG_MODE_LOG_TAG, "SetPoint changed:%dw PowerTable Result: %d", adjustedTarget, tableResult); + if (tableResult != ss2k->getCurrentPosition()) { // add some time to wait while the knob moves to target position. + isDelayed = true; + long int stepDistance = abs(ss2k->getCurrentPosition() - tableResult); + // Calculate time to add based on step distance and stepper speed + long int timeToAdd = round(((double)stepDistance * 1000.0) / (double)userConfig->getStepperSpeed()); + if (timeToAdd > 3000) { // 3 seconds + SS2K_LOG(ERG_MODE_LOG_TAG, "Capping ERG seek time to 3 seconds"); + timeToAdd = 3000; + } + ergTimer += timeToAdd; } - ergTimer += timeToAdd; + ergTimer += (ERG_MODE_DELAY); // Wait for power meter to register new watts } - ergTimer += (ERG_MODE_DELAY); // Wait for power meter to register new watts + return tableResult; } // INTRODUCING PID CONTROL LOOP @@ -206,7 +213,7 @@ void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) { // Derivative term: rate of change of error // PrevError -void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) { +int32_t ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) { // Setting Gains For PID Loop float Kp = userConfig->getERGSensitivity(); float Ki = 0.5; @@ -258,11 +265,8 @@ void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) { // Calculate new incline float newIncline = ss2k->getCurrentPosition() + PID_output; - - prevError = error; - - // Apply the new values - _updateValues(newCadence, newWatts, newIncline); + prevError = error; + return newIncline; } void ErgMode::_updateValues(int newCadence, Measurement& newWatts, float newIncline) { diff --git a/src/PowerTable_Helpers.cpp b/src/PowerTable_Helpers.cpp index 6a71970c..e1227f94 100644 --- a/src/PowerTable_Helpers.cpp +++ b/src/PowerTable_Helpers.cpp @@ -141,6 +141,7 @@ int32_t PTHelpers::lookup(int watts, int cad, PTData& ptData) { } } } + if (resistance != RETURN_ERROR) { SS2K_LOG(PTDATA_LOG_TAG, "Extrapolated resistance: %d for watts=%d, cad=%d", resistance, watts, cad); // Return early if we found a valid extrapolated value @@ -148,7 +149,7 @@ int32_t PTHelpers::lookup(int watts, int cad, PTData& ptData) { SS2K_LOG(PTDATA_LOG_TAG, "Extrapolation failed for watts=%d, cad=%d", watts, cad); } - return resistance; // All lookup methods failed + return resistance; } /** diff --git a/src/SensorCollector.cpp b/src/SensorCollector.cpp index cfbff994..356b126a 100644 --- a/src/SensorCollector.cpp +++ b/src/SensorCollector.cpp @@ -58,7 +58,7 @@ void collectAndSet(NimBLEUUID charUUID, NimBLEUUID serviceUUID, std::string& uni if ((charUUID == PELOTON_DATA_UUID) && !(strcmp(userConfig->getConnectedPowerMeter(), NONE) == 0 || strcmp(userConfig->getConnectedPowerMeter(), ANY) == 0)) { // Peloton connected but using BLE Power Meter. So skip cad for Peloton UUID. } else { - float cadence = sensorData->getCadence(); + int cadence = round(sensorData->getCadence()); rtConfig->cad.setValue(cadence); spinBLEClient.connectedCD = true; logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " CD(%.2f)", fmodf(cadence, 1000.0)); @@ -69,7 +69,7 @@ void collectAndSet(NimBLEUUID charUUID, NimBLEUUID serviceUUID, std::string& uni if ((charUUID == PELOTON_DATA_UUID) && !((strcmp(userConfig->getConnectedPowerMeter(), NONE) == 0) || (strcmp(userConfig->getConnectedPowerMeter(), ANY) == 0))) { // Peloton connected but using BLE Power Meter. So skip power for Peloton UUID. } else { - int power = sensorData->getPower() * userConfig->getPowerCorrectionFactor(); + int power = round(sensorData->getPower() * userConfig->getPowerCorrectionFactor()); rtConfig->watts.setValue(power); spinBLEClient.connectedPM = true; logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " PW(%d)", power % 10000); @@ -77,10 +77,9 @@ void collectAndSet(NimBLEUUID charUUID, NimBLEUUID serviceUUID, std::string& uni } if (sensorData->hasSpeed()) { - float speed = sensorData->getSpeed(); - rtConfig->setSimulatedSpeed(speed); + rtConfig->setSimulatedSpeed(sensorData->getSpeed()); spinBLEClient.connectedSpeed = true; - logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " SD(%.2f)", fmodf(speed, 1000.0)); + logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " SD(%.2f)", fmodf(sensorData->getSpeed(), 1000.0)); } if (sensorData->hasResistance()) { @@ -88,9 +87,8 @@ void collectAndSet(NimBLEUUID charUUID, NimBLEUUID serviceUUID, std::string& uni if ((ss2k->pelotonIsConnected) && (charUUID != PELOTON_DATA_UUID)) { // Peloton connected but using BLE Power Meter. So skip resistance for UUID's that aren't Peloton. } else { - int resistance = sensorData->getResistance(); - rtConfig->resistance.setValue(resistance); - logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " RS(%d)", resistance % 1000); + rtConfig->resistance.setValue(sensorData->getResistance()); + logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " RS(%d)", sensorData->getResistance() % 1000); } } From 0161d052750f261ff2c7fbdfe69cb5c94c8b4ead Mon Sep 17 00:00:00 2001 From: Anthony Doud Date: Tue, 30 Dec 2025 17:35:01 -0800 Subject: [PATCH 07/12] Revert "- More ERG tweaks for Marc Roy." This reverts commit ae53db69d04b5b06ce3ca93ab1a85a3d2c8b28c0. --- CHANGELOG.md | 3 -- include/ERG_Mode.h | 4 +-- src/ERG_Mode.cpp | 72 ++++++++++++++++++-------------------- src/PowerTable_Helpers.cpp | 3 +- src/SensorCollector.cpp | 14 ++++---- 5 files changed, 45 insertions(+), 51 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 05822c45..27122624 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,9 +11,6 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed - Removed >0 watts requirement to compute ERG. -- Added proper rounding from float to int for power and cadence. -- More ERG tweaks for Marc Roy. -- If homed, we throw out negative PowerTable returns. ### Hardware diff --git a/include/ERG_Mode.h b/include/ERG_Mode.h index c01c5650..aece7a91 100644 --- a/include/ERG_Mode.h +++ b/include/ERG_Mode.h @@ -35,10 +35,10 @@ class ErgMode { bool _userIsSpinning(int cadence, float incline); // calculate incline if setpoint (from Zwift) changes - int32_t _setPointChangeState(int newCadence, Measurement& newWatts); + void _setPointChangeState(int newCadence, Measurement& newWatts); // calculate incline if setpoint is unchanged - int32_t _inSetpointState(int newCadence, Measurement& newWatts); + void _inSetpointState(int newCadence, Measurement& newWatts); // update localvalues + incline, creates a log void _updateValues(int newCadence, Measurement& newWatts, float newIncline); diff --git a/src/ERG_Mode.cpp b/src/ERG_Mode.cpp index b7c663a0..cdc7974e 100644 --- a/src/ERG_Mode.cpp +++ b/src/ERG_Mode.cpp @@ -121,7 +121,6 @@ void ErgMode::runERG() { void ErgMode::computeErg() { Measurement newWatts = rtConfig->watts; int newCadence = rtConfig->cad.getValue(); - int32_t result = RETURN_ERROR; bool isUserSpinning = this->_userIsSpinning(newCadence, ss2k->getCurrentPosition()); if (!isUserSpinning) { @@ -142,28 +141,28 @@ void ErgMode::computeErg() { } #ifdef ERG_MODE_USE_POWER_TABLE +// SetPoint changed +#ifdef ERG_MODE_USE_PID if (abs(this->setPoint - newWatts.getTarget()) > ERG_MODE_PID_WINDOW && rtConfig->getHomed()) { - result = _setPointChangeState(newCadence, newWatts); +#endif + _setPointChangeState(newCadence, newWatts); + return; +#ifdef ERG_MODE_USE_PID } #endif +#endif + #ifdef ERG_MODE_USE_PID // Setpoint unchanged - if (result == INT32_MIN) { - result = _inSetpointState(newCadence, newWatts); - } + _inSetpointState(newCadence, newWatts); #endif - _updateValues(newCadence, newWatts, result); } -int32_t ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) { - // It's better to undershoot increasing watts and overshoot decreasing watts, so lets set the lookup target to the nearest side of POWERTABLE_WATT_INCREMENT - int adjustedTarget = newWatts.getTarget() >= newWatts.getValue() ? newWatts.getTarget() - POWERTABLE_WATT_INCREMENT : newWatts.getTarget() + POWERTABLE_WATT_INCREMENT; - int32_t tableResult = powerTable->lookup(adjustedTarget, newCadence); +void ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) { + // It's better to undershoot increasing watts and overshoot decreasing watts, so lets set the lookup target to the nearest side of ERG_MODE_PID_WINDOW + int adjustedTarget = newWatts.getTarget() >= newWatts.getValue() ? newWatts.getTarget() - ERG_MODE_PID_WINDOW : newWatts.getTarget() + ERG_MODE_PID_WINDOW; - // A lot of times this likes to undershoot going from High to low. Lets fix it. - if (adjustedTarget < newWatts.getValue() && adjustedTarget < 200) { - adjustedTarget = (adjustedTarget + ss2k->getCurrentPosition()) / 2; - } + int32_t tableResult = powerTable->lookup(adjustedTarget, newCadence); // Test current watts against the table result. If We're already lower or higher than target, flag the result as a return error. if (tableResult != RETURN_ERROR) { @@ -177,32 +176,26 @@ int32_t ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) { } } - // Sanity check - with homing enabled, we should never have a negative result. If we do, something went wrong. - if (rtConfig->getHomed() && tableResult < 0) { - SS2K_LOG(ERG_MODE_LOG_TAG, "PowerTable returned negative result with homing enabled. Using PID"); - tableResult = RETURN_ERROR; - } - // Handle return errors if (tableResult == RETURN_ERROR) { SS2K_LOG(ERG_MODE_LOG_TAG, "Lookup Error. Using PID"); - tableResult = _inSetpointState(newCadence, newWatts); - } else { - SS2K_LOG(ERG_MODE_LOG_TAG, "SetPoint changed:%dw PowerTable Result: %d", adjustedTarget, tableResult); - if (tableResult != ss2k->getCurrentPosition()) { // add some time to wait while the knob moves to target position. - isDelayed = true; - long int stepDistance = abs(ss2k->getCurrentPosition() - tableResult); - // Calculate time to add based on step distance and stepper speed - long int timeToAdd = round(((double)stepDistance * 1000.0) / (double)userConfig->getStepperSpeed()); - if (timeToAdd > 3000) { // 3 seconds - SS2K_LOG(ERG_MODE_LOG_TAG, "Capping ERG seek time to 3 seconds"); - timeToAdd = 3000; - } - ergTimer += timeToAdd; + _inSetpointState(newCadence, newWatts); + return; + } + + SS2K_LOG(ERG_MODE_LOG_TAG, "SetPoint changed:%dw PowerTable Result: %d", adjustedTarget, tableResult); + _updateValues(newCadence, newWatts, tableResult); + + if (rtConfig->getTargetIncline() != ss2k->getCurrentPosition()) { // add some time to wait while the knob moves to target position. + isDelayed = true; + int timeToAdd = abs(ss2k->getCurrentPosition() - rtConfig->getTargetIncline()); + if (timeToAdd > 3000) { // 3 seconds + SS2K_LOG(ERG_MODE_LOG_TAG, "Capping ERG seek time to 3 seconds"); + timeToAdd = 3000; } - ergTimer += (ERG_MODE_DELAY); // Wait for power meter to register new watts + ergTimer += timeToAdd; } - return tableResult; + ergTimer += (ERG_MODE_DELAY); // Wait for power meter to register new watts } // INTRODUCING PID CONTROL LOOP @@ -213,7 +206,7 @@ int32_t ErgMode::_setPointChangeState(int newCadence, Measurement& newWatts) { // Derivative term: rate of change of error // PrevError -int32_t ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) { +void ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) { // Setting Gains For PID Loop float Kp = userConfig->getERGSensitivity(); float Ki = 0.5; @@ -265,8 +258,11 @@ int32_t ErgMode::_inSetpointState(int newCadence, Measurement& newWatts) { // Calculate new incline float newIncline = ss2k->getCurrentPosition() + PID_output; - prevError = error; - return newIncline; + + prevError = error; + + // Apply the new values + _updateValues(newCadence, newWatts, newIncline); } void ErgMode::_updateValues(int newCadence, Measurement& newWatts, float newIncline) { diff --git a/src/PowerTable_Helpers.cpp b/src/PowerTable_Helpers.cpp index e1227f94..6a71970c 100644 --- a/src/PowerTable_Helpers.cpp +++ b/src/PowerTable_Helpers.cpp @@ -141,7 +141,6 @@ int32_t PTHelpers::lookup(int watts, int cad, PTData& ptData) { } } } - if (resistance != RETURN_ERROR) { SS2K_LOG(PTDATA_LOG_TAG, "Extrapolated resistance: %d for watts=%d, cad=%d", resistance, watts, cad); // Return early if we found a valid extrapolated value @@ -149,7 +148,7 @@ int32_t PTHelpers::lookup(int watts, int cad, PTData& ptData) { SS2K_LOG(PTDATA_LOG_TAG, "Extrapolation failed for watts=%d, cad=%d", watts, cad); } - return resistance; + return resistance; // All lookup methods failed } /** diff --git a/src/SensorCollector.cpp b/src/SensorCollector.cpp index 356b126a..cfbff994 100644 --- a/src/SensorCollector.cpp +++ b/src/SensorCollector.cpp @@ -58,7 +58,7 @@ void collectAndSet(NimBLEUUID charUUID, NimBLEUUID serviceUUID, std::string& uni if ((charUUID == PELOTON_DATA_UUID) && !(strcmp(userConfig->getConnectedPowerMeter(), NONE) == 0 || strcmp(userConfig->getConnectedPowerMeter(), ANY) == 0)) { // Peloton connected but using BLE Power Meter. So skip cad for Peloton UUID. } else { - int cadence = round(sensorData->getCadence()); + float cadence = sensorData->getCadence(); rtConfig->cad.setValue(cadence); spinBLEClient.connectedCD = true; logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " CD(%.2f)", fmodf(cadence, 1000.0)); @@ -69,7 +69,7 @@ void collectAndSet(NimBLEUUID charUUID, NimBLEUUID serviceUUID, std::string& uni if ((charUUID == PELOTON_DATA_UUID) && !((strcmp(userConfig->getConnectedPowerMeter(), NONE) == 0) || (strcmp(userConfig->getConnectedPowerMeter(), ANY) == 0))) { // Peloton connected but using BLE Power Meter. So skip power for Peloton UUID. } else { - int power = round(sensorData->getPower() * userConfig->getPowerCorrectionFactor()); + int power = sensorData->getPower() * userConfig->getPowerCorrectionFactor(); rtConfig->watts.setValue(power); spinBLEClient.connectedPM = true; logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " PW(%d)", power % 10000); @@ -77,9 +77,10 @@ void collectAndSet(NimBLEUUID charUUID, NimBLEUUID serviceUUID, std::string& uni } if (sensorData->hasSpeed()) { - rtConfig->setSimulatedSpeed(sensorData->getSpeed()); + float speed = sensorData->getSpeed(); + rtConfig->setSimulatedSpeed(speed); spinBLEClient.connectedSpeed = true; - logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " SD(%.2f)", fmodf(sensorData->getSpeed(), 1000.0)); + logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " SD(%.2f)", fmodf(speed, 1000.0)); } if (sensorData->hasResistance()) { @@ -87,8 +88,9 @@ void collectAndSet(NimBLEUUID charUUID, NimBLEUUID serviceUUID, std::string& uni if ((ss2k->pelotonIsConnected) && (charUUID != PELOTON_DATA_UUID)) { // Peloton connected but using BLE Power Meter. So skip resistance for UUID's that aren't Peloton. } else { - rtConfig->resistance.setValue(sensorData->getResistance()); - logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " RS(%d)", sensorData->getResistance() % 1000); + int resistance = sensorData->getResistance(); + rtConfig->resistance.setValue(resistance); + logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " RS(%d)", resistance % 1000); } } From 59a3ffca591dbe2930872a71652354e90b6415fb Mon Sep 17 00:00:00 2001 From: Anthony Doud Date: Tue, 30 Dec 2025 21:00:17 -0800 Subject: [PATCH 08/12] Added ERG commands, attempted to send gear keypresses. --- include/BLE_KickrBikeService.h | 1 + src/BLE_KickrBikeService.cpp | 55 +++++++++++++++++++++++++++++----- src/BLE_Server.cpp | 4 +-- 3 files changed, 51 insertions(+), 9 deletions(-) diff --git a/include/BLE_KickrBikeService.h b/include/BLE_KickrBikeService.h index 53ef5b82..fd8a3083 100644 --- a/include/BLE_KickrBikeService.h +++ b/include/BLE_KickrBikeService.h @@ -37,6 +37,7 @@ class BLE_KickrBikeService { void sendRideOnResponse(); void sendKeepAlive(); void sendRideData(); + void sendButtonPress(uint8_t buttonId); // Opcode message handlers void handleGetRequest(const uint8_t* data, size_t length); diff --git a/src/BLE_KickrBikeService.cpp b/src/BLE_KickrBikeService.cpp index 3e44674e..0105efa3 100644 --- a/src/BLE_KickrBikeService.cpp +++ b/src/BLE_KickrBikeService.cpp @@ -154,7 +154,7 @@ BLE_KickrBikeService::BLE_KickrBikeService() void BLE_KickrBikeService::setupService(NimBLEServer *pServer, MyCharacteristicCallbacks *chrCallbacks) { // Create the Zwift Ride service (KICKR BIKE protocol) - pKickrBikeService = spinBLEServer.pServer->createService(ZWIFT_CUSTOM_SERVICE_UUID); + pKickrBikeService = spinBLEServer.pServer->createService(ZWIFT_RIDE_CUSTOM_SERVICE_UUID_SHORT); // Create the three characteristics according to KICKR BIKE specification: // 1. Sync RX - Write characteristic for receiving commands from Zwift @@ -218,6 +218,8 @@ void BLE_KickrBikeService::shiftUp() { if (currentGear < KICKR_BIKE_NUM_GEARS - 1) { currentGear++; applyGearChange(); + // Send button press notification to Zwift (SHFT_UP_L_BTN = 0x00200) + sendButtonPress(2); SS2K_LOG(BLE_SERVER_LOG_TAG, "Shifted UP to gear %d (ratio: %.2f)", currentGear + 1, getCurrentGearRatio()); } else { @@ -229,6 +231,8 @@ void BLE_KickrBikeService::shiftDown() { if (currentGear > 0) { currentGear--; applyGearChange(); + // Send button press notification to Zwift (SHFT_DN_L_BTN = 0x00400) + sendButtonPress(5); SS2K_LOG(BLE_SERVER_LOG_TAG, "Shifted DOWN to gear %d (ratio: %.2f)", currentGear + 1, getCurrentGearRatio()); } else { @@ -416,12 +420,8 @@ void BLE_KickrBikeService::processWrite(const std::string& value) { isHandshakeComplete = true; lastKeepAliveTime = millis(); - // Send initial gear state to the app - const GearProtoFields gearFields = buildGearProtoFields(currentGear); - if (gearFields.token != 0) { - emitGearFrame(syncTxCharacteristic, gearFields, ZWIFT_OPCODE_GEAR_EVENT); - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sent initial gear state: gear %d", currentGear + 1); - } + // Don't send initial gear state - let Zwift request it if needed + // Sending it here can interfere with the handshake sequence return; } @@ -533,6 +533,29 @@ void BLE_KickrBikeService::sendRideData() { spinBLEServer.notifyBleAndDircon(asyncTxCharacteristic, payload.data(), payload.size()); } +void BLE_KickrBikeService::sendButtonPress(uint8_t buttonMask) { + if (!isHandshakeComplete) { + return; + } + + // Send button press notification using opcode 0x23 (RideKeyPadStatus) + // Based on Zwift Hub protocol RideButtonMask: + // SHFT_UP_L_BTN = 0x00200 (512) + // SHFT_DN_L_BTN = 0x00400 (1024) + // ButtonMap field contains bitmask of pressed buttons + std::vector payload; + payload.reserve(8); + payload.push_back(0x23); // Opcode for RideKeyPadStatus + + // Field 1: ButtonMap - bitmask of pressed buttons + uint32_t buttonMapValue = (buttonMask == 2) ? 0x00200 : 0x00400; // 2=up(512), 5=down(1024) + appendVarintField(payload, 1, buttonMapValue); + + spinBLEServer.notifyBleAndDircon(asyncTxCharacteristic, payload.data(), payload.size()); + + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sent RideKeyPadStatus (ButtonMap: 0x%04X)", buttonMapValue); +} + // Opcode message handlers void BLE_KickrBikeService::handleGetRequest(const uint8_t* data, size_t length) { @@ -603,6 +626,24 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) } } + // ERG mode power target command: + // 0x18 - Field 3 (PowerTarget) from HubCommand message + if (data[0] == 0x18) { + size_t index = 1; + uint32_t powerTarget = 0; + if (!decodeVarint32(data, length, index, powerTarget)) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET ERG power varint overflow"); + sendStatusResponse(0x02); + return; + } + + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET ERG mode power target %lu W", + static_cast(powerTarget)); + setTargetPower(powerTarget); + sendStatusResponse(0x00); + return; + } + // Zwift/Rouvy send simulation parameters as: // 0x22 [0x08 ] [0x10 ] [0x18 ] [0x20 ] if (data[0] == 0x22 && data[1] >= 2) { diff --git a/src/BLE_Server.cpp b/src/BLE_Server.cpp index 051e24fa..4186bb73 100644 --- a/src/BLE_Server.cpp +++ b/src/BLE_Server.cpp @@ -60,12 +60,12 @@ void startBLEServer() { ss2kCustomCharacteristic.setupService(spinBLEServer.pServer); deviceInformationService.setupService(spinBLEServer.pServer); //add all service UUIDs to advertisement vector - oServiceUUIDs.push_back(CSCSERVICE_UUID); + // oServiceUUIDs.push_back(CSCSERVICE_UUID); oServiceUUIDs.push_back(CYCLINGPOWERSERVICE_UUID); oServiceUUIDs.push_back(HEARTSERVICE_UUID); oServiceUUIDs.push_back(FITNESSMACHINESERVICE_UUID); //oServiceUUIDs.push_back(ZWIFT_RIDE_SERVICE_UUID); - //oServiceUUIDs.push_back(ZWIFT_RIDE_CUSTOM_SERVICE_UUID_SHORT); + oServiceUUIDs.push_back(ZWIFT_RIDE_CUSTOM_SERVICE_UUID_SHORT); oAdvertisementData.setFlags(0x06); // General Discoverable, BR/EDR Not Supported oAdvertisementData.setCompleteServices16(oServiceUUIDs); pAdvertising->setAdvertisementData(oAdvertisementData); From 59b34fc12c52c0913f3908623e05d42e4dcbe835 Mon Sep 17 00:00:00 2001 From: Anthony Doud Date: Tue, 30 Dec 2025 21:28:39 -0800 Subject: [PATCH 09/12] ERG being decoded correctly. --- src/BLE_KickrBikeService.cpp | 151 ++++++++++++++++++++++++++++++----- 1 file changed, 131 insertions(+), 20 deletions(-) diff --git a/src/BLE_KickrBikeService.cpp b/src/BLE_KickrBikeService.cpp index 0105efa3..1b958c59 100644 --- a/src/BLE_KickrBikeService.cpp +++ b/src/BLE_KickrBikeService.cpp @@ -413,6 +413,15 @@ void BLE_KickrBikeService::processWrite(const std::string& value) { return; } + // Debug: Print all incoming data + String hexDump; + for (size_t i = 0; i < value.length(); ++i) { + char buf[4]; + snprintf(buf, sizeof(buf), "%02X ", (uint8_t)value[i]); + hexDump += buf; + } + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: RX (len=%d): %s", value.length(), hexDump.c_str()); + // Check if this is the RideOn handshake (no opcode, just raw bytes) if (isRideOnMessage(value)) { SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Received RideOn handshake"); @@ -584,18 +593,53 @@ void BLE_KickrBikeService::handleGetRequest(const uint8_t* data, size_t length) } void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) { - if (length < 3) { + if (length < 1) { SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET request too short (%d)", length); sendStatusResponse(0x02); return; } + // ERG mode power target command: + // 0x18 - Field 3 (PowerTarget) from HubCommand message + if (data[0] == 0x18) { + size_t index = 1; + uint32_t powerTarget = 0; + if (!decodeVarint32(data, length, index, powerTarget)) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET ERG power varint overflow"); + sendStatusResponse(0x02); + return; + } + + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET ERG mode - Field 3 (PowerTarget) = %lu W", + static_cast(powerTarget)); + setTargetPower(powerTarget); + sendStatusResponse(0x00); + return; + } + + // Need at least 3 bytes for other SET commands + if (length < 3) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET request too short for this command (%d)", length); + sendStatusResponse(0x02); + return; + } + // Zwift gear select (from some controllers) arrives as: // 2A 10 + // OR physical parameters in SIM mode: + // 2A [10 ] [20 ] [28 ] if (data[0] == 0x2A && data[1] >= 2) { const uint8_t payloadLen = data[1]; const size_t payloadEnd = 2 + payloadLen; - if (payloadEnd <= length && data[2] == 0x10) { + + if (payloadEnd > length) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET 0x2A payload truncated"); + sendStatusResponse(0x02); + return; + } + + // Check if this is a gear token (starts with field tag 0x10 and has short payload) + if (data[2] == 0x10 && payloadLen <= 4) { size_t index = 3; uint32_t token = 0; @@ -606,6 +650,9 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) } const int gearNumber = gearNumberFromInboundToken(token); + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Decoded gear token - Field 1 (token) = %lu, mapped to gear %d", + static_cast(token), gearNumber); + if (gearNumber > 0) { // Sync internal + external representation immediately (avoid incremental shifting logic). currentGear = std::clamp(gearNumber - 1, 0, KICKR_BIKE_NUM_GEARS - 1); @@ -613,33 +660,83 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) rtConfig->setShifterPosition(gearNumber); applyGearChange(true); // fromZwift = true, don't echo back - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET gear token %lu -> gear %d", static_cast(token), gearNumber); + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Applied gear change to gear %d (0-indexed: %d)", gearNumber, currentGear); sendStatusResponse(0x00); return; } SS2K_LOG(BLE_SERVER_LOG_TAG, - "KICKR BIKE: SET unknown gear token %lu (add to zwiftInboundGearTokensObserved)", + "KICKR BIKE: Unknown gear token %lu not in lookup table (add to zwiftInboundGearTokensObserved)", static_cast(token)); sendStatusResponse(0x00); return; } - } - - // ERG mode power target command: - // 0x18 - Field 3 (PowerTarget) from HubCommand message - if (data[0] == 0x18) { - size_t index = 1; - uint32_t powerTarget = 0; - if (!decodeVarint32(data, length, index, powerTarget)) { - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET ERG power varint overflow"); - sendStatusResponse(0x02); - return; + + // Otherwise, it's a PhysicalParam message (field 5 in HubCommand) + // Parse fields: field 2 = GearRatioX10000, field 4 = BikeWeightx100, field 5 = RiderWeightx100 + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Decoding PhysicalParam message (0x2A, len=%d)", payloadLen); + size_t index = 2; + uint32_t gearRatio = 0; + uint32_t bikeWeight = 0; + uint32_t riderWeight = 0; + bool hasGearRatio = false; + bool hasBikeWeight = false; + bool hasRiderWeight = false; + + while (index < payloadEnd) { + uint8_t fieldTag = data[index++]; + + switch (fieldTag) { + case 0x10: // Field 2: GearRatioX10000 + if (!decodeVarint32(data, payloadEnd, index, gearRatio)) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET gear ratio varint overflow"); + sendStatusResponse(0x02); + return; + } + hasGearRatio = true; + break; + + case 0x20: // Field 4: BikeWeightx100 + if (!decodeVarint32(data, payloadEnd, index, bikeWeight)) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET bike weight varint overflow"); + sendStatusResponse(0x02); + return; + } + hasBikeWeight = true; + break; + + case 0x28: // Field 5: RiderWeightx100 + if (!decodeVarint32(data, payloadEnd, index, riderWeight)) { + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET rider weight varint overflow"); + sendStatusResponse(0x02); + return; + } + hasRiderWeight = true; + break; + + default: + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET unknown PhysicalParam field tag 0x%02X", fieldTag); + break; + } + } + + // Log the physical parameters with field numbers and raw values + if (hasGearRatio) { + double ratio = static_cast(gearRatio) / 10000.0; + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 2 (GearRatioX10000) = %lu (raw), decoded ratio = %.4f", + static_cast(gearRatio), ratio); + } + if (hasBikeWeight) { + double weightKg = static_cast(bikeWeight) / 100.0; + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 4 (BikeWeightx100) = %lu (raw), decoded weight = %.2f kg", + static_cast(bikeWeight), weightKg); + } + if (hasRiderWeight) { + double weightKg = static_cast(riderWeight) / 100.0; + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 5 (RiderWeightx100) = %lu (raw), decoded weight = %.2f kg", + static_cast(riderWeight), weightKg); } - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET ERG mode power target %lu W", - static_cast(powerTarget)); - setTargetPower(powerTarget); sendStatusResponse(0x00); return; } @@ -709,19 +806,33 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) } } + // Log simulation message details + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Decoded SimulationParam message (0x22, len=%d)", payloadLen); + // Apply grade if present if (hasGrade) { int32_t signedGrade = decodeZigZag32(gradeRaw); double gradientPercent = static_cast(signedGrade) / 100.0; - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET gradient to %.2f%%", gradientPercent); + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 2 (InclineX100) = %lu (raw), zigzag decoded = %ld, gradient = %.2f%%", + static_cast(gradeRaw), static_cast(signedGrade), gradientPercent); setBaseGradient(gradientPercent); } // Log other parameters for debugging if (hasPower) { - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET power target %lu W (ignored in SIM mode)", + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 1 (Power) = %lu W (ignored in SIM mode)", static_cast(powerTarget)); } + if (windSpeed > 0) { + double windMs = static_cast(static_cast(windSpeed)) / 100.0; + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 3 (Wind) = %lu (raw), %.2f m/s", + static_cast(windSpeed), windMs); + } + if (rollingResistance > 0) { + double crr = static_cast(rollingResistance) / 100000.0; + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 4 (Crr) = %lu (raw), %.5f", + static_cast(rollingResistance), crr); + } sendStatusResponse(0x00); return; From ca5061b190fc97823d28aba99ca8838936799608 Mon Sep 17 00:00:00 2001 From: Anthony Doud Date: Wed, 31 Dec 2025 12:32:55 -0800 Subject: [PATCH 10/12] Cleaning up --- include/BLE_KickrBikeService.h | 21 +- lib/SS2K/include/Constants.h | 5 + src/BLE_KickrBikeService.cpp | 471 +++++++++++++++------------------ 3 files changed, 224 insertions(+), 273 deletions(-) diff --git a/include/BLE_KickrBikeService.h b/include/BLE_KickrBikeService.h index fd8a3083..df101a56 100644 --- a/include/BLE_KickrBikeService.h +++ b/include/BLE_KickrBikeService.h @@ -26,7 +26,6 @@ class BLE_KickrBikeService { // Gear management void shiftUp(); void shiftDown(); - int getCurrentGear() const { return currentGear; } double getCurrentGearRatio() const; // Function to check shifter position and modify incline accordingly @@ -39,6 +38,9 @@ class BLE_KickrBikeService { void sendRideData(); void sendButtonPress(uint8_t buttonId); + // Wahoo gearing service notifications + void sendGearingNotification(); + // Opcode message handlers void handleGetRequest(const uint8_t* data, size_t length); void handleSetRequest(const uint8_t* data, size_t length); @@ -50,12 +52,8 @@ class BLE_KickrBikeService { void sendStatusResponse(uint8_t status); // Gradient/resistance control (independent of FTMS) - void setBaseGradient(double gradientPercent); - double getBaseGradient() const { return baseGradient; } - double getEffectiveGradient() const; - void applyGradientToTrainer(); - void applyGearChange(); - void applyGearChange(bool fromZwift); + void applyGradientToTrainer(float gradient); + void applyGearChange(bool fromZwift = false); // Power control for ERG mode void setTargetPower(int watts); @@ -74,13 +72,14 @@ class BLE_KickrBikeService { BLECharacteristic *debugCharacteristic; // Optional debug characteristic BLECharacteristic *unknown6Characteristic; // Optional unknown characteristic + // Wahoo gearing service (separate from KICKR BIKE protocol) + BLEService *pGearingService; + BLECharacteristic *gearingCharacteristic; // Notify characteristic for gear display + // Gear system state - int currentGear; int lastShifterPosition; // Gradient and resistance state (independent of FTMS) - double baseGradient; // Base gradient set by Zwift (%) - double effectiveGradient; // Gradient after gear ratio applied (%) int targetPower; // Target power for ERG mode (watts) // Service state @@ -89,6 +88,7 @@ class BLE_KickrBikeService { unsigned long lastKeepAliveTime; unsigned long lastGradientUpdateTime; unsigned long lastRideDataTime; + unsigned long lastGearingUpdateTime; // Gear ratio table (24 gears) static const double gearRatios[KICKR_BIKE_NUM_GEARS]; @@ -96,7 +96,6 @@ class BLE_KickrBikeService { // Helper methods double calculateEffectiveGrade(double baseGrade, double gearRatio); bool isRideOnMessage(const std::string& data); - void updateTrainerPosition(); }; // Custom callback class for KickrBike Sync RX characteristic diff --git a/lib/SS2K/include/Constants.h b/lib/SS2K/include/Constants.h index c78dc41f..5a70e535 100644 --- a/lib/SS2K/include/Constants.h +++ b/lib/SS2K/include/Constants.h @@ -102,6 +102,11 @@ #define ZWIFT_UNKNOWN_6_CHARACTERISTIC_UUID NimBLEUUID("00000006-19CA-4651-86E5-FA29DCDD09D1") #define ZWIFT_RIDE_CUSTOM_SERVICE_UUID_SHORT NimBLEUUID((uint16_t)0xFC82) +// Wahoo Gearing Service (KICKR BIKE gear display - separate from Zwift protocol) +// This service provides gear information to Wahoo ELEMNT displays and other compatible devices +#define WAHOO_GEARING_SERVICE_UUID NimBLEUUID("a026ee0d-0a7d-4ab3-97fa-f1500f9feb8b") +#define WAHOO_GEARING_CHARACTERISTIC_UUID NimBLEUUID("a026e03a-0a7d-4ab3-97fa-f1500f9feb8b") + // BLE HID #define APPEARANCE_HID_GENERIC_UUID NimBLEUUID((uint16_t)0x3C0) #define APPEARANCE_HID_KEYBOARD_UUID NimBLEUUID((uint16_t)0x3C1) diff --git a/src/BLE_KickrBikeService.cpp b/src/BLE_KickrBikeService.cpp index 1b958c59..9706b31b 100644 --- a/src/BLE_KickrBikeService.cpp +++ b/src/BLE_KickrBikeService.cpp @@ -29,12 +29,10 @@ inline void appendVarintField(std::vector& buffer, uint8_t fieldNumber, appendVarint(buffer, value); } -inline int32_t decodeZigZag32(uint32_t value) { - return static_cast((value >> 1) ^ static_cast(-static_cast(value & 0x01))); -} +inline int32_t decodeZigZag32(uint32_t value) { return static_cast((value >> 1) ^ static_cast(-static_cast(value & 0x01))); } inline bool decodeVarint32(const uint8_t* data, size_t endIndex, size_t& index, uint32_t& result) { - result = 0; + result = 0; uint32_t shift = 0; while (index < endIndex) { uint8_t byte = data[index++]; @@ -51,25 +49,22 @@ inline bool decodeVarint32(const uint8_t* data, size_t endIndex, size_t& index, } // Zwift Play opcodes/tokens derived from qdomyos-zwift reverse engineering. -constexpr uint8_t ZWIFT_OPCODE_GEAR_EVENT = 0x03; +constexpr uint8_t ZWIFT_OPCODE_GEAR_EVENT = 0x03; constexpr uint8_t ZWIFT_OPCODE_GEAR_RESPONSE = 0x3C; -constexpr size_t ZWIFT_CHAINRING_COUNT = 2; -constexpr size_t ZWIFT_GEARS_PER_RING = - KICKR_BIKE_NUM_GEARS >= ZWIFT_CHAINRING_COUNT ? (KICKR_BIKE_NUM_GEARS / ZWIFT_CHAINRING_COUNT) : KICKR_BIKE_NUM_GEARS; +constexpr size_t ZWIFT_CHAINRING_COUNT = 2; +constexpr size_t ZWIFT_GEARS_PER_RING = KICKR_BIKE_NUM_GEARS >= ZWIFT_CHAINRING_COUNT ? (KICKR_BIKE_NUM_GEARS / ZWIFT_CHAINRING_COUNT) : KICKR_BIKE_NUM_GEARS; struct GearProtoFields { - uint16_t token = 0; + uint16_t token = 0; uint8_t frontIndex = 0; - uint8_t rearIndex = 0; - uint8_t gearIndex = 0; + uint8_t rearIndex = 0; + uint8_t gearIndex = 0; }; // Actual Zwift gear tokens captured from real Zwift communication. // Gears 1-24 from easiest to hardest. -constexpr std::array zwiftGearTokens = { - 7500, 8700, 9900, 11100, 12300, 13800, 15300, 16800, - 18600, 20400, 22200, 24000, 26099, 28200, 30300, 32400, - 34900, 37400, 39900, 42399, 45400, 48400, 51400, 54899}; +constexpr std::array zwiftGearTokens = {7500, 8700, 9900, 11100, 12300, 13800, 15300, 16800, 18600, 20400, 22200, 24000, + 26099, 28200, 30300, 32400, 34900, 37400, 39900, 42399, 45400, 48400, 51400, 54899}; uint16_t gearTokenFromIndex(int gearIndex) { if (gearIndex < 0 || gearIndex >= static_cast(zwiftGearTokens.size())) { @@ -96,12 +91,12 @@ GearProtoFields buildGearProtoFields(int gearIndex) { if (fields.token == 0) { return fields; } - fields.gearIndex = static_cast(gearIndex + 1); - const size_t perRing = ZWIFT_GEARS_PER_RING == 0 ? 1 : ZWIFT_GEARS_PER_RING; + fields.gearIndex = static_cast(gearIndex + 1); + const size_t perRing = ZWIFT_GEARS_PER_RING == 0 ? 1 : ZWIFT_GEARS_PER_RING; const size_t frontIdx = static_cast(gearIndex) / perRing; - const size_t rearIdx = static_cast(gearIndex) % perRing; - fields.frontIndex = static_cast(frontIdx + 1); - fields.rearIndex = static_cast(rearIdx + 1); + const size_t rearIdx = static_cast(gearIndex) % perRing; + fields.frontIndex = static_cast(frontIdx + 1); + fields.rearIndex = static_cast(rearIdx + 1); return fields; } @@ -119,9 +114,9 @@ void emitGearFrame(NimBLECharacteristic* characteristic, const GearProtoFields& appendVarintField(payload, 2, fields.frontIndex); appendVarintField(payload, 3, fields.rearIndex); appendVarintField(payload, 4, fields.gearIndex); - + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sending gear event opcode 0x%02X, token %u, size %d", opcode, fields.token, payload.size()); - + spinBLEServer.notifyBleAndDircon(characteristic, payload.data(), payload.size()); } } // namespace @@ -141,10 +136,9 @@ BLE_KickrBikeService::BLE_KickrBikeService() syncTxCharacteristic(nullptr), debugCharacteristic(nullptr), unknown6Characteristic(nullptr), - currentGear(KICKR_BIKE_DEFAULT_GEAR), - lastShifterPosition(-1), - baseGradient(0.0), - effectiveGradient(0.0), + pGearingService(nullptr), + gearingCharacteristic(nullptr), + lastShifterPosition(1), targetPower(0), isHandshakeComplete(false), isEnabled(false), @@ -152,47 +146,51 @@ BLE_KickrBikeService::BLE_KickrBikeService() lastGradientUpdateTime(0), lastRideDataTime(0) {} -void BLE_KickrBikeService::setupService(NimBLEServer *pServer, MyCharacteristicCallbacks *chrCallbacks) { +void BLE_KickrBikeService::setupService(NimBLEServer* pServer, MyCharacteristicCallbacks* chrCallbacks) { // Create the Zwift Ride service (KICKR BIKE protocol) pKickrBikeService = spinBLEServer.pServer->createService(ZWIFT_RIDE_CUSTOM_SERVICE_UUID_SHORT); - + // Create the three characteristics according to KICKR BIKE specification: // 1. Sync RX - Write characteristic for receiving commands from Zwift - syncRxCharacteristic = pKickrBikeService->createCharacteristic( - ZWIFT_SYNC_RX_UUID, - NIMBLE_PROPERTY::WRITE_NR); - + syncRxCharacteristic = pKickrBikeService->createCharacteristic(ZWIFT_SYNC_RX_UUID, NIMBLE_PROPERTY::WRITE_NR); + // 2. Async TX - Notify characteristic for asynchronous events (button presses, battery) - asyncTxCharacteristic = pKickrBikeService->createCharacteristic( - ZWIFT_ASYNC_TX_UUID, - NIMBLE_PROPERTY::NOTIFY); - + asyncTxCharacteristic = pKickrBikeService->createCharacteristic(ZWIFT_ASYNC_TX_UUID, NIMBLE_PROPERTY::NOTIFY); + // 3. Sync TX - Notify characteristic for synchronous responses - syncTxCharacteristic = pKickrBikeService->createCharacteristic( - ZWIFT_SYNC_TX_UUID, - NIMBLE_PROPERTY::INDICATE); + syncTxCharacteristic = pKickrBikeService->createCharacteristic(ZWIFT_SYNC_TX_UUID, NIMBLE_PROPERTY::INDICATE); // Optional: Debug characteristic for logging/debug info - debugCharacteristic = pKickrBikeService->createCharacteristic( - ZWIFT_DEBUG_CHARACTERISTIC_UUID, - NIMBLE_PROPERTY::NOTIFY | NIMBLE_PROPERTY::READ); + debugCharacteristic = pKickrBikeService->createCharacteristic(ZWIFT_DEBUG_CHARACTERISTIC_UUID, NIMBLE_PROPERTY::NOTIFY | NIMBLE_PROPERTY::READ); // Optional: Unknown characteristic 6 - unknown6Characteristic = pKickrBikeService->createCharacteristic( - ZWIFT_UNKNOWN_6_CHARACTERISTIC_UUID, - NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY | NIMBLE_PROPERTY::INDICATE); - + unknown6Characteristic = pKickrBikeService->createCharacteristic(ZWIFT_UNKNOWN_6_CHARACTERISTIC_UUID, + NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY | NIMBLE_PROPERTY::INDICATE); + // Set custom callback for Sync RX to handle RideOn handshake static KickrBikeCharacteristicCallbacks kickrBikeCallbacks; syncRxCharacteristic->setCallbacks(&kickrBikeCallbacks); - + // Start the service pKickrBikeService->start(); - + // Add service UUID to DirCon MDNS (for discovery) DirConManager::addBleServiceUuid(ZWIFT_RIDE_CUSTOM_SERVICE_UUID_SHORT); - + + // Create Wahoo Gearing Service (for ELEMNT displays and compatible devices) + pGearingService = spinBLEServer.pServer->createService(WAHOO_GEARING_SERVICE_UUID); + + // Create gearing characteristic (notify only) + gearingCharacteristic = pGearingService->createCharacteristic(WAHOO_GEARING_CHARACTERISTIC_UUID, NIMBLE_PROPERTY::NOTIFY); + + // Start the gearing service + pGearingService->start(); + + // Add gearing service UUID to DirCon MDNS + DirConManager::addBleServiceUuid(WAHOO_GEARING_SERVICE_UUID); + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE Service initialized with %d gears", KICKR_BIKE_NUM_GEARS); + SS2K_LOG(BLE_SERVER_LOG_TAG, "Wahoo Gearing Service initialized"); } void BLE_KickrBikeService::update() { @@ -215,52 +213,40 @@ void BLE_KickrBikeService::update() { } void BLE_KickrBikeService::shiftUp() { - if (currentGear < KICKR_BIKE_NUM_GEARS - 1) { - currentGear++; + if (rtConfig->getShifterPosition() < KICKR_BIKE_NUM_GEARS - 1) { + rtConfig->setShifterPosition(rtConfig->getShifterPosition() + 1); applyGearChange(); // Send button press notification to Zwift (SHFT_UP_L_BTN = 0x00200) sendButtonPress(2); - SS2K_LOG(BLE_SERVER_LOG_TAG, "Shifted UP to gear %d (ratio: %.2f)", - currentGear + 1, getCurrentGearRatio()); + SS2K_LOG(BLE_SERVER_LOG_TAG, "Shifted UP to gear %d (ratio: %.2f)", rtConfig->getShifterPosition(), getCurrentGearRatio()); } else { SS2K_LOG(BLE_SERVER_LOG_TAG, "Already in highest gear"); } } void BLE_KickrBikeService::shiftDown() { - if (currentGear > 0) { - currentGear--; + if (rtConfig->getShifterPosition() > 0) { + rtConfig->setShifterPosition(rtConfig->getShifterPosition() - 1); applyGearChange(); // Send button press notification to Zwift (SHFT_DN_L_BTN = 0x00400) sendButtonPress(5); - SS2K_LOG(BLE_SERVER_LOG_TAG, "Shifted DOWN to gear %d (ratio: %.2f)", - currentGear + 1, getCurrentGearRatio()); + SS2K_LOG(BLE_SERVER_LOG_TAG, "Shifted DOWN to gear %d (ratio: %.2f)", rtConfig->getShifterPosition(), getCurrentGearRatio()); } else { SS2K_LOG(BLE_SERVER_LOG_TAG, "Already in lowest gear"); } } double BLE_KickrBikeService::getCurrentGearRatio() const { - if (currentGear >= 0 && currentGear < KICKR_BIKE_NUM_GEARS) { - return gearRatios[currentGear]; + if (rtConfig->getShifterPosition() >= 0 && rtConfig->getShifterPosition() < KICKR_BIKE_NUM_GEARS) { + return gearRatios[rtConfig->getShifterPosition()]; } return 1.0; // Default to neutral ratio } -void BLE_KickrBikeService::applyGearChange() { - applyGearChange(false); -} - void BLE_KickrBikeService::applyGearChange(bool fromZwift) { // Recalculate effective gradient with new gear - effectiveGradient = calculateEffectiveGrade(baseGradient, getCurrentGearRatio()); - - // Apply to trainer if this service is enabled - if (isEnabled) { - applyGradientToTrainer(); - } - const GearProtoFields gearFields = buildGearProtoFields(currentGear); + const GearProtoFields gearFields = buildGearProtoFields(rtConfig->getShifterPosition()); if (gearFields.token != 0) { // Only send gear response notification if we initiated the change (not Zwift) // Use GEAR_RESPONSE opcode (0x3C) like real KICKR BIKE, not GEAR_EVENT (0x03) @@ -268,98 +254,49 @@ void BLE_KickrBikeService::applyGearChange(bool fromZwift) { emitGearFrame(syncTxCharacteristic, gearFields, ZWIFT_OPCODE_GEAR_RESPONSE); } lastReportedGearToken = gearFields.token; - const double ratio = getCurrentGearRatio(); - SS2K_LOG(BLE_SERVER_LOG_TAG, - "KICKR BIKE: Zwift Play gear token 0x%03X -> gear %u (front %u, rear %u, ratio %.2f)", - gearFields.token, - gearFields.gearIndex, - gearFields.frontIndex, - gearFields.rearIndex, - ratio); - } -} - -void BLE_KickrBikeService::setBaseGradient(double gradientPercent) { - baseGradient = gradientPercent; - effectiveGradient = calculateEffectiveGrade(baseGradient, getCurrentGearRatio()); - - // Apply to trainer if enabled - if (isEnabled) { - applyGradientToTrainer(); + const double ratio = getCurrentGearRatio(); + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Zwift Play gear token 0x%03X -> gear %u (front %u, rear %u, ratio %.2f)", gearFields.token, gearFields.gearIndex, + gearFields.frontIndex, gearFields.rearIndex, ratio); } - - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Base gradient set to %.2f%%", baseGradient); -} -double BLE_KickrBikeService::getEffectiveGradient() const { - return effectiveGradient; + // Send Wahoo gearing notification for ELEMNT displays + sendGearingNotification(); } -void BLE_KickrBikeService::applyGradientToTrainer() { - // Only update if enough time has passed (100ms debounce) - unsigned long currentTime = millis(); - if (currentTime - lastGradientUpdateTime < 100) { - return; - } - lastGradientUpdateTime = currentTime; - - // Clamp to valid trainer limits (-20% to +20%) - double clampedGradient = effectiveGradient; - if (clampedGradient < -20.0) clampedGradient = -20.0; - if (clampedGradient > 20.0) clampedGradient = 20.0; - +void BLE_KickrBikeService::applyGradientToTrainer(float gradient) { // Convert to 0.01% units for rtConfig - int gradientUnits = static_cast(clampedGradient * 100); - + int gradientUnits = static_cast(gradient * 100); + // Update the target incline directly rtConfig->setTargetIncline(gradientUnits); - - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Applied gradient %.2f%% (gear %d, ratio %.2f)", - clampedGradient, currentGear + 1, getCurrentGearRatio()); + + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Applied gradient %.2f%%", gradient); } void BLE_KickrBikeService::setTargetPower(int watts) { targetPower = watts; - // In ERG mode, the power is fixed and gears affect the "feel" // This is handled by the trainer's power control logic SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Target power set to %d watts", targetPower); -} - -void BLE_KickrBikeService::updateTrainerPosition() { - // This method updates the physical trainer position based on effective gradient - // Only applies if the service is enabled and controlling the trainer - if (!isEnabled) { - return; - } - - applyGradientToTrainer(); -} - -double BLE_KickrBikeService::calculateEffectiveGrade(double baseGrade, double gearRatio) { - // Calculate effective grade by multiplying base grade with gear ratio - // This simulates the feeling of shifting gears: - // - Lower gear (ratio < 1.0) makes hills feel easier - // - Higher gear (ratio > 1.0) makes hills feel harder - return baseGrade * gearRatio; + rtConfig->watts.setTarget(watts); } void BLE_KickrBikeService::updateGearFromShifterPosition() { // Get current shifter position int currentShifterPosition = rtConfig->getShifterPosition(); - + // Check if shifter position has changed if (lastShifterPosition == -1) { // First run, just store the position lastShifterPosition = currentShifterPosition; return; } - + if (currentShifterPosition == lastShifterPosition) { // No change, nothing to do return; } - + // Determine direction of shift if (currentShifterPosition > lastShifterPosition) { // Shifter moved up - shift to harder gear @@ -368,7 +305,7 @@ void BLE_KickrBikeService::updateGearFromShifterPosition() { // Shifter moved down - shift to easier gear shiftDown(); } - + // Update last position lastShifterPosition = currentShifterPosition; } @@ -412,7 +349,7 @@ void BLE_KickrBikeService::processWrite(const std::string& value) { SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Received empty write"); return; } - + // Debug: Print all incoming data String hexDump; for (size_t i = 0; i < value.length(); ++i) { @@ -421,58 +358,58 @@ void BLE_KickrBikeService::processWrite(const std::string& value) { hexDump += buf; } SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: RX (len=%d): %s", value.length(), hexDump.c_str()); - + // Check if this is the RideOn handshake (no opcode, just raw bytes) if (isRideOnMessage(value)) { SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Received RideOn handshake"); sendRideOnResponse(); isHandshakeComplete = true; - lastKeepAliveTime = millis(); - + lastKeepAliveTime = millis(); + // Don't send initial gear state - let Zwift request it if needed // Sending it here can interfere with the handshake sequence - + return; } - + // Process opcode-based messages - uint8_t opcode = (uint8_t)value[0]; + uint8_t opcode = (uint8_t)value[0]; const uint8_t* messageData = (const uint8_t*)value.data() + 1; - size_t messageLength = value.length() - 1; - + size_t messageLength = value.length() - 1; + switch (opcode) { case 0x00: // INFO_REQUEST - device information query handleInfoRequest(messageData, messageLength); break; - + case 0x04: // SET - Update trainer state handleSetRequest(messageData, messageLength); break; - + case 0x08: // GET - Request data object handleGetRequest(messageData, messageLength); break; - + case 0x22: // RESET - Reset device handleReset(); break; - + case 0x41: // LOG_LEVEL_SET - Set log level handleSetLogLevel(messageData, messageLength); break; - + case 0x32: // VENDOR_MESSAGE - Vendor-specific message handleVendorMessage(messageData, messageLength); break; - + case 0x07: // CONTROLLER_NOTIFICATION - Button events (shouldn't be written to us, we send these) SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Unexpected CONTROLLER_NOTIFICATION write"); break; - + case 0x19: // BATTERY_NOTIF - Battery updates (shouldn't be written to us, we send these) SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Unexpected BATTERY_NOTIF write"); break; - + default: // Log full unknown message for debugging String hexDump; @@ -489,12 +426,12 @@ void BLE_KickrBikeService::processWrite(const std::string& value) { void BLE_KickrBikeService::sendRideOnResponse() { // Respond with "RideOn" + signature bytes (0x01 0x03) uint8_t response[8] = { - 0x52, 0x69, 0x64, 0x65, 0x4F, 0x6E, // "RideOn" - 0x01, 0x03 // Signature + 0x52, 0x69, 0x64, 0x65, 0x4F, 0x6E, // "RideOn" + 0x01, 0x03 // Signature }; - + spinBLEServer.notifyBleAndDircon(syncTxCharacteristic, response, sizeof(response)); - + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sent RideOn response"); } @@ -502,16 +439,11 @@ void BLE_KickrBikeService::sendKeepAlive() { // Keep-alive message to maintain connection with Zwift // This is a protobuf-encoded message that tells Zwift we're still alive // The exact format comes from the BikeControl reference implementation - uint8_t keepAliveData[] = { - 0xB7, 0x01, 0x00, 0x00, 0x20, 0x41, 0x20, 0x1C, - 0x00, 0x18, 0x00, 0x04, 0x00, 0x1B, 0x4F, 0x00, - 0xB7, 0x01, 0x00, 0x00, 0x20, 0x79, 0x8E, 0xC5, - 0xBD, 0xEF, 0xCB, 0xE4, 0x56, 0x34, 0x18, 0x26, - 0x9E, 0x49, 0x26, 0xFB, 0xE1 - }; - + uint8_t keepAliveData[] = {0xB7, 0x01, 0x00, 0x00, 0x20, 0x41, 0x20, 0x1C, 0x00, 0x18, 0x00, 0x04, 0x00, 0x1B, 0x4F, 0x00, 0xB7, 0x01, 0x00, + 0x00, 0x20, 0x79, 0x8E, 0xC5, 0xBD, 0xEF, 0xCB, 0xE4, 0x56, 0x34, 0x18, 0x26, 0x9E, 0x49, 0x26, 0xFB, 0xE1}; + spinBLEServer.notifyBleAndDircon(syncTxCharacteristic, keepAliveData, sizeof(keepAliveData)); - + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sent keep-alive"); } @@ -520,14 +452,14 @@ void BLE_KickrBikeService::sendRideData() { return; } - int power = std::max(0, rtConfig->watts.getValue()); - int cadence = std::max(0, static_cast(rtConfig->cad.getValue())); + int power = std::max(0, rtConfig->watts.getValue()); + int cadence = std::max(0, static_cast(rtConfig->cad.getValue())); double speedKmh = rtConfig->getSimulatedSpeed() > 0 ? rtConfig->getSimulatedSpeed() : spinBLEServer.calculateSpeed(); if (speedKmh < 0) { speedKmh = 0; } uint32_t speedX100 = static_cast(speedKmh * 100.0 + 0.5); - int heartRate = std::max(0, rtConfig->hr.getValue()); + int heartRate = std::max(0, rtConfig->hr.getValue()); std::vector payload; payload.reserve(20); @@ -536,8 +468,8 @@ void BLE_KickrBikeService::sendRideData() { appendVarintField(payload, 2, static_cast(cadence)); appendVarintField(payload, 3, speedX100); appendVarintField(payload, 4, static_cast(heartRate)); - appendVarintField(payload, 5, 0); // Unknown field (possibly flags or state) - appendVarintField(payload, 6, static_cast(currentGear + 1)); // Current gear (1-based) + appendVarintField(payload, 5, 0); // Unknown field (possibly flags or state) + appendVarintField(payload, 6, static_cast(rtConfig->getShifterPosition())); // Current gear (1-based) spinBLEServer.notifyBleAndDircon(asyncTxCharacteristic, payload.data(), payload.size()); } @@ -555,13 +487,13 @@ void BLE_KickrBikeService::sendButtonPress(uint8_t buttonMask) { std::vector payload; payload.reserve(8); payload.push_back(0x23); // Opcode for RideKeyPadStatus - + // Field 1: ButtonMap - bitmask of pressed buttons uint32_t buttonMapValue = (buttonMask == 2) ? 0x00200 : 0x00400; // 2=up(512), 5=down(1024) appendVarintField(payload, 1, buttonMapValue); spinBLEServer.notifyBleAndDircon(asyncTxCharacteristic, payload.data(), payload.size()); - + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sent RideKeyPadStatus (ButtonMap: 0x%04X)", buttonMapValue); } @@ -570,13 +502,13 @@ void BLE_KickrBikeService::sendButtonPress(uint8_t buttonMask) { void BLE_KickrBikeService::handleGetRequest(const uint8_t* data, size_t length) { // GET request - Zwift is requesting a data object // The data should contain an object ID (protobuf encoded) - + if (length < 1) { SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: GET request with no data"); sendStatusResponse(0x02); // Error status return; } - + // For now, we'll parse a simple object ID from the first bytes // In a full implementation, this would be protobuf decoded uint16_t objectId = 0; @@ -585,9 +517,9 @@ void BLE_KickrBikeService::handleGetRequest(const uint8_t* data, size_t length) } else { objectId = data[0]; } - + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: GET request for object ID 0x%04X", objectId); - + // Respond with empty data for now (full implementation would return actual object data) sendGetResponse(objectId, nullptr, 0); } @@ -602,16 +534,15 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) // ERG mode power target command: // 0x18 - Field 3 (PowerTarget) from HubCommand message if (data[0] == 0x18) { - size_t index = 1; + size_t index = 1; uint32_t powerTarget = 0; if (!decodeVarint32(data, length, index, powerTarget)) { SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET ERG power varint overflow"); sendStatusResponse(0x02); return; } - - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET ERG mode - Field 3 (PowerTarget) = %lu W", - static_cast(powerTarget)); + + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET ERG mode - Field 3 (PowerTarget) = %lu W", static_cast(powerTarget)); setTargetPower(powerTarget); sendStatusResponse(0x00); return; @@ -630,17 +561,17 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) // 2A [10 ] [20 ] [28 ] if (data[0] == 0x2A && data[1] >= 2) { const uint8_t payloadLen = data[1]; - const size_t payloadEnd = 2 + payloadLen; - + const size_t payloadEnd = 2 + payloadLen; + if (payloadEnd > length) { SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET 0x2A payload truncated"); sendStatusResponse(0x02); return; } - + // Check if this is a gear token (starts with field tag 0x10 and has short payload) if (data[2] == 0x10 && payloadLen <= 4) { - size_t index = 3; + size_t index = 3; uint32_t token = 0; if (!decodeVarint32(data, payloadEnd, index, token)) { @@ -650,42 +581,38 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) } const int gearNumber = gearNumberFromInboundToken(token); - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Decoded gear token - Field 1 (token) = %lu, mapped to gear %d", - static_cast(token), gearNumber); - + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Decoded gear token - Field 1 (token) = %lu, mapped to gear %d", static_cast(token), gearNumber); + if (gearNumber > 0) { // Sync internal + external representation immediately (avoid incremental shifting logic). - currentGear = std::clamp(gearNumber - 1, 0, KICKR_BIKE_NUM_GEARS - 1); lastShifterPosition = gearNumber; rtConfig->setShifterPosition(gearNumber); applyGearChange(true); // fromZwift = true, don't echo back - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Applied gear change to gear %d (0-indexed: %d)", gearNumber, currentGear); + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Applied gear change to gear %d", rtConfig->getShifterPosition()); sendStatusResponse(0x00); return; } - SS2K_LOG(BLE_SERVER_LOG_TAG, - "KICKR BIKE: Unknown gear token %lu not in lookup table (add to zwiftInboundGearTokensObserved)", - static_cast(token)); + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Unknown gear token %lu not in lookup table (add to zwiftInboundGearTokensObserved)", static_cast(token)); sendStatusResponse(0x00); return; } - + // Otherwise, it's a PhysicalParam message (field 5 in HubCommand) // Parse fields: field 2 = GearRatioX10000, field 4 = BikeWeightx100, field 5 = RiderWeightx100 SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Decoding PhysicalParam message (0x2A, len=%d)", payloadLen); - size_t index = 2; - uint32_t gearRatio = 0; - uint32_t bikeWeight = 0; + size_t index = 2; + uint32_t gearRatio = 0; + uint32_t bikeWeight = 0; uint32_t riderWeight = 0; - bool hasGearRatio = false; - bool hasBikeWeight = false; - bool hasRiderWeight = false; - + bool hasGearRatio = false; + bool hasBikeWeight = false; + bool hasRiderWeight = false; + while (index < payloadEnd) { uint8_t fieldTag = data[index++]; - + switch (fieldTag) { case 0x10: // Field 2: GearRatioX10000 if (!decodeVarint32(data, payloadEnd, index, gearRatio)) { @@ -695,7 +622,7 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) } hasGearRatio = true; break; - + case 0x20: // Field 4: BikeWeightx100 if (!decodeVarint32(data, payloadEnd, index, bikeWeight)) { SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET bike weight varint overflow"); @@ -704,7 +631,7 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) } hasBikeWeight = true; break; - + case 0x28: // Field 5: RiderWeightx100 if (!decodeVarint32(data, payloadEnd, index, riderWeight)) { SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET rider weight varint overflow"); @@ -713,30 +640,27 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) } hasRiderWeight = true; break; - + default: SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET unknown PhysicalParam field tag 0x%02X", fieldTag); break; } } - + // Log the physical parameters with field numbers and raw values if (hasGearRatio) { double ratio = static_cast(gearRatio) / 10000.0; - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 2 (GearRatioX10000) = %lu (raw), decoded ratio = %.4f", - static_cast(gearRatio), ratio); + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 2 (GearRatioX10000) = %lu (raw), decoded ratio = %.4f", static_cast(gearRatio), ratio); } if (hasBikeWeight) { double weightKg = static_cast(bikeWeight) / 100.0; - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 4 (BikeWeightx100) = %lu (raw), decoded weight = %.2f kg", - static_cast(bikeWeight), weightKg); + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 4 (BikeWeightx100) = %lu (raw), decoded weight = %.2f kg", static_cast(bikeWeight), weightKg); } if (hasRiderWeight) { double weightKg = static_cast(riderWeight) / 100.0; - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 5 (RiderWeightx100) = %lu (raw), decoded weight = %.2f kg", - static_cast(riderWeight), weightKg); + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 5 (RiderWeightx100) = %lu (raw), decoded weight = %.2f kg", static_cast(riderWeight), weightKg); } - + sendStatusResponse(0x00); return; } @@ -745,25 +669,25 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) // 0x22 [0x08 ] [0x10 ] [0x18 ] [0x20 ] if (data[0] == 0x22 && data[1] >= 2) { uint8_t payloadLen = data[1]; - size_t payloadEnd = 2 + payloadLen; + size_t payloadEnd = 2 + payloadLen; if (payloadEnd > length) { SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET simulation params payload truncated"); sendStatusResponse(0x02); return; } - size_t index = 2; - uint32_t powerTarget = 0; - uint32_t gradeRaw = 0; - uint32_t windSpeed = 0; + size_t index = 2; + uint32_t powerTarget = 0; + uint32_t gradeRaw = 0; + uint32_t windSpeed = 0; uint32_t rollingResistance = 0; - bool hasPower = false; - bool hasGrade = false; + bool hasPower = false; + bool hasGrade = false; // Parse all fields in the simulation message while (index < payloadEnd) { uint8_t fieldTag = data[index++]; - + switch (fieldTag) { case 0x08: // Field 1: Power target (watts) if (!decodeVarint32(data, payloadEnd, index, powerTarget)) { @@ -773,7 +697,7 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) } hasPower = true; break; - + case 0x10: // Field 2: Grade (zigzag encoded, value * 100) if (!decodeVarint32(data, payloadEnd, index, gradeRaw)) { SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET grade varint overflow"); @@ -782,7 +706,7 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) } hasGrade = true; break; - + case 0x18: // Field 3: Wind speed (m/s * 100, zigzag?) if (!decodeVarint32(data, payloadEnd, index, windSpeed)) { SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET wind varint overflow"); @@ -790,7 +714,7 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) return; } break; - + case 0x20: // Field 4: Rolling resistance coefficient if (!decodeVarint32(data, payloadEnd, index, rollingResistance)) { SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET rolling resistance varint overflow"); @@ -798,7 +722,7 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) return; } break; - + default: // Unknown field, skip it SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET unknown simulation field tag 0x%02X", fieldTag); @@ -808,30 +732,27 @@ void BLE_KickrBikeService::handleSetRequest(const uint8_t* data, size_t length) // Log simulation message details SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Decoded SimulationParam message (0x22, len=%d)", payloadLen); - + // Apply grade if present if (hasGrade) { - int32_t signedGrade = decodeZigZag32(gradeRaw); + int32_t signedGrade = decodeZigZag32(gradeRaw); double gradientPercent = static_cast(signedGrade) / 100.0; - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 2 (InclineX100) = %lu (raw), zigzag decoded = %ld, gradient = %.2f%%", - static_cast(gradeRaw), static_cast(signedGrade), gradientPercent); - setBaseGradient(gradientPercent); + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 2 (InclineX100) = %lu (raw), zigzag decoded = %ld, gradient = %.2f%%", static_cast(gradeRaw), + static_cast(signedGrade), gradientPercent); + applyGradientToTrainer(static_cast(gradientPercent)); } - + // Log other parameters for debugging if (hasPower) { - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 1 (Power) = %lu W (ignored in SIM mode)", - static_cast(powerTarget)); + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 1 (Power) = %lu W (ignored in SIM mode)", static_cast(powerTarget)); } if (windSpeed > 0) { double windMs = static_cast(static_cast(windSpeed)) / 100.0; - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 3 (Wind) = %lu (raw), %.2f m/s", - static_cast(windSpeed), windMs); + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 3 (Wind) = %lu (raw), %.2f m/s", static_cast(windSpeed), windMs); } if (rollingResistance > 0) { double crr = static_cast(rollingResistance) / 100000.0; - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 4 (Crr) = %lu (raw), %.5f", - static_cast(rollingResistance), crr); + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Field 4 (Crr) = %lu (raw), %.5f", static_cast(rollingResistance), crr); } sendStatusResponse(0x00); @@ -857,10 +778,10 @@ void BLE_KickrBikeService::handleInfoRequest(const uint8_t* data, size_t length) } uint32_t requestId = 0; - bool parsed = false; + bool parsed = false; if (data[0] == 0x08 && length >= 2) { - size_t index = 1; + size_t index = 1; uint8_t shift = 0; while (index < length) { uint8_t byte = data[index++]; @@ -897,18 +818,8 @@ void BLE_KickrBikeService::handleInfoRequest(const uint8_t* data, size_t length) void BLE_KickrBikeService::handleReset() { // RESET command - Reset the device to default state SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: RESET command received"); - - // Reset to default gear - currentGear = KICKR_BIKE_DEFAULT_GEAR; - baseGradient = 0.0; - effectiveGradient = 0.0; targetPower = 0; - - // Apply reset state to trainer - if (isEnabled) { - applyGradientToTrainer(); - } - + // Send success status sendStatusResponse(0x00); // Success } @@ -919,10 +830,9 @@ void BLE_KickrBikeService::handleSetLogLevel(const uint8_t* data, size_t length) SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET_LOG_LEVEL with no data"); return; } - - uint8_t logLevel = data[0]; - SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET_LOG_LEVEL to %d", logLevel); - + + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: SET_LOG_LEVEL to %d", data[0]); + // For now, just acknowledge - full implementation would adjust logging sendStatusResponse(0x00); // Success } @@ -930,12 +840,12 @@ void BLE_KickrBikeService::handleSetLogLevel(const uint8_t* data, size_t length) void BLE_KickrBikeService::handleVendorMessage(const uint8_t* data, size_t length) { // VENDOR_MESSAGE - Vendor-specific message SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: VENDOR_MESSAGE received (%d bytes)", length); - + // Log the message content for debugging if (length > 0) { SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Vendor message first byte: 0x%02X", data[0]); } - + // Send success status sendStatusResponse(0x00); } @@ -944,29 +854,66 @@ void BLE_KickrBikeService::sendGetResponse(uint16_t objectId, const uint8_t* dat // Send GET_RESPONSE (opcode 0x3C) with the requested object data std::vector response; response.push_back(0x3C); // GET_RESPONSE opcode - + // Add object ID (little-endian) response.push_back(objectId & 0xFF); response.push_back((objectId >> 8) & 0xFF); - + // Add data if provided if (data && length > 0) { response.insert(response.end(), data, data + length); } - + spinBLEServer.notifyBleAndDircon(syncTxCharacteristic, response.data(), response.size()); - + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sent GET_RESPONSE for object 0x%04X", objectId); } void BLE_KickrBikeService::sendStatusResponse(uint8_t status) { // Send STATUS_RESPONSE (opcode 0x12) with status code uint8_t response[2] = { - 0x12, // STATUS_RESPONSE opcode - status // Status code (0x00 = success, others = error) + 0x12, // STATUS_RESPONSE opcode + status // Status code (0x00 = success, others = error) }; - + spinBLEServer.notifyBleAndDircon(syncTxCharacteristic, response, sizeof(response)); - + SS2K_LOG(BLE_SERVER_LOG_TAG, "KICKR BIKE: Sent STATUS_RESPONSE (status: 0x%02X)", status); } + +void BLE_KickrBikeService::sendGearingNotification() { + if (!gearingCharacteristic) { + return; + } + + // Build gearing notification packet + // Format from KICKR BIKE reference implementation: + // [0][1] = Unknown header bytes (always 0x00 0x00 based on captures) + // [2] = Selected front gear (1-indexed) + // [3] = Selected rear gear (1-indexed) + // [4] = Total front gears + // [5] = Total rear gears + + // For our 24-gear virtual system, simulate a 2x12 drivetrain + constexpr uint8_t SIMULATED_FRONT_GEARS = 2; + constexpr uint8_t SIMULATED_REAR_GEARS = 12; + + // Map our 24 gears (0-23) to front/rear combination (1-indexed) + // Gears 0-11 = small ring (front 1), rear 1-12 + // Gears 12-23 = big ring (front 2), rear 1-12 + uint8_t selectedFrontGear = (rtConfig->getShifterPosition() / SIMULATED_REAR_GEARS) + 1; // 1 or 2 + uint8_t selectedRearGear = (rtConfig->getShifterPosition() % SIMULATED_REAR_GEARS) + 1; // 1-12 + + uint8_t gearingData[6] = { + 0x00, // [0] Header byte + 0x00, // [1] Header byte + selectedFrontGear, // [2] Selected front gear (1-2) + selectedRearGear, // [3] Selected rear gear (1-12) + SIMULATED_FRONT_GEARS, // [4] Total front gears (2) + SIMULATED_REAR_GEARS // [5] Total rear gears (12) + }; + + spinBLEServer.notifyBleAndDircon(gearingCharacteristic, gearingData, sizeof(gearingData)); + + SS2K_LOG(BLE_SERVER_LOG_TAG, "Wahoo Gearing: Sent gear %d:%d (front:rear, gear %d of %d total)", selectedFrontGear, selectedRearGear, rtConfig->getShifterPosition(), KICKR_BIKE_NUM_GEARS); +} From cdcbece5d9768d92b4b5ab2e38a3891083b39abc Mon Sep 17 00:00:00 2001 From: Anthony Doud Date: Sat, 28 Feb 2026 19:24:26 -0600 Subject: [PATCH 11/12] Add BLE appender and enhance logging for target positions --- dependencies.lock | 2 +- src/Main.cpp | 102 ++++++++++++++++++++++++++-------------------- 2 files changed, 59 insertions(+), 45 deletions(-) diff --git a/dependencies.lock b/dependencies.lock index b7f40e3b..a3a69572 100644 --- a/dependencies.lock +++ b/dependencies.lock @@ -96,6 +96,6 @@ direct_dependencies: - espressif/network_provisioning - idf - joltwallet/littlefs -manifest_hash: e7ce7a886dfdb3cc5cd59acbdb4ff333c0a91c6e4de55b9fd6323d866c7cc691 +manifest_hash: df9baa925ebc2a028ee0e349be53f2169bb7ac38b2cb9c63b3ebf69123c43ef8 target: esp32 version: 2.0.0 diff --git a/src/Main.cpp b/src/Main.cpp index 640e1c44..e55d4cac 100644 --- a/src/Main.cpp +++ b/src/Main.cpp @@ -17,6 +17,7 @@ #include "Power_Table.h" #include "UdpAppender.h" #include "WebsocketAppender.h" +#include "BleAppender.h" #include "BLE_Custom_Characteristic.h" #include "BLE_Definitions.h" #include @@ -52,6 +53,7 @@ RuntimeParameters* rtConfig = new RuntimeParameters; ///////////// Log Appender ///////////// UdpAppender udpAppender; WebSocketAppender webSocketAppender; +BleAppender bleAppender; ///////////// BEGIN SETUP ///////////// #ifndef UNIT_TEST @@ -141,6 +143,7 @@ extern "C" void app_main() { // Configure and Initialize Logger logHandler.addAppender(&webSocketAppender); logHandler.addAppender(&udpAppender); + logHandler.addAppender(&bleAppender); logHandler.initialize(); ss2k->startTasks(); httpServer.start(); @@ -322,9 +325,9 @@ void SS2K::maintenanceLoop(void* pvParameters) { } #endif // DEBUG_STACK // Log userParameters - SS2K_LOG(MAIN_LOG_TAG, "PM Con %d, CAD con %d, HRM Con %d, W %d, Cad %d, HR %d, Gear %d, Res %d, Target Position %d", spinBLEClient.connectedPM, spinBLEClient.connectedCD, - spinBLEClient.connectedHRM, rtConfig->watts.getValue(), rtConfig->cad.getValue(), rtConfig->hr.getValue(), rtConfig->getShifterPosition(), - rtConfig->resistance.getValue(), ss2k->targetPosition); + SS2K_LOG(MAIN_LOG_TAG, "PM Con %d, CAD con %d, HRM Con %d, W %d, Cad %d, HR %d, Gear %d, Res %d, Current Pos %d, Target Pos %d", spinBLEClient.connectedPM, + spinBLEClient.connectedCD, spinBLEClient.connectedHRM, rtConfig->watts.getValue(), rtConfig->cad.getValue(), rtConfig->hr.getValue(), rtConfig->getShifterPosition(), + rtConfig->resistance.getValue(), ss2k->getCurrentPosition(), ss2k->getTargetPosition()); intervalTimer2 = millis(); } @@ -349,7 +352,7 @@ void SS2K::FTMSModeShiftModifier() { SS2K_LOG(MAIN_LOG_TAG, "ERG Shift. New Target: %dw", rtConfig->watts.getTarget()); // Format output for FTMS passthrough #ifndef INTERNAL_ERG_4EXT_FTMS - int adjustedTarget = rtConfig->watts.getTarget() / userConfig->getPowerCorrectionFactor(); + int adjustedTarget = round(rtConfig->watts.getTarget() / userConfig->getPowerCorrectionFactor()); const uint8_t translated[] = {FitnessMachineControlPointProcedure::SetTargetPower, (uint8_t)(adjustedTarget & 0xff), (uint8_t)(adjustedTarget >> 8)}; spinBLEClient.FTMSControlPointWrite(translated, 3); #endif @@ -377,8 +380,8 @@ void SS2K::FTMSModeShiftModifier() { default: // Sim Mode { - SS2K_LOG(MAIN_LOG_TAG, "Shift %+d pos %d tgt %d min %d max %d r_min %d r_max %d", shiftDelta, rtConfig->getShifterPosition(), ss2k->targetPosition, rtConfig->getMinStep(), - rtConfig->getMaxStep(), rtConfig->getMinResistance(), rtConfig->getMaxResistance()); + SS2K_LOG(MAIN_LOG_TAG, "Shift %+d pos %d tgt %d min %d max %d r_min %d r_max %d", shiftDelta, rtConfig->getShifterPosition(), ss2k->getTargetPosition(), + rtConfig->getMinStep(), rtConfig->getMaxStep(), rtConfig->getMinResistance(), rtConfig->getMaxResistance()); // Block Shifts further out of bounds if (((ss2k->targetPosition + shiftDelta * userConfig->getShiftStep()) < rtConfig->getMinStep()) && (shiftDelta < 0)) { SS2K_LOG(MAIN_LOG_TAG, "Shift Blocked by stepper limits."); @@ -458,36 +461,49 @@ void SS2K::moveStepper() { ss2k->syncMode = false; } + bool _closeToTarget = (abs(stepper->getCurrentPosition() - rtConfig->getMinStep()) <= (userConfig->getShiftStep() / 2)) || + (abs(stepper->getCurrentPosition() - rtConfig->getMaxStep()) <= (userConfig->getShiftStep() / 2)); + if (ss2k->pelotonIsConnected && !rtConfig->getHomed()) { - if ((rtConfig->resistance.getValue() > rtConfig->getMinResistance()) && (rtConfig->resistance.getValue() < rtConfig->getMaxResistance())) { - stepper->moveTo(ss2k->targetPosition); - } else if (rtConfig->resistance.getValue() <= rtConfig->getMinResistance()) { // Limit Stepper to Min Resistance - if (rtConfig->resistance.getValue() != rtConfig->getMinResistance()) { - stepper->moveTo(stepper->getCurrentPosition() + 20); - } - // Let the user Shift Out of this Position - if (ss2k->targetPosition > stepper->getCurrentPosition()) { - stepper->moveTo(ss2k->targetPosition); + // Peloton + not homed: gently walk away from the edges unless the user is actively shifting past them + if (rtConfig->resistance.getValue() < rtConfig->getMinResistance()) { // Below allowed resistance + // Nudge upward unless the user already asked to move higher + if (ss2k->targetPosition <= ss2k->getCurrentPosition()) { + ss2k->targetPosition = ss2k->getCurrentPosition() + 20; } - } else { // Limit Stepper to Max Resistance - if (rtConfig->resistance.getValue() != rtConfig->getMaxResistance()) { - stepper->moveTo(stepper->getCurrentPosition() - 20); - } - // Let the user Shift Out of this Position - if (ss2k->targetPosition < stepper->getCurrentPosition()) { - stepper->moveTo(ss2k->targetPosition); + } + if (rtConfig->resistance.getValue() > rtConfig->getMaxResistance()) { + // Nudge downward unless the user already asked to move lower + if (ss2k->targetPosition > ss2k->getCurrentPosition()) { + ss2k->targetPosition = ss2k->getCurrentPosition() - 20; } } - } else { // Normal move code for non-Peloton - if ((ss2k->targetPosition >= rtConfig->getMinStep()) && (ss2k->targetPosition <= rtConfig->getMaxStep())) { - stepper->moveTo(ss2k->targetPosition); - } else if (ss2k->targetPosition <= rtConfig->getMinStep()) { // Limit Stepper to Min Position - stepper->moveTo(rtConfig->getMinStep() + 1); - } else { // Limit Stepper to Max Position - stepper->moveTo(rtConfig->getMaxStep() - 1); + } else if (!rtConfig->getHomed()) { // Not homed: keep target inside the provisional range and learn bounds when power looks valid + // Flag when current position is within half a shift step of either bound + bool _closeToTarget = (abs(ss2k->getCurrentPosition() - rtConfig->getMinStep()) <= (userConfig->getShiftStep() / 2)) || + (abs(ss2k->getCurrentPosition() - rtConfig->getMaxStep()) <= (userConfig->getShiftStep() / 2)); + + if (ss2k->targetPosition < rtConfig->getMinStep()) { + // if (_closeToTarget && rtConfig->cad.getValue() > 0 && rtConfig->watts.getValue() > userConfig->getMinWatts() + POWERTABLE_WATT_INCREMENT) { + // rtConfig->setMinStep(ss2k->targetPosition); // Learn a tighter min bound from real effort + // } + ss2k->targetPosition = rtConfig->getMinStep() + 1; + } else if (ss2k->targetPosition > rtConfig->getMaxStep()) { + // if (_closeToTarget && rtConfig->cad.getValue() > 0 && rtConfig->watts.getValue() < userConfig->getMaxWatts() - POWERTABLE_WATT_INCREMENT) { + // rtConfig->setMaxStep(ss2k->targetPosition); // Learn a tighter max bound from real effort + // } + ss2k->targetPosition = rtConfig->getMaxStep() - 1; + } + } else { // Homed: simple clamp to the known good range + if (ss2k->targetPosition < rtConfig->getMinStep()) { + ss2k->targetPosition = rtConfig->getMinStep() + 1; + } else if (ss2k->targetPosition > rtConfig->getMaxStep()) { + ss2k->targetPosition = rtConfig->getMaxStep() - 1; } } + stepper->moveTo(ss2k->targetPosition); + if (rtConfig->cad.getValue() > 1) { stepper->enableOutputs(); stepper->setAutoEnable(false); @@ -525,7 +541,7 @@ void SS2K::_resistanceMove() { if (resistancePercent < 0) resistancePercent = 0; if (resistancePercent > 100) resistancePercent = 100; int64_t span = (int64_t)maxPos - (int64_t)minPos; - int32_t pos = minPos + (int32_t)((span * resistancePercent) / 100); + int32_t pos = minPos + (int32_t)round((span * resistancePercent) / 100.0f); if (usePwr) { // fallback to using ERG rtConfig->watts.setTarget(pos); rtConfig->setFTMSMode(FitnessMachineControlPointProcedure::SetTargetPower); @@ -654,7 +670,7 @@ void SS2K::_findEndStop(bool moveForward) { totalSgResult += driver.SG_RESULT(); delay(10); // Small delay between samples } - threshold = totalSgResult / SAMPLES_TO_AVERAGE; + threshold = round(totalSgResult / (float)SAMPLES_TO_AVERAGE); SS2K_LOG(MAIN_LOG_TAG, "Homing %s. Stable Threshold: %d, Sensitivity: %d", moveForward ? "forward (max)" : "backward (min)", threshold, userConfig->getHomingSensitivity()); @@ -745,26 +761,18 @@ void SS2K::_findFTMSHome(bool bothDirections) { i++; } - bool reachedTarget = (rtConfig->resistance.getValue() == targetResistance); + bool reachedTarget = (rtConfig->resistance.getValue() == targetResistance); int32_t travelDelta = abs(ss2k->getCurrentPosition() - lastPosition); bool iterExceeded = (i >= iMax); bool travelSatisfied = (travelDelta >= minTravel); - SS2K_LOG(MAIN_LOG_TAG, - "FTMS Homing sweep exit: target=%d current=%d reached=%s iter=%d/%d travelΔ=%d minTravel=%d travelMet=%s", - targetResistance, - rtConfig->resistance.getValue(), - reachedTarget ? "true" : "false", - i, - iMax, - travelDelta, - minTravel, - travelSatisfied ? "true" : "false"); + SS2K_LOG(MAIN_LOG_TAG, "FTMS Homing sweep exit: target=%d current=%d reached=%s iter=%d/%d travelΔ=%d minTravel=%d travelMet=%s", targetResistance, + rtConfig->resistance.getValue(), reachedTarget ? "true" : "false", i, iMax, travelDelta, minTravel, travelSatisfied ? "true" : "false"); }; ss2k->updateStepperSpeed(1500); // Use a slow-medium speed for homing // first back off of the stop if we're already there - int midTarget = (rtConfig->resistance.getMax() - rtConfig->resistance.getMin()) / 4; + int midTarget = round((rtConfig->resistance.getMax() - rtConfig->resistance.getMin()) / 4.0f); rtConfig->resistance.setTarget(midTarget); runHomingSweep(midTarget, nullptr, false); runHomingSweep(rtConfig->resistance.getMin(), "Homing to Min Resistance... Current: %d, Target: %d", false); @@ -796,7 +804,13 @@ void SS2K::_findFTMSHome(bool bothDirections) { void SS2K::goHome(bool bothDirections) { SS2K_LOG(MAIN_LOG_TAG, "Starting homing procedure..."); - if (bothDirections) fitnessMachineService.spinDown(FitnessMachineStatus::SpinDown_SpinDownRequested); + if (bothDirections) { + fitnessMachineService.spinDown(FitnessMachineStatus::SpinDown_SpinDownRequested); + if (!userConfig->getPTab4Pwr()) { + // clean slate for homing + powerTable->reset(); + } + } // if we're using real resistance from a FTMS bike, find those values for the reported min and max resistance instead of using hard stops. if (!rtConfig->resistance.getSimulate() && userConfig->getConnectedPowerMeter() != NONE && rtConfig->resistance.getMax() > 0) { From 19f65a85a38379aa06bc81eed7c196fc96c380ad Mon Sep 17 00:00:00 2001 From: GitHub Action Date: Sun, 1 Mar 2026 01:25:48 +0000 Subject: [PATCH 12/12] Update changelog for version 25.12.25 --- CHANGELOG.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 27122624..5cfeaf6e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,15 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added +### Changed + +### Hardware + + +## [25.12.25] + +### Added + ### Changed - Removed >0 watts requirement to compute ERG.