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

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;
        }
    }
}