moparisthebest/USBHost_t36
1
0
Fork 0
mirror of https://github.com/gdsports/USBHost_t36 synced 2024-11-21 16:45:04 -05:00
Code Issues Releases Wiki Activity
bd5f7d9486
USBHost_t36/antplus.cpp

1232 lines
35 KiB
C++
Raw Blame History

/* USB EHCI Host for Teensy 3.6
* Copyright 2017 Michael McElligott
* Copyright 2017 Paul Stoffregen (paul@pjrc.com)
*
* Permission is hereby granted, free of charge, to any person obtaining a
* copy of this software and associated documentation files (the
* "Software"), to deal in the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
* The above copyright notice and this permission notice shall be included
* in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
* OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
// https://forum.pjrc.com/threads/43110
// https://github.com/PaulStoffregen/antplus
#include <Arduino.h>
#include "USBHost_t36.h" // Read this header first for key info
#include "antplusdefs.h" // Ant internal defines, not for Arduino sketches
#define ANTPLUS_VID 0x0FCF
#define ANTPLUS_2_PID 0x1008
#define ANTPLUS_M_PID 0x1009
#define print USBHost::print_
#define println USBHost::println_
#define ENABLE_SERIALPRINTF 1
#if ENABLE_SERIALPRINTF
#undef printf
#define printf(...) Serial.printf(__VA_ARGS__); Serial.write("\r\n")
#else
#undef printf
#define printf(...)
#endif
void AntPlus::init()
{
contribute_Pipes(mypipes, sizeof(mypipes)/sizeof(Pipe_t));
contribute_Transfers(mytransfers, sizeof(mytransfers)/sizeof(Transfer_t));
contribute_String_Buffers(mystring_bufs, sizeof(mystring_bufs)/sizeof(strbuf_t));
driver_ready_for_device(this);
user_onStatusChange = NULL;
user_onDeviceID = NULL;
user_onHeartRateMonitor = NULL;
user_onSpeedCadence = NULL;
user_onSpeed = NULL;
user_onCadence = NULL;
wheelCircumference = WHEEL_CIRCUMFERENCE;
}
bool AntPlus::claim(Device_t *dev, int type, const uint8_t *descriptors, uint32_t len)
{
if (type != 1) return false;
println("AntPlus claim this=", (uint32_t)this, HEX);
if (dev->idVendor != ANTPLUS_VID) return false;
if (dev->idProduct != ANTPLUS_2_PID && dev->idProduct != ANTPLUS_M_PID) return false;
println("found AntPlus, pid=", dev->idProduct, HEX);
rxpipe = txpipe = NULL;
const uint8_t *p = descriptors;
const uint8_t *end = p + len;
int descriptorLength = p[0];
int descriptorType = p[1];
if (descriptorLength < 9 || descriptorType != 4) return false;
p += descriptorLength;
while (p < end) {
descriptorLength = p[0];
if (p + descriptorLength > end) return false; // reject if beyond end of data
descriptorType = p[1];
if (descriptorType == 5) { // 5 = endpoint
uint8_t epAddr = p[2];
uint8_t epType = p[3] & 0x03;
uint16_t epSize = p[4] | (p[5] << 8);
if (epType == 2 && (epAddr & 0xF0) == 0x00) { // Bulk OUT
txpipe = new_Pipe(dev, 2, epAddr, 0, epSize);
} else if (epType == 2 && (epAddr & 0xF0) == 0x80) { // Bulk IN
rxpipe = new_Pipe(dev, 2, epAddr & 0x0F, 1, epSize);
}
}
p += descriptorLength;
}
if (rxpipe && txpipe) {
rxpipe->callback_function = rx_callback;
txpipe->callback_function = tx_callback;
txhead = 0;
txtail = 0;
//rxhead = 0;
//rxtail = 0;
memset(txbuffer, 0, sizeof(txbuffer));
first_update = true;
txready = true;
updatetimer.start(500000);
queue_Data_Transfer(rxpipe, rxpacket, 64, this);
rxlen = 0;
do_polling = false;
return true;
}
return false;
}
void AntPlus::disconnect()
{
updatetimer.stop();
//txtimer.stop();
}
void AntPlus::rx_callback(const Transfer_t *transfer)
{
if (!transfer->driver) return;
((AntPlus *)(transfer->driver))->rx_data(transfer);
}
void AntPlus::tx_callback(const Transfer_t *transfer)
{
if (!transfer->driver) return;
((AntPlus *)(transfer->driver))->tx_data(transfer);
}
void AntPlus::rx_data(const Transfer_t *transfer)
{
uint32_t len = transfer->length - ((transfer->qtd.token >> 16) & 0x7FFF);
//println("ant rx, len=", len);
//print_hexbytes(transfer->buffer, len);
if (len < 1 || len > 64) {
queue_Data_Transfer(rxpipe, rxpacket, 64, this);
rxlen = 0;
} else {
rxlen = len; // signal arrival of data to Task()
// TODO: should someday use EventResponder to call from yield()
}
}
void AntPlus::tx_data(const Transfer_t *transfer)
{
uint8_t *p = (uint8_t *)transfer->buffer;
//print("tx_data, len=", *(p-1));
//print(", tail=", (p-1) - txbuffer);
//println(", tail=", txtail);
uint32_t tail = txtail;
uint8_t size = *(p-1);
tail += size + 1;
if (tail >= sizeof(txbuffer)) tail -= sizeof(txbuffer);
txtail = tail;
//println("new tail=", tail);
txready = true;
transmit();
//txtimer.start(8000);
// adjust tail...
// start timer if more data to send
}
size_t AntPlus::write(const void *data, const size_t size)
{
//print("write ", size);
//print(" bytes: ");
//print_hexbytes(data, size);
if (size > 64) return 0;
uint32_t head = txhead;
if (++head >= sizeof(txbuffer)) head = 0;
uint32_t remain = sizeof(txbuffer) - head;
if (remain < size + 1) {
// not enough space at end of buffer
txbuffer[head] = 0xFF;
head = 0;
}
uint32_t avail;
do {
uint32_t tail = txtail;
if (head > tail) {
avail = sizeof(txbuffer) - head + tail;
} else {
avail = tail - head;
}
} while (avail < size + 1); // wait for space in buffer
txbuffer[head] = size;
memcpy(txbuffer + head + 1, data, size);
txhead = head + size;
//print("head=", txhead);
//println(", tail=", txtail);
//print_hexbytes(txbuffer, 60);
NVIC_DISABLE_IRQ(IRQ_USBHS);
transmit();
NVIC_ENABLE_IRQ(IRQ_USBHS);
return size;
}
void AntPlus::transmit()
{
if (!txready) return;
uint32_t head = txhead;
uint32_t tail = txtail;
if (head == tail) {
//println("no data to transmit");
return; // no data to transit
}
if (++tail >= sizeof(txbuffer)) tail = 0;
uint32_t size = txbuffer[tail];
//print("tail=", tail);
//println(", tx size=", size);
if (size == 0xFF) {
txtail = 0;
tail = 0;
size = txbuffer[0];
//println("tx size=", size);
}
//txtail = tail + size;
queue_Data_Transfer(txpipe, txbuffer + tail + 1, size, this);
//txtimer.start(8000);
txready = false;
}
void AntPlus::timer_event(USBDriverTimer *whichTimer)
{
if (whichTimer == &updatetimer) {
updatetimer.start(250000);
if (first_update) {
ResetSystem();
first_update = false;
} else {
do_polling = true;
}
//println("ant update timer");
}
/* else if (whichTimer == &txtimer) {
println("ant tx timer");
//txtimer.stop(); // TODO: why is this needed?
txready = true;
transmit();
} */
}
void AntPlus::Task()
{
uint32_t len = rxlen;
if (len) {
handleMessages(rxpacket, len);
NVIC_DISABLE_IRQ(IRQ_USBHS);
queue_Data_Transfer(rxpipe, rxpacket, 64, this);
rxlen = 0;
NVIC_ENABLE_IRQ(IRQ_USBHS);
}
if (do_polling) {
do_polling = false;
for (int i = 0; i < PROFILE_TOTAL; i++) {
TDCONFIG *cfg = &ant.dcfg[i];
if (!(cfg->flags.profileValid)) continue;
//printf("#### %i %i: %i %i %i ####", i, cfg->channel,
// cfg->flags.channelStatus, cfg->flags.keyAccepted,
// cfg->flags.chanIdOnce);
if (cfg->flags.channelStatus) {
RequestMessage(cfg->channel, MESG_CHANNEL_STATUS_ID);
} else {
AssignChannel(cfg->channel, cfg->channelType, cfg->networkNumber);
RequestMessage(cfg->channel, MESG_CHANNEL_STATUS_ID);
if (!cfg->flags.keyAccepted && !cfg->flags.chanIdOnce) {
SetNetworkKey(cfg->networkNumber, getAntKey(ant.key));
}
}
}
}
}
enum _akeys {
KEY_ANTSPORT,
KEY_SUUNTO,
KEY_GARMIN,
KEY_ANTPLUS,
KEY_TOTAL,
KEY_DEFAULT = KEY_ANTSPORT
};
static const uint8_t antkeys[KEY_TOTAL][8] = {
{0xB9,0xA5,0x21,0xFB,0xBD,0x72,0xC3,0x45}, // Ant+ sport key KEY_ANTSPORT
{0xB9,0xAD,0x32,0x28,0x75,0x7E,0xC7,0x4D}, // Suunto KEY_SUUNTO
{0xA8,0xA4,0x23,0xB9,0xF5,0x5E,0x63,0xC1}, // Garmin KEY_GARMIN
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00} // Ant+ key KEY_ANTPLUS (add your key here)
//{0xFD,0x38,0xBE,0xA6,0x40,0x5D,0x26,0x99}
};
uint8_t AntPlus::calcMsgChecksum(const uint8_t *buffer, const uint8_t len)
{
uint8_t checksum = 0x00;
for (uint8_t i = 0; i < len; i++)
checksum ^= buffer[i];
return checksum;
}
uint8_t * AntPlus::findStreamSync(uint8_t *stream, const size_t rlen, int *pos)
{
// find and sync with input stream
*pos = 0;
while (*pos < (int)rlen /*&& *pos < INPUTBUFFERSIZE-3*/){
if (stream[*pos] == MESG_TX_SYNC)
return stream + *pos;
(*pos)++;
}
return NULL;
}
int AntPlus::msgCheckIntegrity(uint8_t *stream, const int len)
{
// min message length is 5
if (len < 5) return 0;
int crc = stream[STREAM_SYNC];
crc ^= stream[STREAM_LENGTH];
crc ^= stream[STREAM_MESSAGE];
int mlen = 0;
do{
crc ^= stream[STREAM_DATA+mlen];
} while (++mlen < stream[STREAM_LENGTH]);
//printf("crc == 0x%X: msg crc = 0x%X\n", crc, stream[stream[STREAM_LENGTH] + 3]);
return (crc == stream[stream[STREAM_LENGTH] + 3]);
}
int AntPlus::msgGetLength (uint8_t *stream)
{
// eg; {A4 1 6F 20 EA} = {SYNC DATALEN MSGID DATA CRC}
return stream[STREAM_LENGTH] + 4;
}
int AntPlus::handleMessages(uint8_t *buffer, int tBytes)
{
int syncOffset = 0;
//uint8_t buffer[ANTPLUS_MAXPACKETSIZE];
uint8_t *stream = buffer;
//int tBytes = antplus_read(ant, buffer, ant->ioVar.readBufferSize);
//if (tBytes <= 0) return tBytes;
//int tBytes = ANTPLUS_MAXPACKETSIZE;
while (tBytes > 0){
stream = findStreamSync(stream, tBytes, &syncOffset);
if (stream == NULL){
//printf("stream sync not found {size:%i}\n", tBytes);
return 0;
}
tBytes -= syncOffset;
if (!msgCheckIntegrity(stream, tBytes)){
//printf("stream integrity failed {size:%i}\n", tBytes);
return 0;
}
//we have a valid message
//if (dispatchMessage(stream, tBytes) == -1){
//printf("quiting..\n");
//return 0;
//}
message_event(stream[STREAM_CHANNEL], stream[STREAM_MESSAGE],
&stream[STREAM_DATA], (size_t)stream[STREAM_LENGTH]);
int len = msgGetLength(stream);
stream += len;
tBytes -= len;
}
return 1;
}
void AntPlus::sendMessageChannelStatus(TDCONFIG *cfg, const uint32_t channelStatus)
{
cfg->flags.channelStatus = channelStatus;
if (cfg->flags.channelStatus != cfg->flags.channelStatusOld) {
if (user_onStatusChange) {
(*user_onStatusChange)(cfg->channel, cfg->flags.channelStatus);
}
cfg->flags.channelStatusOld = cfg->flags.channelStatus;
}
}
void AntPlus::message_channel(const int chan, const int eventId,
const uint8_t *payload, const size_t dataLength)
{
//printf(" $ chan event: chan:%i, msgId:0x%.2X, payload:%p, dataLen:%i, uPtr:%p", chan, eventId, payload, (int)dataLength, uPtr);
//dump_hexbytes(payload, dataLength);
TDCONFIG *cfg = &(ant.dcfg[chan]);
switch (eventId){
case EVENT_RX_SEARCH_TIMEOUT:
printf(" $ event RX search timeout");
break;
case EVENT_RX_FAIL:
//printf(" $ event RX fail");
break;
case EVENT_TX:
//printf(" $ event TX");
break;
case EVENT_RX_BROADCAST:
//printf(" $ event RX broadcast ");
if (!cfg->flags.chanIdOnce) {
cfg->flags.chanIdOnce = 1;
RequestMessage(cfg->channel, MESG_CHANNEL_ID_ID);
}
//dump_hexbytes(payload, dataLength);
dispatchPayload(cfg, payload, dataLength);
break;
}
}
void AntPlus::message_response(const int chan, const int msgId,
const uint8_t *payload, const size_t dataLength)
{
//printf(" # response event: msgId:0x%.2X, payload:%p, dataLen:%i, uPtr:%p", msgId, payload, dataLength, uPtr);
TDCONFIG *cfg = (TDCONFIG*)&(ant.dcfg[chan]);
switch (msgId){
case MESG_EVENT_ID:
//printf(" * event");
message_channel(chan, payload[STREAM_EVENT_EVENTID], payload, dataLength);
break;
case MESG_NETWORK_KEY_ID:
printf("[%i] * network key accepted", chan);
cfg->flags.keyAccepted = 1;
if (cfg->transType == ANT_TRANSMISSION_MASTER)
AssignChannel(cfg->channel, PARAMETER_TX_NOT_RX, cfg->networkNumber);
else
AssignChannel(cfg->channel, cfg->channelType, cfg->networkNumber);
break;
case MESG_ASSIGN_CHANNEL_ID:
printf("[%i] * channel assign accepted", chan);
SetChannelPeriod(cfg->channel, cfg->channelPeriod);
break;
case MESG_CHANNEL_MESG_PERIOD_ID:
printf("[%i] * channel mesg period accepted", chan);
SetChannelSearchTimeout(cfg->channel, cfg->searchTimeout);
break;
case MESG_CHANNEL_SEARCH_TIMEOUT_ID:
printf("[%i] * search timeout period accepted", chan);
SetChannelRFFreq(cfg->channel, cfg->RFFreq);
break;
case MESG_CHANNEL_RADIO_FREQ_ID:
printf("[%i] * radio freq accepted", chan);
SetSearchWaveform(cfg->channel, cfg->searchWaveform);
break;
case MESG_SEARCH_WAVEFORM_ID:
printf("[%i] * search waveform accepted", chan);
SetChannelId(cfg->channel, cfg->deviceNumber, cfg->deviceType, cfg->transType);
break;
case MESG_CHANNEL_ID_ID:
printf("[%i] * set channel id accepted", chan);
OpenChannel(cfg->channel);
break;
case MESG_OPEN_CHANNEL_ID:
printf("[%i] * open channel accepted", chan);
//cfg->flags.channelStatus = 1;
RequestMessage(cfg->channel, MESG_CHANNEL_STATUS_ID);
RequestMessage(cfg->channel, MESG_CAPABILITIES_ID);
RequestMessage(cfg->channel, MESG_VERSION_ID);
break;
case MESG_UNASSIGN_CHANNEL_ID:
printf("[%i] * channel Unassigned", chan);
break;
case MESG_CLOSE_CHANNEL_ID:
printf("[%i] * channel CLOSED", chan);
cfg->flags.keyAccepted = 0;
sendMessageChannelStatus(cfg, ANT_CHANNEL_STATUS_UNASSIGNED);
break;
case CHANNEL_IN_WRONG_STATE:
printf("[%i] * channel in wrong state", chan);
break;
case CHANNEL_NOT_OPENED:
printf("[%i] * channel not opened", chan);
break;
case CHANNEL_ID_NOT_SET: //??
printf("[%i] * channel ID not set", chan);
break;
case CLOSE_ALL_CHANNELS: // Start RX Scan mode
printf("[%i] * close all channels", chan);
break;
case TRANSFER_IN_PROGRESS: // TO ack message ID
printf("[%i] * tranfer in progress", chan);
break;
case TRANSFER_SEQUENCE_NUMBER_ERROR:
printf("[%i] * transfer sequence number error", chan);
break;
case TRANSFER_IN_ERROR:
printf("[%i] * transfer in error", chan);
break;
case INVALID_MESSAGE:
printf("[%i] * invalid message", chan);
break;
case INVALID_NETWORK_NUMBER:
printf("[%i] * invalid network number", chan);
break;
case INVALID_LIST_ID:
printf("[%i] * invalid list ID", chan);
break;
case INVALID_SCAN_TX_CHANNEL:
printf("[%i] * invalid Scanning transmit channel", chan);
break;
case INVALID_PARAMETER_PROVIDED:
printf("[%i] * invalid parameter provided", chan);
break;
case EVENT_QUE_OVERFLOW:
printf("[%i] * queue overflow", chan);
break;
default:
printf("[i] #### unhandled response id %i", chan, msgId);
;
};
}
void AntPlus::message_event(const int channel, const int msgId,
const uint8_t *payload, const size_t dataLength)
{
//printf(" @ msg event cb: Chan:%i, Id:0x%.2X, payload:%p, len:%i, ptr:%p", channel, msgId, payload, (int)dataLength, uPtr);
//dump_hexbytes(payload, dataLength);
uint8_t chan = 0;
if (channel >= 0 && channel < PROFILE_TOTAL) chan = channel;
switch(msgId) {
case MESG_BROADCAST_DATA_ID:
//printf(" @ broadcast data \n");
//dumpPayload(payload, dataLength);
message_channel(chan, EVENT_RX_BROADCAST, payload, dataLength);
break;
case MESG_STARTUP_MESG_ID:
// reason == ANT_STARTUP_RESET_xxxx
//printf(" @ start up mesg reason: 0x%X", payload[STREAM_STARTUP_REASON]);
//TDCONFIG *cfg = &(ant.dcfg[0]);
//SetNetworkKey(cfg->networkNumber, getAntKey(cfg->keyIdx));
SetNetworkKey(ant.dcfg[0].networkNumber, getAntKey(ant.key));
break;
case MESG_RESPONSE_EVENT_ID:
message_response(payload[STREAM_EVENT_CHANNEL_ID],
payload[STREAM_EVENT_RESPONSE_ID], payload, dataLength);
break;
case MESG_CHANNEL_STATUS_ID:
//printf(" @ channel status for channel %i is %i",
// payload[STREAM_CHANNEL_ID], payload[STREAM_CHANNEL_STATUS]);
//TDCONFIG *cfg = (TDCONFIG*)&ant->dcfg[payload[STREAM_CHANNEL_ID]];
sendMessageChannelStatus(&(ant.dcfg[payload[STREAM_CHANNEL_ID]]),
payload[STREAM_CHANNELSTATUS_STATUS] & ANT_CHANNEL_STATUS_MASK);
//if (cfg->flags.channelStatus != STATUS_TRACKING_CHANNEL)
// printf("channel %i status: %s", payload[STREAM_CHANNEL_ID],
// channelStatusStr[cfg->flags.channelStatus]);
break;
case MESG_CAPABILITIES_ID:
printf(" @ capabilities:");
printf(" Max ANT Channels: %i",payload[STREAM_CAP_MAXCHANNELS]);
printf(" Max ANT Networks: %i",payload[STREAM_CAP_MAXNETWORKS]);
printf(" Std. option: 0x%X",payload[STREAM_CAP_STDOPTIONS]);
printf(" Advanced: 0x%X",payload[STREAM_CAP_ADVANCED]);
printf(" Advanced2: 0x%X",payload[STREAM_CAP_ADVANCED2]);
break;
case MESG_CHANNEL_ID_ID:
//TDCONFIG *cfg = (TDCONFIG*)&ant->dcfg[chan];
//ant.dcfg[chan].dev.deviceId = payload[STREAM_CHANNELID_DEVNO_LO] | (payload[STREAM_CHANNELID_DEVNO_HI] << 8);
//ant.dcfg[chan].dev.deviceType = payload[STREAM_CHANNELID_DEVTYPE];
//ant.dcfg[chan].dev.transType = payload[STREAM_CHANNELID_TRANTYPE];
//printf(" @ CHANNEL ID: channel %i, deviceId:%i, deviceType:%i, transType:%i)", chan, cfg->dev.deviceId, cfg->dev.deviceType, cfg->dev.transType);
if (user_onDeviceID) {
int devid = payload[STREAM_CHANNELID_DEVNO_LO];
devid |= payload[STREAM_CHANNELID_DEVNO_HI] << 8;
int devtype = payload[STREAM_CHANNELID_DEVTYPE];
int transtype = payload[STREAM_CHANNELID_TRANTYPE];
(*user_onDeviceID)(chan, devid, devtype, transtype);
}
#if 0
if (cfg->dev.scidDeviceType != cfg->deviceType){
printf(" @ CHANNEL ID: this is not the device we're looking for");
printf(" @ CHANNEL ID: expecting 0x%X but found 0x%X", cfg->deviceType, cfg->dev.scidDeviceType);
//CloseChannel(cfg->channel);
return;
}
#endif
break;
case MESG_VERSION_ID:
printf(" @ version: '%s'", (char*)&payload[STREAM_VERSION_STRING]);
break;
};
}
int AntPlus::ResetSystem()
{
//println("libantplus_ResetSystem");
uint8_t msg[5];
msg[0] = MESG_TX_SYNC; // sync
msg[1] = 1; // length
msg[2] = MESG_SYSTEM_RESET_ID; // msg id
msg[3] = 0; // nop
msg[4] = calcMsgChecksum(msg, 4);
return write(msg, 5);
}
int AntPlus::RequestMessage(const int channel, const int message)
{
//println("libantplus_RequestMessage");
uint8_t msg[6];
msg[0] = MESG_TX_SYNC; // sync
msg[1] = 2; // length
msg[2] = MESG_REQUEST_ID; // msg id
msg[3] = (uint8_t)channel;
msg[4] = (uint8_t)message;
msg[5] = calcMsgChecksum(msg, 5);
return write(msg, 6);
}
int AntPlus::SetNetworkKey(const int netNumber, const uint8_t *key)
{
uint8_t msg[13];
msg[0] = MESG_TX_SYNC;
msg[1] = 9;
msg[2] = MESG_NETWORK_KEY_ID;
msg[3] = (uint8_t)netNumber;
msg[4] = key[0];
msg[5] = key[1];
msg[6] = key[2];
msg[7] = key[3];
msg[8] = key[4];
msg[9] = key[5];
msg[10] = key[6];
msg[11] = key[7];
msg[12] = calcMsgChecksum(msg, 12); // xor checksum
return write(msg, 13);
}
int AntPlus::SetChannelSearchTimeout(const int channel, const int searchTimeout)
{
uint8_t msg[6];
msg[0] = MESG_TX_SYNC; // sync
msg[1] = 2; // length
msg[2] = MESG_CHANNEL_SEARCH_TIMEOUT_ID;
msg[3] = (uint8_t)channel;
msg[4] = (uint8_t)searchTimeout;
msg[5] = calcMsgChecksum(msg, 5);
return write(msg, 6);
}
int AntPlus::SetChannelPeriod(const int channel, const int period)
{
uint8_t msg[7];
msg[0] = MESG_TX_SYNC; // sync
msg[1] = 3; // length
msg[2] = MESG_CHANNEL_MESG_PERIOD_ID;
msg[3] = (uint8_t)channel;
msg[4] = (uint8_t)(period & 0xFF);
msg[5] = (uint8_t)(period >> 8);
msg[6] = calcMsgChecksum(msg, 6);
return write(msg, 7);
}
int AntPlus::SetChannelRFFreq(const int channel, const int freq)
{
uint8_t msg[6];
msg[0] = MESG_TX_SYNC; // sync
msg[1] = 2; // length
msg[2] = MESG_CHANNEL_RADIO_FREQ_ID;
msg[3] = (uint8_t)channel;
msg[4] = (uint8_t)freq;
msg[5] = calcMsgChecksum(msg, 5);
return write(msg, 6);
}
int AntPlus::SetSearchWaveform(const int channel, const int wave)
{
uint8_t msg[7];
msg[0] = MESG_TX_SYNC; // sync
msg[1] = 3; // length
msg[2] = MESG_SEARCH_WAVEFORM_ID; // msg id
msg[3] = (uint8_t)channel;
msg[4] = (uint8_t)wave & 0xFF;
msg[5] = (uint8_t)wave >> 8;
msg[6] = calcMsgChecksum(msg, 6);
return write(msg, 7);
}
int AntPlus::OpenChannel(const int channel)
{
uint8_t msg[5];
msg[0] = MESG_TX_SYNC; // sync
msg[1] = 1; // length
msg[2] = MESG_OPEN_CHANNEL_ID; // msg id
msg[3] = (uint8_t)channel;
msg[4] = calcMsgChecksum(msg, 4);
return write(msg, 5);
}
int AntPlus::CloseChannel(const int channel)
{
uint8_t msg[5];
msg[0] = MESG_TX_SYNC; // sync
msg[1] = 1; // length
msg[2] = MESG_CLOSE_CHANNEL_ID; // msg id
msg[3] = (uint8_t)channel;
msg[4] = calcMsgChecksum(msg, 4);
return write(msg, 5);
}
int AntPlus::AssignChannel(const int channel, const int channelType, const int network)
{
uint8_t msg[7];
msg[0] = MESG_TX_SYNC;
msg[1] = 3;
msg[2] = MESG_ASSIGN_CHANNEL_ID;
msg[3] = (uint8_t)channel;
msg[4] = (uint8_t)channelType;
msg[5] = (uint8_t)network;
msg[6] = calcMsgChecksum(msg, 6);
return write(msg, 7);
}
int AntPlus::SetChannelId(const int channel, const int deviceNum, const int deviceType, const int transmissionType)
{
uint8_t msg[9];
msg[0] = MESG_TX_SYNC;
msg[1] = 5;
msg[2] = MESG_CHANNEL_ID_ID;
msg[3] = (uint8_t)channel;
msg[4] = (uint8_t)(deviceNum & 0xFF);
msg[5] = (uint8_t)(deviceNum >> 8);
msg[6] = (uint8_t)deviceType;
msg[7] = (uint8_t)transmissionType;
msg[8] = calcMsgChecksum(msg, 8);
return write(msg, 9);
}
int AntPlus::SendBurstTransferPacket(const int channelSeq, const uint8_t *data)
{
uint8_t msg[13];
msg[0] = MESG_TX_SYNC;
msg[1] = 9;
msg[2] = MESG_BURST_DATA_ID;
msg[3] = (uint8_t)channelSeq;
msg[4] = data[0];
msg[5] = data[1];
msg[6] = data[2];
msg[7] = data[3];
msg[8] = data[4];
msg[9] = data[5];
msg[10] = data[6];
msg[11] = data[7];
msg[12] = calcMsgChecksum(msg, 12); // xor checksum
return write(msg, 13);
}
int AntPlus::SendBurstTransfer(const int channel, const uint8_t *data, const int nunPackets)
{
int ret = 0;
int seq = 0;
for (int i = 0; i < nunPackets; i++) {
if (i == nunPackets-1) seq |= 0x04;
ret = SendBurstTransferPacket((seq<<5) | (channel&0x1F), data+(i<<3));
seq = (seq+1)&0x03;
}
return ret;
}
int AntPlus::SendBroadcastData(const int channel, const uint8_t *data)
{
uint8_t msg[13];
msg[0] = MESG_TX_SYNC;
msg[1] = 9;
msg[2] = MESG_BROADCAST_DATA_ID;
msg[3] = (uint8_t)channel;
msg[4] = data[0];
msg[5] = data[1];
msg[6] = data[2];
msg[7] = data[3];
msg[8] = data[4];
msg[9] = data[5];
msg[10] = data[6];
msg[11] = data[7];
msg[12] = calcMsgChecksum(msg, 12);
return write(msg, 13);
}
int AntPlus::SendAcknowledgedData(const int channel, const uint8_t *data)
{
uint8_t msg[13];
msg[0] = MESG_TX_SYNC;
msg[1] = 9;
msg[2] = MESG_ACKNOWLEDGED_DATA_ID;
msg[3] = (uint8_t)channel;
msg[4] = data[0];
msg[5] = data[1];
msg[6] = data[2];
msg[7] = data[3];
msg[8] = data[4];
msg[9] = data[5];
msg[10] = data[6];
msg[11] = data[7];
msg[12] = calcMsgChecksum(msg, 12);
return write(msg, 13);
}
int AntPlus::SendExtAcknowledgedData(const int channel, const int devNum, const int devType, const int TranType, const uint8_t *data)
{
uint8_t msg[17];
msg[0] = MESG_TX_SYNC;
msg[1] = 13;
msg[2] = MESG_EXT_ACKNOWLEDGED_DATA_ID;
msg[3] = (uint8_t)channel;
msg[4] = (uint8_t)(devNum & 0xFF);
msg[5] = (uint8_t)(devNum >> 8);
msg[6] = (uint8_t)devType;
msg[7] = (uint8_t)TranType;
msg[8] = data[0];
msg[9] = data[1];
msg[10] = data[2];
msg[11] = data[3];
msg[12] = data[4];
msg[13] = data[5];
msg[14] = data[6];
msg[15] = data[7];
msg[16] = calcMsgChecksum(msg, 16);
return write(msg, 17);
}
int AntPlus::SendExtBroadcastData(const int channel, const int devNum, const int devType, const int TranType, const uint8_t *data)
{
uint8_t msg[17];
msg[0] = MESG_TX_SYNC;
msg[1] = 13;
msg[2] = MESG_EXT_BROADCAST_DATA_ID;
msg[3] = (uint8_t)channel;
msg[4] = (uint8_t)(devNum & 0xFF);
msg[5] = (uint8_t)(devNum >> 8);
msg[6] = (uint8_t)devType;
msg[7] = (uint8_t)TranType;
msg[8] = data[0];
msg[9] = data[1];
msg[10] = data[2];
msg[11] = data[3];
msg[12] = data[4];
msg[13] = data[5];
msg[14] = data[6];
msg[15] = data[7];
msg[16] = calcMsgChecksum(msg, 16);
return write(msg, 17);
}
int AntPlus::SendExtBurstTransferPacket(const int chanSeq, const int devNum, const int devType,
const int TranType, const uint8_t *data)
{
uint8_t msg[17];
msg[0] = MESG_TX_SYNC;
msg[1] = 13;
msg[2] = MESG_EXT_BROADCAST_DATA_ID;
msg[3] = (uint8_t)chanSeq;
msg[4] = (uint8_t)(devNum & 0xFF);
msg[5] = (uint8_t)(devNum >> 8);
msg[6] = (uint8_t)devType;
msg[7] = (uint8_t)TranType;
msg[8] = data[0];
msg[9] = data[1];
msg[10] = data[2];
msg[11] = data[3];
msg[12] = data[4];
msg[13] = data[5];
msg[14] = data[6];
msg[15] = data[7];
msg[16] = calcMsgChecksum(msg, 16);
return write(msg, 17);
}
int AntPlus::SendExtBurstTransfer(const int channel, const int devNum, const int devType,
const int tranType, const uint8_t *data, const int nunPackets)
{
int ret = 0;
int seq = 0;
for (int i = 0; i < nunPackets; i++){
if (i == nunPackets-1) seq |= 0x04;
ret = SendExtBurstTransferPacket((seq<<5) | (channel&0x1F),
devNum, devType, tranType, data+(i<<3));
seq = (seq+1)&0x03;
}
return ret;
}
void AntPlus::profileSetup_HRM(TDCONFIG *cfg, const uint32_t deviceId)
{
cfg->deviceNumber = deviceId; // 0
cfg->deviceType = ANT_DEVICE_HRM;
cfg->transType = ANT_TRANSMISSION_SLAVE;
cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;
cfg->networkNumber = 0;
cfg->channel = PROFILE_HRM;
cfg->channelPeriod = ANT_PERIOD_HRM;
cfg->RFFreq = ANT_FREQUENCY_SPORT;
cfg->searchTimeout = 255;
cfg->searchWaveform = 0x53;//316;//0x53;
//cfg->keyIdx = KEY_ANTSPORT;
cfg->flags.chanIdOnce = 0;
cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;
cfg->flags.channelStatusOld = 0xFF;
cfg->flags.keyAccepted = 0;
cfg->flags.profileValid = 1;
}
void AntPlus::profileSetup_SPDCAD(TDCONFIG *cfg, const uint32_t deviceId)
{
cfg->deviceNumber = deviceId; // 0
cfg->deviceType = ANT_DEVICE_SPDCAD;
cfg->transType = ANT_TRANSMISSION_SLAVE; // 5
cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;
cfg->networkNumber = 0;
cfg->channel = PROFILE_SPDCAD;
cfg->channelPeriod = ANT_PERIOD_SPDCAD;
cfg->RFFreq = ANT_FREQUENCY_SPORT;
cfg->searchTimeout = 255;
cfg->searchWaveform = 0x53;
//cfg->keyIdx = KEY_ANTSPORT;
cfg->flags.chanIdOnce = 0;
cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;
cfg->flags.channelStatusOld = 0xFF;
cfg->flags.keyAccepted = 0;
cfg->flags.profileValid = 1;
}
void AntPlus::profileSetup_POWER(TDCONFIG *cfg, const uint32_t deviceId)
{
cfg->deviceNumber = deviceId; // 0
cfg->deviceType = ANT_DEVICE_POWER;
cfg->transType = ANT_TRANSMISSION_SLAVE; // 5
cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;
cfg->networkNumber = 0;
cfg->channel = PROFILE_POWER;
cfg->channelPeriod = ANT_PERIOD_POWER;
cfg->RFFreq = ANT_FREQUENCY_SPORT;
cfg->searchTimeout = 255;
cfg->searchWaveform = 0x53;
//cfg->keyIdx = KEY_ANTSPORT;
cfg->flags.chanIdOnce = 0;
cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;
cfg->flags.channelStatusOld = 0xFF;
cfg->flags.keyAccepted = 0;
cfg->flags.profileValid = 1;
}
void AntPlus::profileSetup_STRIDE(TDCONFIG *cfg, const uint32_t deviceId)
{
cfg->deviceNumber = deviceId; // 0
cfg->deviceType = ANT_DEVICE_STRIDE;
cfg->transType = ANT_TRANSMISSION_SLAVE; // 5
cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;
cfg->networkNumber = 0;
cfg->channel = PROFILE_STRIDE;
cfg->channelPeriod = ANT_PERIOD_STRIDE;
cfg->RFFreq = ANT_FREQUENCY_STRIDE;
cfg->searchTimeout = 255;
cfg->searchWaveform = 0x53;
//cfg->keyIdx = KEY_ANTSPORT;
cfg->flags.chanIdOnce = 0;
cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;
cfg->flags.channelStatusOld = 0xFF;
cfg->flags.keyAccepted = 0;
cfg->flags.profileValid = 1;
}
void AntPlus::profileSetup_SPEED(TDCONFIG *cfg, const uint32_t deviceId)
{
cfg->deviceNumber = deviceId; // 0
cfg->deviceType = ANT_DEVICE_SPEED;
cfg->transType = ANT_TRANSMISSION_SLAVE; // 5
cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;
cfg->networkNumber = 0;
cfg->channel = PROFILE_SPEED;
cfg->channelPeriod = ANT_PERIOD_SPEED;
cfg->RFFreq = ANT_FREQUENCY_SPORT;
cfg->searchTimeout = 255;
cfg->searchWaveform = 0x53;
//cfg->keyIdx = KEY_ANTSPORT;
cfg->flags.chanIdOnce = 0;
cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;
cfg->flags.channelStatusOld = 0xFF;
cfg->flags.keyAccepted = 0;
cfg->flags.profileValid = 1;
}
void AntPlus::profileSetup_CADENCE(TDCONFIG *cfg, const uint32_t deviceId)
{
cfg->deviceNumber = deviceId; // 0
cfg->deviceType = ANT_DEVICE_CADENCE;
cfg->transType = ANT_TRANSMISSION_SLAVE; // 5
cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;
cfg->networkNumber = 0;
cfg->channel = PROFILE_CADENCE;
cfg->channelPeriod = ANT_PERIOD_CADENCE;
cfg->RFFreq = ANT_FREQUENCY_SPORT;
cfg->searchTimeout = 255;
cfg->searchWaveform = 0x53;
//cfg->keyIdx = KEY_ANTSPORT;
cfg->flags.chanIdOnce = 0;
cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;
cfg->flags.channelStatusOld = 0xFF;
cfg->flags.keyAccepted = 0;
cfg->flags.profileValid = 1;
}
/*
uint64_t factory_passkey (uint64_t device_id, uint8_t *buffer)
{
uint64_t n = (((uint64_t)device_id ^ 0x7d215abb) << 32) + ((uint64_t)device_id ^ 0x42b93f06);
for (uint8_t i = 0; i < 8; i++)
buffer[i] = n >> (8*i)&0xFF;
return n;
}
*/
void AntPlus::begin(const uint8_t key)
{
ant.key = (key < KEY_TOTAL) ? key : 0;
}
void AntPlus::dispatchPayload(TDCONFIG *cfg, const uint8_t *payload, const int len)
{
switch (cfg->channel) {
case PROFILE_HRM:
payload_HRM(cfg, payload, len);
break;
case PROFILE_SPDCAD:
payload_SPDCAD(cfg, payload, len);
break;
case PROFILE_POWER:
payload_POWER(cfg, payload, len);
break;
case PROFILE_STRIDE:
payload_STRIDE(cfg, payload, len);
break;
case PROFILE_SPEED:
payload_SPEED(cfg, payload, len);
break;
case PROFILE_CADENCE:
payload_CADENCE(cfg, payload, len);
break;
}
}
const uint8_t * AntPlus::getAntKey(const uint8_t keyIdx)
{
if (keyIdx >= KEY_TOTAL) return NULL;
return antkeys[keyIdx];
}
void AntPlus::payload_HRM(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)
{
uint8_t bpm = data[STREAM_RXBROADCAST_DEV120_HR];
uint8_t sequence = data[STREAM_RXBROADCAST_DEV120_SEQ];
if ((sequence == hrm.previous.sequence && bpm == hrm.previous.bpm) || bpm == 0) {
return;
}
uint16_t time = (data[STREAM_RXBROADCAST_DEV120_BEATLO]
+ (data[STREAM_RXBROADCAST_DEV120_BEATHI] << 8));
int interval = (uint16_t)(time - hrm.previous.time) * (uint32_t)1000 / (uint32_t)1024;
if (user_onHeartRateMonitor) {
(*user_onHeartRateMonitor)(bpm, interval, sequence);
}
hrm.previous.time = time;
hrm.previous.sequence = sequence;
hrm.previous.bpm = bpm;
//printf("payload_HRM: page:%i, Sequence:%i, BPM:%i, %i %i",
// data[1]&0x0F, sequence, bpm, time, interval);
}
void AntPlus::payload_SPDCAD(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)
{
uint16_t cadenceTime = data[1] | (data[2] << 8);
uint16_t cadenceCt = data[3] | (data[4] << 8);
uint16_t speedTime = data[5] | (data[6] << 8);
uint16_t speedCt = data[7] | (data[8] << 8);
if (cadenceTime == spdcad.previous.cadenceTime
&& cadenceCt == spdcad.previous.cadenceCt
&& speedTime == spdcad.previous.speedTime
&& speedCt == spdcad.previous.speedCt) {
return; // no change
}
uint16_t cadence = (60 * (cadenceCt - spdcad.previous.cadenceCt) * 1024) / (cadenceTime - spdcad.previous.cadenceTime);
// number wheel revolutions
uint32_t speedRotationDelta = speedCt - spdcad.previous.speedCt;
// time for above revolutions
float speedTimeDelta = (float)(speedTime - spdcad.previous.speedTime) / 1024.0f;
// calculated distance (meters) travelled as per above
float distance = (speedRotationDelta * (float)wheelCircumference) / 1000.0f;
spdcad.distance += distance;
float speed = (distance / (speedTimeDelta / 3600.0f)) / 1000.0f;
if (user_onSpeedCadence) {
(*user_onSpeedCadence)(speed, spdcad.distance * 0.001f, cadence);
}
spdcad.previous.cadenceTime = cadenceTime;
spdcad.previous.cadenceCt = cadenceCt;
spdcad.previous.speedTime = speedTime;
spdcad.previous.speedCt = speedCt;
//printf("payload_SPDCAD: speed: %.2f, cadence: %i, total distance: %.2f",
// speed, cadence, spdcad.distance);
}
void AntPlus::payload_POWER(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)
{
//printf("payload_POWER: len:%i", dataLength);
#if 0
uint8_t eventCount = data[2];
uint8_t pedalPowerContribution = ((data[3] != 0xFF) && (data[3]&0x80)) ; // left/right is defined if NOT 0xFF (= no Pedal Power) AND BIT 7 is set
uint8_t pedalPower = (data[3]&0x7F); // right pedalPower % - stored in bit 0-6
uint8_t instantCadence = data[4];
uint16_t sumPower = data[5] + (data[6]<<8);
uint16_t instantPower = data[7] + (data[8]<<8);
#endif
//sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)pwr, PROFILE_POWER);
}
void AntPlus::payload_STRIDE(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)
{
//printf("payload_STRIDE: len:%i", dataLength);
int page = data[1];
if (page == 0) {
//stride.current.strides = data[7];
//sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)&stride, PROFILE_STRIDE);
//stride.previous.strides = stride.current.strides;
} else if (page == 1) {
//stride.current.speed = ((float)(data[4]&0x0f) + (float)(data[5]/256.0f));
//stride.current.cadence = ((float)data[3] + (float)((data[4] << 4) / 16.0f));
//sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)&stride, PROFILE_STRIDE);
//stride.previous.speed = stride.current.speed;
//stride.previous.cadence = stride.current.cadence;
}
}
void AntPlus::payload_SPEED(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)
{
//printf("payload_SPEED: len:%i", dataLength);
uint16_t speedTime = data[5] | (data[6] << 8);
uint16_t speedCt = data[7] | (data[8] << 8);
if (speedTime == spd.previous.speedTime && speedCt == spd.previous.speedCt) {
return; // no change
}
// number wheel revolutions
uint32_t speedRotationDelta = speedCt - spd.previous.speedCt;
// time for above revolutions
float speedTimeDelta = (float)(speedTime - spd.previous.speedTime) / 1024.0f;
// calculated distance (meters) travelled as per above
float distance = (speedRotationDelta * (float)wheelCircumference) / 1000.0f;
spd.distance += distance;
float speed = (distance / (speedTimeDelta / 3600.0f)) / 1000.0f;
if (user_onSpeed) {
(*user_onSpeed)(speed, spd.distance);
}
spd.previous.speedTime = speedTime;
spd.previous.speedCt = speedCt;
}
void AntPlus::payload_CADENCE(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)
{
//printf("payload_CADENCE: len:%i", dataLength);
uint16_t cadenceTime = data[5] | (data[6] << 8);
uint16_t cadenceCt = data[7] | (data[8] << 8);
if (cadenceTime == cad.previous.cadenceTime
&& cadenceCt == cad.previous.cadenceCt) {
return; // no change
}
uint16_t cadence = (60 * (cadenceCt - cad.previous.cadenceCt) * 1024) / (cadenceTime - cad.previous.cadenceTime);
if (user_onCadence) {
(*user_onCadence)(cadence);
}
cad.previous.cadenceTime = cadenceTime;
cad.previous.cadenceCt = cadenceCt;
}
Reference in New Issue View Git Blame Copy Permalink