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 { private readonly Dictionary sameEventCounter = new Dictionary {{0x10, 0}, {0x12, 0}}; private readonly Dictionary updateEventCount = new Dictionary {{0x10, 0}, {0x12, 0}}; private AntChannel backgroundScanChannel; private AntChannel deviceChannel; private byte[] pageToSend; private int previousAccumulatedTorqueValue; private int previousTorqueEventCount; private PowermeterSensorData sensorData; public PowerMeterReceiver() { } public PowerMeterReceiver(int deviceID) { DeviceID = deviceID; } public int DeviceID { get; } public bool Connected { get; private set; } public List ScanResult { get; private set; } public PowermeterSensorData SensorData => sensorData; public void StartScan() { Debug.Log("Looking for ANT + power meter sensor"); AntManager.Instance.Init(); ScanResult = new List(); backgroundScanChannel = AntManager.Instance.OpenBackgroundScanChannel(0); backgroundScanChannel.onReceiveData += ReceivedBackgroundScanData; } private void ReceivedBackgroundScanData(byte[] data) { var deviceType = data[12]; // extended info Device Type byte switch (deviceType) { case AntplusDeviceType.BikePower: { 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 = 8182; foundDevice.radiofreq = 57; foundDevice.name = "Powermeter(" + foundDevice.deviceNumber + ")"; ScanResult.Add(foundDevice); if (deviceNumber == DeviceID) { Debug.Log($"Desired Power Sensor with id {deviceNumber} found!"); ConnectToDevice(foundDevice); } else { Debug.Log($"Power 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) { if (data[0] == 0x10) // Standard Power-Only Main Data Page { 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]; } } if (data[0] == 0x12) // Standard Crank Torque Main Data Page { //Debug.Log($"Torque Data {data}"); if (data[1] == updateEventCount[0x12]) sameEventCounter[0x12]++; else sameEventCounter[0x12] = 0; updateEventCount[0x12] = data[1]; if (sameEventCounter[0x12] > 3) { 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]; } } } 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) { if (response.getChannelEventCode() == ANT_ReferenceLibrary.ANTEventID.EVENT_TRANSFER_TX_FAILED_0x06) deviceChannel.sendAcknowledgedData(pageToSend); //send the page again if the transfer failed } } }