HoHCI-BikeSim/Assets/Scripts/Sensors/ANT/SpeedSensorReceiver.cs

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

namespace Sensors.ANT
{
    [System.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
    {
        private AntChannel backgroundScanChannel;
        private AntChannel deviceChannel;

        private int stopRevCounterSpeed = 0;
        private int prevMeasTimeSpeed = 0;
        private int prevRevCountSpeed = 0;
        private int revCountZero = 0;
        private SpeedSensorData sensorData;

        public SpeedSensorConfig Config { get; }
        public bool Connected { get; private set; }
        public List<AntDevice> ScanResult { get; private set; }
        public SpeedSensorData SensorData => sensorData;

        public SpeedSensorReceiver()
        {
        }

        public SpeedSensorReceiver(SpeedSensorConfig config) => this.Config = config;

        public SpeedSensorReceiver(int deviceId) => Config = new SpeedSensorConfig(deviceId);

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

        //Start a background Scan to find the device
        public void StartScan()
        {
            Debug.Log("Looking for ANT + Speed sensor");
            AntManager.Instance.Init();
            ScanResult = new List<AntDevice>();
            backgroundScanChannel = AntManager.Instance.OpenBackgroundScanChannel(0);
            backgroundScanChannel.onReceiveData += ReceivedBackgroundScanData;
        }

        //If the device is found
        void ReceivedBackgroundScanData(Byte[] data)
        {
            byte deviceType = (data[12]); // extended info Device Type byte

            switch (deviceType)
            {
                case AntplusDeviceType.BikeSpeed:
                {
                    int deviceNumber = (data[10]) | data[11] << 8;
                    byte transType = data[13];
                    foreach (AntDevice d in ScanResult)
                    {
                        if (d.deviceNumber == deviceNumber && d.transType == transType) //device already found
                            return;
                    }


                    AntDevice foundDevice = new AntDevice();
                    foundDevice.deviceType = deviceType;
                    foundDevice.deviceNumber = deviceNumber;
                    foundDevice.transType = transType;
                    foundDevice.period = 8118;
                    foundDevice.radiofreq = 57;
                    foundDevice.name = "BikeSpeed(" + foundDevice.deviceNumber + ")";
                    ScanResult.Add(foundDevice);
                    if (deviceNumber == Config.deviceId)
                    {
                        Debug.Log($"Desired Speed Sensor with id {deviceNumber} found!");
                        ConnectToDevice(foundDevice);
                    }
                    else
                    {
                        Debug.Log($"Speed sensor ({deviceNumber}) found and added to ScanResult");
                    }

                    break;
                }
            }
        }

        void ConnectToDevice(AntDevice device)
        {
            AntManager.Instance.CloseBackgroundScanChannel();
            byte 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
            int measTime_speed = (data[4]) | data[5] << 8;
            int 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);
        }


        void ChannelResponse(ANT_Response response)
        {
        }
    }
}