/* 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 #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; } }