HoHCI-BikeSim/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
    {
        private AntChannel backgroundScanChannel;
        private AntChannel deviceChannel;
        private int prevMeasTimeSpeed;
        private int prevRevCountSpeed;
        private int revCountZero;
        private SpeedSensorData sensorData;

        private int stopRevCounterSpeed;

        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 SpeedSensorConfig Config { get; }
        public bool Connected { get; private set; }
        public List<AntDevice> ScanResult { get; private set; }
        public SpeedSensorData SensorData => sensorData;

        //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
        private void ReceivedBackgroundScanData(byte[] data)
        {
            var deviceType = data[12]; // extended info Device Type byte

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


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

        private void ConnectToDevice(AntDevice device)
        {
            AntManager.Instance.CloseBackgroundScanChannel();
            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)
        {
        }
    }
}