1
0

PowerMeterReceiver.cs 6.4 KB

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