moparisthebest/USBHost_t36
1
0
Fork 0
mirror of https://github.com/gdsports/USBHost_t36 synced 2024-11-14 05:05:09 -05:00
Code Issues Releases Wiki Activity
94b2decbca
USBHost_t36/antplus.cpp
PaulStoffregen 94b2decbca AntPlus polling and example (still incomplete)
2017-10-20 06:45:27 -07:00

1484 lines
42 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);
callbackFunc = NULL;
}
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;
tail += *(p-1) + 1;
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;
}
// TODO: replace this with multiple Arduino style OnXYZ callbacks
void AntPlus::setCallbackFunc(int (*func)(uint32_t msg, intptr_t *value1, uint32_t value2))
{
callbackFunc = func;
}
// TODO: replace this with multiple Arduino style OnXYZ callbacks
void AntPlus::sendMessage(uint32_t msg, intptr_t *value1, uint32_t value2)
{
if (callbackFunc) (*callbackFunc)(msg, value1, value2);
}
void AntPlus::sendMessageChannelStatus(TDCONFIG *cfg, const uint32_t channelStatus)
{
cfg->flags.channelStatus = channelStatus;
if (cfg->flags.channelStatus != cfg->flags.channelStatusOld) {
uint32_t status = cfg->flags.channelStatus&0x0F;
status |= ((cfg->channel&0x0F)<<4);
sendMessage(ANTP_MSG_CHANNELSTATUS, NULL, status);
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);
sendMessage(ANTP_MSG_DEVICEID, (intptr_t *)&(ant.dcfg[chan].dev), chan);
#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;
}
#if 0
/*
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;
}
*/
int libantplus_Start ()
{
#if 0
uint8_t buffer[8];
factory_passkey(3825666043, buffer);
dump_hexbytes(buffer, 8);
#endif
int ct = 0;
uint32_t deviceId;
for (int i = 0; i < PROFILE_TOTAL; i++){
deviceId = 0;
if (antplus_sendMessage(ANTP_MSG_PROFILE_SELECT, (intptr_t*)&deviceId, i) != 1)
continue;
ct++;
//printf("enabling profile %i", i);
switch (i){
case PROFILE_HRM:
profileSetup_HRM(&ant->dcfg[PROFILE_HRM], deviceId);
libantplus_SetPayloadHandler(PROFILE_HRM, (void*)payload_HRM, (void*)NULL);
break;
case PROFILE_SPDCAD:
profileSetup_SPDCAD(&ant->dcfg[PROFILE_SPDCAD], deviceId);
libantplus_SetPayloadHandler(PROFILE_SPDCAD, (void*)payload_SPDCAD, (void*)NULL);
break;
case PROFILE_POWER:
profileSetup_POWER(&ant->dcfg[PROFILE_POWER], deviceId);
libantplus_SetPayloadHandler(PROFILE_POWER, (void*)payload_POWER, (void*)NULL);
break;
case PROFILE_STRIDE:
profileSetup_STRIDE(&ant->dcfg[PROFILE_STRIDE], deviceId);
libantplus_SetPayloadHandler(PROFILE_STRIDE, (void*)payload_STRIDE, (void*)NULL);
break;
case PROFILE_SPEED:
profileSetup_SPEED(&ant->dcfg[PROFILE_SPEED], deviceId);
libantplus_SetPayloadHandler(PROFILE_SPEED, (void*)payload_SPEED, (void*)NULL);
break;
case PROFILE_CADENCE:
profileSetup_CADENCE(&ant->dcfg[PROFILE_CADENCE], deviceId);
libantplus_SetPayloadHandler(PROFILE_CADENCE, (void*)payload_CADENCE, (void*)NULL);
break;
}
}
return ct;
}
TLIBANTPLUS *libantplus_Init (const uint8_t networkKey)
{
if (networkKey >= KEY_TOTAL){
//printf("libantplus_Init(): invalid networkKey (%i)");
//return NULL;
ant->key = KEY_DEFAULT;
}else{
ant->key = networkKey;
}
libantplus_SetEventHandler(EVENTI_MESSAGE, (void*)message_event, (void*)ant);
return ant;
}
#endif
void AntPlus::begin(const uint8_t key)
{
ant.key = (key < KEY_TOTAL) ? key : 0;
int deviceId = 0; // TODO: user API to set this?
profileSetup_HRM(&ant.dcfg[PROFILE_HRM], deviceId);
profileSetup_SPDCAD(&ant.dcfg[PROFILE_SPDCAD], deviceId);
//profileSetup_POWER(&ant.dcfg[PROFILE_POWER], deviceId);
//profileSetup_STRIDE(&ant.dcfg[PROFILE_STRIDE], deviceId);
//profileSetup_SPEED(&ant.dcfg[PROFILE_SPEED], deviceId);
//profileSetup_CADENCE(&ant.dcfg[PROFILE_CADENCE], deviceId);
//ant.eventCb[EVENTI_MESSAGE].cbPtr = &message_event;
//SetEventHandler(EVENTI_MESSAGE, (void*)message_event, (void*)ant);
}
/*
int AntPlus::registerEventCallback(const int which, void *eventFunc, void *userPtr)
{
if (which < EVENTI_TOTAL) {
ant.eventCb[which].cbPtr = (int (*)(int, int, const uint8_t*, size_t, void*))eventFunc;
*/
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];
}
#define WHEEL_CIRCUMFERENCE 2122
typedef struct {
struct {
uint16_t time;
uint16_t interval;
uint8_t bpm; // heart rate in beats per minute
uint8_t sequence;
}current;
struct {
uint8_t bpm;
uint8_t sequence;
uint16_t time;
uint16_t interval;
}previous;
}payload_HRM_t;
typedef struct {
struct {
uint16_t cadenceTime;
uint16_t cadence;
uint16_t cadenceCt;
uint16_t speedTime;
uint16_t speed;
uint16_t speedCt;
uint32_t distance;
}current;
struct {
uint16_t cadenceTime;
uint16_t cadence;
uint16_t cadenceCt;
uint16_t speedTime;
uint16_t speed;
uint16_t speedCt;
uint32_t distance;
}previous;
uint16_t wheelCircumference; // default is WHEEL_CIRCUMFERENCE (2122cm)
uint8_t spdChange;
uint8_t cadChange;
}payload_SPDCAD_t;
typedef struct {
struct {
uint8_t sequence;
uint16_t pedalPowerContribution;
uint8_t pedalPower;
uint8_t instantCadence;
uint16_t sumPower;
uint16_t instantPower;
} current;
struct {
uint16_t stub;
} previous;
}payload_POWER_t;
typedef struct {
struct {
uint16_t speed;
uint16_t cadence;
uint8_t strides;
}current;
struct {
uint8_t strides;
uint16_t speed;
uint16_t cadence;
}previous;
}payload_STRIDE_t;
typedef struct {
struct {
uint16_t speedTime;
uint16_t speed;
uint16_t speedCt;
uint32_t distance;
}current;
struct {
uint16_t speedTime;
uint16_t speed;
uint16_t speedCt;
uint32_t distance;
}previous;
uint16_t wheelCircumference; // default is WHEEL_CIRCUMFERENCE (2122cm)
uint8_t spdChange;
}payload_SPEED_t;
typedef struct {
struct {
uint16_t cadenceTime;
uint16_t cadence;
uint16_t cadenceCt;
}current;
struct {
uint16_t cadenceTime;
uint16_t cadence;
uint16_t cadenceCt;
}previous;
uint8_t cadChange;
}payload_CADENCE_t;
typedef struct {
payload_HRM_t hrm;
payload_SPDCAD_t spdcad;
payload_POWER_t power;
payload_STRIDE_t stride;
payload_SPEED_t spd;
payload_CADENCE_t cad;
}payload_genstorage_t;
static payload_genstorage_t payload; // FIXME: should be in the AntPlus object
void AntPlus::payload_HRM(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)
{
payload_HRM_t *hrm = &payload.hrm;
hrm->current.bpm = data[STREAM_RXBROADCAST_DEV120_HR];
hrm->current.sequence = data[STREAM_RXBROADCAST_DEV120_SEQ];
//const int page = data[1]&0x0F;
if (hrm->previous.sequence != hrm->current.sequence || hrm->previous.bpm != hrm->current.bpm){
if (hrm->current.bpm){
hrm->current.time = (data[STREAM_RXBROADCAST_DEV120_BEATLO] + (data[STREAM_RXBROADCAST_DEV120_BEATHI] << 8));
hrm->current.interval = hrm->current.time - hrm->previous.time;
hrm->current.interval = (((int32_t)hrm->current.interval) * (int32_t)1000) / (int32_t)1024;
//printf("page %i", page);
sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)hrm, PROFILE_HRM);
hrm->previous.time = hrm->current.time;
hrm->previous.interval = hrm->current.interval;
hrm->previous.sequence = hrm->current.sequence;
hrm->previous.bpm = hrm->current.bpm;
}
}
//int page = data[1]&0x0F;
//printf("payload_HRM: page:%i, Sequence:%i, BPM:%i, %i %i", page, hrm->current.sequence, hrm->current.bpm, hrm->current.time, hrm->current.interval);
}
void AntPlus::payload_SPDCAD(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)
{
payload_SPDCAD_t *spdcad = &payload.spdcad;
spdcad->current.cadenceTime = data[1];
spdcad->current.cadenceTime |= (data[2] << 8);
spdcad->current.cadenceCt = data[3];
spdcad->current.cadenceCt |= (data[4] << 8);
spdcad->current.speedTime = data[5];
spdcad->current.speedTime |= (data[6] << 8);
spdcad->current.speedCt = data[7];
spdcad->current.speedCt |= (data[8] << 8);
spdcad->cadChange = (spdcad->current.cadenceTime != spdcad->previous.cadenceTime || spdcad->current.cadenceCt != spdcad->previous.cadenceCt);
spdcad->spdChange = (spdcad->current.speedTime != spdcad->previous.speedTime || spdcad->current.speedCt != spdcad->previous.speedCt);
if (spdcad->cadChange || spdcad->spdChange){
uint16_t cadence = (60 * (spdcad->current.cadenceCt - spdcad->previous.cadenceCt) * 1024) / (spdcad->current.cadenceTime - spdcad->previous.cadenceTime);
spdcad->current.cadence = cadence;
if (!spdcad->wheelCircumference) spdcad->wheelCircumference = WHEEL_CIRCUMFERENCE;
uint32_t speedRotationDelta = spdcad->current.speedCt - spdcad->previous.speedCt; // number wheel revolutions
float speedTimeDelta = (float)(spdcad->current.speedTime - spdcad->previous.speedTime) / 1024.0f; // time for above revolutions
float distance = (speedRotationDelta * (float)spdcad->wheelCircumference) / 1000.0f; // calculated distance (meters) travelled as per above
float speed = (distance / (speedTimeDelta / 3600.0f)) / 1000.0f; // its why we're here
spdcad->current.speed = speed * 100;
spdcad->current.distance += distance;
sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)spdcad, PROFILE_SPDCAD);
spdcad->previous.cadenceTime = spdcad->current.cadenceTime;
spdcad->previous.cadence = spdcad->current.cadence;
spdcad->previous.cadenceCt = spdcad->current.cadenceCt;
spdcad->previous.speedTime = spdcad->current.speedTime;
spdcad->previous.speedCt = spdcad->current.speedCt;
spdcad->previous.speed = spdcad->current.speed;
spdcad->previous.distance = spdcad->current.distance;
//printf("payload_SPDCAD: speed: %.2f, cadence: %i, total distance: %.2f", spdcad->current.speed/100.0f, spdcad->current.cadence, spdcad->current.distance/1000.0f);
}
}
void AntPlus::payload_POWER(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)
{
//printf("payload_POWER: len:%i", dataLength);
payload_POWER_t *pwr = &payload.power;
/*
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);
*/
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);
payload_STRIDE_t *stride = &payload.stride;
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);
payload_SPEED_t *spd = &payload.spd;
spd->current.speedTime = data[5];
spd->current.speedTime |= (data[6] << 8);
spd->current.speedCt = data[7];
spd->current.speedCt |= (data[8] << 8);
spd->spdChange = (spd->current.speedTime != spd->previous.speedTime || spd->current.speedCt != spd->previous.speedCt);
if (spd->spdChange){
uint32_t speedRotationDelta = spd->current.speedCt - spd->previous.speedCt; // number wheel revolutions
float speedTimeDelta = (float)(spd->current.speedTime - spd->previous.speedTime) / 1024.0f; // time for above revolutions
if (!spd->wheelCircumference) spd->wheelCircumference = WHEEL_CIRCUMFERENCE;
float distance = (speedRotationDelta * (float)spd->wheelCircumference) / 1000.0f; // calculated distance (meters) travelled as per above
float speed = (distance / (speedTimeDelta / 3600.0f)) / 1000.0f; // its why we're here
spd->current.speed = speed * 100;
spd->current.distance += distance;
sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)spd, PROFILE_SPEED);
spd->previous.speedTime = spd->current.speedTime;
spd->previous.speedCt = spd->current.speedCt;
spd->previous.speed = spd->current.speed;
spd->previous.distance = spd->current.distance;
}
}
void AntPlus::payload_CADENCE(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)
{
//printf("payload_CADENCE: len:%i", dataLength);
payload_CADENCE_t *cad = &payload.cad;
cad->current.cadenceTime = data[5];
cad->current.cadenceTime |= (data[6] << 8);
cad->current.cadenceCt = data[7];
cad->current.cadenceCt |= (data[8] << 8);
cad->cadChange = (cad->current.cadenceTime != cad->previous.cadenceTime || cad->current.cadenceCt != cad->previous.cadenceCt);
if (cad->cadChange){
uint16_t cadence = (60 * (cad->current.cadenceCt - cad->previous.cadenceCt) * 1024) / (cad->current.cadenceTime - cad->previous.cadenceTime);
cad->current.cadence = cadence;
sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)cad, PROFILE_CADENCE);
cad->previous.cadenceTime = cad->current.cadenceTime;
cad->previous.cadence = cad->current.cadence;
cad->previous.cadenceCt = cad->current.cadenceCt;
}
}
Reference in New Issue View Git Blame Copy Permalink