From 991e03a7c37ebe10e97f1aa50e9f6af62598ec3e Mon Sep 17 00:00:00 2001
From: ptx2 <68594395+ptx2@users.noreply.github.com>
Date: Thu, 11 Feb 2021 12:12:27 -0500
Subject: [PATCH] Add Cycling Speed and Cadence to server. (#39)

This enables cadence broadcasting via the BLE CSC service. e.g. for apps
like Peloton on iOS/Android.
---
 src/servers/ble/index.js                      |  8 +++-
 .../characteristics/csc-feature.js            | 20 ++++++++
 .../characteristics/csc-measurement.js        | 47 +++++++++++++++++++
 .../cycling-speed-and-cadence/index.js        | 32 +++++++++++++
 4 files changed, 105 insertions(+), 2 deletions(-)
 create mode 100644 src/servers/ble/services/cycling-speed-and-cadence/characteristics/csc-feature.js
 create mode 100644 src/servers/ble/services/cycling-speed-and-cadence/characteristics/csc-measurement.js
 create mode 100644 src/servers/ble/services/cycling-speed-and-cadence/index.js

diff --git a/src/servers/ble/index.js b/src/servers/ble/index.js
index 7c4dd039..262f6dfc 100644
--- a/src/servers/ble/index.js
+++ b/src/servers/ble/index.js
@@ -1,4 +1,5 @@
 import {CyclingPowerService} from './services/cycling-power'
+import {CyclingSpeedAndCadenceService} from './services/cycling-speed-and-cadence'
 import {BleServer} from '../../util/ble-server'
 
 export const DEFAULT_NAME = 'Gymnasticon';
@@ -14,7 +15,8 @@ export class GymnasticonServer extends BleServer {
 	 */
   constructor(bleno, name=DEFAULT_NAME) {
     super(bleno, name, [
-      new CyclingPowerService()
+      new CyclingPowerService(),
+      new CyclingSpeedAndCadenceService(),
     ])
   }
 
@@ -27,6 +29,8 @@ export class GymnasticonServer extends BleServer {
    * @param {number} measurement.crank.timestamp - timestamp at last crank event.
    */
   updateMeasurement(measurement) {
-    this.services[0].updateMeasurement(measurement)
+    for (let s of this.services) {
+      s.updateMeasurement(measurement)
+    }
   }
 }
diff --git a/src/servers/ble/services/cycling-speed-and-cadence/characteristics/csc-feature.js b/src/servers/ble/services/cycling-speed-and-cadence/characteristics/csc-feature.js
new file mode 100644
index 00000000..f612357e
--- /dev/null
+++ b/src/servers/ble/services/cycling-speed-and-cadence/characteristics/csc-feature.js
@@ -0,0 +1,20 @@
+import {Characteristic, Descriptor} from '@abandonware/bleno';
+
+/**
+ * Bluetooth LE GATT CSC Feature Characteristic implementation.
+ */
+export class CscFeatureCharacteristic extends Characteristic {
+  constructor() {
+    super({
+      uuid: '2a5c',
+      properties: ['read'],
+      descriptors: [
+        new Descriptor({
+          uuid: '2901',
+          value: 'CSC Feature'
+        })
+      ],
+      value: Buffer.from([2,0]) // crank revolution data
+    })
+  }
+}
diff --git a/src/servers/ble/services/cycling-speed-and-cadence/characteristics/csc-measurement.js b/src/servers/ble/services/cycling-speed-and-cadence/characteristics/csc-measurement.js
new file mode 100644
index 00000000..d3d80a24
--- /dev/null
+++ b/src/servers/ble/services/cycling-speed-and-cadence/characteristics/csc-measurement.js
@@ -0,0 +1,47 @@
+import {Characteristic, Descriptor} from '@abandonware/bleno';
+
+const FLAG_HASCRANKDATA = (1<<1);
+const CRANK_TIMESTAMP_SCALE = 1024 / 1000; // timestamp resolution is 1/1024 sec
+
+/**
+ * Bluetooth LE GATT CSC Measurement Characteristic implementation.
+ */
+export class CscMeasurementCharacteristic extends Characteristic {
+  constructor() {
+    super({
+      uuid: '2a5b',
+      properties: ['notify'],
+      descriptors: [
+        new Descriptor({
+          uuid: '2903',
+          value: Buffer.alloc(2)
+        })
+      ]
+    })
+  }
+
+  /**
+   * Notify subscriber (e.g. Zwift) of new CSC Measurement.
+   * @param {object} measurement - new csc measurement.
+   * @param {object} measurement.crank - last crank event.
+   * @param {number} measurement.crank.revolutions - revolution count at last crank event.
+   * @param {number} measurement.crank.timestamp - timestamp at last crank event.
+   */
+  updateMeasurement({ crank }) {
+    let flags = 0;
+
+    const value = Buffer.alloc(5);
+
+    const revolutions16bit = crank.revolutions & 0xffff;
+    const timestamp16bit = Math.round(crank.timestamp * CRANK_TIMESTAMP_SCALE) & 0xffff;
+    value.writeUInt16LE(revolutions16bit, 1);
+    value.writeUInt16LE(timestamp16bit, 3);
+    flags |= FLAG_HASCRANKDATA;
+
+    value.writeUInt8(flags, 0);
+
+    if (this.updateValueCallback) {
+      this.updateValueCallback(value)
+    }
+  }
+}
diff --git a/src/servers/ble/services/cycling-speed-and-cadence/index.js b/src/servers/ble/services/cycling-speed-and-cadence/index.js
new file mode 100644
index 00000000..fae4a116
--- /dev/null
+++ b/src/servers/ble/services/cycling-speed-and-cadence/index.js
@@ -0,0 +1,32 @@
+import {PrimaryService} from '@abandonware/bleno';
+import {CscMeasurementCharacteristic} from './characteristics/csc-measurement';
+import {CscFeatureCharacteristic} from './characteristics/csc-feature';
+
+/**
+ * Bluetooth LE GATT Cycling Speed and Cadence Service implementation.
+ */
+export class CyclingSpeedAndCadenceService extends PrimaryService {
+  /**
+   * Create a CyclingSpeedAndCadenceService instance.
+   */
+  constructor() {
+    super({
+      uuid: '1816',
+      characteristics: [
+        new CscMeasurementCharacteristic(),
+        new CscFeatureCharacteristic(),
+      ]
+    })
+  }
+
+  /**
+   * Notify subscriber (e.g. Zwift) of new CSC Measurement.
+   * @param {object} measurement - new csc measurement.
+   * @param {object} measurement.crank - last crank event.
+   * @param {number} measurement.crank.revolutions - revolution count at last crank event.
+   * @param {number} measurement.crank.timestamp - timestamp at last crank event.
+   */
+  updateMeasurement(measurement) {
+    this.characteristics[0].updateMeasurement(measurement)
+  }
+}