88 lines
3.0 KiB
C#
88 lines
3.0 KiB
C#
using Assets.Scripts.Devices.Ant.Interfaces;
|
|
using Assets.Scripts.Devices.Ant.LegacyPages;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.Threading.Tasks;
|
|
|
|
namespace Assets.Scripts.Devices.Ant
|
|
{
|
|
public class BikeSpdCadDevice : AbstractAntDevice, ISpeedDevice, ICadenceDevice
|
|
{
|
|
public const int MAX_NO_EVENT_STOP_COUNT = 12;
|
|
public const double DEFAULT_WHEEL_CIRCUMFERENCE_m = 2096; //Average 700cx23mm road tire
|
|
|
|
public readonly double wheelCircumfrence_m;
|
|
|
|
private RotationData _speedData = new RotationData();
|
|
private RotationData _cadenceData = new RotationData();
|
|
|
|
public BikeSpdCadDevice(double wheelCircumfrence_m = DEFAULT_WHEEL_CIRCUMFERENCE_m)
|
|
: base("Ant+ Speed&Cadence", racerSportType.Biking, SensorType.SpeedCadence)
|
|
{
|
|
Priority = 1;
|
|
this.wheelCircumfrence_m = wheelCircumfrence_m;
|
|
}
|
|
|
|
public double Speed { get; set; }
|
|
private int _Cadence;
|
|
public int Cadence
|
|
{
|
|
get
|
|
{
|
|
return _Cadence;
|
|
}
|
|
set
|
|
{
|
|
_Cadence = DeviceValueFilter.Cadence(value);
|
|
}
|
|
}
|
|
|
|
|
|
DateTime now = DateTime.Now;
|
|
protected override AntChannelProfile getDefaultSearchProfile()
|
|
{
|
|
return new AntChannelProfile()
|
|
{
|
|
rfOffset = 57,
|
|
transType = 0,
|
|
deviceType = 121,
|
|
deviceNumber = 0,
|
|
messagePeriod = 8086,
|
|
pairingEnabled = false,
|
|
};
|
|
}
|
|
|
|
public override void handleChannelResponse(ANT_Managed_Library.ANT_Response response)
|
|
{
|
|
//商品Manufacturer Information
|
|
if (response.messageContents[1] == 2)
|
|
{
|
|
//Console.WriteLine(string.Join(",",response.messageContents));
|
|
base.ManufacturerId = response.messageContents[2];
|
|
}
|
|
if (response.responseID == (byte)ANT_Managed_Library.ANT_ReferenceLibrary.ANTMessageID.BROADCAST_DATA_0x4E)
|
|
{
|
|
var dataPayload = response.messageContents.Skip(1).ToArray();
|
|
|
|
int newUpdate1 = (int)dataPayload[0] | (int)dataPayload[1] << 8;
|
|
int newCount1 = (int)dataPayload[2] | (int)dataPayload[3] << 8;
|
|
int newUpdate2 = (int)dataPayload[4] | (int)dataPayload[5] << 8;
|
|
int newCount2 = (int)dataPayload[6] | (int)dataPayload[7] << 8;
|
|
|
|
var cadence = this._cadenceData.CalculateRpm(now, newUpdate1, newCount1);
|
|
if (cadence.HasValue)
|
|
{
|
|
Cadence = cadence.Value;
|
|
}
|
|
var a2 = this._speedData.CalculateRpm(now, newUpdate2, newCount2);
|
|
if (a2.HasValue)
|
|
{
|
|
Speed = DoubleExtensions.CalculateSpeed(a2.Value, this.wheelCircumfrence_m);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|