marcin/Aether-Engine
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first push!

Browse Source
This commit is contained in:
marcin
2026-02-20 17:53:43 +01:00
parent ab073a6a39
commit e723ab86b9
274 changed files with 89671 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

381
Assets/Scripts/PM5/CSAFEProtocol.cs Normal file
View File

@@ -0,0 +1,381 @@
using System;
using System.Linq;
using System.Collections.Generic;
public static class CSAFEDictionary
{
public static readonly byte ExtendedFrameStartFlag = 0xF0;
public static readonly byte StandardFrameStartFlag = 0xF1;
public static readonly byte StopFrameFlag = 0xF2;
public static readonly byte ByteStuffingFlag = 0xF3;
public static readonly Dictionary<string, List<object>> Cmd = new Dictionary<string, List<object>>
{
// Standard Short Commands
{ "CSAFE_GETSTATUS_CMD", new List<object> { 0x80, new int[] { } } },
{ "CSAFE_RESET_CMD", new List<object> { 0x81, new int[] { } } },
{ "CSAFE_GOIDLE_CMD", new List<object> { 0x82, new int[] { } } },
{ "CSAFE_GOHAVEID_CMD", new List<object> { 0x83, new int[] { } } },
{ "CSAFE_GOINUSE_CMD", new List<object> { 0x85, new int[] { } } },
{ "CSAFE_GOFINISHED_CMD", new List<object> { 0x86, new int[] { } } },
{ "CSAFE_GOREADY_CMD", new List<object> { 0x87, new int[] { } } },
{ "CSAFE_BADID_CMD", new List<object> { 0x88, new int[] { } } },
{ "CSAFE_GETVERSION_CMD", new List<object> { 0x91, new int[] { } } },
{ "CSAFE_GETID_CMD", new List<object> { 0x92, new int[] { } } },
{ "CSAFE_GETUNITS_CMD", new List<object> { 0x93, new int[] { } } },
{ "CSAFE_GETSERIAL_CMD", new List<object> { 0x94, new int[] { } } },
{ "CSAFE_GETODOMETER_CMD", new List<object> { 0x9B, new int[] { } } },
{ "CSAFE_GETERRORCODE_CMD", new List<object> { 0x9C, new int[] { } } },
{ "CSAFE_GETTWORK_CMD", new List<object> { 0xA0, new int[] { } } },
{ "CSAFE_GETHORIZONTAL_CMD", new List<object> { 0xA1, new int[] { } } },
{ "CSAFE_GETCALORIES_CMD", new List<object> { 0xA3, new int[] { } } },
{ "CSAFE_GETPROGRAM_CMD", new List<object> { 0xA4, new int[] { } } },
{ "CSAFE_GETPACE_CMD", new List<object> { 0xA6, new int[] { } } },
{ "CSAFE_GETCADENCE_CMD", new List<object> { 0xA7, new int[] { } } },
{ "CSAFE_GETUSERINFO_CMD", new List<object> { 0xAB, new int[] { } } },
{ "CSAFE_GETHRCUR_CMD", new List<object> { 0xB0, new int[] { } } },
{ "CSAFE_GETPOWER_CMD", new List<object> { 0xB4, new int[] { } } },
// Configuration / Set Commands
{ "CSAFE_AUTOUPLOAD_CMD", new List<object> { 0x01, new int[] { 1 } } },
{ "CSAFE_IDDIGITS_CMD", new List<object> { 0x10, new int[] { 1 } } },
{ "CSAFE_SETTIME_CMD", new List<object> { 0x11, new int[] { 1, 1, 1 } } },
{ "CSAFE_SETDATE_CMD", new List<object> { 0x12, new int[] { 1, 1, 1 } } },
{ "CSAFE_SETTIMEOUT_CMD", new List<object> { 0x13, new int[] { 1 } } },
{ "CSAFE_SETUSERCFG1_CMD", new List<object> { 0x1A, new int[] { 0 } } },
{ "CSAFE_SETTWORK_CMD", new List<object> { 0x20, new int[] { 1, 1, 1 } } },
{ "CSAFE_SETHORIZONTAL_CMD", new List<object> { 0x21, new int[] { 2, 1 } } },
{ "CSAFE_SETCALORIES_CMD", new List<object> { 0x23, new int[] { 2 } } },
{ "CSAFE_SETPROGRAM_CMD", new List<object> { 0x24, new int[] { 1, 1 } } },
{ "CSAFE_SETPOWER_CMD", new List<object> { 0x34, new int[] { 2, 1 } } },
{ "CSAFE_GETCAPS_CMD", new List<object> { 0x70, new int[] { 1 } } },
// PM5 Specific Long Commands (Wrapper 0x1A)
{ "CSAFE_PM_GET_WORKOUTTYPE", new List<object> { 0x89, new int[] { }, 0x1A } },
{ "CSAFE_PM_GET_DRAGFACTOR", new List<object> { 0xC1, new int[] { }, 0x1A } },
{ "CSAFE_PM_GET_STROKESTATE", new List<object> { 0xBF, new int[] { }, 0x1A } },
{ "CSAFE_PM_GET_WORKTIME", new List<object> { 0xA0, new int[] { }, 0x1A } },
{ "CSAFE_PM_GET_WORKDISTANCE", new List<object> { 0xA3, new int[] { }, 0x1A } },
{ "CSAFE_PM_GET_ERRORVALUE", new List<object> { 0xC9, new int[] { }, 0x1A } },
{ "CSAFE_PM_GET_WORKOUTSTATE", new List<object> { 0x8D, new int[] { }, 0x1A } },
{ "CSAFE_PM_GET_WORKOUTINTERVALCOUNT", new List<object> { 0x9F, new int[] { }, 0x1A } },
{ "CSAFE_PM_GET_INTERVALTYPE", new List<object> { 0x8E, new int[] { }, 0x1A } },
{ "CSAFE_PM_GET_RESTTIME", new List<object> { 0xCF, new int[] { }, 0x1A } },
{ "CSAFE_PM_SET_SPLITDURATION", new List<object> { 0x05, new int[] { 1, 4 }, 0x1A } }, // Fixed ID
{ "CSAFE_PM_GET_FORCEPLOTDATA", new List<object> { 0x6B, new int[] { 1 }, 0x1A } }, // Fixed to 0x6B, 1 arg (block size)
{ "CSAFE_PM_SET_SCREENERRORMODE", new List<object> { 0x27, new int[] { 1 }, 0x1A } },
{ "CSAFE_PM_GET_HEARTBEATDATA", new List<object> { 0x6C, new int[] { }, 0x1A } } // FIXED: No arguments required
};
public static readonly Dictionary<int, List<object>> Resp = new Dictionary<int, List<object>>
{
{ 0x80, new List<object> { "CSAFE_GETSTATUS_CMD", new int[] { 0 } } },
{ 0x81, new List<object> { "CSAFE_RESET_CMD", new int[] { 0 } } },
{ 0x82, new List<object> { "CSAFE_GOIDLE_CMD", new int[] { 0 } } },
{ 0x83, new List<object> { "CSAFE_GOHAVEID_CMD", new int[] { 0 } } },
{ 0x85, new List<object> { "CSAFE_GOINUSE_CMD", new int[] { 0 } } },
{ 0x86, new List<object> { "CSAFE_GOFINISHED_CMD", new int[] { 0 } } },
{ 0x87, new List<object> { "CSAFE_GOREADY_CMD", new int[] { 0 } } },
{ 0x88, new List<object> { "CSAFE_BADID_CMD", new int[] { 0 } } },
{ 0x91, new List<object> { "CSAFE_GETVERSION_CMD", new int[] { 1, 1, 1, 2, 2 } } },
{ 0x92, new List<object> { "CSAFE_GETID_CMD", new int[] { -5 } } },
{ 0x93, new List<object> { "CSAFE_GETUNITS_CMD", new int[] { 1 } } },
{ 0x94, new List<object> { "CSAFE_GETSERIAL_CMD", new int[] { -9 } } },
{ 0x9B, new List<object> { "CSAFE_GETODOMETER_CMD", new int[] { 4, 1 } } },
{ 0x9C, new List<object> { "CSAFE_GETERRORCODE_CMD", new int[] { 3 } } },
{ 0xA0, new List<object> { "CSAFE_GETTWORK_CMD", new int[] { 1, 1, 1 } } },
{ 0xA1, new List<object> { "CSAFE_GETHORIZONTAL_CMD", new int[] { 2, 1 } } },
{ 0xA3, new List<object> { "CSAFE_GETCALORIES_CMD", new int[] { 2 } } },
{ 0xA4, new List<object> { "CSAFE_GETPROGRAM_CMD", new int[] { 1 } } },
{ 0xA6, new List<object> { "CSAFE_GETPACE_CMD", new int[] { 2, 1 } } },
{ 0xA7, new List<object> { "CSAFE_GETCADENCE_CMD", new int[] { 2, 1 } } },
{ 0xAB, new List<object> { "CSAFE_GETUSERINFO_CMD", new int[] { 2, 1, 1, 1 } } },
{ 0xB0, new List<object> { "CSAFE_GETHRCUR_CMD", new int[] { 1 } } },
{ 0xB4, new List<object> { "CSAFE_GETPOWER_CMD", new int[] { 2, 1 } } },
{ 0x01, new List<object> { "CSAFE_AUTOUPLOAD_CMD", new int[] { 0 } } },
{ 0x10, new List<object> { "CSAFE_IDDIGITS_CMD", new int[] { 0 } } },
{ 0x11, new List<object> { "CSAFE_SETTIME_CMD", new int[] { 0 } } },
{ 0x12, new List<object> { "CSAFE_SETDATE_CMD", new int[] { 0 } } },
{ 0x13, new List<object> { "CSAFE_SETTIMEOUT_CMD", new int[] { 0 } } },
{ 0x1A, new List<object> { "CSAFE_SETUSERCFG1_CMD", new int[] { 0 } } },
{ 0x20, new List<object> { "CSAFE_SETTWORK_CMD", new int[] { 0 } } },
{ 0x21, new List<object> { "CSAFE_SETHORIZONTAL_CMD", new int[] { 0 } } },
{ 0x23, new List<object> { "CSAFE_SETCALORIES_CMD", new int[] { 0 } } },
{ 0x24, new List<object> { "CSAFE_SETPROGRAM_CMD", new int[] { 0 } } },
{ 0x34, new List<object> { "CSAFE_SETPOWER_CMD", new int[] { 0 } } },
{ 0x70, new List<object> { "CSAFE_GETCAPS_CMD", new int[] { 11 } } },
{ 0x1A89, new List<object> { "CSAFE_PM_GET_WORKOUTTYPE", new int[] { 1 } } },
{ 0x1AC1, new List<object> { "CSAFE_PM_GET_DRAGFACTOR", new int[] { 1 } } },
{ 0x1ABF, new List<object> { "CSAFE_PM_GET_STROKESTATE", new int[] { 1 } } },
{ 0x1AA0, new List<object> { "CSAFE_PM_GET_WORKTIME", new int[] { 4, 1 } } },
{ 0x1AA3, new List<object> { "CSAFE_PM_GET_WORKDISTANCE", new int[] { 4, 1 } } },
{ 0x1AC9, new List<object> { "CSAFE_PM_GET_ERRORVALUE", new int[] { 2 } } },
{ 0x1A8D, new List<object> { "CSAFE_PM_GET_WORKOUTSTATE", new int[] { 1 } } },
{ 0x1A9F, new List<object> { "CSAFE_PM_GET_WORKOUTINTERVALCOUNT", new int[] { 1 } } },
{ 0x1A8E, new List<object> { "CSAFE_PM_GET_INTERVALTYPE", new int[] { 1 } } },
{ 0x1ACF, new List<object> { "CSAFE_PM_GET_RESTTIME", new int[] { 2 } } },
{ 0x1A05, new List<object> { "CSAFE_PM_SET_SPLITDURATION", new int[] { 0 } } },
{ 0x1A6B, new List<object> { "CSAFE_PM_GET_FORCEPLOTDATA", new int[] { -1 } } },
{ 0x1A27, new List<object> { "CSAFE_PM_SET_SCREENERRORMODE", new int[] { 0 } } },
{ 0x1A6C, new List<object> { "CSAFE_PM_GET_HEARTBEATDATA", new int[] { -1 } } }
};
}
public static class CSAFECommand
{
public static List<byte> Write(string[] arguments)
{
int i = 0;
List<byte> message = new List<byte>();
List<byte> wrapper = new List<byte>();
int wrapped = 0;
while (i < arguments.Length)
{
string argument = arguments[i];
int commandPropertiesIdInt = Convert.ToInt32(CSAFEDictionary.Cmd[argument][0]);
int[] cmdProps = CSAFEDictionary.Cmd[argument][1] as int[];
int wrapperInt = 0;
if (CSAFEDictionary.Cmd[argument].Count > 2)
wrapperInt = Convert.ToInt32(CSAFEDictionary.Cmd[argument][2]);
List<byte> command = new List<byte>();
if (cmdProps.Length != 0)
{
for (int bytes = 0; bytes < cmdProps.Length; bytes++)
{
i++;
int intValue = int.Parse(arguments[i]);
byte[] value = IntToBytes(intValue, cmdProps[bytes]);
command.AddRange(value);
}
command.Insert(0, Convert.ToByte(command.Count));
}
command.Insert(0, Convert.ToByte(commandPropertiesIdInt));
if (wrapperInt == 0x1A)
{
if (wrapped == 0)
{
wrapper.Insert(0, Convert.ToByte(wrapperInt));
wrapped = 1;
}
wrapper.AddRange(command);
}
else
{
message.AddRange(command);
}
i++;
}
if (wrapped == 1)
{
wrapper.Insert(1, Convert.ToByte(wrapper.Count - 1));
message.AddRange(wrapper);
}
int checksum = 0;
for (int j = 0; j < message.Count; j++)
{
checksum ^= message[j];
}
int k = 0;
while (k < message.Count)
{
if (0xF0 <= message[k] && message[k] <= 0xF3)
{
byte stuffValue = Convert.ToByte(message[k] & 0x03);
message[k] = CSAFEDictionary.ByteStuffingFlag;
message.Insert(k + 1, stuffValue);
k++;
}
k++;
}
if (0xF0 <= checksum && checksum <= 0xF3)
{
message.Add(CSAFEDictionary.ByteStuffingFlag);
message.Add(Convert.ToByte(checksum & 0x03));
}
else
{
message.Add(Convert.ToByte(checksum));
}
message.Insert(0, CSAFEDictionary.StandardFrameStartFlag);
message.Add(CSAFEDictionary.StopFrameFlag);
return message;
}
public static Dictionary<string, List<string>> Read(byte[] transmission)
{
Dictionary<string, List<string>> response = new Dictionary<string, List<string>>();
List<byte> message = transmission.ToList();
int startFlagIndex = message.IndexOf(CSAFEDictionary.StandardFrameStartFlag);
int extStartFlagIndex = message.IndexOf(CSAFEDictionary.ExtendedFrameStartFlag);
int startIndex = Math.Max(startFlagIndex, extStartFlagIndex);
int stopFlagIndex = message.IndexOf(CSAFEDictionary.StopFrameFlag);
if (startIndex == -1 || stopFlagIndex == -1 || stopFlagIndex < startIndex)
return response;
message = message.GetRange(startIndex + 1, stopFlagIndex - startIndex - 1);
message = CheckMessage(message);
if (message == null || message.Count == 0) return response;
int k = 0;
int msgState = message[k];
response["CSAFE_GETSTATUS_CMD"] = new List<string> { msgState.ToString() };
k++;
// NEW: Track the end index of the PM5 wrapper block
int wrapperEndIndex = -1;
while (k < message.Count)
{
try
{
int k_before = k; // Track where this specific command started
int responseId = message[k];
k++;
// Wrapper Check
if (responseId == 0x1A)
{
if (k >= message.Count) break;
int wrapperLength = message[k];
k++;
if (wrapperLength > 0)
{
wrapperEndIndex = k + wrapperLength; // Mark the boundary of wrapped data
if (k < message.Count)
{
responseId = message[k];
responseId = responseId | (0x1A << 8);
k++;
}
}
else
{
continue;
}
}
// NEW: If we are still reading bytes that fall inside the wrapper length,
// automatically apply the 0x1A PM5 prefix!
else if (k_before < wrapperEndIndex)
{
responseId = responseId | (0x1A << 8);
}
if (!CSAFEDictionary.Resp.ContainsKey(responseId))
{
if (k < message.Count) k += message[k] + 1;
continue;
}
List<object> commandResponse = CSAFEDictionary.Resp[responseId];
string commandName = commandResponse[0] as string;
int[] commandByteCountArray = commandResponse[1] as int[];
List<string> result = new List<string>();
// SAFEGUARD: Prevent out of bounds
if (k >= message.Count) break;
int payloadLength = message[k];
k++;
int bytesReadForCmd = 0;
foreach (int commandByteCount in commandByteCountArray)
{
if (bytesReadForCmd >= payloadLength) break;
int bytesToRead = commandByteCount >= 0 ? commandByteCount : (payloadLength - bytesReadForCmd);
if (k + bytesToRead > message.Count) break;
byte[] rawBytes = message.GetRange(k, bytesToRead).ToArray();
string value = "";
if (commandByteCount >= 0)
{
int intValue = 0;
for (int b = 0; b < rawBytes.Length; b++)
{
intValue |= (rawBytes[b] << (8 * b));
}
value = intValue.ToString();
}
else
{
value = string.Join(",", rawBytes.Select(b => b.ToString()));
}
result.Add(value);
k += bytesToRead;
bytesReadForCmd += bytesToRead;
}
if (bytesReadForCmd < payloadLength)
{
k += (payloadLength - bytesReadForCmd);
}
response[commandName] = result;
}
catch (Exception)
{
break;
}
}
return response;
}
private static List<byte> CheckMessage(List<byte> message)
{
int i = 0;
int checksum = 0;
while (i < message.Count)
{
if (message[i] == CSAFEDictionary.ByteStuffingFlag)
{
if (i + 1 >= message.Count) break;
byte stuffValue = message[i + 1];
message.RemoveAt(i + 1);
message[i] = Convert.ToByte(0xF0 | stuffValue);
}
checksum ^= message[i];
i++;
}
if (checksum != 0) return new List<byte>();
if (message.Count > 0) message.RemoveAt(message.Count - 1);
return message;
}
private static byte[] IntToBytes(int value, int length)
{
byte[] result = new byte[length];
for (int i = 0; i < length; i++)
{
result[i] = (byte)((value >> (i * 8)) & 0xFF);
}
return result;
}
}

11
Assets/Scripts/PM5/CSAFEProtocol.cs.meta Normal file
View File

@@ -0,0 +1,11 @@
fileFormatVersion: 2
guid: fcecce28b8a4248ac86b498638edf8ca
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

21
Assets/Scripts/PM5/PerfomanceMonitorDictionary.cs Normal file
View File

@@ -0,0 +1,21 @@
public static class PerformanceMonitorDictionary
{
// Concept2 PM5 UUIDs
public const string DeviceUUID = "ce060000-43e5-11e4-916c-0800200c9a66";
// Services
public const string DeviceInfoService = "ce060010-43e5-11e4-916c-0800200c9a66";
public const string ControlService = "ce060020-43e5-11e4-916c-0800200c9a66";
public const string RowingService = "ce060030-43e5-11e4-916c-0800200c9a66";
// Characteristics - Rowing Service
// 0x31: Row Status (Distance, Time)
// 0x32: Extra Status (Split, Interval)
// 0x35: Stroke Data (Watts, SPM, Drive Info)
// 0x3D: Multiplexed (The main firehose of data)
public const string MultiplexedCharacteristic = "ce060080-43e5-11e4-916c-0800200c9a66";
// Characteristics - Control Service (CSAFE)
public const string ControlReceiveChar = "ce060021-43e5-11e4-916c-0800200c9a66"; // Write here
public const string ControlTransmitChar = "ce060022-43e5-11e4-916c-0800200c9a66"; // Read here
}

11
Assets/Scripts/PM5/PerfomanceMonitorDictionary.cs.meta Normal file
View File

@@ -0,0 +1,11 @@
fileFormatVersion: 2
guid: ad857e139c72d41edb392b5719f88390
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

288
Assets/Scripts/PM5/PerformanceMonitorManager.cs Normal file
View 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); }
}

11
Assets/Scripts/PM5/PerformanceMonitorManager.cs.meta Normal file
View File

@@ -0,0 +1,11 @@
fileFormatVersion: 2
guid: 598ae3859dc5b48b68087a6740b22b00
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant: