288 lines
10 KiB
C#
288 lines
10 KiB
C#
using System;
|
|
using System.Collections;
|
|
using System.Collections.Generic;
|
|
using UnityEngine;
|
|
using System.Linq;
|
|
|
|
public class PerformanceMonitorManager : MonoBehaviour
|
|
{
|
|
public static PerformanceMonitorManager Instance;
|
|
|
|
public Action<RowingStats> OnStatsUpdated;
|
|
public Action<List<float>> OnForceCurveUpdated;
|
|
public Action<bool> OnConnectionStateChanged;
|
|
public Action<string> OnLog;
|
|
|
|
[Serializable]
|
|
public class RowingStats
|
|
{
|
|
public bool IsConnected;
|
|
public float Distance;
|
|
public float ElapsedTime;
|
|
public int Watts;
|
|
public int SPM;
|
|
public int HeartRate;
|
|
public int Calories;
|
|
public List<float> CurrentForceCurve = new List<float>();
|
|
}
|
|
|
|
public RowingStats Stats = new RowingStats();
|
|
|
|
[Header("Settings")]
|
|
[Tooltip("10Hz is the sweet spot for BLE. Faster may cause packet collisions.")]
|
|
[SerializeField] private float pollRate = 0.10f;
|
|
[SerializeField] private float activityTimeout = 3.5f; // Time before we consider the user "stopped"
|
|
private string _deviceAddress;
|
|
private bool _isSubscribedToControl = false;
|
|
private List<byte> _csafeBuffer = new List<byte>();
|
|
|
|
// Watchdog
|
|
private float _lastDataTime = 0f;
|
|
private int _lastStrokeState = -1;
|
|
|
|
public static class UUIDs
|
|
{
|
|
public const string Svc_Rowing = "ce060030-43e5-11e4-916c-0800200c9a66";
|
|
public const string Char_Multiplexed = "ce060080-43e5-11e4-916c-0800200c9a66";
|
|
public const string Svc_Control = "ce060020-43e5-11e4-916c-0800200c9a66";
|
|
public const string Char_ControlWrite = "ce060021-43e5-11e4-916c-0800200c9a66";
|
|
public const string Char_ControlRead = "ce060022-43e5-11e4-916c-0800200c9a66";
|
|
}
|
|
|
|
private void Awake()
|
|
{
|
|
if (Instance == null) Instance = this;
|
|
else Destroy(gameObject);
|
|
DontDestroyOnLoad(gameObject);
|
|
}
|
|
|
|
private void Start()
|
|
{
|
|
InitializeBLE();
|
|
}
|
|
|
|
private void Update()
|
|
{
|
|
if (Stats.IsConnected)
|
|
{
|
|
// Instead of checking SPM == 0, we check how long it's been since we got ANY multiplexed data.
|
|
// This prevents Watts dropping to 0 during a long, slow recovery phase (like in Stone Giant mode).
|
|
if (Time.time - _lastDataTime > activityTimeout)
|
|
{
|
|
if (Stats.Watts != 0 || Stats.SPM != 0)
|
|
{
|
|
Stats.Watts = 0;
|
|
Stats.SPM = 0;
|
|
OnStatsUpdated?.Invoke(Stats);
|
|
Log("Rowing stopped (Timeout). Resetting active stats.");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
public void InitializeBLE()
|
|
{
|
|
BluetoothLEHardwareInterface.Initialize(true, false, () => Log("BLE Init"), (err) => Log("Error: " + err));
|
|
}
|
|
|
|
public void StartScan()
|
|
{
|
|
if (Stats.IsConnected) return;
|
|
Log("Scanning...");
|
|
BluetoothLEHardwareInterface.ScanForPeripheralsWithServices(null, (address, name) => {
|
|
if (!string.IsNullOrEmpty(name) && (name.Contains("Concept2") || name.Contains("PM5")))
|
|
{
|
|
Log($"Found {name}");
|
|
_deviceAddress = address;
|
|
BluetoothLEHardwareInterface.StopScan();
|
|
StartCoroutine(ConnectSequence());
|
|
}
|
|
}, null);
|
|
}
|
|
|
|
private IEnumerator ConnectSequence()
|
|
{
|
|
yield return new WaitForSeconds(0.5f);
|
|
Connect();
|
|
}
|
|
|
|
private void Connect()
|
|
{
|
|
BluetoothLEHardwareInterface.ConnectToPeripheral(_deviceAddress, (addr) => {
|
|
Stats.IsConnected = true;
|
|
OnConnectionStateChanged?.Invoke(true);
|
|
Log("Connected!");
|
|
}, null, (addr, svc, chr) => {
|
|
|
|
if (svc.ToUpper().Contains("0030") && chr.ToUpper().Contains("0080"))
|
|
BluetoothLEHardwareInterface.SubscribeCharacteristicWithDeviceAddress(_deviceAddress, svc, chr, null, OnMultiplexedDataReceived);
|
|
|
|
if (svc.ToUpper().Contains("0020") && chr.ToUpper().Contains("0022"))
|
|
{
|
|
if (!_isSubscribedToControl)
|
|
{
|
|
BluetoothLEHardwareInterface.SubscribeCharacteristicWithDeviceAddress(_deviceAddress, svc, chr, null, OnControlDataReceived);
|
|
_isSubscribedToControl = true;
|
|
StartCoroutine(CSAFEPollLoop());
|
|
}
|
|
}
|
|
});
|
|
}
|
|
|
|
private IEnumerator CSAFEPollLoop()
|
|
{
|
|
while (_isSubscribedToControl && Stats.IsConnected)
|
|
{
|
|
// 1. Get Stroke State
|
|
// 2. Get Force Plot Data (Command 0x6B) with Block Size 32 (0x20)
|
|
// 3. Get Power
|
|
string[] commands = new string[]
|
|
{
|
|
"CSAFE_PM_GET_STROKESTATE",
|
|
"CSAFE_PM_GET_FORCEPLOTDATA", "32",
|
|
"CSAFE_GETPOWER_CMD",
|
|
"CSAFE_GETHRCUR_CMD"
|
|
};
|
|
|
|
byte[] payload = CSAFECommand.Write(commands).ToArray();
|
|
|
|
BluetoothLEHardwareInterface.WriteCharacteristic(
|
|
_deviceAddress,
|
|
UUIDs.Svc_Control,
|
|
UUIDs.Char_ControlWrite,
|
|
payload,
|
|
payload.Length,
|
|
true,
|
|
(x) => {}
|
|
);
|
|
|
|
yield return new WaitForSeconds(pollRate);
|
|
}
|
|
}
|
|
|
|
private void OnMultiplexedDataReceived(string addr, string chr, byte[] data)
|
|
{
|
|
if (data == null || data.Length < 1) return;
|
|
_lastDataTime = Time.time;
|
|
|
|
int packetID = data[0];
|
|
switch (packetID)
|
|
{
|
|
case 0x31:
|
|
if (data.Length >= 7) {
|
|
Stats.ElapsedTime = (data[1] | (data[2] << 8) | (data[3] << 16)) * 0.01f;
|
|
Stats.Distance = (data[4] | (data[5] << 8) | (data[6] << 16)) * 0.1f;
|
|
}
|
|
break;
|
|
case 0x32:
|
|
if (data.Length >= 7) {
|
|
if (data[6] != 255) Stats.SPM = data[6];
|
|
}
|
|
break;
|
|
case 0x33:
|
|
if (data.Length >= 7) {
|
|
Stats.Calories = (data[5] | (data[6] << 8));
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
private void OnControlDataReceived(string addr, string chr, byte[] data)
|
|
{
|
|
_csafeBuffer.AddRange(data);
|
|
while (_csafeBuffer.Contains(0xF2))
|
|
{
|
|
int stopIdx = _csafeBuffer.IndexOf(0xF2);
|
|
byte[] completeFrame = _csafeBuffer.GetRange(0, stopIdx + 1).ToArray();
|
|
_csafeBuffer.RemoveRange(0, stopIdx + 1);
|
|
ParseCSAFE(completeFrame);
|
|
}
|
|
}
|
|
|
|
private void ParseCSAFE(byte[] frame)
|
|
{
|
|
try
|
|
{
|
|
var res = CSAFECommand.Read(frame);
|
|
|
|
// --- 1. STROKE STATE ---
|
|
if (res.ContainsKey("CSAFE_PM_GET_STROKESTATE"))
|
|
{
|
|
// We keep this variable for debugging, but no longer use it as the curve trigger
|
|
_lastStrokeState = int.Parse(res["CSAFE_PM_GET_STROKESTATE"][0]);
|
|
}
|
|
|
|
// --- 2. FORCE CURVE PARSING ---
|
|
if (res.ContainsKey("CSAFE_PM_GET_FORCEPLOTDATA"))
|
|
{
|
|
string rawCsvData = res["CSAFE_PM_GET_FORCEPLOTDATA"][0];
|
|
string[] byteStrings = rawCsvData.Split(',');
|
|
|
|
if (byteStrings.Length > 0)
|
|
{
|
|
int bytesRead = int.Parse(byteStrings[0]);
|
|
|
|
// NEW BULLETPROOF TRIGGER: The PM5 sends 0 bytes during the recovery phase.
|
|
// If bytesRead is 0, but we have data accumulated, the drive JUST finished.
|
|
if (bytesRead == 0)
|
|
{
|
|
if (Stats.CurrentForceCurve.Count > 0)
|
|
{
|
|
// 1. Emit an empty list to signal ForceCurveDataLogger to print the completed stroke
|
|
OnForceCurveUpdated?.Invoke(new List<float>());
|
|
|
|
// 2. Clear our internal buffer to prep for the next stroke
|
|
Stats.CurrentForceCurve.Clear();
|
|
}
|
|
}
|
|
else if (bytesRead > 0 && byteStrings.Length >= bytesRead + 1)
|
|
{
|
|
List<float> newPoints = new List<float>();
|
|
|
|
// Start at index 1. Each point consists of 2 bytes.
|
|
for (int i = 1; i < bytesRead; i += 2)
|
|
{
|
|
if (i + 1 >= byteStrings.Length) break;
|
|
|
|
// ENDIANNESS FIX: Page 91 of CSAFE docs explicitly shows LSB first, then MSB.
|
|
int lsb = int.Parse(byteStrings[i]); // Byte 1: LSB
|
|
int msb = int.Parse(byteStrings[i + 1]); // Byte 2: MSB
|
|
|
|
// Combine MSB and LSB correctly (Little Endian)
|
|
int forceValue = (msb << 8) | lsb;
|
|
|
|
newPoints.Add(forceValue);
|
|
}
|
|
|
|
if (newPoints.Count > 0)
|
|
{
|
|
Stats.CurrentForceCurve.AddRange(newPoints);
|
|
OnForceCurveUpdated?.Invoke(Stats.CurrentForceCurve); // Emit accumulated list
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// --- 3. POWER & HR ---
|
|
if (res.ContainsKey("CSAFE_GETPOWER_CMD"))
|
|
{
|
|
if (int.TryParse(res["CSAFE_GETPOWER_CMD"][0], out int watts))
|
|
Stats.Watts = watts;
|
|
}
|
|
|
|
if (res.ContainsKey("CSAFE_GETHRCUR_CMD"))
|
|
{
|
|
if (int.TryParse(res["CSAFE_GETHRCUR_CMD"][0], out int hr))
|
|
Stats.HeartRate = hr;
|
|
}
|
|
|
|
// Emit the overall stats update
|
|
OnStatsUpdated?.Invoke(Stats);
|
|
}
|
|
catch (Exception e)
|
|
{
|
|
Debug.Log("CSAFE Parse Error: " + e.Message);
|
|
}
|
|
}
|
|
private void Log(string m) { Debug.Log("[PM5] " + m); OnLog?.Invoke(m); }
|
|
} |