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

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

[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)
    {
    }
}