first push!
This commit is contained in:
288
Assets/Scripts/PM5/PerformanceMonitorManager.cs
Normal file
288
Assets/Scripts/PM5/PerformanceMonitorManager.cs
Normal file
@@ -0,0 +1,288 @@
|
||||
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); }
|
||||
}
|
||||
Reference in New Issue
Block a user