powerfun-unity/Assets/Scripts/Devices/Ant/PowerDevice.cs

using Assets.Scripts.Devices.Ant.Interfaces;
using Assets.Scripts.Devices.Ant.Messages;
using Assets.Scripts.Devices.Ant.Pages;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Timers;

namespace Assets.Scripts.Devices.Ant
{
    /// <summary>
    /// 功率计
    /// </summary>
    public class PowerDevice : AbstractAntDevice, IPowerDevice //, ICadenceDevice
    {
        private DateTime _lastEventChange = DateTime.UtcNow;
        //private byte _previousEventCount;
        private readonly TorquePowerDataCalculator _torquePowerDataCalculator = new TorquePowerDataCalculator();
        private readonly CrankTorqueFrequencyData _crankTorqueFrequency = new CrankTorqueFrequencyData();
        private System.Timers.Timer timer;

        //List<IPageHandler> pageHandlers;

        public PowerDevice(string id)
            : base(id, "Ant+ Power", racerSportType.Biking, SensorType.Power)
        {
            Priority = 2;
            //pageHandlers = new List<IPageHandler>();
            //pageHandlers.Add(new ManufacturerDataPageHandler());
            pageHandlers.Add(new PowerOnlyPageHandler());
            pageHandlers.Add(new CrankPowerPageHandler());
            pageHandlers.Add(new CtfPageHandler());
            pageHandlers.Add(new WheelPowerPageHandler());
            pageHandlers.Add(new CalibrationPageHandler());


            this.StateChange = (state) =>
            {
                if (state == DeviceState.Connected)
                {
                    Calibrate();
                }
                else if (state == DeviceState.Disconnected)
                {
                    count = 0;
                    if (timer != null)
                    {
                        timer.Stop();
                        timer = null;
                        DoCalibrationFailure();
                    }
                }
            };
        }

        private int _Power;
        public int Power
        {
            get
            {
                return _Power;
            }
            set
            {
                _Power = DeviceValueFilter.Power(value);
            }
        }
        private int _Cadence;
        public int Cadence
        {
            get
            {
                return _Cadence;
            }
            set
            {
                _Cadence = DeviceValueFilter.Cadence(value);
            }
        }

        public DateTime LastCalibration { get; protected set; }
        public int? Offset { get; set; }

        public AutoZeroStatus AutoZeroStatus { get; private set; }

        public bool IsCalibrating { get; set; }

        public bool IsCalibrationReady { get; set; }

        public bool CalibrationCompleted { get; set; }

        protected override AntChannelProfile getDefaultSearchProfile()
        {
            return new AntChannelProfile()
            {
                rfOffset = 57,
                transType = 0,
                deviceType = 11,
                deviceNumber = 0,
                messagePeriod = 8182,
                pairingEnabled = false,
            };
        }
        //List<int> list = new List<int>();
        public override void handleChannelResponse(ANT_Managed_Library.ANT_Response response)
        {
            //if (!list.Contains(response.messageContents[1]))
            //{
            //    list.Add(response.messageContents[1]);
            //    Console.WriteLine(response.messageContents[1]);
            //}
            //Console.WriteLine(string.Join(",", response.messageContents));
            if (response.responseID == (byte)ANT_Managed_Library.ANT_ReferenceLibrary.ANTMessageID.BROADCAST_DATA_0x4E)
            {
                var pageNumber = response.messageContents[1];

                foreach (var handler in pageHandlers)
                {
                    if (handler.CanHandle(pageNumber))
                    {
                        handler.Handle(response.messageContents.Skip(1).ToArray(), this);
                    }
                }
            }
            //System.IO.File.AppendAllText(System.Environment.CurrentDirectory + "\\data.txt", string.Join(",", response.messageContents) + ";" + Power+"\r\n");
            //Console.WriteLine();
        }



        internal void DoCalibrationSuccess(int? offset, AutoZeroStatus autoZeroStatus)
        {
            this.AutoZeroStatus = autoZeroStatus;
            this.LastCalibration = DateTime.UtcNow;
            if (this.Offset.HasValue == false)
            {
                this.Offset = offset;
                //this.Offset = 570;
            }
            this.IsCalibrating = false;
            this.CalibrationCompleted = true;

            //Trace.WriteLine($"校准成功：{ offset }");
        }

        internal void DoCalibrationFailure()
        {
            this.IsCalibrating = false;
            this.CalibrationCompleted = false;
        }

        /// <summary>
        /// 发送校准命令
        /// </summary>
        public void Calibrate()
        {
            if (this.IsCalibrating)
                return;

            this.IsCalibrating = true;
            var channelId = GetChannelId();

            base.SendMessage(new ManualCalibrate(channelId));

            if (timer == null)
            {
                timer = new System.Timers.Timer(10000);
                timer.Elapsed += Timer_Elapsed;
                timer.Start();
            }
        }

        int count = 0;
        /// <summary>
        /// 检查是否已自动校准,自动发送3次
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void Timer_Elapsed(object sender, ElapsedEventArgs e)
        {
            if (count > 3)
            {
                timer.Stop();
                return;
            }
            if (this.CalibrationCompleted)
            {
                timer.Stop();
                return;
            }

            count++;
            this.IsCalibrating = false;
            this.Calibrate();
        }
    }


    public class TorquePowerDataCalculator
    {
        private DateTime _lastEventChange = DateTime.UtcNow;
        private byte _previousEventCount;
        private ushort _previousPeriod;
        private ushort _previousAccumulatedTorque;

        public int Power { get; private set; }

        public int Rpm { get; private set; }

        public DateTime LastUpdatedTime { get; private set; }

        public TorquePowerDataCalculator()
        {
            this.LastUpdatedTime = DateTime.MinValue;
        }

        public void Update(byte eventCount, ushort period, ushort accumulatedTorque)
        {
            byte difference1 = ByteExtensions.GetDifference(eventCount, this._previousEventCount);
            ushort difference2 = UshortExtensions.GetDifference(period, this._previousPeriod);
            if ((int)difference1 > 0)
                this._lastEventChange = DateTime.UtcNow;
            if (this.PeriodRolloverPossible() || this.IsNotMovingDueToNoEventChanges() || (int)difference2 == 0 && (int)difference1 > 0)
            {
                this.Power = 0;
                this.Rpm = 0;
            }
            else if ((int)difference1 > 0)
            {
                this.Power = (int)(402.123859659494 * ((double)UshortExtensions.GetDifference(accumulatedTorque, this._previousAccumulatedTorque) / (double)difference2));
                this.Rpm = 122880 * (int)difference1 / (int)difference2;
            }
            this.LastUpdatedTime = DateTime.UtcNow;
            this._previousEventCount = eventCount;
            this._previousPeriod = period;
            this._previousAccumulatedTorque = accumulatedTorque;
        }

        private bool PeriodRolloverPossible()
        {
            return DateTime.UtcNow - this.LastUpdatedTime > TimeSpan.FromSeconds(32.0);
        }

        private bool IsNotMovingDueToNoEventChanges()
        {
            return this._lastEventChange.AddSeconds(3.0) <= DateTime.UtcNow;
        }
    }

    public class CrankTorqueFrequencyData
    {
        private byte _previousEventCount;
        private ushort _previousTimestamp;
        private ushort _previousTorqueTicksStamp;
        private int _noEventCount;

        public int? Power { get; private set; }

        public int Cadence { get; private set; }

        public DateTime LastUpdatedTime { get; set; }

        public CrankTorqueFrequencyData()
        {
            this.LastUpdatedTime = DateTime.MinValue;
        }

        public void Update(byte eventCount, ushort timestamp, ushort torqueTicksStamp, ushort slope, int offset)
        {
            byte difference1 = ByteExtensions.GetDifference(eventCount, this._previousEventCount);
            ushort difference2 = UshortExtensions.GetDifference(timestamp, this._previousTimestamp);
            ushort difference3 = UshortExtensions.GetDifference(torqueTicksStamp, this._previousTorqueTicksStamp);
            this._previousEventCount = eventCount;
            this._previousTimestamp = timestamp;
            this._previousTorqueTicksStamp = torqueTicksStamp;
            if (this.HasMoreThanOneRolloverOccured())
            {
                this.LastUpdatedTime = DateTime.UtcNow;
                this.Power = new int?(0);
                this.Cadence = 0;
            }
            else
            {
                this.LastUpdatedTime = DateTime.UtcNow;
                if ((int)difference1 > 0)
                {
                    this._noEventCount = 0;
                    double num = (double)difference2 / (double)difference1 * 0.0005;
                    if ((int)difference2 == 0)
                        return;
                    this.Cadence = (int)Math.Round(60.0 / num);
                    if ((int)difference3 == 0)
                        this.Power = new int?();
                    else
                        this.Power = new int?((int)((1.0 / ((double)difference2 * 0.0005 / (double)difference3) - (double)offset) / ((double)slope / 10.0) * (double)this.Cadence * 3.14159265358979 / 30.0));
                }
                else
                {
                    int num = this._noEventCount + 1;
                    this._noEventCount = num;
                    if (num < 12)
                        return;
                    this.Power = new int?(0);
                    this.Cadence = 0;
                }
            }
        }

        private bool HasMoreThanOneRolloverOccured()
        {
            return DateTime.UtcNow - this.LastUpdatedTime > TimeSpan.FromSeconds(32.7);
        }
    }
}