PowerMeterReceiver.cs 6.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182
  1. using UnityEngine;
  2. using System.Collections;
  3. using System.Collections.Generic;
  4. using ANT_Managed_Library;
  5. using System;
  6. /*
  7. Frequencies such as 4.06 Hz (ANT+ Heart
  8. Rate) and 4.005 Hz (ANT+ Bike Power) will
  9. periodically “drift” into each other, or into
  10. other channel periods that may be present
  11. in the vicinity. During this overlap, channel
  12. collisions may occur as the radio can only
  13. service channel at a time.
  14. */
  15. public struct PowermeterSensorData
  16. {
  17. public int InstantaneousPower; // the instantaneous power in watt
  18. public int InstantaneousCadence; // crank cadence in RPM if available ( 255 indicates invalid)
  19. public float CrankTorque;
  20. }
  21. public class PowerMeterReceiver
  22. {
  23. private AntChannel backgroundScanChannel;
  24. private AntChannel deviceChannel;
  25. private byte[] pageToSend;
  26. private PowermeterSensorData sensorData;
  27. public int DeviceID { get; } = 0;
  28. public bool Connected { get; private set; } = false;
  29. public List<AntDevice> ScanResult { get; private set; }
  30. public PowermeterSensorData SensorData => sensorData;
  31. public PowerMeterReceiver()
  32. {
  33. }
  34. public PowerMeterReceiver(int deviceID) => DeviceID = deviceID;
  35. public void StartScan()
  36. {
  37. Debug.Log("Looking for ANT + power meter sensor");
  38. AntManager.Instance.Init();
  39. ScanResult = new List<AntDevice>();
  40. backgroundScanChannel = AntManager.Instance.OpenBackgroundScanChannel(0);
  41. backgroundScanChannel.onReceiveData += ReceivedBackgroundScanData;
  42. }
  43. void ReceivedBackgroundScanData(Byte[] data)
  44. {
  45. byte deviceType = (data[12]); // extended info Device Type byte
  46. switch (deviceType)
  47. {
  48. case AntplusDeviceType.BikePower:
  49. {
  50. int deviceNumber = (data[10]) | data[11] << 8;
  51. byte transType = data[13];
  52. foreach (AntDevice d in ScanResult)
  53. {
  54. if (d.deviceNumber == deviceNumber && d.transType == transType) //device already found
  55. return;
  56. }
  57. AntDevice foundDevice = new AntDevice();
  58. foundDevice.deviceType = deviceType;
  59. foundDevice.deviceNumber = deviceNumber;
  60. foundDevice.transType = transType;
  61. foundDevice.period = 8182;
  62. foundDevice.radiofreq = 57;
  63. foundDevice.name = "Powermeter(" + foundDevice.deviceNumber + ")";
  64. ScanResult.Add(foundDevice);
  65. if (deviceNumber == DeviceID)
  66. {
  67. Debug.Log($"Desired Power Sensor with id {deviceNumber} found!");
  68. ConnectToDevice(foundDevice);
  69. }
  70. else
  71. {
  72. Debug.Log($"Power sensor ({deviceNumber}) found and added to ScanResult");
  73. }
  74. break;
  75. }
  76. }
  77. }
  78. void ConnectToDevice(AntDevice device)
  79. {
  80. AntManager.Instance.CloseBackgroundScanChannel();
  81. byte channelID = AntManager.Instance.GetFreeChannelID();
  82. deviceChannel = AntManager.Instance.OpenChannel(ANT_ReferenceLibrary.ChannelType.BASE_Slave_Receive_0x00,
  83. channelID, (ushort) device.deviceNumber, device.deviceType, device.transType, (byte) device.radiofreq,
  84. (ushort) device.period, false);
  85. Connected = true;
  86. deviceChannel.onReceiveData += Data;
  87. deviceChannel.onChannelResponse += ChannelResponse;
  88. deviceChannel.hideRXFAIL = true;
  89. }
  90. private readonly Dictionary<int, int> updateEventCount = new Dictionary<int, int> {{0x10, 0}, {0x12, 0}};
  91. private readonly Dictionary<int, int> sameEventCounter = new Dictionary<int, int> {{0x10, 0}, {0x12, 0}};
  92. private int previousAccumulatedTorqueValue = 0;
  93. private int previousTorqueEventCount = 0;
  94. //Deal with the received Data
  95. private void Data(Byte[] data)
  96. {
  97. if (data[0] == 0x10) // Standard Power-Only Main Data Page
  98. {
  99. if (data[1] == updateEventCount[0x10])
  100. sameEventCounter[0x10]++;
  101. else
  102. sameEventCounter[0x10] = 0;
  103. updateEventCount[0x10] = data[1];
  104. if (sameEventCounter[0x10] > 3)
  105. {
  106. sensorData.InstantaneousPower = 0;
  107. sensorData.InstantaneousCadence = 0;
  108. }
  109. else
  110. {
  111. sensorData.InstantaneousPower = data[6] | data[7] << 8;
  112. sensorData.InstantaneousCadence = data[3];
  113. }
  114. }
  115. if (data[0] == 0x12) // Standard Crank Torque Main Data Page
  116. {
  117. Debug.Log($"Torque Data {data}");
  118. if (data[1] == updateEventCount[0x12])
  119. sameEventCounter[0x12]++;
  120. else
  121. sameEventCounter[0x12] = 0;
  122. updateEventCount[0x12] = data[1];
  123. if (sameEventCounter[0x12] > 3)
  124. {
  125. sensorData.CrankTorque = 0;
  126. }
  127. else
  128. {
  129. int accumulatedTorque = data[6] | data[7] << 8;
  130. float divisor = (32 * (updateEventCount[0x12] - previousTorqueEventCount));
  131. if (divisor > 0)
  132. {
  133. sensorData.CrankTorque = (accumulatedTorque - previousAccumulatedTorqueValue) / divisor;
  134. }
  135. previousAccumulatedTorqueValue = accumulatedTorque;
  136. previousTorqueEventCount = updateEventCount[0x12];
  137. }
  138. }
  139. }
  140. public void Calibrate()
  141. {
  142. Debug.Log("Sending : Manual Zero Calibration request");
  143. pageToSend = new byte[8] {0x01, 0xAA, 0x0FF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
  144. deviceChannel.sendAcknowledgedData(pageToSend);
  145. }
  146. void ChannelResponse(ANT_Response response)
  147. {
  148. if (response.getChannelEventCode() == ANT_ReferenceLibrary.ANTEventID.EVENT_TRANSFER_TX_FAILED_0x06)
  149. {
  150. deviceChannel.sendAcknowledgedData(pageToSend); //send the page again if the transfer failed
  151. }
  152. }
  153. }