/*InitBeacon*/ // Initialize Type B timer and input capture on digital input pin for BEACON_IN /*PostBeacon*/ // Allows external code to post an event to this queue /*RunBeacon*/ // ES_INIT or ES_TIMEOUT event // Calculate time since last beacon alignment time // If time is greater than 10ms // If misalignment index++ % 5 == 0 or aligned flag is set // Clear aligned flag // Post HW_MISALIGNED to leader service // Initialize align time timer // HW_ALIGNED // Calculate frequency based on period param // If has never aligned yet // Set isRed or isBlue flag depending on detected beacon frequency (with tolerance) // If either color flag is set // Set first alignment flag // Change LED team indicator color // If same frequency of original detected team frequency is detected again // Set aligned flag // Post HW_BEACON to leader service /*IC3 Handler*/ Input capture for BEACON_IN pin // If corresponding timer interrupt flag is set and input capture buffer is less than 0x8000 // Increment rollover counter and clear timer interrupt flag // Record last beacon alignment time as current time // Add calculated beacon period to buffer // If buffer is full (i.e., has MEDIAN_LENGTH elements) and aligned flag not set // Calculate median of buffer using MedianBeaconTimes() // Clear buffer // Post HW_ALIGNED to BeaconService with median as period param /*MedianBeaconTimes*/ // Bubble sort on buffer (buffer is very small) // If length of buffer is even // Return average of middle two elements // If length of buffer is odd // Return middle element