60 lines
1.9 KiB
C#
60 lines
1.9 KiB
C#
|
using ANT_Managed_Library;
|
|||
|
|
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 SpeedDevice : AbstractAntDevice, ISpeedDevice
|
|||
|
|
{
|
|||
|
|
public double Speed { get; set; }
|
|||
|
|
private DateTime now = DateTime.Now;
|
|||
|
|
public const double DEFAULT_WHEEL_CIRCUMFERENCE_m = 2096; //Average 700cx23mm road tire
|
|||
|
|
public readonly double wheelCircumfrence_m;
|
|||
|
|
|
|||
|
|
|
|||
|
|
private RotationData _rotationData = new RotationData();
|
|||
|
|
public SpeedDevice(string id, double wheelCircumfrence_m = DEFAULT_WHEEL_CIRCUMFERENCE_m)
|
|||
|
|
: base(id, "Ant+ Speed", racerSportType.Unknown, SensorType.Speed)
|
|||
|
|
{
|
|||
|
|
Priority = 2;
|
|||
|
|
this.wheelCircumfrence_m = wheelCircumfrence_m;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public override void handleChannelResponse(ANT_Response response)
|
|||
|
|
{
|
|||
|
|
//throw new NotImplementedException();
|
|||
|
|
var page = response.messageContents.Skip(1).ToArray();
|
|||
|
|
int? value = _rotationData.CalculateRpm(now, page[4] | page[5] << 8, page[6] | page[7] << 8);
|
|||
|
|
if (value.HasValue)
|
|||
|
|
{
|
|||
|
|
Speed = DoubleExtensions.CalculateSpeed(value.Value, wheelCircumfrence_m);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
Speed = 0;
|
|||
|
|
}
|
|||
|
|
Console.WriteLine(Math.Round(Speed, 1));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
protected override AntChannelProfile getDefaultSearchProfile()
|
|||
|
|
{
|
|||
|
|
//throw new NotImplementedException();
|
|||
|
|
|
|||
|
|
return new AntChannelProfile()
|
|||
|
|
{
|
|||
|
|
rfOffset = 57,
|
|||
|
|
transType = 0,
|
|||
|
|
deviceType = 123,
|
|||
|
|
deviceNumber = 0,
|
|||
|
|
messagePeriod = 8118,
|
|||
|
|
pairingEnabled = false
|
|||
|
|
};
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|