471 lines
18 KiB
C#
471 lines
18 KiB
C#
using Assets.Scripts.Devices.Ant.Interfaces;
|
||
using Assets.Scripts.Devices.Ant.Messages;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using System.Text;
|
||
using System.Threading.Tasks;
|
||
using UnityEngine;
|
||
|
||
namespace Assets.Scripts.Devices.Ant
|
||
{
|
||
public class FitDevice : AbstractAntDevice, ISpeedDevice, IPowerDevice, ICadenceDevice, IRequiresRiderWeight, ITrainerDevice
|
||
{
|
||
public const int MAX_NO_EVENT_STOP_COUNT = 12;
|
||
|
||
//byte lastElapsedTimeAcc;
|
||
//byte lastDistAcc;
|
||
//double calcSpeed = 0; //Averaged Speed over several msgs in case instSpeed is not available
|
||
|
||
int lastInstCadence = -1;
|
||
ushort lastInstPower = 0xFFFF;
|
||
|
||
/// <summary>
|
||
/// 当前坡度，当切换到其他模式的时候，需要把坡度设置为0
|
||
/// </summary>
|
||
private double _grade = 0;
|
||
private bool IsCalibrating { get; set; }
|
||
public FitDevice(string id)
|
||
: base(id, "Ant+ Trainer", racerSportType.Unknown, SensorType.Trainer)
|
||
{
|
||
Priority = 0;
|
||
this.StateChange = (state) =>
|
||
{
|
||
if (state == DeviceState.Connected)
|
||
{
|
||
Calibrate();
|
||
}
|
||
};
|
||
}
|
||
|
||
/// <summary>
|
||
/// 发送校准命令
|
||
/// </summary>
|
||
private void Calibrate()
|
||
{
|
||
if (this.IsCalibrating)
|
||
return;
|
||
|
||
this.IsCalibrating = true;
|
||
|
||
base.SendMessage(new FecCalibrate(GetChannelId()));
|
||
}
|
||
|
||
protected override AntChannelProfile getDefaultSearchProfile()
|
||
{
|
||
return new AntChannelProfile()
|
||
{
|
||
rfOffset = 57,
|
||
transType = 0,
|
||
deviceType = 17,
|
||
deviceNumber = 0,
|
||
messagePeriod = 8192,
|
||
pairingEnabled = false,
|
||
};
|
||
}
|
||
//private byte _previousEventCount;
|
||
|
||
// Token: 0x0400054F RID: 1359
|
||
//private ushort _previousPeriod;
|
||
|
||
// Token: 0x04000550 RID: 1360
|
||
//private ushort _previousAccumulatedTorque;
|
||
|
||
private DateTime _lastEventChange = DateTime.UtcNow;
|
||
|
||
public double Speed { get; set; }
|
||
|
||
private int _Power;
|
||
public int Power
|
||
{
|
||
get
|
||
{
|
||
return _Power;
|
||
}
|
||
set
|
||
{
|
||
_Power = DeviceValueFilter.Power(value);
|
||
//Debug.Log(_Power);
|
||
}
|
||
}
|
||
|
||
private int _Cadence;
|
||
public int Cadence
|
||
{
|
||
get
|
||
{
|
||
return _Cadence;
|
||
}
|
||
set
|
||
{
|
||
_Cadence = DeviceValueFilter.Cadence(value);
|
||
}
|
||
}
|
||
|
||
private double _riderWeight = 75.0;
|
||
|
||
public double RiderWeight
|
||
{
|
||
get => _riderWeight;
|
||
set
|
||
{
|
||
if (value <= 0) return;
|
||
this._riderWeight = value;
|
||
this.UpdateUserConfiguration();
|
||
}
|
||
}
|
||
|
||
public DateTime LastCalibration { get; protected set; }
|
||
public int? Offset { get; set; }
|
||
|
||
|
||
public override void handleChannelResponse(ANT_Managed_Library.ANT_Response response)
|
||
{
|
||
//Console.WriteLine(string.Join(",", response.messageContents));
|
||
//Log.LogInfo(string.Join(",", response.messageContents));
|
||
if (response.responseID == (byte)ANT_Managed_Library.ANT_ReferenceLibrary.ANTMessageID.BROADCAST_DATA_0x4E)
|
||
{
|
||
var pageNumber = response.messageContents[1];
|
||
switch (pageNumber)
|
||
{
|
||
case 16: //Page 16 - General Page
|
||
{
|
||
HandleGeneralDataPage(response.messageContents.Skip(1).ToArray());
|
||
}
|
||
break;
|
||
//case 17: //general settings Page, not on bike, shows on rower, but not very useful
|
||
// {
|
||
// System.Console.Out.WriteLine("Cycle Length:" + response.messageContents[4] + ",Incline:" + (response.messageContents[5] + (response.messageContents[5] << 8)) + ",Resist:" + response.messageContents[7]);
|
||
// }
|
||
// break;
|
||
//case 18: //metabolic data, doesn't show on rower or bike
|
||
// {
|
||
// System.Console.Out.WriteLine("BurnRate:" + (response.messageContents[5] + (response.messageContents[6] << 8)) + ",Cal:" + response.messageContents[7]);
|
||
// }
|
||
// break;
|
||
case 19: //treadmill data
|
||
{
|
||
lastInstCadence = response.messageContents[5];
|
||
}
|
||
break;
|
||
case 21: //bike data
|
||
case 22: //Row data, same format as bike for cad and power
|
||
case 20: //Elliptical, same format as bike for cad and power
|
||
case 24: //Nordic Skier, same format as bike for cad and power
|
||
{
|
||
lastInstCadence = response.messageContents[5];
|
||
lastInstPower = (ushort)(response.messageContents[6] + (response.messageContents[7] << 8));
|
||
//System.Console.Out.WriteLine("Cadence:" + response.messageContents[5] + ",Power:" + (response.messageContents[6] + (response.messageContents[7] << 8)));
|
||
//System.Console.Out.WriteLine("SPM:"+response.messageContents[5]+"Power:"+(response.messageContents[6] + (response.messageContents[7] << 8)));
|
||
}
|
||
break;
|
||
case 25:
|
||
HandleTrainerDataPage(response.messageContents.Skip(1).ToArray());
|
||
break;
|
||
//case 80:
|
||
// new ManufacturerDataPageHandler().Handle(response.messageContents.Skip(1).ToArray(), this);
|
||
// break;
|
||
case 122:
|
||
break;
|
||
default:
|
||
foreach (var handler in pageHandlers)
|
||
{
|
||
if (handler.CanHandle(pageNumber))
|
||
{
|
||
handler.Handle(response.messageContents.Skip(1).ToArray(), this);
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
private void HandleGeneralDataPage(byte[] dataPayload)
|
||
{
|
||
try
|
||
{
|
||
byte equipmentType = dataPayload[1];
|
||
byte elapsedTime = dataPayload[2];
|
||
byte distanceTraveled = dataPayload[3];
|
||
|
||
//byte heartRate = dataPayload[6];
|
||
//FecPageHandler.HeartRateDataSource heartRateDataSource = (FecPageHandler.HeartRateDataSource)(dataPayload[7] & 3);
|
||
//bool traveledEnabled = (dataPayload[7] & 4) == 1;
|
||
//bool distanceTraveledEnabled = (dataPayload[7] & 8) == 1;
|
||
//FecPageHandler.FeState feState = (FecPageHandler.FeState)(dataPayload[7] & 128);
|
||
//bool lapToggle = (dataPayload[7] & 128) == 1;
|
||
//this._fecGeneralData = new FecGeneralData(equipmentType, elapsedTime, distanceTraveled, (short)num, feState, lapToggle, heartRate, heartRateDataSource, traveledEnabled, distanceTraveledEnabled);
|
||
bool traveledEnabled = ((int)dataPayload[7] & 4) == 1;
|
||
bool distanceTraveledEnabled = ((int)dataPayload[7] & 8) == 1;
|
||
|
||
int num1 = (int)dataPayload[4] | (int)dataPayload[5] << 8; // Instantaneous speed, m/s
|
||
Speed = (double)num1 / 1000.0 * 60.0 * 60.0 / 1000.0; //转换成公里/小时
|
||
//Console.WriteLine(num1 +"-----"+Speed);
|
||
//fecDevice.UpdateSpeed(new double?(value));
|
||
//PubCommData.Speed = num2;
|
||
|
||
//byte? b2 = (heartRate == 255) ? null : new byte?(heartRate);
|
||
//int? xl = b2.HasValue ? new int?((int)b2.GetValueOrDefault()) : 0;
|
||
//Console.WriteLine("最新速度" + Speed);
|
||
//Console.WriteLine("最新心率" + xl);
|
||
|
||
//string str = "";
|
||
//for (int i = 0; i < response.messageContents.Length; i++)
|
||
//{
|
||
// str += "," + response.messageContents[i];
|
||
//}
|
||
//PubCommData.sp = "速度：" + str;
|
||
}
|
||
catch (Exception)
|
||
{
|
||
}
|
||
}
|
||
|
||
private void HandleTrainerDataPage(byte[] dataPayload)
|
||
{
|
||
//if (response.messageContents.Length < 11) break;
|
||
//double value = 0;
|
||
//for (int i = 0; i < response.messageContents.Length; i++)
|
||
//{
|
||
// if (i < 8)
|
||
// {
|
||
// value += response.messageContents[i];
|
||
// }
|
||
//}
|
||
|
||
//double value1 = response.messageContents[10] * 256 + response.messageContents[9];
|
||
|
||
|
||
//double power = response.messageContents[7] * 256 + response.messageContents[6];
|
||
//Console.WriteLine("power:" + power);
|
||
//PubCommData.power = power;
|
||
|
||
try
|
||
{
|
||
if (dataPayload[5] > 0)
|
||
{
|
||
//Console.WriteLine(dataPayload[5] + "," + dataPayload[6]);//瞬时功率
|
||
}
|
||
byte num1 = dataPayload[1];
|
||
byte num2 = dataPayload[2];
|
||
int accumulatedPower = (int)dataPayload[3] | (int)dataPayload[4] << 8;
|
||
int instantaneousPower = (int)dataPayload[5] | ((int)dataPayload[6] & 15) << 8; //Watts
|
||
bool bikePowerCalibrationRequired = ((int)dataPayload[6] >> 4 & 1) == 1;
|
||
bool resistanceCalibrationRequired = ((int)dataPayload[6] >> 5 & 1) == 1;
|
||
bool userConfigurationRequired = ((int)dataPayload[6] >> 6 & 1) == 1;
|
||
bool speedIsTooLow = ((int)dataPayload[7] & 1) == 1;
|
||
bool speedIsTooHigh = ((int)dataPayload[7] & 2) == 1;
|
||
|
||
//PubCommData.power = instantaneousPower;
|
||
Power = instantaneousPower;
|
||
|
||
//bool bikePowerCalibrationRequired = (response.messageContents[6] >> 4 & 1) == 1;
|
||
//bool resistanceCalibrationRequired = (response.messageContents[6] >> 5 & 1) == 1;
|
||
//bool flag = (response.messageContents[6] >> 6 & 1) == 1;
|
||
//bool speedIsTooLow = (response.messageContents[7] & 1) == 1;
|
||
//bool speedIsTooHigh = (response.messageContents[7] & 2) == 1;
|
||
//FecPageHandler.FeState feState = (FecPageHandler.FeState)(dataPayload[7] & 112);
|
||
//bool lapToggle = (dataPayload[7] & 128) == 1;
|
||
//this._fecTrainerData = new FecTrainerData((int)eventCount, (int)b, accumulatedPower, num, bikePowerCalibrationRequired, resistanceCalibrationRequired, flag, speedIsTooLow, speedIsTooHigh, feState, lapToggle);
|
||
//device.UpdatePower(new int?(num));
|
||
byte? nullable = (int)num2 == (int)byte.MaxValue ? new byte?() : new byte?(num2);
|
||
int? cadence = nullable.HasValue ? new int?((int)nullable.GetValueOrDefault()) : new int?();
|
||
this.Cadence = cadence.GetValueOrDefault();
|
||
|
||
//Console.WriteLine($"power:{ instantaneousPower },Cadence:{ cadence }, data:{ string.Join(",", dataPayload) }");
|
||
|
||
if (!userConfigurationRequired)
|
||
return;
|
||
UpdateUserConfiguration();
|
||
}
|
||
catch (Exception)
|
||
{
|
||
}
|
||
}
|
||
|
||
|
||
public void SetErgMode(int targetPower)
|
||
{
|
||
if (this.State != DeviceState.Connected)
|
||
return;
|
||
if (_grade > 0)
|
||
{
|
||
SetTrackResistance(0);
|
||
}
|
||
|
||
var channelId = GetChannelId();
|
||
|
||
//byte id = (byte)79;
|
||
//byte[] Data = new byte[9]
|
||
// {
|
||
// channelId,
|
||
// (byte) 49,
|
||
// byte.MaxValue,
|
||
// byte.MaxValue,
|
||
// byte.MaxValue,
|
||
// byte.MaxValue,
|
||
// byte.MaxValue,
|
||
// (byte)(targetPower & (int) byte.MaxValue),
|
||
// (byte)(targetPower >> 8 & (int) byte.MaxValue)
|
||
// };
|
||
AntConnector.Instance().SendMessage(new FecErgMode(channelId, targetPower).GetMessage());
|
||
}
|
||
|
||
public void SetResistanceMode(double value)
|
||
{
|
||
if (this.State != DeviceState.Connected)
|
||
return;
|
||
if (_grade > 0)
|
||
{
|
||
SetTrackResistance(0);
|
||
}
|
||
Console.WriteLine($"阻力模式{ value }");
|
||
var channelId = GetChannelId();
|
||
|
||
double resistance = value / 100 * 200;
|
||
byte id = (byte)79;
|
||
byte[] Data = new byte[9]
|
||
{
|
||
channelId,
|
||
(byte) 48,
|
||
byte.MaxValue,
|
||
byte.MaxValue,
|
||
byte.MaxValue,
|
||
byte.MaxValue,
|
||
byte.MaxValue,
|
||
byte.MaxValue,
|
||
(byte)resistance
|
||
};
|
||
AntConnector.Instance().SendMessage(id, Data);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 风阻
|
||
/// </summary>
|
||
/// <param name="height">海拔高度，单位米</param>
|
||
public void SetWindResistance(double? height = null)
|
||
{
|
||
if (this.State != DeviceState.Connected)
|
||
return;
|
||
if (_grade > 0)
|
||
{
|
||
SetTrackResistance(0);
|
||
}
|
||
Console.WriteLine($"自由骑{ height }");
|
||
var channelId = GetChannelId();
|
||
//double resistance = value / 100 * 200;
|
||
|
||
byte windResistance = 0; //Product of Frontal Surface Area, Drag Coefficient and Air Density. Use default value: 0xFF, range: 0.00 – 1.86 kg/m
|
||
|
||
if (height.HasValue)
|
||
{
|
||
//Wind Resistance Coefficient [kg/m] = Frontal Surface Area [m2] x Drag Coefficient x Air Density[kg / m3]
|
||
var wr = 0.40 * 1.0 * AirDensity.GetAirDensity(height.Value);
|
||
if (wr > 1.86)
|
||
{
|
||
wr = 1.86;
|
||
}
|
||
windResistance = (byte)(Convert.ToInt32(Math.Round(wr, 2) / 0.01));
|
||
|
||
Console.WriteLine($"风阻系数：{ windResistance }");
|
||
}
|
||
|
||
byte id = (byte)79;
|
||
byte[] Data = new byte[9]
|
||
{
|
||
channelId,
|
||
(byte) 50,
|
||
byte.MaxValue,
|
||
byte.MaxValue,
|
||
byte.MaxValue,
|
||
byte.MaxValue,
|
||
//Wind Resistance Coefficient 0.00 – 1.86kg/m Product of Frontal Surface Area, Drag Coefficient and Air Density.Use default value: 0xFF
|
||
windResistance,
|
||
//Wind Speed -127 – +127km/h
|
||
0xFF,
|
||
0xFF
|
||
};
|
||
AntConnector.Instance().SendMessage(id, Data);
|
||
}
|
||
/// <summary>
|
||
/// 轨道阻力
|
||
/// </summary>
|
||
/// <param name="grade">坡度百分比的值,单位是%</param>
|
||
public void SetTrackResistance(double grade)
|
||
{
|
||
if (this.State != DeviceState.Connected)
|
||
return;
|
||
|
||
_grade = grade;
|
||
if (_grade > 15)
|
||
{
|
||
_grade = 15;
|
||
}
|
||
else if (_grade < -5)
|
||
{
|
||
_grade = -5;
|
||
}
|
||
//Console.WriteLine($"轨道阻力{ grade }");
|
||
var channelId = GetChannelId();
|
||
//double resistance = value / 100 * 200;
|
||
|
||
//var windResistance = 0.00; //Product of Frontal Surface Area, Drag Coefficient and Air Density. Use default value: 0xFF, range: 0.00 – 1.86kg/m
|
||
|
||
byte id = (byte)79;
|
||
|
||
// zwift started
|
||
// 0000 a4 09 4e 00 10 19 18 19 1a 41 00 30 80 00 00
|
||
// reply a4 09 4f 00 33 ff ff ff ff 45 4e ff 25 # p.74 [33] track resistance, ff ff ff ff reserved, [45] slope lsb, [4e] slope msb in 1/100%, [ff] coefficient rolling resistance
|
||
// Simulated Grade(%) = (Raw Grade Value x 0.01%) – 200.00% , 454e = 20037 x 0.01 -200 = 0.37%
|
||
//var gradeBytes = BitConverter.GetBytes(0x4E20);
|
||
|
||
var gradeValue = Convert.ToInt32((grade + 200) * 100);
|
||
var gradeBytes = BitConverter.GetBytes(gradeValue);
|
||
//var gradeBytes = BitConverter.GetBytes(20004);
|
||
byte[] Data = new byte[9]
|
||
{
|
||
channelId,
|
||
(byte) 51,
|
||
byte.MaxValue,
|
||
byte.MaxValue,
|
||
byte.MaxValue,
|
||
byte.MaxValue,
|
||
gradeBytes[0],
|
||
gradeBytes[1],
|
||
byte.MaxValue
|
||
};
|
||
AntConnector.Instance().SendMessage(id, Data);
|
||
}
|
||
|
||
public void UpdateUserConfiguration()
|
||
{
|
||
if (this.State != DeviceState.Connected)
|
||
return;
|
||
|
||
var channelId = GetChannelId();
|
||
//var _weight = (int)(this._riderWeight * 100.0);
|
||
//byte id = (byte)79;
|
||
//byte[] Data = new byte[9]
|
||
// {
|
||
// channelId,
|
||
// (byte) 55,
|
||
// (byte)(_weight & (int) byte.MaxValue),
|
||
// (byte)(_weight >> 8 & (int) byte.MaxValue),
|
||
// byte.MaxValue,
|
||
// (byte) 143,
|
||
// (byte) 12,
|
||
// (byte) 70,
|
||
// (byte) 0
|
||
// };
|
||
//AntConnector.Instance().SendMessage(id, Data);
|
||
AntConnector.Instance().SendMessage(new FecUserConfiguration(channelId, this._riderWeight).GetMessage());
|
||
}
|
||
|
||
internal void DoCalibrationSuccess(int? offset)
|
||
{
|
||
this.LastCalibration = DateTime.UtcNow;
|
||
this.Offset = offset;
|
||
}
|
||
}
|
||
}
|