first push!
This commit is contained in:
381
Assets/Scripts/PM5/CSAFEProtocol.cs
Normal file
381
Assets/Scripts/PM5/CSAFEProtocol.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user