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

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

namespace Sensors.ANT
{
    /*

    Frequencies such as 4.06 Hz (ANT+ Heart
Rate) and 4.005 Hz (ANT+ Bike Power) will
periodically “drift” into each other, or into
other channel periods that may be present
in the vicinity. During this overlap, channel
collisions may occur as the radio can only
service channel at a time. 
*/

    public struct PowermeterSensorData
    {
        public int InstantaneousPower; // the instantaneous power in watt
        public int InstantaneousCadence; // crank cadence in RPM if available ( 255 indicates invalid)
        public float CrankTorque;
    }

    public class PowerMeterReceiver : AwaitDevice
    {
        private readonly Dictionary<int, int> sameEventCounter = new Dictionary<int, int> {{0x10, 0}, {0x12, 0}};

        private readonly Dictionary<int, int> updateEventCount = new Dictionary<int, int> {{0x10, 0}, {0x12, 0}};
        private AntChannel backgroundScanChannel;
        private AntChannel deviceChannel;
        private byte[] pageToSend;

        private int previousAccumulatedTorqueValue;
        private int previousTorqueEventCount;
        private PowermeterSensorData sensorData;

        private bool connected;

        public override int DeviceId { get; }

        public override bool Connected => connected;
        public PowermeterSensorData SensorData => sensorData;

        public PowerMeterReceiver()
        {
        }

        public PowerMeterReceiver(int deviceID)
        {
            DeviceId = deviceID;
        }


        public override void Connect(AntDevice device)
        {
            var channelID = AntManager.Instance.GetFreeChannelID();
            deviceChannel = AntManager.Instance.OpenChannel(ANT_ReferenceLibrary.ChannelType.ADV_Shared_0x20,
                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)
        {
            /*if (data[0] == 0x10) // Standard Power-Only Main Data Page
            {
                Debug.Log($"Power only Main Data page [{string.Join(",", data)}]");
                if (data[1] == updateEventCount[0x10])
                    sameEventCounter[0x10]++;
                else
                    sameEventCounter[0x10] = 0;

                updateEventCount[0x10] = data[1];

                if (sameEventCounter[0x10] > 3)
                {
                    sensorData.InstantaneousPower = 0;
                    sensorData.InstantaneousCadence = 0;
                }
                else
                {
                    sensorData.InstantaneousPower = data[6] | (data[7] << 8);
                    sensorData.InstantaneousCadence = data[3];
                }
            }
            else*/ if (data[0] == 0x12) // Standard Crank Torque Main Data Page
            {
                Debug.Log($"Crank Torque Main Data page  [{string.Join(",", data)}]");
                //Debug.Log($"Torque Data {data}");
                if (data[1] == updateEventCount[0x12])
                    sameEventCounter[0x12]++;
                else
                    sameEventCounter[0x12] = 0;

                updateEventCount[0x12] = data[1];
                Debug.Log($"UpdateEventCount = {data[1]}, same as before ? {updateEventCount[0x12] == data[1]}");
                Debug.Log($"Crank ticks = {data[2]}");
                Debug.Log($"Cadence = {data[3]}");

                if (sameEventCounter[0x12] > 3)
                {
                    Debug.Log("SameEventCounter > 3 setting torque to 0");
                    sensorData.CrankTorque = 0;
                }
                else
                {
                    var accumulatedTorque = data[6] | (data[7] << 8);
                    float divisor = 32 * (updateEventCount[0x12] - previousTorqueEventCount);
                    if (divisor > 0)
                        sensorData.CrankTorque = (accumulatedTorque - previousAccumulatedTorqueValue) / divisor;
                    previousAccumulatedTorqueValue = accumulatedTorque;
                    previousTorqueEventCount = updateEventCount[0x12];
                }
            }
            else
            {
                Debug.Log($"Other datapage got data: [{string.Join(",",data)}]");
            }
        }


        /*public void Calibrate()
        {
            Debug.Log("Sending : Manual Zero Calibration request");
            pageToSend = new byte[8] {0x01, 0xAA, 0x0FF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};

            deviceChannel.sendAcknowledgedData(pageToSend);
        }*/

        private void ChannelResponse(ANT_Response response)
        {
            //Debug.Log($"response: {response.getChannelEventCode()}");
            //if (response.getChannelEventCode() == ANT_ReferenceLibrary.ANTEventID.EVENT_TRANSFER_TX_FAILED_0x06)
              //  deviceChannel.sendAcknowledgedData(pageToSend); //send the page again if the transfer failed
        }
    }
}