1
0

DecodeWheelTorque.c 13 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316
  1. /*
  2. This software is subject to the license described in the License.txt file
  3. included with this software distribution. You may not use this file except in compliance
  4. with this license.
  5. Copyright (c) Dynastream Innovations Inc. 2014
  6. All rights reserved.
  7. */
  8. #include "string.h"
  9. #include "stdlib.h"
  10. #define _USE_MATH_DEFINES
  11. #include "math.h"
  12. #include "PowerDecoder.h"
  13. #include "RecordOutput.h"
  14. #include "DecodeWheelTorque.h"
  15. #define PROPAGATE_CADENCE
  16. static BPSAMPLER stState;
  17. static PowerRecordReceiver prrPtr;
  18. static double dRecordInterval;
  19. static double dReSyncInterval;
  20. #define UPDATE_EVENT_BYTE 1
  21. #define WHEEL_TICKS_BYTE 2
  22. #define INST_CADENCE_BYTE 3
  23. #define ACCUM_PERIOD_LSB 4
  24. #define ACCUM_PERIOD_MSB 5
  25. #define ACCUM_TORQUE_LSB 6
  26. #define ACCUM_TORQUE_MSB 7
  27. void DecodeWheelTorque_Init(double dRecordInterval_, double dTimeBasedPeriod_, double dReSyncInterval_, PowerRecordReceiver powerRecordReceiverPtr_)
  28. {
  29. ResamplerOutput_Init(&stState, (int)(dRecordInterval_ * WT_TIME_QUANTIZATION), dRecordInterval_, (int)(dTimeBasedPeriod_ * WT_TIME_QUANTIZATION));
  30. prrPtr = powerRecordReceiverPtr_;
  31. dRecordInterval = dRecordInterval_;
  32. dReSyncInterval = dReSyncInterval_;
  33. }
  34. ///////////////////////////////////////////////////////////////////////////////
  35. // void DecodeCrankTorque_Message(double dTime_, unsigned char messagePayload_[])
  36. ///////////////////////////////////////////////////////////////////////////////
  37. //
  38. // Message event handler interface.
  39. // This is intended to abstract away the top-level messiness of having to
  40. // detect data gaps or duplicates, etc.
  41. //
  42. ///////////////////////////////////////////////////////////////////////////////
  43. void DecodeWheelTorque_Message(double dTime_, unsigned char messagePayload_[])
  44. {
  45. // see if the message is new.
  46. if (stState.ucLastEventCount != messagePayload_[UPDATE_EVENT_BYTE])
  47. {
  48. if ((dTime_ - stState.dLastMessageTime) > dReSyncInterval)
  49. {
  50. DecodeWheelTorque_Resync(dTime_, messagePayload_);
  51. }
  52. else
  53. {
  54. DecodeWheelTorque(dTime_, messagePayload_);
  55. }
  56. stState.dLastMessageTime = dTime_;
  57. stState.ucLastEventCount = messagePayload_[UPDATE_EVENT_BYTE];
  58. }
  59. }
  60. ///////////////////////////////////////////////////////////////////////////////
  61. // void DecodeCrankTorque_Resync(double dCurrentTime_, unsigned char messagePayload_[])
  62. ///////////////////////////////////////////////////////////////////////////////
  63. //
  64. // Re-establish data baseline.
  65. ///////////////////////////////////////////////////////////////////////////////
  66. void DecodeWheelTorque_Resync(double dCurrentTime_, unsigned char messagePayload_[])
  67. {
  68. unsigned short usCurrentAccumTorque;
  69. unsigned short usCurrentAccumPeriod;
  70. // CurrentRecordEpoch is the last time that we should have had a data record.
  71. double dCurrentRecordEpoch = (floor(dCurrentTime_ / dRecordInterval)) * dRecordInterval;
  72. if ((stState.dLastRecordTime != 0) &&
  73. (dCurrentRecordEpoch - stState.dLastRecordTime > 0) &&
  74. (dCurrentRecordEpoch - stState.dLastRecordTime < MAXIMUM_TIME_GAP))
  75. {
  76. stState.ucRecordGapCount = (unsigned char)(dCurrentRecordEpoch - stState.dLastRecordTime + dRecordInterval * 0.5)
  77. / dRecordInterval;
  78. // Transfer the accumulated data to the gap.
  79. stState.fGapEnergy = stState.fAccumEnergy;
  80. stState.fGapRotation = stState.fAccumRotation;
  81. // We need to fill in the gap with records.
  82. RecordOutput_FillGap(prrPtr, &stState);
  83. }
  84. usCurrentAccumPeriod = messagePayload_[ACCUM_PERIOD_LSB];
  85. usCurrentAccumPeriod += ((unsigned short)messagePayload_[ACCUM_PERIOD_MSB]) << 8;
  86. usCurrentAccumTorque = messagePayload_[ACCUM_TORQUE_LSB];
  87. usCurrentAccumTorque += ((unsigned short)messagePayload_[ACCUM_TORQUE_MSB]) << 8;
  88. stState.ucCadence = messagePayload_[INST_CADENCE_BYTE];
  89. stState.fAccumEnergy = 0;
  90. stState.fPendingEnergy = 0;
  91. stState.fGapEnergy = 0;
  92. stState.fAccumRotation = 0;
  93. stState.fPendingRotation = 0;
  94. stState.fGapRotation = 0;
  95. stState.ucRecordGapCount = 0;
  96. stState.ulEventTime = 0;
  97. stState.ulLastRecordTime = 0;
  98. stState.dLastMessageTime = dCurrentTime_;
  99. // Update our saved state.
  100. stState.dLastRecordTime = dCurrentRecordEpoch;
  101. stState.usLastAccumTorque = usCurrentAccumTorque;
  102. stState.usLastAccumPeriod = usCurrentAccumPeriod;
  103. stState.ucLastRotationTicks = messagePayload_[WHEEL_TICKS_BYTE];
  104. stState.ucLastEventCount = messagePayload_[UPDATE_EVENT_BYTE];
  105. }
  106. ///////////////////////////////////////////////////////////////////////////////
  107. // void DecodeWheelTorque(double dTime_, unsigned char messagePayload_[])
  108. ///////////////////////////////////////////////////////////////////////////////
  109. // This is the main decoding function for wheel torque messages.
  110. // Emphasis is placed at this point on handling the specific data
  111. // that comes from the Powertap system since it's the only commercial
  112. // wheel torque power meter.
  113. //
  114. // For time based systems we split the wheel period and the power period,
  115. // since the wheel rate is the reciprocal of the wheel period, but the
  116. // power period is the timebase value.
  117. //
  118. // There is a further (compile-time) split in here to enable the output
  119. // of the averaged cadence OR the wheel rotation rate. Compatibility with
  120. // the other power meter outputs suggests that cadence output is preferable.
  121. // The wheel output can be handled by a separate decoder or eventually
  122. // as a special case... code is left here to illustrate the general method.
  123. ///////////////////////////////////////////////////////////////////////////////
  124. void DecodeWheelTorque(double dTime_, unsigned char messagePayload_[])
  125. {
  126. unsigned long ulNewEventTime;
  127. unsigned long ulEventWheelRPM;
  128. unsigned long ulEventPower;
  129. unsigned short usCurrentAccumTorque;
  130. unsigned short usCurrentAccumPeriod;
  131. unsigned short usDeltaTorque;
  132. unsigned short usDeltaPeriod;
  133. unsigned short usDeltaPowerPeriod;
  134. unsigned char ucDeltaEventCount;
  135. unsigned char ucDeltaTicks;
  136. float fEventEnergy;
  137. usCurrentAccumPeriod = messagePayload_[ACCUM_PERIOD_LSB];
  138. usCurrentAccumPeriod += ((unsigned short)messagePayload_[ACCUM_PERIOD_MSB]) << 8;
  139. usCurrentAccumTorque = messagePayload_[ACCUM_TORQUE_LSB];
  140. usCurrentAccumTorque += ((unsigned short)messagePayload_[ACCUM_TORQUE_MSB]) << 8;
  141. usDeltaTorque = usCurrentAccumTorque - stState.usLastAccumTorque; // make sure this is done in 16 bit word width!
  142. usDeltaPeriod = usCurrentAccumPeriod - stState.usLastAccumPeriod; // make sure this is done in 16 bit word width!
  143. usDeltaPowerPeriod = usDeltaPeriod;
  144. stState.ucCadence = messagePayload_[INST_CADENCE_BYTE];
  145. ucDeltaEventCount = messagePayload_[UPDATE_EVENT_BYTE] - stState.ucLastEventCount;
  146. ucDeltaTicks = messagePayload_[WHEEL_TICKS_BYTE] - stState.ucLastRotationTicks;
  147. if (ucDeltaTicks > 200)
  148. {
  149. // Unlikely to be right...
  150. ucDeltaTicks = 0;
  151. }
  152. // 65535 is an invalid value.
  153. if (usDeltaTorque == 65535)
  154. {
  155. usDeltaTorque = 0;
  156. }
  157. if (usDeltaPeriod && (usDeltaPeriod != 0xFFFF))
  158. {
  159. ulEventPower = ((long)(M_PI*2048.0 + 0.5) * usDeltaTorque / usDeltaPeriod + 8) >> 4;
  160. if (stState.usTimeBase != 0)
  161. {
  162. // time based messages.
  163. ulNewEventTime = stState.ulEventTime + (unsigned long)stState.usTimeBase * ucDeltaEventCount;
  164. #if defined (TIMEBASE_DRIFT_CORRECTION)
  165. // This is a correction for cases where the sensor timebase is fast compared to the
  166. // receiver timebase.
  167. if ((dTime_ - stState.dLastRecordTime) > (RECORD_INTERVAL * 2))
  168. {
  169. //create a gap to fill.
  170. ulNewEventTime += stState.usRecordInterval;
  171. }
  172. #endif
  173. // Maybe we want to up the resolution on the power to energy
  174. // conversion. We round the power to the nearest watt so we
  175. // should be ok in the long term.
  176. usDeltaPowerPeriod = stState.usTimeBase;
  177. fEventEnergy = (float)ulEventPower;
  178. // the reported data reflects one revolution for each message update.
  179. #if defined (PROPAGATE_CADENCE)
  180. if (stState.ucCadence)
  181. {
  182. ucDeltaTicks = ucDeltaEventCount;
  183. }
  184. else
  185. {
  186. ucDeltaTicks = 0;
  187. }
  188. #else
  189. ucDeltaTicks = ucDeltaEventCount;
  190. #endif
  191. }
  192. else
  193. {
  194. // event based messages
  195. ulNewEventTime = stState.ulEventTime + (unsigned long)usDeltaPeriod;
  196. fEventEnergy = (float)(M_PI * (float)usDeltaTorque / 16.0);
  197. }
  198. // This is actually the wheel rotation speed.
  199. ulEventWheelRPM = ((long)ucDeltaTicks * 60L * WT_TIME_QUANTIZATION + (usDeltaPeriod >> 1)) / usDeltaPeriod;
  200. }
  201. else
  202. {
  203. // This is basically a non-event.
  204. ulEventPower = 0;
  205. ulEventWheelRPM = 0;
  206. fEventEnergy = 0;
  207. ulNewEventTime = stState.ulEventTime;
  208. }
  209. if (((unsigned short)(ulNewEventTime - stState.ulLastRecordTime)) >= stState.usRecordInterval)
  210. {
  211. // The event occurred after the end of the current record epoch.
  212. // First, figure out the number of records in a gap if it exists. This calculation uses
  213. // implicit truncation in the division so the subtraction can't be done first.
  214. stState.ucRecordGapCount = (unsigned char)((ulNewEventTime / stState.usRecordInterval) - (stState.ulLastRecordTime / stState.usRecordInterval) - 1);
  215. // Pending energy goes towards the partial accumulated record we currently have.
  216. stState.fPendingEnergy = stState.fAccumEnergy + fEventEnergy * ((float)(stState.usRecordInterval - (stState.ulEventTime % stState.usRecordInterval))) / ((float)usDeltaPowerPeriod);
  217. // accumulated energy goes towards the *next* event.
  218. stState.fAccumEnergy = fEventEnergy * ((float)(ulNewEventTime % stState.usRecordInterval)) / ((float)usDeltaPowerPeriod);
  219. // Gap energy fills the remainder.
  220. stState.fGapEnergy = fEventEnergy * ((unsigned short)stState.ucRecordGapCount * stState.usRecordInterval) / ((float)usDeltaPowerPeriod);
  221. //Same for rotation. Within this framework we can propagate either the wheel speed or the cycling cadence...
  222. #if defined (PROPAGATE_CADENCE)
  223. stState.fPendingRotation = stState.fAccumRotation + (float)ucDeltaTicks * (float)(stState.ucCadence) / 60.0f * ((float)(stState.usRecordInterval - (stState.ulEventTime % stState.usRecordInterval))) / ((float)WT_TIME_QUANTIZATION);
  224. stState.fAccumRotation = (float)ucDeltaTicks * (float)(stState.ucCadence) / 60.0f * ((float)(ulNewEventTime % stState.usRecordInterval)) / ((float)WT_TIME_QUANTIZATION);
  225. stState.fGapRotation = (float)ucDeltaTicks * (float)(stState.ucCadence) / 60.0f * ((unsigned short)stState.ucRecordGapCount * stState.usRecordInterval) / ((float)WT_TIME_QUANTIZATION);
  226. #else
  227. stState.fPendingRotation = stState.fAccumRotation + (float)ucDeltaTicks * ((float)(stState.usRecordInterval - (stState.ulEventTime % stState.usRecordInterval)))/((float)usDeltaPeriod);
  228. stState.fAccumRotation = (float)ucDeltaTicks * ((float)(ulNewEventTime % stState.usRecordInterval))/((float)usDeltaPeriod);
  229. stState.fGapRotation = (float)ucDeltaTicks * ((unsigned short)stState.ucRecordGapCount * stState.usRecordInterval) / ((float)usDeltaPeriod);
  230. #endif
  231. }
  232. else
  233. {
  234. // This event came in before the next record epoch started - this
  235. // will happen when the event period is less than the recording period.
  236. stState.fAccumEnergy += fEventEnergy;
  237. #if defined (PROPAGATE_CADENCE)
  238. stState.fAccumRotation += (float)ucDeltaTicks * (float)(stState.ucCadence) / 60.0f;
  239. #else
  240. stState.fAccumRotation += (float)ucDeltaTicks;
  241. #endif
  242. stState.fPendingEnergy = 0;
  243. stState.fPendingRotation = 0;
  244. stState.ucRecordGapCount = 0;
  245. }
  246. stState.ulEventTime = ulNewEventTime;
  247. if (((unsigned short)(stState.ulEventTime - stState.ulLastRecordTime)) >= stState.usRecordInterval)
  248. {
  249. RecordOutput(prrPtr, &stState);
  250. }
  251. else
  252. {
  253. // We've had an event that either didn't have a rotation associated
  254. // with it (no event time increment) or else it was within the
  255. // recording interval.
  256. if ((dTime_ - stState.dLastRecordTime) > dRecordInterval)
  257. {
  258. while ((dTime_ - stState.dLastRecordTime) > dRecordInterval)
  259. {
  260. stState.dLastRecordTime += ((double)stState.usRecordInterval) / WT_TIME_QUANTIZATION;
  261. (prrPtr)(stState.dLastRecordTime, stState.dTotalRotation, stState.dTotalEnergy, 0.0, 0.0);
  262. }
  263. }
  264. }
  265. // Propagate the message state information.
  266. stState.ucLastEventCount = messagePayload_[UPDATE_EVENT_BYTE];
  267. stState.ucLastRotationTicks = messagePayload_[WHEEL_TICKS_BYTE];
  268. stState.usLastAccumPeriod = usCurrentAccumPeriod;
  269. stState.usLastAccumTorque = usCurrentAccumTorque;
  270. }