VRCycling/Assets/Scripts/Sensors/ANT/SpeedSensorReceiver.cs

using System;
using System.Collections.Generic;
using ANT_Managed_Library;
using UnityEngine;

namespace Sensors.ANT
{
    [Serializable]
    public struct SpeedSensorConfig
    {
        public int deviceId;
        public float wheelCircumference;

        public SpeedSensorConfig(int deviceId = 0, float wheelCircumference = 2.096f)
        {
            this.deviceId = deviceId;
            this.wheelCircumference = wheelCircumference;
        }
    }

    public struct SpeedSensorData
    {
        public float Speed;
        public float SpeedKmh => Speed * 3.6f;
        public float Distance;
    }

    public class SpeedSensorReceiver: AwaitDevice
    {
        private AntChannel backgroundScanChannel;
        private AntChannel deviceChannel;
        private int prevMeasTimeSpeed;
        private int prevRevCountSpeed;
        private int revCountZero;
        private SpeedSensorData sensorData;

        private int stopRevCounterSpeed;

        private bool connected;
        
        public SpeedSensorConfig Config { get; }
        public override bool Connected => connected;
        public SpeedSensorData SensorData => sensorData;

        public override int DeviceId => Config.deviceId;

        public SpeedSensorReceiver()
        {
        }

        public SpeedSensorReceiver(SpeedSensorConfig config)
        {
            Config = config;
        }

        public SpeedSensorReceiver(int deviceId)
        {
            Config = new SpeedSensorConfig(deviceId);
        }

        public SpeedSensorReceiver(int deviceId, float wheelCircumference)
        {
            Config = new SpeedSensorConfig(deviceId, wheelCircumference);
        }

        public override void Connect(AntDevice device)
        {
            var channelID = AntManager.Instance.GetFreeChannelID();
            deviceChannel = AntManager.Instance.OpenChannel(ANT_ReferenceLibrary.ChannelType.BASE_Slave_Receive_0x00,
                channelID, (ushort) device.deviceNumber, device.deviceType, device.transType, (byte) device.radiofreq,
                (ushort) device.period, false);
            connected = true;
            deviceChannel.onReceiveData += Data;
            deviceChannel.onChannelResponse += ChannelResponse;

            deviceChannel.hideRXFAIL = true;
        }

        //Deal with the received Data
        private void Data(byte[] data)
        {
            //SPEED
            var measTime_speed = data[4] | (data[5] << 8);
            var revCount_speed = data[6] | (data[7] << 8);


            if (prevMeasTimeSpeed != 0 && measTime_speed != prevMeasTimeSpeed && prevMeasTimeSpeed < measTime_speed &&
                prevRevCountSpeed < revCount_speed)
            {
                sensorData.Speed = Config.wheelCircumference * (revCount_speed - prevRevCountSpeed) * 1024 /
                                   (measTime_speed - prevMeasTimeSpeed);
                stopRevCounterSpeed = 0;
            }
            else
            {
                stopRevCounterSpeed++;
            }

            if (stopRevCounterSpeed >= 5)
            {
                stopRevCounterSpeed = 5;
                sensorData.Speed = 0;
            }


            prevMeasTimeSpeed = measTime_speed;
            prevRevCountSpeed = revCount_speed;

            //DISTANCE
            if (revCountZero == 0)
                revCountZero = revCount_speed;

            sensorData.Distance = Config.wheelCircumference * (revCount_speed - revCountZero);
        }


        private void ChannelResponse(ANT_Response response)
        {
        }
    }
}