2
0

DecodePowerOnly.c 10 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271
  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 "stdbool.h"
  10. #include "stdlib.h"
  11. #define _USE_MATH_DEFINES
  12. #include "math.h"
  13. #include "PowerDecoder.h"
  14. #include "RecordOutput.h"
  15. #include "DecodePowerOnly.h"
  16. static BPSAMPLER stState;
  17. static PowerRecordReceiver prrPtr;
  18. static double dRecordInterval;
  19. static double dReSyncInterval;
  20. #define UPDATE_EVENT_BYTE 1
  21. #define PEDAL_BALANCE_BYTE 2
  22. #define INST_CADENCE_BYTE 3
  23. #define ACCUM_POWER_LSB 4
  24. #define ACCUM_POWER_MSB 5
  25. #define INST_POWER_LSB 6
  26. #define INST_POWER_MSB 7
  27. void DecodePowerOnly_Init(double dRecordInterval_, double dTimeBase_, double dReSyncInterval_, PowerRecordReceiver powerRecordReceiverPtr_)
  28. {
  29. ResamplerOutput_Init(&stState, (int)(dRecordInterval_* PO_TIME_QUANTIZATION), dRecordInterval_, (int)(dTimeBase_ * PO_TIME_QUANTIZATION));
  30. prrPtr = powerRecordReceiverPtr_;
  31. dRecordInterval = dRecordInterval_;
  32. dReSyncInterval = dReSyncInterval_;
  33. }
  34. //
  35. // Message event handler interface.
  36. // This is intended to abstract away the top-level messiness of having to detect data gaps or duplicates, etc.
  37. //
  38. void DecodePowerOnly_Message(double dTime_, unsigned char messagePayload_[])
  39. {
  40. // see if the message is new.
  41. if (stState.ucLastEventCount != messagePayload_[1])
  42. {
  43. if ((dTime_ - stState.dLastMessageTime) > dReSyncInterval)
  44. {
  45. DecodePowerOnly_Resync(dTime_, messagePayload_);
  46. }
  47. else
  48. {
  49. DecodePowerOnly(dTime_, messagePayload_);
  50. }
  51. stState.dLastMessageTime = dTime_;
  52. stState.ucLastEventCount = messagePayload_[1];
  53. }
  54. }
  55. ///////////////////////////////////////////////////////////////////////
  56. //
  57. // If the power-only messages are associated with torque based messages
  58. // then we need to process them differently in order to make sure the
  59. // total energy is properly calculated.
  60. //
  61. ///////////////////////////////////////////////////////////////////////
  62. void DecodePowerOnly_SetTimeBase(double dTimeBase_)
  63. {
  64. // reset the timebase
  65. stState.usTimeBase = (int)(dTimeBase_ * PO_TIME_QUANTIZATION);
  66. }
  67. ///////////////////////////////////////////////////////////////////////
  68. //
  69. // Re-establish data baseline.
  70. ///////////////////////////////////////////////////////////////////////
  71. void DecodePowerOnly_Resync(double dCurrentTime_, unsigned char messagePayload_[])
  72. {
  73. unsigned short usCurrentAccumPower;
  74. unsigned char ucCurrentEventCount = messagePayload_[UPDATE_EVENT_BYTE];
  75. double dCurrentRecordEpoch = (floor(dCurrentTime_ / dRecordInterval)) * dRecordInterval;
  76. if ((stState.dLastRecordTime != 0)
  77. && (dCurrentRecordEpoch - stState.dLastRecordTime > 0)
  78. && (dCurrentRecordEpoch - stState.dLastRecordTime < MAXIMUM_TIME_GAP))
  79. {
  80. stState.ucRecordGapCount = (unsigned char)((dCurrentRecordEpoch - stState.dLastRecordTime + dRecordInterval * 0.5)
  81. / dRecordInterval);
  82. // Transfer the accumulated data to the gap.
  83. stState.fGapEnergy = stState.fAccumEnergy;
  84. stState.fGapRotation = stState.fAccumRotation;
  85. // We need to fill in the gap with records.
  86. RecordOutput_FillGap(prrPtr, &stState);
  87. }
  88. usCurrentAccumPower = messagePayload_[ACCUM_POWER_LSB];
  89. usCurrentAccumPower += ((unsigned short)messagePayload_[ACCUM_POWER_MSB]) << 8;
  90. stState.ucCadence = messagePayload_[INST_CADENCE_BYTE];
  91. stState.fAccumEnergy = 0;
  92. stState.fPendingEnergy = 0;
  93. stState.fGapEnergy = 0;
  94. stState.fAccumRotation = 0;
  95. stState.fPendingRotation = 0;
  96. stState.fGapRotation = 0;
  97. stState.ucRecordGapCount = 0;
  98. stState.ulEventTime = 0;
  99. stState.ulLastRecordTime = 0;
  100. stState.dLastMessageTime = dCurrentTime_;
  101. // Update our saved state.
  102. stState.dLastRecordTime = dCurrentRecordEpoch;
  103. stState.usLastAccumPeriod = 0;
  104. stState.ucLastRotationTicks = messagePayload_[UPDATE_EVENT_BYTE];
  105. stState.ucLastEventCount = messagePayload_[UPDATE_EVENT_BYTE];
  106. stState.usLastAccumTorque = usCurrentAccumPower; // use the accumtorque field to store the accum power data
  107. }
  108. ///////////////////////////////////////////////////////////////////////////////
  109. //
  110. //
  111. ///////////////////////////////////////////////////////////////////////////////
  112. void DecodePowerOnly(double dTime_, unsigned char messagePayload_[])
  113. {
  114. unsigned long ulNewEventTime;
  115. unsigned short usCurrentAccumPower;
  116. unsigned short usDeltaPeriod;
  117. unsigned short usDeltaPowerPeriod;
  118. unsigned short usDeltaPower;
  119. unsigned short usInstPower;
  120. unsigned char ucEventBalance = messagePayload_[PEDAL_BALANCE_BYTE];
  121. unsigned char ucCurrentEventCount = messagePayload_[UPDATE_EVENT_BYTE];
  122. unsigned char ucDeltaTicks;
  123. float fEventEnergy;
  124. usCurrentAccumPower = messagePayload_[ACCUM_POWER_LSB];
  125. usCurrentAccumPower += ((unsigned short)messagePayload_[ACCUM_POWER_MSB]) << 8;
  126. usInstPower = messagePayload_[INST_POWER_LSB];
  127. usInstPower += ((unsigned short)messagePayload_[INST_POWER_MSB]) << 8;
  128. usDeltaPower = usCurrentAccumPower - stState.usLastAccumTorque; // make sure this is done in 16 bit word width!
  129. ucDeltaTicks = messagePayload_[UPDATE_EVENT_BYTE] - stState.ucLastEventCount;
  130. stState.ucCadence = messagePayload_[INST_CADENCE_BYTE];
  131. // Sanity check on delta power vs. instantaneous.
  132. if ((usInstPower > 0) && (usDeltaPower > 100 * usInstPower))
  133. {
  134. usDeltaPower = usInstPower;
  135. }
  136. if (stState.ucCadence > 0)
  137. {
  138. usDeltaPeriod = (unsigned short)(((unsigned long)ucDeltaTicks * PO_TIME_QUANTIZATION * 60L + (stState.ucCadence >> 1)) / stState.ucCadence);
  139. }
  140. else
  141. {
  142. usDeltaPeriod = 0xFFFF;
  143. usDeltaPower = 0;
  144. }
  145. if (stState.usTimeBase != 0)
  146. {
  147. // time based messages.
  148. ulNewEventTime = stState.ulEventTime + (unsigned long)stState.usTimeBase*ucDeltaTicks;
  149. #if defined (TIMEBASE_DRIFT_CORRECTION)
  150. // This is a correction for cases where the sensor timebase is fast compared to the
  151. // receiver timebase.
  152. if ((dTime_ - stState.dLastRecordTime) > (RECORD_INTERVAL * 2))
  153. {
  154. //create a gap to fill.
  155. ulNewEventTime += stState.usRecordInterval;
  156. }
  157. #endif
  158. // Maybe we want to up the resolution on the power to energy
  159. // conversion. We round the power to the nearest watt so we
  160. // should be ok in the long term.
  161. usDeltaPowerPeriod = stState.usTimeBase*ucDeltaTicks;
  162. fEventEnergy = (float)usDeltaPower;
  163. }
  164. else
  165. {
  166. // event based messages
  167. usDeltaPowerPeriod = usDeltaPeriod;
  168. ulNewEventTime = stState.ulEventTime + (unsigned long)usDeltaPeriod;
  169. fEventEnergy = (float)usDeltaPower*usDeltaPeriod / PO_TIME_QUANTIZATION / ucDeltaTicks;
  170. }
  171. if (((unsigned short)(ulNewEventTime - stState.ulLastRecordTime)) >= stState.usRecordInterval)
  172. {
  173. // The event occurred after the end of the current record epoch.
  174. // First, figure out the number of records in a gap if it exists. This calculation uses
  175. // implicit truncation in the division so the subtraction can't be done first.
  176. stState.ucRecordGapCount = (unsigned char)((ulNewEventTime / stState.usRecordInterval) - (stState.ulLastRecordTime / stState.usRecordInterval) - 1);
  177. // Pending energy goes towards the partial accumulated record we currently have.
  178. stState.fPendingEnergy = stState.fAccumEnergy + fEventEnergy * ((float)(stState.usRecordInterval - (stState.ulEventTime % stState.usRecordInterval))) / ((float)usDeltaPowerPeriod);
  179. // accumulated energy goes towards the *next* event.
  180. stState.fAccumEnergy = fEventEnergy * ((float)(ulNewEventTime % stState.usRecordInterval)) / ((float)usDeltaPowerPeriod);
  181. // Gap energy fills the remainder.
  182. stState.fGapEnergy = fEventEnergy * ((unsigned short)stState.ucRecordGapCount * stState.usRecordInterval) / ((float)usDeltaPowerPeriod);
  183. //Same for rotation.
  184. stState.fPendingRotation = stState.fAccumRotation + (float)ucDeltaTicks * ((float)(stState.usRecordInterval - (stState.ulEventTime % stState.usRecordInterval))) / ((float)usDeltaPeriod);
  185. stState.fAccumRotation = (float)ucDeltaTicks * ((float)(ulNewEventTime % stState.usRecordInterval)) / ((float)usDeltaPeriod);
  186. stState.fGapRotation = (float)ucDeltaTicks * ((unsigned short)stState.ucRecordGapCount * stState.usRecordInterval) / ((float)usDeltaPeriod);
  187. }
  188. else
  189. {
  190. // This event came in before the next record epoch started - this
  191. // will happen when the event period is less than the recording period.
  192. stState.fAccumEnergy += fEventEnergy;
  193. if (stState.usTimeBase != 0)
  194. {
  195. stState.fAccumRotation += (float)ucDeltaTicks * (float)(stState.ucCadence) / 60.0f;
  196. }
  197. else
  198. {
  199. stState.fAccumRotation += (float)ucDeltaTicks;
  200. }
  201. stState.fPendingEnergy = 0;
  202. stState.fPendingRotation = 0;
  203. stState.ucRecordGapCount = 0;
  204. }
  205. stState.ulEventTime = ulNewEventTime;
  206. if (((unsigned short)(stState.ulEventTime - stState.ulLastRecordTime)) >= stState.usRecordInterval)
  207. {
  208. RecordOutput(prrPtr, &stState);
  209. }
  210. else
  211. {
  212. // We've had an event that either didn't have a rotation associated
  213. // with it (no event time increment) or else it was within the
  214. // recording interval.
  215. if ((dTime_ - stState.dLastRecordTime) > dRecordInterval)
  216. {
  217. while ((dTime_ - stState.dLastRecordTime) > dRecordInterval)
  218. {
  219. stState.dLastRecordTime += ((double)stState.usRecordInterval) / PO_TIME_QUANTIZATION;
  220. (prrPtr)(stState.dLastRecordTime, stState.dTotalRotation, stState.dTotalEnergy, 0.0, 0.0);
  221. }
  222. }
  223. }
  224. // Propagate the message state information.
  225. stState.ucLastRotationTicks = messagePayload_[UPDATE_EVENT_BYTE];
  226. stState.ucLastEventCount = messagePayload_[UPDATE_EVENT_BYTE];
  227. stState.usLastAccumTorque = usCurrentAccumPower;
  228. }