String decodeChannelFeedback(ANT_Response response)
{
StringBuilder stringToPrint; //We use a stringbuilder for speed and better memory usage, but, it doesn't really matter for the demo.
stringToPrint = new StringBuilder("Channel: ", 100); //Begin the string and allocate some more space
//In the channel feedback we will get either RESPONSE_EVENTs or receive events,
//If it is a response event we display what the event was and the error code if it failed.
//Mostly, these response_events will all be broadcast events from a Master channel.
if (response.responseID == (byte)ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40)
{
stringToPrint.AppendLine(((ANT_ReferenceLibrary.ANTEventID)response.messageContents[2]).ToString());
}
else //This is a receive event, so display the ID
{
stringToPrint.AppendLine("Received " + ((ANT_ReferenceLibrary.ANTMessageID)response.responseID).ToString());
}
//Always print the raw contents in hex, with leading '::' for easy visibility/parsing
//If this is a receive event it will contain the payload of the message
stringToPrint.Append(" :: ");
stringToPrint.Append(Convert.ToString(response.responseID, 16));
stringToPrint.Append(", ");
stringToPrint.Append(BitConverter.ToString(response.messageContents) + Environment.NewLine);
if (response.responseID == (byte)ANT_ReferenceLibrary.ANTMessageID.BROADCAST_DATA_0x4E)
{
// if (log != null)
// log.WriteLine(BitConverter.ToString(response.messageContents));
}
/*
*/
return(stringToPrint.ToString());
}
public void antChannel_channelResponse_DataFetch(ANT_Response response)
{
if (curStatus == ConnState.Connected)
{
//If the channel closes we need to know
if (response.responseID == (byte)ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40 &&
response.messageContents[1] == (byte)ANT_ReferenceLibrary.ANTMessageID.EVENT_0x01)
{
if (response.messageContents[2] == (byte)ANT_ReferenceLibrary.ANTEventID.EVENT_RX_FAIL_GO_TO_SEARCH_0x08)
{
dataSource.isInitialized = false;
curStatus = ConnState.DrpdToSrch;
}
else if (response.messageContents[2] == (byte)ANT_ReferenceLibrary.ANTEventID.EVENT_CHANNEL_CLOSED_0x07)
{
curStatus = ConnState.Closed; //TODO: qc Ensure search channel is cycling, or we will never get this back
}
}
}
else if (curStatus == ConnState.DrpdToSrch) //If we are in search and see a new broadcast
{
if (response.responseID == (byte)ANT_Managed_Library.ANT_ReferenceLibrary.ANTMessageID.BROADCAST_DATA_0x4E)
{
curStatus = ConnState.Connected;
}
}
dataSource.handleChannelResponse(response);
}
void ChannelResponse(ANT_Response response)
{
try
{
switch ((ANTMessageID)response.responseID)
{
case ANTMessageID.RESPONSE_EVENT_0x40:
ChannelEventResponse(response);
break;
case ANTMessageID.BROADCAST_DATA_0x4E:
case ANTMessageID.ACKNOWLEDGED_DATA_0x4F:
case ANTMessageID.BURST_DATA_0x50:
case ANTMessageID.EXT_BROADCAST_DATA_0x5D:
case ANTMessageID.EXT_ACKNOWLEDGED_DATA_0x5E:
case ANTMessageID.EXT_BURST_DATA_0x5F:
ChannelDataResponse(response);
break;
default:
Debug.WriteLine("Unknown Message " + response.responseID);
break;
}
}
catch (Exception ex)
{
Debug.WriteLine("Channel response processing failed with exception: " + ex.Message);
}
}
private void action_ReqVersion()
{
//To request the version we use the requestMessageAndResponse function. To request messages you could
//also use the requestMessage function, but then you need to monitor the device feedback for the response.
//The version string is a null terminated ascii string, so we decode it and display it
threadSafePrintLine("Requesting Device 0 Version", textBox_Display);
//On failure an exception is thrown so we watch for it
try
{
ANT_Response version = device0.requestMessageAndResponse(ANT_ReferenceLibrary.RequestMessageID.VERSION_0x3E, 500);
threadSafePrintLine("Device 0 ANT Version: " + new ASCIIEncoding().GetString(version.messageContents, 0, version.messageContents.Length - 1), textBox_Display);
}
catch (Exception)
{
threadSafePrintLine("Requesting Device 0 Version Timed Out", textBox_Display);
}
//if device 1 is open, request its version too
if (device1 != null)
{
threadSafePrintLine("Requesting Device 1 Version", textBox_Display);
try
{
ANT_Response version = device1.requestMessageAndResponse(ANT_ReferenceLibrary.RequestMessageID.VERSION_0x3E, 500);
threadSafePrintLine("Device 1 ANT Version: " + new ASCIIEncoding().GetString(version.messageContents, 0, version.messageContents.Length - 1), textBox_Display);
}
catch (Exception)
{
threadSafePrintLine("Requesting Device 1 Version Timed Out", textBox_Display);
}
}
}
/*
* DeviceResponse
* Called whenever a message is received from ANT unless that message is a
* channel event message.
* response: ANT message
*/
void DeviceResponse(ANT_Response response)
{
if (response.responseID == (byte)ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40)
{
messageQueue.Enqueue(response);
}
}
private void PublishReading(ANT_Response response)
{
Debug.Log("Publishing BROADCAST_DATA_0x4E !!!");
byte[] payload = response.getDataPayload();
// What the heck is going on here?!
ushort[] dataArray1 = new ushort[4]
{
(ushort)(((int)payload [3] & (int)byte.MaxValue) << 2 | (int)payload [4] >> 6),
(ushort)(((int)payload [4] & 63) << 4 | (int)payload [5] >> 4),
(ushort)(((int)payload [5] & 15) << 6 | (int)payload [6] >> 2),
(ushort)((uint)(((int)payload [6] & 3) << 8) | (uint)payload [7])
};
// Why are there 4 readings per sample?
for (int index = 0; index < 4; index++)
{
/* logic to hanle sample history
* this._gSampleHistorySum -= (uint)this._gSampleHistory [this._gBufferIndex];
* this._gSampleHistory [this._gBufferIndex] = dataArray1 [index];
* this._gSampleHistorySum += (uint)this._gSampleHistory [this._gBufferIndex];
* this._gSampleAverageHistory [this._gBufferIndex] = (ushort)(this._gSampleHistorySum / this._gNumTaps);
* this._gSampleAverageDifferenceSquaredHistory [this._gBufferIndex] =
* (uint)Math.Pow ((double)((int)this._gSampleHistory [this._gBufferIndex]
* - (int)this._gSampleAverageHistory [this._gBufferIndex]), 2.0);
* uint num1 = 0U;
* foreach (uint num2 in this._gSampleAverageDifferenceSquaredHistory)
* num1 += num2;
* this._gPseudoStandardDeviationHistory [index] = (ushort)Math.Sqrt ((double)num1 / (double)(this._gNumTaps - 1U));
* this._gBufferIndex = ++this._gBufferIndex % this._gNumTaps;
*/
SomaxisReading reading = new SomaxisReading(payload [0], dataArray1 [index]);
owner.PublishReading(reading, this);
}
}
void ChannelResponse(ANT_Response response)
{
if (response.getChannelEventCode() == ANT_ReferenceLibrary.ANTEventID.EVENT_TRANSFER_TX_FAILED_0x06)
{
deviceChannel.sendAcknowledgedData(pageToSend); //send the page again if the transfer failed
}
}
void ChannelResponse(ANT_Response response)
{
switch ((ANT_ReferenceLibrary.ANTMessageID)response.responseID)
{
case ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40:
{
//TODO: Handle many EVENT_RX_FAIL (close channel, throw exception?)
// Debug.Log(response.getChannelEventCode());
break;
}
case ANT_ReferenceLibrary.ANTMessageID.BROADCAST_DATA_0x4E:
case ANT_ReferenceLibrary.ANTMessageID.ACKNOWLEDGED_DATA_0x4F:
case ANT_ReferenceLibrary.ANTMessageID.BURST_DATA_0x50:
case ANT_ReferenceLibrary.ANTMessageID.EXT_BROADCAST_DATA_0x5D:
case ANT_ReferenceLibrary.ANTMessageID.EXT_ACKNOWLEDGED_DATA_0x5E:
case ANT_ReferenceLibrary.ANTMessageID.EXT_BURST_DATA_0x5F:
{
var dataPayload = response.getDataPayload();
ExtractHeartRateInfo(dataPayload);
break;
}
default:
{
Debug.Log("Unknown Message " + response.responseID);
break;
}
}
}
//This function decodes the message code into human readable form, shows the error value on failures, and also shows the raw message contents
String decodeDeviceFeedback(ANT_Response response)
{
string toDisplay = "Device: ";
//The ANTReferenceLibrary contains all the message and event codes in user-friendly enums
//This allows for more readable code and easy conversion to human readable strings for displays
// So, for the device response we first check if it is an event, then we check if it failed,
// and display the failure if that is the case. "Events" use message code 0x40.
if (response.responseID == (byte)ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40)
{
//We cast the byte to its messageID string and add the channel number byte associated with the message
toDisplay += (ANT_ReferenceLibrary.ANTMessageID)response.messageContents[1] + ", Ch:" + response.messageContents[0];
//Check if the eventID shows an error, if it does, show the error message
if ((ANT_ReferenceLibrary.ANTEventID)response.messageContents[2] != ANT_ReferenceLibrary.ANTEventID.RESPONSE_NO_ERROR_0x00)
{
toDisplay += Environment.NewLine + ((ANT_ReferenceLibrary.ANTEventID)response.messageContents[2]).ToString();
}
}
else //If the message is not an event, we just show the messageID
{
toDisplay += ((ANT_ReferenceLibrary.ANTMessageID)response.responseID).ToString();
}
//Finally we display the raw byte contents of the response, converting it to hex
toDisplay += Environment.NewLine + "::" + Convert.ToString(response.responseID, 16) + ", " + BitConverter.ToString(response.messageContents) + Environment.NewLine;
return(toDisplay);
}
void cadenceSensorResponse(ANT_Response response)
{
if (response.responseID == 0x4E && response.messageContents[1] == 0x00)
{
ushort bs = BitConverter.ToUInt16(new byte[2] {
response.messageContents[5], response.messageContents[6]
}, 0);
ushort crc = BitConverter.ToUInt16(new byte[2] {
response.messageContents[7], response.messageContents[8]
}, 0);
rpmData[0].eventTime = (ushort)(bs);
rpmData[0].revCount = crc;
rpmData[0] = rpmData[1];
rpmData[1].eventTime = (ushort)(bs);
rpmData[1].revCount = crc;
if (rpmData[0].Equals(rpmData[1]) == false)
{
current_rpm = (60 * ((rpmData[1].revCount - rpmData[0].revCount)) * 1024) / (rpmData[1].eventTime - rpmData[0].eventTime);
Debug.Log("RPM : " + current_rpm.ToString() + " rpm");
}
}
}
//Print the channel response to the textbox
void speedSensorResponse(ANT_Response response)
{
//if (response.responseID == 0x4E && response.messageContents[1] == 0x05)
if (response.responseID == 0x4E)
{
stop = false;
ushort bs = BitConverter.ToUInt16(new byte[2] {
response.messageContents[5], response.messageContents[6]
}, 0);
ushort crc = BitConverter.ToUInt16(new byte[2] {
response.messageContents[7], response.messageContents[8]
}, 0);
speedData[0].eventTime = (ushort)(bs);
speedData[0].revCount = crc;
speedData[0] = speedData[1];
speedData[1].eventTime = (ushort)(bs);
speedData[1].revCount = crc;
if (speedData[0].Equals(speedData[1]) == false)
{
current_speed = (1.8 * ((speedData[1].revCount - speedData[0].revCount))) / (speedData[1].eventTime - speedData[0].eventTime) * 3600;
if (current_speed > 100.0 || current_speed < 0)
{
current_speed = 0;
}
Debug.Log("Speed : " + current_speed.ToString() + " km/h");
}
}
}
private void action_ReqChannelID()
{
//To request the ID we also use the requestMessageAndResponse function
threadSafePrintLine("Requesting Device 0 Channel ID", textBox_Display);
//On failure an exception is thrown so we watch for it
try
{
ANT_Response IDresp = device0.requestMessageAndResponse(ANT_ReferenceLibrary.RequestMessageID.CHANNEL_ID_0x51, 500);
threadSafePrintLine("Device 0 Channel ID: " + "Dev#: " + ((IDresp.messageContents[2] << 8) + IDresp.messageContents[1]) + ", DevType: " + IDresp.messageContents[3] + ", TransType: " + IDresp.messageContents[4], textBox_Display);
}
catch (Exception)
{
threadSafePrintLine("Requesting Device 0 Channel ID Timed Out", textBox_Display);
}
//if device 1 is open, request its version too
if (device1 != null)
{
threadSafePrintLine("Requesting Device 1 Channel ID", textBox_Display);
try
{
ANT_Response IDresp = device1.requestMessageAndResponse(ANT_ReferenceLibrary.RequestMessageID.CHANNEL_ID_0x51, 500);
threadSafePrintLine("Device 1 Channel ID: " + "Dev#: " + ((IDresp.messageContents[2] << 8) + IDresp.messageContents[1]) + ", DevType: " + IDresp.messageContents[3] + ", TransType: " + IDresp.messageContents[4], textBox_Display);
}
catch (Exception)
{
threadSafePrintLine("Requesting Device 1 Channel ID Timed Out", textBox_Display);
}
}
}
void DeviceEventResponse(ANT_Response response)
{
var ec = response.getChannelEventCode();
switch (response.getMessageID())
{
case ANTMessageID.CHANNEL_SEARCH_TIMEOUT_0x44:
Debug.WriteLine("DeviceEventResponse CHANNEL_SEARCH_TIMEOUT_0x44");
break;
case ANTMessageID.CLOSE_CHANNEL_0x4C:
if (ec == ANTEventID.CHANNEL_IN_WRONG_STATE_0x15)
{
Debug.WriteLine("CHANNEL_IN_WRONG_STATE_0x15: Channel is already closed");
}
break;
case ANTMessageID.NETWORK_KEY_0x46:
case ANTMessageID.ASSIGN_CHANNEL_0x42:
case ANTMessageID.CHANNEL_ID_0x51:
case ANTMessageID.CHANNEL_RADIO_FREQ_0x45:
case ANTMessageID.CHANNEL_MESG_PERIOD_0x43:
case ANTMessageID.OPEN_CHANNEL_0x4B:
case ANTMessageID.UNASSIGN_CHANNEL_0x41:
if (ec != ANTEventID.RESPONSE_NO_ERROR_0x00)
{
PrintErrorMessage(response, "UNASSIGN_CHANNEL_0x41 RESPONSE_NO_ERROR_0x00");
}
break;
case ANTMessageID.RX_EXT_MESGS_ENABLE_0x66:
if (ec == ANTEventID.INVALID_MESSAGE_0x28)
{
Debug.WriteLine("Extended messages not supported in this ANT product");
break;
}
else if (ec != ANTEventID.RESPONSE_NO_ERROR_0x00)
{
PrintErrorMessage(response, "RX_EXT_MESGS_ENABLE_0x66 RESPONSE_NO_ERROR_0x00");
break;
}
// Extended messages enabled
break;
case ANTMessageID.REQUEST_0x4D:
if (ec == ANTEventID.INVALID_MESSAGE_0x28)
{
Debug.WriteLine("Requested message not supported in this ANT product");
break;
}
break;
default:
PrintErrorMessage(response, "Unhandled response");
break;
}
}
//Print the channel response to the textbox
void d0channel0_channelResponse(ANT_Response response)
{
//threadSafePrintLine(decodeChannelFeedback(response), textBox_device0);
antEventArgs aEA = new antEventArgs();
aEA.data = response.messageContents;
if (null != channelMessageHandler)
{
channelMessageHandler(this, aEA);
}
}
void antChannel_channelResponse_FeSearch(ANT_Response response)
{
switch ((ANT_ReferenceLibrary.ANTMessageID)response.responseID)
{
case ANT_ReferenceLibrary.ANTMessageID.BROADCAST_DATA_0x4E:
AntPlus_Connection newConn = deviceList[searchingDeviceIndex];
ANT_Response idResp = null;
try
{
idResp = antStick.requestMessageAndResponse(searchChannel.getChannelNum(), ANT_ReferenceLibrary.RequestMessageID.CHANNEL_ID_0x51, 1000);
}
catch (Exception)
{
//Don't know what to do if we can't get id...could retry somewhere...
break;
}
lock (newConn)
{
searchChannel.channelResponse -= antChannel_channelResponse_FeSearch;
newConn.dataSource.searchProfile.deviceNumber = (ushort)(idResp.messageContents[1] + ((ushort)idResp.messageContents[2] << 8)); //Save to the search profile so we keep this id after dropouts
newConn.connectedChannel = searchChannel;
//Note: the low pri search happens before the high pri, so this isn't even doing anything.
//newConn.connectedChannel.setChannelSearchTimeout(2, 500); //If we drop, we really want it back soon because it may be in use in a race, but we can't afford to ruin other channels staying in high priority. With the default search waveform, 5s is supposed to give us a really good rate of acquisition
newConn.connectedChannel.setLowPrioritySearchTimeout(255, 500); //Search indefinitely
newConn.antChannel_channelResponse_DataFetch(response);
newConn.connectedChannel.channelResponse += newConn.antChannel_channelResponse_DataFetch;
newConn.setConnStatus(AntPlus_Connection.ConnState.Connected);
}
searchChannel = null; //invalidate this channel as a search channel
startNextSearch();
break;
case ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40:
if (response.messageContents[1] == (byte)ANT_ReferenceLibrary.ANTMessageID.EVENT_0x01)
{
//if(response.messageContents[2] == 0x01) //Search timeout causes close channel, so wait for that
if (response.messageContents[2] == (byte)ANT_ReferenceLibrary.ANTEventID.EVENT_CHANNEL_CLOSED_0x07) //Closed channel
{
startNextSearch();
}
}
break;
}
}
private byte[] waitForMsg(byte[] leadingBytes, int wait_ms)
{
Queue <ANT_Response> msgSave = new Queue <ANT_Response>();
int i = 0;
ANT_Response curResponse;
System.Diagnostics.Stopwatch waitTimer = new System.Diagnostics.Stopwatch();
waitTimer.Start();
do
{
while (responseBuf.Count > 0)
{
curResponse = responseBuf.Dequeue();
msgSave.Enqueue(curResponse);
//If the leading bytes match our expected response
if (curResponse.responseID.Equals(leadingBytes[0]))
{
//Check message id matches
for (i = 1; i < leadingBytes.Length; ++i)
{
//Check remaining bytes to check
if (!curResponse.messageContents[i - 1].Equals(leadingBytes[i]))
{
break;
}
}
//If all bytes matched return the remaining bytes of the message
if (i == leadingBytes.Length)
{
//Save the seen messages back to the response buffer
responseBuf = new Queue <ANT_Response>(msgSave.Concat(responseBuf));
//Send out the information
lastAcceptedResponse = curResponse;
i -= 1; //Start at the byte we were at
return(curResponse.messageContents.Skip(i).ToArray());
}
}
}
} while (waitTimer.ElapsedMilliseconds < wait_ms);
//If we didn't find a matching response
responseBuf = new Queue <ANT_Response>(msgSave.Concat(responseBuf));
lastAcceptedResponse = null;
throw new Exception("Expected Message Timeout");
}
void DeviceResponse(ANT_Response response)
{
switch ((ANTMessageID)response.responseID)
{
case ANTMessageID.STARTUP_MESG_0x6F:
// RESET Complete
break;
case ANTMessageID.VERSION_0x3E:
Debug.WriteLine("VERSION: " + new ASCIIEncoding().GetString(response.messageContents));
break;
case ANTMessageID.RESPONSE_EVENT_0x40:
DeviceEventResponse(response);
break;
}
}
/// <summary>
/// Wait for a message matching the desired pattern
/// </summary>
/// <param name="matchPattern">Pattern to match</param>
/// <param name="wait_ms">time to wait, in ms</param>
/// <returns>True if the matching message is seen in the configured timeout</returns>
public bool WaitForNextMatchMsg(int[] matchPattern, int wait_ms)
{
Queue <ANT_Response> msgSave = new Queue <ANT_Response>();
ANT_Response curResponse;
int i;
responseBuf.Clear();
System.Diagnostics.Stopwatch waitTimer = new System.Diagnostics.Stopwatch();
waitTimer.Start();
do
{
while (responseBuf.Count > 0)
{
System.Threading.Thread.Sleep(10); //Allow time for a new message to arrive
curResponse = responseBuf.Dequeue();
msgSave.Enqueue(curResponse);
//Check each byte with the conditional match function against the given conditional byte match array
if (compareConditionalByte(curResponse.responseID, matchPattern[0]) && curResponse.messageContents.Length == (matchPattern.Length - 1))
{
//Check message id matches
for (i = 1; i < matchPattern.Length; ++i)
{
//Check remaining bytes to check
if (!compareConditionalByte(curResponse.messageContents[i - 1], matchPattern[i]))
{
break;
}
}
//If all bytes matched then we succeeded
if (i == matchPattern.Length)
{
//Save all the seen messages back to the buffer
responseBuf = new Queue <ANT_Response>(msgSave.Concat(responseBuf));
lastAcceptedResponse = curResponse;
return(true);
}
}
}
} while (waitTimer.ElapsedMilliseconds < wait_ms);
//If we didn't find a match
responseBuf = new Queue <ANT_Response>(msgSave.Concat(responseBuf));
lastAcceptedResponse = null;
return(false);
}
/// <summary>
/// Handle ANT channel responses, while in broadcast mode
/// </summary>
/// <param name="response">ANT channel event</param>
public void HandleChannelResponses(ANT_Response response)
{
// Make sure we are not processing responses if ANT-FS is active
if (antfsHost.GetStatus() != ANTFS_HostChannel.State.Idle)
{
return;
}
if (response.responseID == (byte)ANT_ReferenceLibrary.ANTMessageID.BROADCAST_DATA_0x4E)
{
Console.Write("Rx: " + Demo.CursorStrings[cursorIndex++] + "\r");
cursorIndex &= 3;
if (response.getDataPayload()[7] >= 40) // Receive data for about 10 seconds
{
antfsHost.RequestSession(Demo.SearchRF, ConnectRF); // Request ANT-FS session
}
}
}
/// <summary>
/// Handle channel responses, while in broadcast mode
/// </summary>
/// <param name="response">ANT channel events</param>
public void HandleChannelResponses(ANT_Response response)
{
// Make sure we are not processing responses if ANT-FS is active
if (antfsClient.GetStatus() != ANTFS_ClientChannel.State.Idle)
{
return;
}
if ((response.responseID == (byte)ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40))
{
if (response.getChannelEventCode() == ANT_ReferenceLibrary.ANTEventID.EVENT_TX_0x03)
{
channel0.sendBroadcastData(txBuffer);
txBuffer[7]++;
Console.Write("Tx: " + Demo.CursorStrings[cursorIndex++] + "\r");
cursorIndex &= 3;
}
else if (response.getChannelEventCode() == ANT_ReferenceLibrary.ANTEventID.EVENT_CHANNEL_CLOSED_0x07)
{
Console.WriteLine("Channel Closed");
Console.WriteLine("Unassigning Channel...");
if (channel0.unassignChannel(500))
{
Console.WriteLine("Unassigned Channel");
Console.WriteLine("Press enter to exit");
demoDone = true;
}
}
}
else if (response.responseID == (byte)ANT_ReferenceLibrary.ANTMessageID.ACKNOWLEDGED_DATA_0x4F)
{
// Look for request page
if (response.messageContents[1] == 0x46)
{
if (response.messageContents[6] == 0 &&
response.messageContents[7] == 0x43 &&
response.messageContents[8] == 0x02)
{
Console.WriteLine("Remote device requesting ANT-FS session");
}
antfsClient.OpenBeacon();
}
}
}
void OnChannelResponse(ANT_Response response)
{
if (response.getChannelEventCode() == ANT_ReferenceLibrary.ANTEventID.EVENT_RX_FAIL_0x02)
{
DisplayText("channel " + response.antChannel.ToString() + " " + "RX fail");
}
else if (response.getChannelEventCode() == ANT_ReferenceLibrary.ANTEventID.EVENT_RX_FAIL_GO_TO_SEARCH_0x08)
{
DisplayText("channel " + response.antChannel.ToString() + " " + "Go to search");
}
else if (response.getChannelEventCode() == ANT_ReferenceLibrary.ANTEventID.EVENT_TX_0x03)
{
DisplayText("channel " + response.antChannel.ToString() + " " + "Tx: (" + response.antChannel.ToString() + ")" + BitConverter.ToString(hrChannel.txBuffer));
}
else
{
DisplayText("channel " + response.antChannel.ToString() + " " + response.getChannelEventCode());
}
}
private void handleANTResponses(ANT_Response response)
{
if ((int)response.responseID == 64)
{
if (response.getChannelEventCode() == ANT_ReferenceLibrary.ANTEventID.RESPONSE_NO_ERROR_0x00)
{
Debug.LogError("SET " + ((object)response.getMessageID()).ToString() + "... OK");
}
else
{
Debug.LogError("Response: " + ((object)response.getChannelEventCode()).ToString()
+ " to " + ((object)response.getMessageID()).ToString());
}
}
else
{
Debug.LogError(((object)(ANT_ReferenceLibrary.ANTMessageID)response.responseID).ToString());
}
}
String decodeDeviceFeedback(ANT_Response response)
{
string toDisplay = "Device: ";
if (response.responseID == (byte)ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40)
{
toDisplay += (ANT_ReferenceLibrary.ANTMessageID)response.messageContents[1] + ", Ch:" + response.messageContents[0];
if ((ANT_ReferenceLibrary.ANTEventID)response.messageContents[2] != ANT_ReferenceLibrary.ANTEventID.RESPONSE_NO_ERROR_0x00)
{
toDisplay += Environment.NewLine + ((ANT_ReferenceLibrary.ANTEventID)response.messageContents[2]).ToString();
}
}
else //If the message is not an event, we just show the messageID
{
toDisplay += ((ANT_ReferenceLibrary.ANTMessageID)response.responseID).ToString();
}
toDisplay += Environment.NewLine + "::" + Convert.ToString(response.responseID, 16) + ", " + BitConverter.ToString(response.messageContents) + Environment.NewLine;
return(toDisplay);
}
String decodeChannelFeedback(ANT_Response response)
{
StringBuilder stringToPrint; //We use a stringbuilder for speed and better memory usage, but, it doesn't really matter for the demo.
stringToPrint = new StringBuilder("Channel: ", 100); //Begin the string and allocate some more space
if (response.responseID == (byte)ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40)
{
stringToPrint.AppendLine(((ANT_ReferenceLibrary.ANTEventID)response.messageContents[2]).ToString());
}
else //This is a receive event, so display the ID
{
stringToPrint.AppendLine("Received " + ((ANT_ReferenceLibrary.ANTMessageID)response.responseID).ToString());
}
stringToPrint.Append(" :: ");
stringToPrint.Append(Convert.ToString(response.responseID, 16));
stringToPrint.Append(", ");
stringToPrint.Append(BitConverter.ToString(response.messageContents) + Environment.NewLine);
return(stringToPrint.ToString());
}
public sealed override void handleChannelResponse(ANT_Response response)
{
//Watch for collisions or failures, if we miss a message, interpolate from the last known speed
if (response.responseID == (byte)ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40 &&
response.messageContents[1] == (byte)ANT_ReferenceLibrary.ANTMessageID.EVENT_0x01 &&
(response.messageContents[2] == (byte)ANT_ReferenceLibrary.ANTEventID.EVENT_RX_FAIL_0x02 ||
response.messageContents[2] == (byte)ANT_ReferenceLibrary.ANTEventID.EVENT_CHANNEL_COLLISION_0x09))
{
DataSourcePacket lastData = getLastDataRcvd();
if (lastData != null)
{
DateTime curEventTime = DateTime.Now;
double timeMissed_s = curEventTime.Subtract(lastEventTime).TotalSeconds;
double distMissed_m = timeMissed_s * averageSpeed; //Use the recent average speed so we get a closer estimate
interpolatedDistanceDiff += distMissed_m;
lastEventTime = curEventTime;
base.incrementDistanceAndUpdate(distMissed_m, averageSpeed, lastData.cadence, lastData.heartRate, lastData.power);
////DEBUG
//System.Console.Out.WriteLine((ANT_ReferenceLibrary.ANTEventID)response.messageContents[2] + " interpolated " + distMissed_m);
}
}
if (response.messageContents[2] == (byte)ANT_ReferenceLibrary.ANTEventID.EVENT_RX_FAIL_GO_TO_SEARCH_0x08)
{
//Sorry, if we drop to search there is not much we can do to recover, let the race display know we are dead
base.incrementDistanceAndUpdate(0, 0, 0, 0, 0);
}
else //If it is not an rx failure, let the derived class handle it
{
//Don't worry about isInitialized=false on rx_drop_to_search here, because we hope the broadcast
//comes back soon enough for it to not matter, if we have to wait long enough for it to matter,
//we have waited too long anyway
////DEBUG
//if (response.responseID == (byte)ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40)
// System.Console.Out.WriteLine((ANT_ReferenceLibrary.ANTEventID)response.messageContents[2]);
handleNonRxFailChannelResponse(response);
}
}
void channel_channelResponse(ANT_Response response)
{
switch ((ANT_ReferenceLibrary.ANTMessageID)response.responseID)
{
case ANT_ReferenceLibrary.ANTMessageID.ACKNOWLEDGED_DATA_0x4F:
statusReport = String.Format("Open - Last Activity = {0}:{1}", DateTime.Now.ToLongTimeString(), "Rcvd Ack");
lastMsgBytes = response.messageContents.Skip(1).ToArray();
decodeCmd(lastMsgBytes);
break;
case ANT_ReferenceLibrary.ANTMessageID.BURST_DATA_0x50:
statusReport = String.Format("Open - Last Activity = {0}:{1}", DateTime.Now.ToLongTimeString(), "Rcvd Burst");
if ((response.messageContents[0] & 0xE0) == 0) //first packet
{
lastMsgBytes = response.messageContents.Skip(1).ToArray();
}
else
{
lastMsgBytes = lastMsgBytes.Concat(response.messageContents.Skip(1)).ToArray();
if ((response.messageContents[0] & 0x80) == 0x80) //last packet
{
decodeCmd(lastMsgBytes);
}
}
break;
case ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40:
//Print all messages except EVENT_TX to status
if ((ANT_ReferenceLibrary.ANTEventID)response.messageContents[2] != ANT_ReferenceLibrary.ANTEventID.EVENT_TX_0x03)
{
statusReport = String.Format("Open - Last Activity = {0}:{1}", DateTime.Now.ToLongTimeString(), (ANT_ReferenceLibrary.ANTEventID)response.messageContents[2]);
}
break;
default:
statusReport = String.Format("Open - Last Activity = {0}:{1}", DateTime.Now.ToLongTimeString(), (ANT_ReferenceLibrary.ANTMessageID)response.responseID);
break;
}
}
String decodeChannelFeedback(ANT_Response response)
{
//Decoding channel events is an important consideration when building applications.
//The channel events will be where broadcast, ack and burst messages are received, you
//are also notified of messages sent and, for acknowledged and burst messages, their success or failure.
//In a burst transfer, or if the device is operating at a small message period, then it is important that
//decoding the messages is done quickly and efficiently.
//The way the response retrieval works, messages should never be lost. If you decode messages too slow, they are simply queued.
//However, you will be retrieving messages at a longer and longer delay from when they were actually received by the device.
//One further consideration when handling channel events is to remember that the feedback functions will be triggered on a different thread
//which is operating within the managed library to retrieve the feedback from the device. So, if you need to interact with your UI objects
//you will need to use a Dispatcher invoke with a prority that will not make the UI unresponsive (see the threadSafePrint function used here).
StringBuilder stringToPrint; //We use a stringbuilder for speed and better memory usage, but, it doesn't really matter for the demo.
stringToPrint = new StringBuilder("Channel: ", 100); //Begin the string and allocate some more space
//In the channel feedback we will get either RESPONSE_EVENTs or receive events,
//If it is a response event we display what the event was and the error code if it failed.
//Mostly, these response_events will all be broadcast events from a Master channel.
if (response.responseID == (byte)ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40)
{
stringToPrint.AppendLine(((ANT_ReferenceLibrary.ANTEventID)response.messageContents[2]).ToString());
}
else //This is a receive event, so display the ID
{
stringToPrint.AppendLine("Received " + ((ANT_ReferenceLibrary.ANTMessageID)response.responseID).ToString());
}
//Always print the raw contents in hex, with leading '::' for easy visibility/parsing
//If this is a receive event it will contain the payload of the message
stringToPrint.Append(" :: ");
stringToPrint.Append(Convert.ToString(response.responseID, 16));
stringToPrint.Append(", ");
stringToPrint.Append(BitConverter.ToString(response.messageContents) + Environment.NewLine);
return(stringToPrint.ToString());
}
private void ChannelResponse(ANT_Response response)
{
// Debug.Log("[AntStick] " + decodeChannelFeedback(response));
if (response.responseID == (byte)ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40)
{
if (response.messageContents[2] == (byte)ANT_ReferenceLibrary.ANTEventID.EVENT_RX_SEARCH_TIMEOUT_0x01)
{
Debug.LogWarning("[AntStick] Failed to connect to any device.");
Debug.LogWarning("[AntStick] <b>WARNING --- UNITY MAY CRASH!</b> Try lowering AntStick.CONNECT_TIMEOUT value.");
UpdateState(AntState.ConnectFail);
Stop();
}
if (response.messageContents[2] == (byte)ANT_ReferenceLibrary.ANTEventID.EVENT_RX_FAIL_0x02)
{
Stats.IncrementRxFailCount();
}
if (response.messageContents[2] == (byte)ANT_ReferenceLibrary.ANTEventID.EVENT_RX_FAIL_GO_TO_SEARCH_0x08)
{
UpdateState(AntState.ReConnecting);
StartConnectTimeout();
}
}
else
{
if (state == AntState.Starting || state == AntState.ReConnecting)
{
Debug.Log("[AntStick] Successfully began receiving data.");
UpdateState(AntState.Connected);
}
DataPage dataPage = DataPage.BuildDataPageFromReceivedData(response.messageContents);
Stats.IncrementPageCount(dataPage.DataPageNumber);
dataPage.FireReceived();
}
}
////////////////////////////////////////////////////////////////////////////////
// ChannelResponse
//
// Called whenever a channel event is recieved.
//
// response: ANT message
////////////////////////////////////////////////////////////////////////////////
void ChannelResponse(ANT_Response response)
{
//With the messageQueue we can deal with ANT response in the Unity main thread
if (response.responseID == (byte)ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40)
{
messageQueue.Enqueue(response);
}
try
{
switch ((ANT_ReferenceLibrary.ANTMessageID)response.responseID)
{
case ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40:
{
switch (response.getChannelEventCode())
{
// This event indicates that a message has just been
// sent over the air. We take advantage of this event to set
// up the data for the next message period.
case ANT_ReferenceLibrary.ANTEventID.EVENT_TX_0x03:
{
// Broadcast data will be sent over the air on
// the next message period
if (broadcasting)
{
channel.sendBroadcastData(txBuffer);
}
break;
}
case ANT_ReferenceLibrary.ANTEventID.EVENT_RX_SEARCH_TIMEOUT_0x01:
{
Debug.Log("Search Timeout");
break;
}
case ANT_ReferenceLibrary.ANTEventID.EVENT_RX_FAIL_0x02:
{
if (!hideRXFAIL)
{
Debug.Log("Rx Fail");
}
break;
}
case ANT_ReferenceLibrary.ANTEventID.EVENT_TRANSFER_RX_FAILED_0x04:
{
Debug.Log("Burst receive has failed");
break;
}
case ANT_ReferenceLibrary.ANTEventID.EVENT_TRANSFER_TX_COMPLETED_0x05:
{
Debug.Log("Transfer Completed");
break;
}
case ANT_ReferenceLibrary.ANTEventID.EVENT_TRANSFER_TX_FAILED_0x06:
{
Debug.Log("Transfer Failed");
break;
}
case ANT_ReferenceLibrary.ANTEventID.EVENT_CHANNEL_CLOSED_0x07:
{
channel.unassignChannel(500);
break;
}
case ANT_ReferenceLibrary.ANTEventID.EVENT_RX_FAIL_GO_TO_SEARCH_0x08:
{
Debug.Log("Go to Search");
break;
}
case ANT_ReferenceLibrary.ANTEventID.EVENT_CHANNEL_COLLISION_0x09:
{
Debug.Log("Channel Collision");
break;
}
case ANT_ReferenceLibrary.ANTEventID.EVENT_TRANSFER_TX_START_0x0A:
{
Debug.Log("Burst Started");
break;
}
default:
{
Debug.Log("Unhandled Channel Event " + response.getChannelEventCode());
break;
}
}
break;
}
case ANT_ReferenceLibrary.ANTMessageID.BROADCAST_DATA_0x4E:
case ANT_ReferenceLibrary.ANTMessageID.ACKNOWLEDGED_DATA_0x4F:
case ANT_ReferenceLibrary.ANTMessageID.BURST_DATA_0x50:
case ANT_ReferenceLibrary.ANTMessageID.EXT_BROADCAST_DATA_0x5D:
case ANT_ReferenceLibrary.ANTMessageID.EXT_ACKNOWLEDGED_DATA_0x5E:
case ANT_ReferenceLibrary.ANTMessageID.EXT_BURST_DATA_0x5F:
{
if (response.isExtended() && isBackgroundScan == true)
{
RXQueue.Enqueue(response.messageContents);
}
else
{
RXQueue.Enqueue(response.getDataPayload());
}
break;
}
default:
{
Debug.Log("Unknown Message " + response.responseID);
break;
}
}
}
catch (Exception ex)
{
Debug.Log("Channel response processing failed with exception: " + ex.Message);
}
}
////////////////////////////////////////////////////////////////////////////////
// DeviceResponse
//
// Called whenever a message is received from ANT unless that message is a
// channel event message.
//
// response: ANT message
////////////////////////////////////////////////////////////////////////////////
static void DeviceResponse(ANT_Response response)
{
switch ((ANT_ReferenceLibrary.ANTMessageID)response.responseID)
{
case ANT_ReferenceLibrary.ANTMessageID.STARTUP_MESG_0x6F:
{
Console.Write("RESET Complete, reason: ");
byte ucReason = response.messageContents[0];
if (ucReason == (byte)ANT_ReferenceLibrary.StartupMessage.RESET_POR_0x00)
{
Console.WriteLine("RESET_POR");
}
if (ucReason == (byte)ANT_ReferenceLibrary.StartupMessage.RESET_RST_0x01)
{
Console.WriteLine("RESET_RST");
}
if (ucReason == (byte)ANT_ReferenceLibrary.StartupMessage.RESET_WDT_0x02)
{
Console.WriteLine("RESET_WDT");
}
if (ucReason == (byte)ANT_ReferenceLibrary.StartupMessage.RESET_CMD_0x20)
{
Console.WriteLine("RESET_CMD");
}
if (ucReason == (byte)ANT_ReferenceLibrary.StartupMessage.RESET_SYNC_0x40)
{
Console.WriteLine("RESET_SYNC");
}
if (ucReason == (byte)ANT_ReferenceLibrary.StartupMessage.RESET_SUSPEND_0x80)
{
Console.WriteLine("RESET_SUSPEND");
}
break;
}
case ANT_ReferenceLibrary.ANTMessageID.VERSION_0x3E:
{
Console.WriteLine("VERSION: " + new ASCIIEncoding().GetString(response.messageContents));
break;
}
case ANT_ReferenceLibrary.ANTMessageID.RESPONSE_EVENT_0x40:
{
switch (response.getMessageID())
{
case ANT_ReferenceLibrary.ANTMessageID.CLOSE_CHANNEL_0x4C:
{
if (response.getChannelEventCode() == ANT_ReferenceLibrary.ANTEventID.CHANNEL_IN_WRONG_STATE_0x15)
{
Console.WriteLine("Channel is already closed");
Console.WriteLine("Unassigning Channel...");
if (channel0.unassignChannel(500))
{
Console.WriteLine("Unassigned Channel");
Console.WriteLine("Press enter to exit");
bDone = true;
}
}
break;
}
case ANT_ReferenceLibrary.ANTMessageID.NETWORK_KEY_0x46:
case ANT_ReferenceLibrary.ANTMessageID.ASSIGN_CHANNEL_0x42:
case ANT_ReferenceLibrary.ANTMessageID.CHANNEL_ID_0x51:
case ANT_ReferenceLibrary.ANTMessageID.CHANNEL_RADIO_FREQ_0x45:
case ANT_ReferenceLibrary.ANTMessageID.CHANNEL_MESG_PERIOD_0x43:
case ANT_ReferenceLibrary.ANTMessageID.OPEN_CHANNEL_0x4B:
case ANT_ReferenceLibrary.ANTMessageID.UNASSIGN_CHANNEL_0x41:
{
if (response.getChannelEventCode() != ANT_ReferenceLibrary.ANTEventID.RESPONSE_NO_ERROR_0x00)
{
Console.WriteLine(String.Format("Error {0} configuring {1}", response.getChannelEventCode(), response.getMessageID()));
}
break;
}
case ANT_ReferenceLibrary.ANTMessageID.RX_EXT_MESGS_ENABLE_0x66:
{
if (response.getChannelEventCode() == ANT_ReferenceLibrary.ANTEventID.INVALID_MESSAGE_0x28)
{
Console.WriteLine("Extended messages not supported in this ANT product");
break;
}
else if (response.getChannelEventCode() != ANT_ReferenceLibrary.ANTEventID.RESPONSE_NO_ERROR_0x00)
{
Console.WriteLine(String.Format("Error {0} configuring {1}", response.getChannelEventCode(), response.getMessageID()));
break;
}
Console.WriteLine("Extended messages enabled");
break;
}
case ANT_ReferenceLibrary.ANTMessageID.REQUEST_0x4D:
{
if (response.getChannelEventCode() == ANT_ReferenceLibrary.ANTEventID.INVALID_MESSAGE_0x28)
{
Console.WriteLine("Requested message not supported in this ANT product");
break;
}
break;
}
default:
{
Console.WriteLine("Unhandled response " + response.getChannelEventCode() + " to message " + response.getMessageID()); break;
}
}
break;
}
}
}
private byte[] waitForMsg(byte[] leadingBytes, int wait_ms)
{
Queue<ANT_Response> msgSave = new Queue<ANT_Response>();
int i = 0;
ANT_Response curResponse;
System.Diagnostics.Stopwatch waitTimer = new System.Diagnostics.Stopwatch();
waitTimer.Start();
do
{
while (responseBuf.Count > 0)
{
curResponse = responseBuf.Dequeue();
msgSave.Enqueue(curResponse);
//If the leading bytes match our expected response
if (curResponse.responseID.Equals(leadingBytes[0]))
{
//Check message id matches
for (i = 1; i < leadingBytes.Length; ++i)
{
//Check remaining bytes to check
if (!curResponse.messageContents[i - 1].Equals(leadingBytes[i]))
break;
}
//If all bytes matched return the remaining bytes of the message
if (i == leadingBytes.Length)
{
//Save the seen messages back to the response buffer
responseBuf = new Queue<ANT_Response>(msgSave.Concat(responseBuf));
//Send out the information
lastAcceptedResponse = curResponse;
i -= 1; //Start at the byte we were at
return curResponse.messageContents.Skip(i).ToArray();
}
}
}
} while (waitTimer.ElapsedMilliseconds < wait_ms);
//If we didn't find a matching response
responseBuf = new Queue<ANT_Response>(msgSave.Concat(responseBuf));
lastAcceptedResponse = null;
throw new Exception("Expected Message Timeout");
}
void dev_hostdeviceEvents(ANT_Response response)
{
hostdeviceEvents = response.messageContents;
flagHostDeviceEvents = true;
}
//TODO: This buffer gets really big if we sit around for very long
void dev_deviceResponse(ANT_Response response)
{
responseBuf.Enqueue(response);
}
/// <summary>
/// Wait for a message matching the desired pattern
/// </summary>
/// <param name="matchPattern">Pattern to match</param>
/// <param name="wait_ms">time to wait, in ms</param>
/// <returns>True if the matching message is seen in the configured timeout</returns>
public bool WaitForNextMatchMsg(int[] matchPattern, int wait_ms)
{
Queue<ANT_Response> msgSave = new Queue<ANT_Response>();
ANT_Response curResponse;
int i;
responseBuf.Clear();
System.Diagnostics.Stopwatch waitTimer = new System.Diagnostics.Stopwatch();
waitTimer.Start();
do
{
while (responseBuf.Count > 0)
{
System.Threading.Thread.Sleep(10); //Allow time for a new message to arrive
curResponse = responseBuf.Dequeue();
msgSave.Enqueue(curResponse);
//Check each byte with the conditional match function against the given conditional byte match array
if (compareConditionalByte(curResponse.responseID, matchPattern[0]) && curResponse.messageContents.Length == (matchPattern.Length - 1))
{
//Check message id matches
for (i = 1; i < matchPattern.Length; ++i)
{
//Check remaining bytes to check
if (!compareConditionalByte(curResponse.messageContents[i - 1], matchPattern[i]))
break;
}
//If all bytes matched then we succeeded
if (i == matchPattern.Length)
{
//Save all the seen messages back to the buffer
responseBuf = new Queue<ANT_Response>(msgSave.Concat(responseBuf));
lastAcceptedResponse = curResponse;
return true;
}
}
}
} while (waitTimer.ElapsedMilliseconds < wait_ms);
//If we didn't find a match
responseBuf = new Queue<ANT_Response>(msgSave.Concat(responseBuf));
lastAcceptedResponse = null;
return false;
}
