visus
/
teensy-usbhost
Sledovat 1
Oblíbit 0
Rozštěpit 0
Zdrojový kód Úkoly 0 Požadavky na natažení 0 Vydání 0 Wiki Aktivita
Nelze vybrat více než 25 témat Téma musí začínat písmenem nebo číslem, může obsahovat pomlčky („-“) a může být dlouhé až 35 znaků.
124 Revize
1 Větev
Strom: 75cb233db6
Větve Značky
${ item.name }
Vytvořit větev ${ searchTerm }
z „75cb233db6“
${ noResults }
teensy-usbhost/antplus.cpp

1518 lines
43KB
Surový Blame Historie 

  1. /* USB EHCI Host for Teensy 3.6

  2.  * Copyright 2017 Michael McElligott

  3.  * Copyright 2017 Paul Stoffregen (paul@pjrc.com)

  4.  *

  5.  * Permission is hereby granted, free of charge, to any person obtaining a

  6.  * copy of this software and associated documentation files (the

  7.  * "Software"), to deal in the Software without restriction, including

  8.  * without limitation the rights to use, copy, modify, merge, publish,

  9.  * distribute, sublicense, and/or sell copies of the Software, and to

  10.  * permit persons to whom the Software is furnished to do so, subject to

  11.  * the following conditions:

  12.  *

  13.  * The above copyright notice and this permission notice shall be included

  14.  * in all copies or substantial portions of the Software.

  15.  *

  16.  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS

  17.  * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF

  18.  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.

  19.  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY

  20.  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,

  21.  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE

  22.  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

  23.  */

  24. 

  25. // https://forum.pjrc.com/threads/43110

  26. // https://github.com/PaulStoffregen/antplus

  27. 

  28. #include <Arduino.h>

  29. #include "USBHost_t36.h"  // Read this header first for key info

  30. 

  31. #include "antplusdefs.h"  // Ant internal defines, not for Arduino sketches

  32. 

  33. #define ANTPLUS_VID     0x0FCF

  34. #define ANTPLUS_2_PID   0x1008

  35. #define ANTPLUS_M_PID   0x1009

  36. 

  37. #define print   USBHost::print_

  38. #define println USBHost::println_

  39. 

  40. 

  41. #define ENABLE_SERIALPRINTF   1

  42. 

  43. #if ENABLE_SERIALPRINTF

  44. #undef printf

  45. #define printf(...) Serial.printf(__VA_ARGS__); Serial.write("\r\n")

  46. #else

  47. #undef printf

  48. #define printf(...)    

  49. #endif

  50. 

  51. 

  52. void AntPlus::init()

  53. {

  54. 	contribute_Pipes(mypipes, sizeof(mypipes)/sizeof(Pipe_t));

  55. 	contribute_Transfers(mytransfers, sizeof(mytransfers)/sizeof(Transfer_t));

  56. 	contribute_String_Buffers(mystring_bufs, sizeof(mystring_bufs)/sizeof(strbuf_t));

  57. 	driver_ready_for_device(this);

  58. 	callbackFunc = NULL;

  59. }

  60. 

  61. bool AntPlus::claim(Device_t *dev, int type, const uint8_t *descriptors, uint32_t len)

  62. {

  63. 	if (type != 1) return false;

  64. 	println("AntPlus claim this=", (uint32_t)this, HEX);

  65. 	if (dev->idVendor != ANTPLUS_VID) return false;

  66. 	if (dev->idProduct != ANTPLUS_2_PID && dev->idProduct != ANTPLUS_M_PID) return false;

  67. 	println("found AntPlus, pid=", dev->idProduct, HEX);

  68. 	rxpipe = txpipe = NULL;

  69. 	const uint8_t *p = descriptors;

  70. 	const uint8_t *end = p + len;

  71. 	int descriptorLength = p[0];

  72. 	int descriptorType = p[1];

  73. 	if (descriptorLength < 9 || descriptorType != 4) return false;

  74. 	p += descriptorLength;

  75. 	while (p < end) {

  76. 		descriptorLength = p[0];

  77. 		if (p + descriptorLength > end) return false; // reject if beyond end of data

  78. 		descriptorType = p[1];

  79. 		if (descriptorType == 5) { // 5 = endpoint

  80. 			uint8_t epAddr = p[2];

  81. 			uint8_t epType = p[3] & 0x03;

  82. 			uint16_t epSize = p[4] | (p[5] << 8);

  83. 			if (epType == 2 && (epAddr & 0xF0) == 0x00) { // Bulk OUT

  84. 				txpipe = new_Pipe(dev, 2, epAddr, 0, epSize);

  85. 			} else if (epType == 2 && (epAddr & 0xF0) == 0x80) { // Bulk IN

  86. 				rxpipe = new_Pipe(dev, 2, epAddr & 0x0F, 1, epSize);

  87. 			}

  88. 		}

  89. 		p += descriptorLength;

  90. 	}

  91. 	if (rxpipe && txpipe) {

  92. 		rxpipe->callback_function = rx_callback;

  93. 		txpipe->callback_function = tx_callback;

  94. 		txhead = 0;

  95. 		txtail = 0;

  96. 		//rxhead = 0;

  97. 		//rxtail = 0;

  98. 		memset(txbuffer, 0, sizeof(txbuffer));

  99. 		first_update = true;

  100. 		txready = true;

  101. 		updatetimer.start(500000);

  102. 		queue_Data_Transfer(rxpipe, rxpacket, 64, this);

  103. 		rxlen = 0;

  104. 		return true;

  105. 	}

  106. 	return false;

  107. }

  108. 

  109. void AntPlus::disconnect()

  110. {

  111. 	updatetimer.stop();

  112. 	//txtimer.stop();

  113. }

  114. 

  115. 

  116. void AntPlus::rx_callback(const Transfer_t *transfer)

  117. {

  118. 	if (!transfer->driver) return;

  119. 	((AntPlus *)(transfer->driver))->rx_data(transfer);

  120. }

  121. 

  122. void AntPlus::tx_callback(const Transfer_t *transfer)

  123. {

  124.         if (!transfer->driver) return;

  125.         ((AntPlus *)(transfer->driver))->tx_data(transfer);

  126. }

  127. 

  128. void AntPlus::rx_data(const Transfer_t *transfer)

  129. {

  130. 	uint32_t len = transfer->length - ((transfer->qtd.token >> 16) & 0x7FFF);

  131. 	//println("ant rx, len=", len);

  132. 	//print_hexbytes(transfer->buffer, len);

  133. 	if (len < 1 || len > 64) {

  134. 		queue_Data_Transfer(rxpipe, rxpacket, 64, this);

  135. 		rxlen = 0;

  136. 	} else {

  137. 		rxlen = len; // signal arrival of data to Task()

  138. 	}

  139. }

  140. 

  141. void AntPlus::tx_data(const Transfer_t *transfer)

  142. {

  143. 	uint8_t *p = (uint8_t *)transfer->buffer;

  144. 	//print("tx_data, len=", *(p-1));

  145. 	//print(", tail=", (p-1) - txbuffer);

  146. 	//println(", tail=", txtail);

  147. 	uint32_t tail = txtail;

  148. 	tail += *(p-1) + 1;

  149. 	txtail = tail;

  150. 	//println("new tail=", tail);

  151. 	txready = true;

  152. 	transmit();

  153. 	//txtimer.start(8000);

  154. 	// adjust tail...

  155. 	// start timer if more data to send

  156. }

  157. 

  158. 

  159. size_t AntPlus::write(const void *data, const size_t size)

  160. {

  161. 	//print("write ", size);

  162. 	//print(" bytes: ");

  163. 	//print_hexbytes(data, size);

  164. 	if (size > 64) return 0;

  165. 	uint32_t head = txhead;

  166. 	if (++head >= sizeof(txbuffer)) head = 0;

  167. 	uint32_t remain = sizeof(txbuffer) - head;

  168. 	if (remain < size + 1) {

  169. 		// not enough space at end of buffer

  170. 		txbuffer[head] = 0xFF;

  171. 		head = 0;

  172. 	}

  173. 	uint32_t avail;

  174. 	do {

  175. 		uint32_t tail = txtail;

  176. 		if (head > tail) {

  177. 			avail = sizeof(txbuffer) - head + tail;

  178. 		} else {

  179. 			avail = tail - head;

  180. 		}

  181. 	} while (avail < size + 1); // wait for space in buffer

  182. 	txbuffer[head] = size;

  183. 	memcpy(txbuffer + head + 1, data, size);

  184. 	txhead = head + size;

  185. 	//print("head=", txhead);

  186. 	//println(", tail=", txtail);

  187. 	//print_hexbytes(txbuffer, 60);

  188. 	NVIC_DISABLE_IRQ(IRQ_USBHS);

  189. 	transmit();

  190. 	NVIC_ENABLE_IRQ(IRQ_USBHS);

  191. 	return size;

  192. }

  193. 

  194. void AntPlus::transmit()

  195. {

  196. 	if (!txready) return;

  197. 	uint32_t head = txhead;

  198. 	uint32_t tail = txtail;

  199. 	if (head == tail) {

  200. 		//println("no data to transmit");

  201. 		return; // no data to transit

  202. 	}

  203. 	if (++tail >= sizeof(txbuffer)) tail = 0;

  204. 	uint32_t size = txbuffer[tail];

  205. 	//print("tail=", tail);

  206. 	//println(", tx size=", size);

  207. 	if (size == 0xFF) {

  208. 		txtail = 0;

  209. 		tail = 0;

  210. 		size = txbuffer[0];

  211. 		//println("tx size=", size);

  212. 	}

  213. 	//txtail = tail + size;

  214. 	queue_Data_Transfer(txpipe, txbuffer + tail + 1, size, this);

  215. 	//txtimer.start(8000);

  216. 	txready = false;

  217. }

  218. 

  219. void AntPlus::timer_event(USBDriverTimer *whichTimer)

  220. {

  221. 	if (whichTimer == &updatetimer) {

  222. 		updatetimer.start(250000);

  223. 		if (first_update) {

  224. 			ResetSystem();

  225. 			first_update = false;

  226. 		}

  227. 		//println("ant update timer");

  228. 	}

  229. 	/* else if (whichTimer == &txtimer) {

  230. 		println("ant tx timer");

  231. 		//txtimer.stop(); // TODO: why is this needed?

  232. 		txready = true;

  233. 		transmit();

  234. 	} */

  235. }

  236. 

  237. 

  238. void AntPlus::Task()

  239. {

  240. 	uint32_t len = rxlen;

  241. 	if (len) {

  242. 		handleMessages(rxpacket, len);

  243. 		NVIC_DISABLE_IRQ(IRQ_USBHS);

  244. 		queue_Data_Transfer(rxpipe, rxpacket, 64, this);

  245. 		rxlen = 0;

  246. 		NVIC_ENABLE_IRQ(IRQ_USBHS);

  247. 	}

  248. 

  249. 

  250. 

  251. }

  252. 

  253. 

  254. 

  255. 

  256. 

  257. 

  258. enum _akeys {

  259. 	KEY_ANTSPORT,

  260. 	KEY_SUUNTO,

  261. 	KEY_GARMIN,

  262. 	KEY_ANTPLUS,

  263. 	KEY_TOTAL,

  264. 	KEY_DEFAULT = KEY_ANTSPORT

  265. };

  266. static const uint8_t antkeys[KEY_TOTAL][8] = {

  267. {0xB9,0xA5,0x21,0xFB,0xBD,0x72,0xC3,0x45},  // Ant+ sport key  KEY_ANTSPORT

  268. {0xB9,0xAD,0x32,0x28,0x75,0x7E,0xC7,0x4D},  // Suunto          KEY_SUUNTO

  269. {0xA8,0xA4,0x23,0xB9,0xF5,0x5E,0x63,0xC1},  // Garmin          KEY_GARMIN

  270. {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}   // Ant+ key        KEY_ANTPLUS (add your key here)

  271. //{0xFD,0x38,0xBE,0xA6,0x40,0x5D,0x26,0x99}

  272. };

  273. 

  274. 

  275. 

  276. /*int AntPlus::dispatchMessage(const uint8_t *stream, const int len)

  277. {

  278. 	return ant.eventCb[EVENTI_MESSAGE].cbPtr(stream[STREAM_CHANNEL],

  279. 		stream[STREAM_MESSAGE], &stream[STREAM_DATA],

  280. 		(size_t)stream[STREAM_LENGTH], ant.eventCb[EVENTI_MESSAGE].uPtr);

  281. }*/

  282. 

  283. uint8_t AntPlus::calcMsgChecksum(const uint8_t *buffer, const uint8_t len)

  284. {

  285. 	uint8_t checksum = 0x00;

  286. 	for (uint8_t i = 0; i < len; i++)

  287. 		checksum ^= buffer[i];

  288. 	return checksum;

  289. }

  290. 

  291. uint8_t * AntPlus::findStreamSync(uint8_t *stream, const size_t rlen, int *pos)

  292. {

  293. 	// find and sync with input stream

  294. 	*pos = 0;

  295. 	while (*pos < (int)rlen /*&& *pos < INPUTBUFFERSIZE-3*/){

  296. 		if (stream[*pos] == MESG_TX_SYNC)

  297. 			return stream + *pos;

  298. 		(*pos)++;

  299. 	}

  300. 	return NULL;

  301. }

  302. 

  303. int AntPlus::msgCheckIntegrity(uint8_t *stream, const int len)

  304. {

  305. 	// min message length is 5

  306. 	if (len < 5) return 0;

  307. 

  308. 	int crc = stream[STREAM_SYNC];

  309. 	crc ^= stream[STREAM_LENGTH];

  310. 	crc ^= stream[STREAM_MESSAGE];

  311. 	int mlen = 0;

  312. 

  313. 	do{

  314. 		crc ^= stream[STREAM_DATA+mlen];

  315. 	} while (++mlen < stream[STREAM_LENGTH]);

  316. 

  317. 	//printf("crc == 0x%X: msg crc = 0x%X\n", crc, stream[stream[STREAM_LENGTH] + 3]);

  318. 	return (crc == stream[stream[STREAM_LENGTH] + 3]);

  319. }

  320. 

  321. int AntPlus::msgGetLength (uint8_t *stream)

  322. {

  323. 	// eg; {A4 1 6F 20 EA} = {SYNC DATALEN MSGID DATA CRC}

  324. 	return stream[STREAM_LENGTH] + 4;

  325. }

  326. 

  327. int AntPlus::handleMessages(uint8_t *buffer, int tBytes)

  328. {

  329. 	int syncOffset = 0;

  330. 	//uint8_t buffer[ANTPLUS_MAXPACKETSIZE];

  331. 	uint8_t *stream = buffer;

  332. 

  333. 	//int tBytes = antplus_read(ant, buffer, ant->ioVar.readBufferSize);

  334. 	//if (tBytes <= 0) return tBytes;

  335. 

  336. 	//int tBytes = ANTPLUS_MAXPACKETSIZE;

  337. 

  338. 	while (tBytes > 0){

  339. 		stream = findStreamSync(stream, tBytes, &syncOffset);

  340. 		if (stream == NULL){

  341. 			//printf("stream sync not found {size:%i}\n", tBytes);

  342. 			return 0;

  343. 		}

  344. 		tBytes -= syncOffset;

  345. 

  346. 		if (!msgCheckIntegrity(stream, tBytes)){

  347. 			//printf("stream integrity failed {size:%i}\n", tBytes);

  348. 			return 0;

  349. 		}

  350. 

  351. 		//we have a valid message

  352. 		//if (dispatchMessage(stream, tBytes) == -1){

  353. 			//printf("quiting..\n");

  354. 			//return 0;

  355. 		//}

  356. 		message_event(stream[STREAM_CHANNEL], stream[STREAM_MESSAGE],

  357. 			&stream[STREAM_DATA], (size_t)stream[STREAM_LENGTH]);

  358. 

  359. 

  360. 

  361. 		int len = msgGetLength(stream);

  362. 		stream += len;

  363. 		tBytes -= len;

  364. 	}

  365. 	return 1;

  366. }

  367. 

  368. /*int AntPlus::registerEventCallback(const int which, void *eventFunc, void *userPtr)

  369. {

  370. 	if (which < EVENTI_TOTAL) {

  371. 		ant.eventCb[which].cbPtr = (int (*)(int, int, const uint8_t*, size_t, void*))eventFunc;

  372. 		ant.eventCb[which].uPtr = userPtr;

  373. 		return 1;

  374. 	}

  375. 	//printf("invalid callback id {%i}\n.", which);

  376. 	return 0;

  377. }*/

  378. 

  379. /*int AntPlus::registerPayloadCallback(const int profile, void *eventFunc, void *userPtr)

  380. {

  381. 	if (profile < PROFILE_TOTAL) {

  382. 		ant.payloadCb[profile].cbPtr = (void (*)(TDCONFIG*, const uint8_t*, size_t, void*))eventFunc;

  383. 		ant.payloadCb[profile].uPtr = userPtr;

  384. 		return 1;

  385. 	}

  386. 	//printf("invalid callback id {%i}\n.", profile);

  387. 	return 0;

  388. }*/

  389. 

  390. 

  391. 

  392. // TODO: replace this with multiple Arduino style OnXYZ callbacks

  393. void AntPlus::setCallbackFunc(int (*func)(uint32_t msg, intptr_t *value1, uint32_t value2))

  394. {

  395. 	callbackFunc = func;

  396. }

  397. // TODO: replace this with multiple Arduino style OnXYZ callbacks

  398. void AntPlus::sendMessage(uint32_t msg, intptr_t *value1, uint32_t value2)

  399. {

  400. 	if (callbackFunc) (*callbackFunc)(msg, value1, value2);

  401. }

  402. 

  403. void AntPlus::sendMessageChannelStatus(TDCONFIG *cfg, const uint32_t channelStatus)

  404. {

  405. 	cfg->flags.channelStatus = channelStatus;

  406. 	if (cfg->flags.channelStatus != cfg->flags.channelStatusOld) {

  407. 		uint32_t status = cfg->flags.channelStatus&0x0F;

  408. 		status |= ((cfg->channel&0x0F)<<4);

  409. 		sendMessage(ANTP_MSG_CHANNELSTATUS, NULL, status);

  410. 		cfg->flags.channelStatusOld = cfg->flags.channelStatus;

  411. 	}

  412. }

  413. 

  414. void AntPlus::message_channel(const int chan, const int eventId,

  415. 	const uint8_t *payload, const size_t dataLength)

  416. {

  417. 	//printf(" $ chan event: chan:%i, msgId:0x%.2X, payload:%p, dataLen:%i, uPtr:%p", chan, eventId, payload, (int)dataLength, uPtr);

  418. 	//dump_hexbytes(payload, dataLength);

  419. 

  420. 	//TLIBANTPLUS *ant = (AntPlus::TLIBANTPLUS*)uPtr;

  421. 	TDCONFIG *cfg = &(ant.dcfg[chan]);

  422. 

  423. 	switch (eventId){

  424. 	  case EVENT_RX_SEARCH_TIMEOUT:

  425. 	  	printf(" $ event RX search timeout");

  426. 	  	break;

  427. 

  428. 	  case EVENT_RX_FAIL:

  429. 	  	//printf(" $ event RX fail");

  430. 	  	break;

  431. 

  432. 	  case EVENT_TX:

  433. 	  	//printf(" $ event TX");

  434. 	  	break;

  435. 

  436. 	  case EVENT_RX_BROADCAST:

  437. 	  	//printf(" $ event RX broadcast ");

  438. 	  	if (!cfg->flags.chanIdOnce) {

  439. 	  		cfg->flags.chanIdOnce = 1;

  440. 	  		RequestMessage(cfg->channel, MESG_CHANNEL_ID_ID);

  441. 	  	}

  442. 		//dump_hexbytes(payload, dataLength);

  443. 		dispatchPayload(cfg, payload, dataLength);

  444. 		break;

  445. 	 }

  446. }

  447. 

  448. void AntPlus::message_response(const int chan, const int msgId,

  449. 	const uint8_t *payload, const size_t dataLength)

  450. {

  451. 	//printf(" # response event: msgId:0x%.2X, payload:%p, dataLen:%i, uPtr:%p", msgId, payload, dataLength, uPtr);

  452. 	TDCONFIG *cfg = (TDCONFIG*)&(ant.dcfg[chan]);

  453. 

  454. 	switch (msgId){

  455. 	  case MESG_EVENT_ID:

  456. 	  	//printf(" * event");

  457. 	  	message_channel(chan, payload[STREAM_EVENT_EVENTID], payload, dataLength);

  458. 	  	break;

  459. 

  460. 	  case MESG_NETWORK_KEY_ID:

  461. 	  	printf("[%i] * network key accepted", chan);

  462. 	  	cfg->flags.keyAccepted = 1;

  463. 	  	if (cfg->transType == ANT_TRANSMISSION_MASTER)

  464. 	  		AssignChannel(cfg->channel, PARAMETER_TX_NOT_RX, cfg->networkNumber);

  465. 	  	else

  466. 	  		AssignChannel(cfg->channel, cfg->channelType, cfg->networkNumber);

  467. 	  	break;

  468. 

  469. 	  case MESG_ASSIGN_CHANNEL_ID:

  470. 	  	printf("[%i]  * channel assign accepted", chan);

  471. 	  	SetChannelPeriod(cfg->channel, cfg->channelPeriod);

  472. 	  	break;

  473. 

  474. 	  case MESG_CHANNEL_MESG_PERIOD_ID:

  475. 		printf("[%i]  * channel mesg period accepted", chan);

  476. 		SetChannelSearchTimeout(cfg->channel, cfg->searchTimeout);

  477. 		break;

  478. 

  479. 	  case MESG_CHANNEL_SEARCH_TIMEOUT_ID:

  480. 	  	printf("[%i]  * search timeout period accepted", chan);

  481. 	  	SetChannelRFFreq(cfg->channel, cfg->RFFreq);

  482. 	  	break;

  483. 

  484. 	  case MESG_CHANNEL_RADIO_FREQ_ID:

  485. 	  	printf("[%i]  * radio freq accepted", chan);

  486. 	  	SetSearchWaveform(cfg->channel, cfg->searchWaveform);

  487. 	  	break;

  488. 

  489. 	  case MESG_SEARCH_WAVEFORM_ID:

  490. 	  	printf("[%i]  * search waveform accepted", chan);

  491. 	  	SetChannelId(cfg->channel, cfg->deviceNumber, cfg->deviceType, cfg->transType);

  492. 	  	break;

  493. 

  494. 	  case MESG_CHANNEL_ID_ID:

  495. 	  	printf("[%i]  * set channel id accepted", chan);

  496. 	  	OpenChannel(cfg->channel);

  497. 	  	break;

  498. 

  499. 	  case MESG_OPEN_CHANNEL_ID:

  500. 	  	printf("[%i]  * open channel accepted", chan);

  501. 	  	//cfg->flags.channelStatus = 1;

  502. 	  	RequestMessage(cfg->channel, MESG_CHANNEL_STATUS_ID);

  503. 	  	RequestMessage(cfg->channel, MESG_CAPABILITIES_ID);

  504. 	  	RequestMessage(cfg->channel, MESG_VERSION_ID);

  505. 	  	break;

  506. 

  507.  	  case MESG_UNASSIGN_CHANNEL_ID:

  508. 		printf("[%i]  * channel Unassigned", chan);

  509. 		break;

  510. 

  511. 	  case MESG_CLOSE_CHANNEL_ID:

  512. 		printf("[%i]  * channel CLOSED", chan);

  513. 		cfg->flags.keyAccepted = 0;

  514. 		sendMessageChannelStatus(cfg, ANT_CHANNEL_STATUS_UNASSIGNED);

  515. 		break;

  516. 

  517.  	  case CHANNEL_IN_WRONG_STATE:

  518. 		printf("[%i]  * channel in wrong state", chan);

  519. 		break;

  520. 

  521.  	  case CHANNEL_NOT_OPENED:

  522. 		printf("[%i]  * channel not opened", chan);

  523. 		break;

  524. 

  525.  	  case CHANNEL_ID_NOT_SET: //??

  526. 		printf("[%i]  * channel ID not set", chan);

  527. 		break;

  528. 

  529.  	  case CLOSE_ALL_CHANNELS: // Start RX Scan mode

  530. 		printf("[%i]  * close all channels", chan);

  531. 		break;

  532. 

  533.  	  case TRANSFER_IN_PROGRESS: // TO ack message ID

  534. 		printf("[%i]  * tranfer in progress", chan);

  535. 		break;

  536. 

  537.  	  case TRANSFER_SEQUENCE_NUMBER_ERROR:

  538. 		printf("[%i]  * transfer sequence number error", chan);

  539. 		break;

  540. 

  541.  	  case TRANSFER_IN_ERROR:

  542. 		printf("[%i]  * transfer in error", chan);

  543. 		break;

  544. 

  545.  	  case INVALID_MESSAGE:

  546. 		printf("[%i]  * invalid message", chan);

  547. 		break;

  548. 

  549.  	  case INVALID_NETWORK_NUMBER:

  550. 		printf("[%i]  * invalid network number", chan);

  551. 		break;

  552. 

  553.  	  case INVALID_LIST_ID:

  554. 		printf("[%i]  * invalid list ID", chan);

  555. 		break;

  556. 

  557.  	  case INVALID_SCAN_TX_CHANNEL:

  558. 		printf("[%i]  * invalid Scanning transmit channel", chan);

  559. 		break;

  560. 

  561.  	  case INVALID_PARAMETER_PROVIDED:

  562. 		printf("[%i]  * invalid parameter provided", chan);

  563.    		break;

  564. 

  565.  	  case EVENT_QUE_OVERFLOW:

  566. 		printf("[%i]  * queue overflow", chan);

  567. 		break;

  568. 

  569. 	  default:

  570. 	  	printf("[i] #### unhandled response id %i", chan, msgId);

  571. 		;

  572. 	};

  573. }

  574. 

  575. 

  576. void AntPlus::message_event(const int channel, const int msgId,

  577. 	const uint8_t *payload, const size_t dataLength)

  578. {

  579. 	//printf(" @ msg event cb: Chan:%i, Id:0x%.2X, payload:%p, len:%i, ptr:%p", channel, msgId, payload, (int)dataLength, uPtr);

  580. 	//dump_hexbytes(payload, dataLength);

  581. 

  582. 	//TLIBANTPLUS *ant = (TLIBANTPLUS*)uPtr;

  583. 	uint8_t chan = 0;

  584. 	if (channel >= 0 && channel < PROFILE_TOTAL) chan = channel;

  585. 

  586. 	switch(msgId) {

  587. 	  case MESG_BROADCAST_DATA_ID:

  588. 	  	//printf(" @ broadcast data \n");

  589. 		//dumpPayload(payload, dataLength);

  590. 		message_channel(chan, EVENT_RX_BROADCAST, payload, dataLength);

  591. 	  	break;

  592. 

  593. 	  case MESG_STARTUP_MESG_ID:

  594. 	  	// reason == ANT_STARTUP_RESET_xxxx

  595. 	  	//printf(" @ start up mesg reason: 0x%X", payload[STREAM_STARTUP_REASON]);

  596. 	  	//TDCONFIG *cfg = &(ant.dcfg[0]);

  597. 		//SetNetworkKey(cfg->networkNumber, getAntKey(cfg->keyIdx));

  598. 		SetNetworkKey(ant.dcfg[0].networkNumber, getAntKey(ant.key));

  599. 		break;

  600. 

  601. 	  case MESG_RESPONSE_EVENT_ID:

  602. 	  	message_response(payload[STREAM_EVENT_CHANNEL_ID],

  603. 			payload[STREAM_EVENT_RESPONSE_ID], payload, dataLength);

  604. 	  	break;

  605. 

  606. 	  case MESG_CHANNEL_STATUS_ID:

  607. 	  	//printf(" @ channel status for channel %i is %i",

  608. 		//   payload[STREAM_CHANNEL_ID], payload[STREAM_CHANNEL_STATUS]);

  609. 	  	//TDCONFIG *cfg = (TDCONFIG*)&ant->dcfg[payload[STREAM_CHANNEL_ID]];

  610. 	  	sendMessageChannelStatus(&(ant.dcfg[payload[STREAM_CHANNEL_ID]]),

  611. 			payload[STREAM_CHANNELSTATUS_STATUS] & ANT_CHANNEL_STATUS_MASK);

  612. 		//if (cfg->flags.channelStatus != STATUS_TRACKING_CHANNEL)

  613. 		//	printf("channel %i status: %s", payload[STREAM_CHANNEL_ID],

  614. 		//	 channelStatusStr[cfg->flags.channelStatus]);

  615. 	  	break;

  616. 

  617. 	  case MESG_CAPABILITIES_ID:

  618. 		//printf(" @ capabilities:");

  619. 		//printf("   Max ANT Channels: %i",payload[STREAM_CAP_MAXCHANNELS]);

  620. 		//printf("   Max ANT Networks: %i",payload[STREAM_CAP_MAXNETWORKS]);

  621. 		//printf("   Std. option: 0x%X",payload[STREAM_CAP_STDOPTIONS]);

  622. 		//printf("   Advanced: 0x%X",payload[STREAM_CAP_ADVANCED]);

  623. 		//printf("   Advanced2: 0x%X",payload[STREAM_CAP_ADVANCED2]);

  624. 	  	break;

  625. 

  626. 	case MESG_CHANNEL_ID_ID:

  627. 		//TDCONFIG *cfg = (TDCONFIG*)&ant->dcfg[chan];

  628.         	ant.dcfg[chan].dev.deviceId = payload[STREAM_CHANNELID_DEVNO_LO] | (payload[STREAM_CHANNELID_DEVNO_HI] << 8);

  629. 		ant.dcfg[chan].dev.deviceType = payload[STREAM_CHANNELID_DEVTYPE];

  630. 		ant.dcfg[chan].dev.transType = payload[STREAM_CHANNELID_TRANTYPE];

  631. 		//printf(" @ CHANNEL ID: channel %i, deviceId:%i, deviceType:%i, transType:%i)", chan, cfg->dev.deviceId, cfg->dev.deviceType, cfg->dev.transType);

  632. 		sendMessage(ANTP_MSG_DEVICEID, (intptr_t *)&(ant.dcfg[chan].dev), chan);

  633. #if 0

  634. 		if (cfg->dev.scidDeviceType != cfg->deviceType){

  635. 			printf(" @ CHANNEL ID: this is not the device we're looking for");

  636. 			printf(" @ CHANNEL ID: expecting 0x%X but found 0x%X", cfg->deviceType, cfg->dev.scidDeviceType);

  637.          		//CloseChannel(cfg->channel);

  638.          		return;

  639.          	}

  640. #endif

  641. 		break;

  642. 

  643. 	case MESG_VERSION_ID:

  644. 		//printf(" @ version: '%s'", (char*)&payload[STREAM_VERSION_STRING]);

  645. 		break;

  646. 	};

  647. }

  648. 

  649. 

  650. //int AntPlus::SetPayloadHandler(const int profile, void *eventFunc, void *userPtr)

  651. //{

  652. 	//return registerPayloadCallback(profile, eventFunc, userPtr);

  653. //}

  654. 

  655. //int AntPlus::SetEventHandler(const int which, void *eventFunc, void *userPtr)

  656. //{

  657. 	//return registerEventCallback(which, eventFunc, userPtr);

  658. //}

  659. 

  660. int AntPlus::ResetSystem()

  661. {

  662. 	//println("libantplus_ResetSystem");

  663. 	uint8_t msg[5];

  664. 	msg[0] = MESG_TX_SYNC;			// sync

  665. 	msg[1] = 1;						// length

  666. 	msg[2] = MESG_SYSTEM_RESET_ID;	// msg id

  667. 	msg[3] = 0;						// nop

  668. 	msg[4] = calcMsgChecksum(msg, 4);

  669. 	return write(msg, 5);

  670. }

  671. 

  672. int AntPlus::RequestMessage(const int channel, const int message)

  673. {

  674. 	//println("libantplus_RequestMessage");

  675. 	uint8_t msg[6];

  676. 	msg[0] = MESG_TX_SYNC;			// sync

  677. 	msg[1] = 2;						// length

  678. 	msg[2] = MESG_REQUEST_ID;		// msg id

  679. 	msg[3] = (uint8_t)channel;

  680. 	msg[4] = (uint8_t)message;

  681. 	msg[5] = calcMsgChecksum(msg, 5);

  682. 	return write(msg, 6);

  683. }

  684. 

  685. int AntPlus::SetNetworkKey(const int netNumber, const uint8_t *key)

  686. {

  687. 	uint8_t msg[13];

  688. 	msg[0] = MESG_TX_SYNC;

  689. 	msg[1] = 9;

  690. 	msg[2] = MESG_NETWORK_KEY_ID;

  691. 	msg[3] = (uint8_t)netNumber;

  692. 	msg[4] = key[0];

  693. 	msg[5] = key[1];

  694. 	msg[6] = key[2];

  695. 	msg[7] = key[3];

  696. 	msg[8] = key[4];

  697. 	msg[9] = key[5];

  698. 	msg[10] = key[6];

  699. 	msg[11] = key[7];

  700. 	msg[12] = calcMsgChecksum(msg, 12); 			// xor checksum

  701. 	return write(msg, 13);

  702. }

  703. 

  704. int AntPlus::SetChannelSearchTimeout(const int channel, const int searchTimeout)

  705. {

  706. 	uint8_t msg[6];

  707. 	msg[0] = MESG_TX_SYNC;			// sync

  708. 	msg[1] = 2;						// length

  709. 	msg[2] = MESG_CHANNEL_SEARCH_TIMEOUT_ID;

  710. 	msg[3] = (uint8_t)channel;

  711. 	msg[4] = (uint8_t)searchTimeout;

  712. 	msg[5] = calcMsgChecksum(msg, 5);

  713. 	return write(msg, 6);

  714. }

  715. 

  716. int AntPlus::SetChannelPeriod(const int channel, const int period)

  717. {

  718. 	uint8_t msg[7];

  719. 	msg[0] = MESG_TX_SYNC;			// sync

  720. 	msg[1] = 3;						// length

  721. 	msg[2] = MESG_CHANNEL_MESG_PERIOD_ID;

  722. 	msg[3] = (uint8_t)channel;

  723. 	msg[4] = (uint8_t)(period & 0xFF);

  724. 	msg[5] = (uint8_t)(period >> 8);

  725. 	msg[6] = calcMsgChecksum(msg, 6);

  726. 	return write(msg, 7);

  727. }

  728. 

  729. int AntPlus::SetChannelRFFreq(const int channel, const int freq)

  730. {

  731. 	uint8_t msg[6];

  732. 	msg[0] = MESG_TX_SYNC;			// sync

  733. 	msg[1] = 2;						// length

  734. 	msg[2] = MESG_CHANNEL_RADIO_FREQ_ID;

  735. 	msg[3] = (uint8_t)channel;

  736. 	msg[4] = (uint8_t)freq;

  737. 	msg[5] = calcMsgChecksum(msg, 5);

  738. 	return write(msg, 6);

  739. }

  740. 

  741. int AntPlus::SetSearchWaveform(const int channel, const int wave)

  742. {

  743. 	uint8_t msg[7];

  744. 	msg[0] = MESG_TX_SYNC;			// sync

  745. 	msg[1] = 3;						// length

  746. 	msg[2] = MESG_SEARCH_WAVEFORM_ID;		// msg id

  747. 	msg[3] = (uint8_t)channel;

  748. 	msg[4] = (uint8_t)wave & 0xFF;

  749. 	msg[5] = (uint8_t)wave >> 8;

  750. 	msg[6] = calcMsgChecksum(msg, 6);

  751. 	return write(msg, 7);

  752. }

  753. 

  754. int AntPlus::OpenChannel(const int channel)

  755. {

  756. 	uint8_t msg[5];

  757. 	msg[0] = MESG_TX_SYNC;			// sync

  758. 	msg[1] = 1;						// length

  759. 	msg[2] = MESG_OPEN_CHANNEL_ID;	// msg id

  760. 	msg[3] = (uint8_t)channel;

  761. 	msg[4] = calcMsgChecksum(msg, 4);

  762. 	return write(msg, 5);

  763. }

  764. 

  765. int AntPlus::CloseChannel(const int channel)

  766. {

  767. 	uint8_t msg[5];

  768. 	msg[0] = MESG_TX_SYNC;			// sync

  769. 	msg[1] = 1;						// length

  770. 	msg[2] = MESG_CLOSE_CHANNEL_ID;	// msg id

  771. 	msg[3] = (uint8_t)channel;

  772. 	msg[4] = calcMsgChecksum(msg, 4);

  773. 	return write(msg, 5);

  774. }

  775. 

  776. int AntPlus::AssignChannel(const int channel, const int channelType, const int network)

  777. {

  778. 	uint8_t msg[7];

  779. 	msg[0] = MESG_TX_SYNC;

  780. 	msg[1] = 3;

  781. 	msg[2] = MESG_ASSIGN_CHANNEL_ID;

  782. 	msg[3] = (uint8_t)channel;

  783. 	msg[4] = (uint8_t)channelType;

  784. 	msg[5] = (uint8_t)network;

  785. 	msg[6] = calcMsgChecksum(msg, 6);

  786. 	return write(msg, 7);

  787. }

  788. 

  789. int AntPlus::SetChannelId(const int channel, const int deviceNum, const int deviceType, const int transmissionType)

  790. {

  791. 	uint8_t msg[9];

  792. 	msg[0] = MESG_TX_SYNC;

  793. 	msg[1] = 5;

  794. 	msg[2] = MESG_CHANNEL_ID_ID;

  795. 	msg[3] = (uint8_t)channel;

  796. 	msg[4] = (uint8_t)(deviceNum & 0xFF);

  797. 	msg[5] = (uint8_t)(deviceNum >> 8);

  798. 	msg[6] = (uint8_t)deviceType;

  799. 	msg[7] = (uint8_t)transmissionType;

  800. 	msg[8] = calcMsgChecksum(msg, 8);

  801. 	return write(msg, 9);

  802. }

  803. 

  804. int AntPlus::SendBurstTransferPacket(const int channelSeq, const uint8_t *data)

  805. {

  806. 	uint8_t msg[13];

  807. 	msg[0] = MESG_TX_SYNC;

  808. 	msg[1] = 9;

  809. 	msg[2] = MESG_BURST_DATA_ID;

  810. 	msg[3] = (uint8_t)channelSeq;

  811. 	msg[4] = data[0];

  812. 	msg[5] = data[1];

  813. 	msg[6] = data[2];

  814. 	msg[7] = data[3];

  815. 	msg[8] = data[4];

  816. 	msg[9] = data[5];

  817. 	msg[10] = data[6];

  818. 	msg[11] = data[7];

  819. 	msg[12] = calcMsgChecksum(msg, 12); 			// xor checksum

  820. 	return write(msg, 13);

  821. }

  822. 

  823. int AntPlus::SendBurstTransfer(const int channel, const uint8_t *data, const int nunPackets)

  824. {

  825. 	int ret = 0;

  826. 	int seq = 0;

  827. 	for (int i = 0; i < nunPackets; i++) {

  828. 		if (i == nunPackets-1) seq |= 0x04;

  829. 		ret = SendBurstTransferPacket((seq<<5) | (channel&0x1F), data+(i<<3));

  830. 		seq = (seq+1)&0x03;

  831. 	}

  832. 	return ret;

  833. }

  834. 

  835. int AntPlus::SendBroadcastData(const int channel, const uint8_t *data)

  836. {

  837. 	uint8_t msg[13];

  838. 	msg[0] = MESG_TX_SYNC;

  839. 	msg[1] = 9;

  840. 	msg[2] = MESG_BROADCAST_DATA_ID;

  841. 	msg[3] = (uint8_t)channel;

  842. 	msg[4] = data[0];

  843. 	msg[5] = data[1];

  844. 	msg[6] = data[2];

  845. 	msg[7] = data[3];

  846. 	msg[8] = data[4];

  847. 	msg[9] = data[5];

  848. 	msg[10] = data[6];

  849. 	msg[11] = data[7];

  850. 	msg[12] = calcMsgChecksum(msg, 12);

  851. 	return write(msg, 13);

  852. }

  853. 

  854. int AntPlus::SendAcknowledgedData(const int channel, const uint8_t *data)

  855. {

  856. 	uint8_t msg[13];

  857. 	msg[0] = MESG_TX_SYNC;

  858. 	msg[1] = 9;

  859. 	msg[2] = MESG_ACKNOWLEDGED_DATA_ID;

  860. 	msg[3] = (uint8_t)channel;

  861. 	msg[4] = data[0];

  862. 	msg[5] = data[1];

  863. 	msg[6] = data[2];

  864. 	msg[7] = data[3];

  865. 	msg[8] = data[4];

  866. 	msg[9] = data[5];

  867. 	msg[10] = data[6];

  868. 	msg[11] = data[7];

  869. 	msg[12] = calcMsgChecksum(msg, 12);

  870. 	return write(msg, 13);

  871. }

  872. 

  873. int AntPlus::SendExtAcknowledgedData(const int channel, const int devNum, const int devType, const int TranType, const uint8_t *data)

  874. {

  875. 	uint8_t msg[17];

  876. 	msg[0] = MESG_TX_SYNC;

  877. 	msg[1] = 13;

  878. 	msg[2] = MESG_EXT_ACKNOWLEDGED_DATA_ID;

  879. 	msg[3] = (uint8_t)channel;

  880. 	msg[4] = (uint8_t)(devNum & 0xFF);

  881. 	msg[5] = (uint8_t)(devNum >> 8);

  882. 	msg[6] = (uint8_t)devType;

  883. 	msg[7] = (uint8_t)TranType;

  884. 	msg[8] = data[0];

  885. 	msg[9] = data[1];

  886. 	msg[10] = data[2];

  887. 	msg[11] = data[3];

  888. 	msg[12] = data[4];

  889. 	msg[13] = data[5];

  890. 	msg[14] = data[6];

  891. 	msg[15] = data[7];

  892. 	msg[16] = calcMsgChecksum(msg, 16);

  893. 	return write(msg, 17);

  894. }

  895. 

  896. int AntPlus::SendExtBroadcastData(const int channel, const int devNum, const int devType, const int TranType, const uint8_t *data)

  897. {

  898. 	uint8_t msg[17];

  899. 	msg[0] = MESG_TX_SYNC;

  900. 	msg[1] = 13;

  901. 	msg[2] = MESG_EXT_BROADCAST_DATA_ID;

  902. 	msg[3] = (uint8_t)channel;

  903. 	msg[4] = (uint8_t)(devNum & 0xFF);

  904. 	msg[5] = (uint8_t)(devNum >> 8);

  905. 	msg[6] = (uint8_t)devType;

  906. 	msg[7] = (uint8_t)TranType;

  907. 	msg[8] = data[0];

  908. 	msg[9] = data[1];

  909. 	msg[10] = data[2];

  910. 	msg[11] = data[3];

  911. 	msg[12] = data[4];

  912. 	msg[13] = data[5];

  913. 	msg[14] = data[6];

  914. 	msg[15] = data[7];

  915. 	msg[16] = calcMsgChecksum(msg, 16);

  916. 	return write(msg, 17);

  917. }

  918. 

  919. int AntPlus::SendExtBurstTransferPacket(const int chanSeq, const int devNum, const int devType,

  920. 	const int TranType, const uint8_t *data)

  921. {

  922. 	uint8_t msg[17];

  923. 	msg[0] = MESG_TX_SYNC;

  924. 	msg[1] = 13;

  925. 	msg[2] = MESG_EXT_BROADCAST_DATA_ID;

  926. 	msg[3] = (uint8_t)chanSeq;

  927. 	msg[4] = (uint8_t)(devNum & 0xFF);

  928. 	msg[5] = (uint8_t)(devNum >> 8);

  929. 	msg[6] = (uint8_t)devType;

  930. 	msg[7] = (uint8_t)TranType;

  931. 	msg[8] = data[0];

  932. 	msg[9] = data[1];

  933. 	msg[10] = data[2];

  934. 	msg[11] = data[3];

  935. 	msg[12] = data[4];

  936. 	msg[13] = data[5];

  937. 	msg[14] = data[6];

  938. 	msg[15] = data[7];

  939. 	msg[16] = calcMsgChecksum(msg, 16);

  940. 	return write(msg, 17);

  941. }

  942. 

  943. int AntPlus::SendExtBurstTransfer(const int channel, const int devNum, const int devType,

  944. 	const int tranType, const uint8_t *data, const int nunPackets)

  945. {

  946. 	int ret = 0;

  947. 	int seq = 0;

  948. 	for (int i = 0; i < nunPackets; i++){

  949. 		if (i == nunPackets-1) seq |= 0x04;

  950. 		ret = SendExtBurstTransferPacket((seq<<5) | (channel&0x1F),

  951. 			devNum, devType, tranType, data+(i<<3));

  952. 		seq = (seq+1)&0x03;

  953. 	}

  954. 	return ret;

  955. }

  956. 

  957. //const uint8_t *libantplus_GetNetworkKey (const uint8_t keyIdx)

  958. //{

  959. 	//return antplus_getAntKey(keyIdx);

  960. //}

  961. 

  962. 

  963. 

  964. 

  965. void AntPlus::profileSetup_HRM(TDCONFIG *cfg, const uint32_t deviceId)

  966. {

  967. 	cfg->deviceNumber = deviceId;		// 0

  968. 	cfg->deviceType = ANT_DEVICE_HRM;

  969. 	cfg->transType = ANT_TRANSMISSION_SLAVE;

  970. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  971. 	cfg->networkNumber = 0;

  972. 	cfg->channel = PROFILE_HRM;

  973. 	cfg->channelPeriod = ANT_PERIOD_HRM;

  974. 	cfg->RFFreq = ANT_FREQUENCY_SPORT;

  975. 	cfg->searchTimeout = 255;

  976. 	cfg->searchWaveform = 0x53;//316;//0x53;

  977. 	//cfg->keyIdx = KEY_ANTSPORT;

  978. 	cfg->flags.chanIdOnce = 0;

  979. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

  980. 	cfg->flags.channelStatusOld = 0xFF;

  981. 	cfg->flags.keyAccepted = 0;

  982. 	cfg->flags.profileValid = 1;

  983. }

  984. 

  985. void AntPlus::profileSetup_SPDCAD(TDCONFIG *cfg, const uint32_t deviceId)

  986. {

  987. 	cfg->deviceNumber = deviceId;		// 0

  988. 	cfg->deviceType = ANT_DEVICE_SPDCAD;

  989. 	cfg->transType = ANT_TRANSMISSION_SLAVE;		// 5

  990. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  991. 	cfg->networkNumber = 0;

  992. 	cfg->channel = PROFILE_SPDCAD;

  993. 	cfg->channelPeriod = ANT_PERIOD_SPDCAD;

  994. 	cfg->RFFreq = ANT_FREQUENCY_SPORT;

  995. 	cfg->searchTimeout = 255;

  996. 	cfg->searchWaveform = 0x53;

  997. 	//cfg->keyIdx = KEY_ANTSPORT;

  998. 	cfg->flags.chanIdOnce = 0;

  999. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

  1000. 	cfg->flags.channelStatusOld = 0xFF;

  1001. 	cfg->flags.keyAccepted = 0;

  1002. 	cfg->flags.profileValid = 1;

  1003. }

  1004. 

  1005. void AntPlus::profileSetup_POWER(TDCONFIG *cfg, const uint32_t deviceId)

  1006. {

  1007. 	cfg->deviceNumber = deviceId;		// 0

  1008. 	cfg->deviceType = ANT_DEVICE_POWER;

  1009. 	cfg->transType = ANT_TRANSMISSION_SLAVE;		// 5

  1010. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  1011. 	cfg->networkNumber = 0;

  1012. 	cfg->channel = PROFILE_POWER;

  1013. 	cfg->channelPeriod = ANT_PERIOD_POWER;

  1014. 	cfg->RFFreq = ANT_FREQUENCY_SPORT;

  1015. 	cfg->searchTimeout = 255;

  1016. 	cfg->searchWaveform = 0x53;

  1017. 	//cfg->keyIdx = KEY_ANTSPORT;

  1018. 	cfg->flags.chanIdOnce = 0;

  1019. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

  1020. 	cfg->flags.channelStatusOld = 0xFF;

  1021. 	cfg->flags.keyAccepted = 0;

  1022. 	cfg->flags.profileValid = 1;

  1023. }

  1024. 

  1025. void AntPlus::profileSetup_STRIDE(TDCONFIG *cfg, const uint32_t deviceId)

  1026. {

  1027. 	cfg->deviceNumber = deviceId;		// 0

  1028. 	cfg->deviceType = ANT_DEVICE_STRIDE;

  1029. 	cfg->transType = ANT_TRANSMISSION_SLAVE;		// 5

  1030. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  1031. 	cfg->networkNumber = 0;

  1032. 	cfg->channel = PROFILE_STRIDE;

  1033. 	cfg->channelPeriod = ANT_PERIOD_STRIDE;

  1034. 	cfg->RFFreq = ANT_FREQUENCY_STRIDE;

  1035. 	cfg->searchTimeout = 255;

  1036. 	cfg->searchWaveform = 0x53;

  1037. 	//cfg->keyIdx = KEY_ANTSPORT;

  1038. 	cfg->flags.chanIdOnce = 0;

  1039. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

  1040. 	cfg->flags.channelStatusOld = 0xFF;

  1041. 	cfg->flags.keyAccepted = 0;

  1042. 	cfg->flags.profileValid = 1;

  1043. }

  1044. 

  1045. void AntPlus::profileSetup_SPEED(TDCONFIG *cfg, const uint32_t deviceId)

  1046. {

  1047. 	cfg->deviceNumber = deviceId;		// 0

  1048. 	cfg->deviceType = ANT_DEVICE_SPEED;

  1049. 	cfg->transType = ANT_TRANSMISSION_SLAVE;		// 5

  1050. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  1051. 	cfg->networkNumber = 0;

  1052. 	cfg->channel = PROFILE_SPEED;

  1053. 	cfg->channelPeriod = ANT_PERIOD_SPEED;

  1054. 	cfg->RFFreq = ANT_FREQUENCY_SPORT;

  1055. 	cfg->searchTimeout = 255;

  1056. 	cfg->searchWaveform = 0x53;

  1057. 	//cfg->keyIdx = KEY_ANTSPORT;

  1058. 	cfg->flags.chanIdOnce = 0;

  1059. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

  1060. 	cfg->flags.channelStatusOld = 0xFF;

  1061. 	cfg->flags.keyAccepted = 0;

  1062. 	cfg->flags.profileValid = 1;

  1063. }

  1064. 

  1065. void AntPlus::profileSetup_CADENCE(TDCONFIG *cfg, const uint32_t deviceId)

  1066. {

  1067. 	cfg->deviceNumber = deviceId;		// 0

  1068. 	cfg->deviceType = ANT_DEVICE_CADENCE;

  1069. 	cfg->transType = ANT_TRANSMISSION_SLAVE;		// 5

  1070. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  1071. 	cfg->networkNumber = 0;

  1072. 	cfg->channel = PROFILE_CADENCE;

  1073. 	cfg->channelPeriod = ANT_PERIOD_CADENCE;

  1074. 	cfg->RFFreq = ANT_FREQUENCY_SPORT;

  1075. 	cfg->searchTimeout = 255;

  1076. 	cfg->searchWaveform = 0x53;

  1077. 	//cfg->keyIdx = KEY_ANTSPORT;

  1078. 	cfg->flags.chanIdOnce = 0;

  1079. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

  1080. 	cfg->flags.channelStatusOld = 0xFF;

  1081. 	cfg->flags.keyAccepted = 0;

  1082. 	cfg->flags.profileValid = 1;

  1083. }

  1084. 

  1085. #if 0

  1086. 

  1087. /*

  1088. uint64_t factory_passkey (uint64_t device_id, uint8_t *buffer)

  1089. {

  1090. 	uint64_t n = (((uint64_t)device_id ^ 0x7d215abb) << 32) + ((uint64_t)device_id ^ 0x42b93f06);

  1091. 

  1092. 	for (uint8_t i = 0; i < 8; i++)

  1093. 		buffer[i] = n >> (8*i)&0xFF;

  1094. 

  1095. 	return n;

  1096. }

  1097. 

  1098. */

  1099. int libantplus_Start ()

  1100. {

  1101. #if 0

  1102. 	uint8_t buffer[8];

  1103. 	factory_passkey(3825666043, buffer);

  1104. 	dump_hexbytes(buffer, 8);

  1105. #endif

  1106. 

  1107. 	int ct = 0;

  1108. 	uint32_t deviceId;

  1109. 

  1110. 	for (int i = 0; i < PROFILE_TOTAL; i++){

  1111. 		deviceId = 0;

  1112. 		if (antplus_sendMessage(ANTP_MSG_PROFILE_SELECT, (intptr_t*)&deviceId, i) != 1)

  1113. 			continue;

  1114. 

  1115. 		ct++;

  1116. 		//printf("enabling profile %i", i);

  1117. 

  1118. 		switch (i){

  1119. 		  case PROFILE_HRM:

  1120. 			profileSetup_HRM(&ant->dcfg[PROFILE_HRM], deviceId);

  1121. 			libantplus_SetPayloadHandler(PROFILE_HRM, (void*)payload_HRM, (void*)NULL);

  1122. 			break;

  1123. 		  case PROFILE_SPDCAD:

  1124. 			profileSetup_SPDCAD(&ant->dcfg[PROFILE_SPDCAD], deviceId);

  1125. 			libantplus_SetPayloadHandler(PROFILE_SPDCAD, (void*)payload_SPDCAD, (void*)NULL);

  1126. 			break;

  1127. 		  case PROFILE_POWER:

  1128. 			profileSetup_POWER(&ant->dcfg[PROFILE_POWER], deviceId);

  1129. 			libantplus_SetPayloadHandler(PROFILE_POWER, (void*)payload_POWER, (void*)NULL);

  1130. 			break;

  1131. 		  case PROFILE_STRIDE:

  1132. 			profileSetup_STRIDE(&ant->dcfg[PROFILE_STRIDE], deviceId);

  1133. 			libantplus_SetPayloadHandler(PROFILE_STRIDE, (void*)payload_STRIDE, (void*)NULL);

  1134. 			break;

  1135. 		  case PROFILE_SPEED:

  1136. 			profileSetup_SPEED(&ant->dcfg[PROFILE_SPEED], deviceId);

  1137. 			libantplus_SetPayloadHandler(PROFILE_SPEED, (void*)payload_SPEED, (void*)NULL);

  1138. 			break;

  1139. 		  case PROFILE_CADENCE:

  1140. 			profileSetup_CADENCE(&ant->dcfg[PROFILE_CADENCE], deviceId);

  1141. 			libantplus_SetPayloadHandler(PROFILE_CADENCE, (void*)payload_CADENCE, (void*)NULL);

  1142. 			break;

  1143. 		}

  1144. 	}

  1145. 

  1146. 	return ct;

  1147. }

  1148. 

  1149. 

  1150. TLIBANTPLUS *libantplus_Init (const uint8_t networkKey)

  1151. {

  1152. 	if (networkKey >= KEY_TOTAL){

  1153. 		//printf("libantplus_Init(): invalid networkKey (%i)");

  1154. 		//return NULL;

  1155. 		ant->key = KEY_DEFAULT;

  1156. 	}else{

  1157. 		ant->key = networkKey;

  1158. 	}

  1159. 

  1160. 	libantplus_SetEventHandler(EVENTI_MESSAGE, (void*)message_event, (void*)ant);

  1161. 	return ant;

  1162. }

  1163. #endif

  1164. 

  1165. 

  1166. void AntPlus::begin(const uint8_t key)

  1167. {

  1168. 	ant.key = (key < KEY_TOTAL) ? key : 0;

  1169. 

  1170. 	int deviceId = 0; // TODO: user API to set this?

  1171. 	profileSetup_HRM(&ant.dcfg[PROFILE_HRM], deviceId);

  1172. 	profileSetup_SPDCAD(&ant.dcfg[PROFILE_SPDCAD], deviceId);

  1173. 	profileSetup_POWER(&ant.dcfg[PROFILE_POWER], deviceId);

  1174. 	profileSetup_STRIDE(&ant.dcfg[PROFILE_STRIDE], deviceId);

  1175. 	profileSetup_SPEED(&ant.dcfg[PROFILE_SPEED], deviceId);

  1176. 	profileSetup_CADENCE(&ant.dcfg[PROFILE_CADENCE], deviceId);

  1177. 

  1178. 	//ant.eventCb[EVENTI_MESSAGE].cbPtr = &message_event;

  1179. 	//SetEventHandler(EVENTI_MESSAGE, (void*)message_event, (void*)ant);

  1180. }

  1181. 

  1182. /*

  1183. int AntPlus::registerEventCallback(const int which, void *eventFunc, void *userPtr)

  1184. {

  1185. 	if (which < EVENTI_TOTAL) {

  1186. 		ant.eventCb[which].cbPtr = (int (*)(int, int, const uint8_t*, size_t, void*))eventFunc;

  1187. 

  1188. */

  1189. 

  1190. 

  1191. 

  1192. 

  1193. 

  1194. 

  1195. 

  1196. void AntPlus::dispatchPayload(TDCONFIG *cfg, const uint8_t *payload, const int len)

  1197. {

  1198. 	switch (cfg->channel) {

  1199. 	  case PROFILE_HRM:

  1200. 		payload_HRM(cfg, payload, len);

  1201. 		break;

  1202. 	  case PROFILE_SPDCAD:

  1203. 		payload_SPDCAD(cfg, payload, len);

  1204. 		break;

  1205. 	  case PROFILE_POWER:

  1206. 		payload_POWER(cfg, payload, len);

  1207. 		break;

  1208. 	  case PROFILE_STRIDE:

  1209. 		payload_STRIDE(cfg, payload, len);

  1210. 		break;

  1211. 	  case PROFILE_SPEED:

  1212. 		payload_SPEED(cfg, payload, len);

  1213. 		break;

  1214. 	  case PROFILE_CADENCE:

  1215. 		payload_CADENCE(cfg, payload, len);

  1216. 		break;

  1217. 	}

  1218. }

  1219. 

  1220. const uint8_t * AntPlus::getAntKey(const uint8_t keyIdx)

  1221. {

  1222. 	if (keyIdx >= KEY_TOTAL) return NULL;

  1223. 	return antkeys[keyIdx];

  1224. }

  1225. 

  1226. 

  1227. #define WHEEL_CIRCUMFERENCE                     2122

  1228. 

  1229. 

  1230. typedef struct {

  1231.         struct {

  1232.                 uint16_t time;

  1233.                 uint16_t interval;

  1234.                 uint8_t bpm;                            // heart rate in beats per minute

  1235.                 uint8_t sequence;

  1236.         }current;

  1237. 

  1238.         struct {

  1239.                 uint8_t bpm;

  1240.                 uint8_t sequence;

  1241.                 uint16_t time;

  1242.                 uint16_t interval;

  1243.         }previous;

  1244. }payload_HRM_t;

  1245. 

  1246. typedef struct {

  1247.         struct {

  1248.                 uint16_t cadenceTime;

  1249.                 uint16_t cadence;

  1250.                 uint16_t cadenceCt;

  1251.                 uint16_t speedTime;

  1252.                 uint16_t speed;

  1253.                 uint16_t speedCt;

  1254.                 uint32_t distance;

  1255.         }current;

  1256. 

  1257.         struct {

  1258.                 uint16_t cadenceTime;

  1259.                 uint16_t cadence;

  1260.                 uint16_t cadenceCt;

  1261.                 uint16_t speedTime;

  1262.                 uint16_t speed;

  1263.                 uint16_t speedCt;

  1264.                 uint32_t distance;

  1265.         }previous;

  1266. 

  1267.         uint16_t wheelCircumference;            // default is WHEEL_CIRCUMFERENCE (2122cm)

  1268.         uint8_t spdChange;

  1269.         uint8_t cadChange;

  1270. }payload_SPDCAD_t;

  1271. 

  1272. 

  1273. 

  1274. typedef struct {

  1275.         struct {

  1276.         	uint8_t sequence;

  1277.         	uint16_t pedalPowerContribution;

  1278.         	uint8_t pedalPower;

  1279.         	uint8_t instantCadence;

  1280.         	uint16_t sumPower;

  1281.         	uint16_t instantPower;

  1282.         } current;

  1283.         struct {

  1284.                 uint16_t stub;

  1285.         } previous;

  1286. }payload_POWER_t;

  1287. 

  1288. typedef struct {

  1289.         struct {

  1290.                 uint16_t speed;

  1291.                 uint16_t cadence;

  1292.                 uint8_t strides;

  1293.         }current;

  1294. 

  1295.         struct {

  1296.                 uint8_t strides;

  1297.                 uint16_t speed;

  1298.                 uint16_t cadence;

  1299.         }previous;

  1300. }payload_STRIDE_t;

  1301. 

  1302. typedef struct {

  1303.         struct {

  1304.                 uint16_t speedTime;

  1305.                 uint16_t speed;

  1306.                 uint16_t speedCt;

  1307.                 uint32_t distance;

  1308.         }current;

  1309. 

  1310.         struct {

  1311.                 uint16_t speedTime;

  1312.                 uint16_t speed;

  1313.                 uint16_t speedCt;

  1314.                 uint32_t distance;

  1315.         }previous;

  1316. 

  1317.         uint16_t wheelCircumference;            // default is WHEEL_CIRCUMFERENCE (2122cm)

  1318.         uint8_t spdChange;

  1319. }payload_SPEED_t;

  1320. 

  1321. typedef struct {

  1322.         struct {

  1323.                 uint16_t cadenceTime;

  1324.                 uint16_t cadence;

  1325.                 uint16_t cadenceCt;

  1326.         }current;

  1327. 

  1328.         struct {

  1329.                 uint16_t cadenceTime;

  1330.                 uint16_t cadence;

  1331.                 uint16_t cadenceCt;

  1332.         }previous;

  1333. 

  1334.         uint8_t cadChange;

  1335. }payload_CADENCE_t;

  1336. 

  1337. 

  1338. 

  1339. typedef struct {

  1340.         payload_HRM_t           hrm;

  1341.         payload_SPDCAD_t        spdcad;

  1342.         payload_POWER_t         power;

  1343.         payload_STRIDE_t        stride;

  1344.         payload_SPEED_t         spd;

  1345.         payload_CADENCE_t       cad;

  1346. }payload_genstorage_t;

  1347. 

  1348. static payload_genstorage_t payload; // FIXME: should be in the AntPlus object

  1349. 

  1350. 

  1351. 

  1352. void AntPlus::payload_HRM(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)

  1353. {

  1354. 	payload_HRM_t *hrm = &payload.hrm;

  1355. 	hrm->current.bpm = data[STREAM_RXBROADCAST_DEV120_HR];

  1356. 	hrm->current.sequence = data[STREAM_RXBROADCAST_DEV120_SEQ];

  1357. 

  1358. 	//const int page = data[1]&0x0F;

  1359. 	if (hrm->previous.sequence != hrm->current.sequence || hrm->previous.bpm != hrm->current.bpm){

  1360. 		if (hrm->current.bpm){

  1361. 			hrm->current.time = (data[STREAM_RXBROADCAST_DEV120_BEATLO] + (data[STREAM_RXBROADCAST_DEV120_BEATHI] << 8));

  1362. 			hrm->current.interval = hrm->current.time - hrm->previous.time;

  1363. 			hrm->current.interval = (((int32_t)hrm->current.interval) * (int32_t)1000) / (int32_t)1024;

  1364. 

  1365. 			//printf("page %i", page);

  1366. 			sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)hrm, PROFILE_HRM);

  1367. 

  1368. 			hrm->previous.time = hrm->current.time;

  1369. 			hrm->previous.interval = hrm->current.interval;

  1370. 			hrm->previous.sequence = hrm->current.sequence;

  1371. 			hrm->previous.bpm = hrm->current.bpm;

  1372. 		}

  1373. 	}

  1374. 

  1375. 	//int page = data[1]&0x0F;

  1376. 	//printf("payload_HRM: page:%i, Sequence:%i, BPM:%i, %i %i", page, hrm->current.sequence, hrm->current.bpm, hrm->current.time, hrm->current.interval);

  1377. }

  1378. 

  1379. void AntPlus::payload_SPDCAD(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)

  1380. {

  1381. 	payload_SPDCAD_t *spdcad = &payload.spdcad;

  1382. 

  1383. 	spdcad->current.cadenceTime = data[1];

  1384. 	spdcad->current.cadenceTime |= (data[2] << 8);

  1385. 	spdcad->current.cadenceCt = data[3];

  1386. 	spdcad->current.cadenceCt |= (data[4] << 8);

  1387. 	spdcad->current.speedTime = data[5];

  1388. 	spdcad->current.speedTime |= (data[6] << 8);

  1389. 	spdcad->current.speedCt = data[7];

  1390. 	spdcad->current.speedCt |= (data[8] << 8);

  1391. 

  1392. 	spdcad->cadChange = (spdcad->current.cadenceTime != spdcad->previous.cadenceTime || spdcad->current.cadenceCt != spdcad->previous.cadenceCt);

  1393. 	spdcad->spdChange = (spdcad->current.speedTime != spdcad->previous.speedTime || spdcad->current.speedCt != spdcad->previous.speedCt);

  1394. 

  1395. 	if (spdcad->cadChange || spdcad->spdChange){

  1396. 		uint16_t cadence = (60 * (spdcad->current.cadenceCt - spdcad->previous.cadenceCt) * 1024) / (spdcad->current.cadenceTime - spdcad->previous.cadenceTime);

  1397. 		spdcad->current.cadence = cadence;

  1398. 

  1399. 		if (!spdcad->wheelCircumference) spdcad->wheelCircumference = WHEEL_CIRCUMFERENCE;

  1400. 		uint32_t speedRotationDelta = spdcad->current.speedCt - spdcad->previous.speedCt;	// number wheel revolutions

  1401. 		float speedTimeDelta = (float)(spdcad->current.speedTime - spdcad->previous.speedTime) / 1024.0f;	// time for above revolutions

  1402. 		float distance = (speedRotationDelta * (float)spdcad->wheelCircumference) / 1000.0f;		// calculated distance (meters) travelled as per above

  1403. 		float speed = (distance / (speedTimeDelta / 3600.0f)) / 1000.0f;		// its why we're here

  1404. 		spdcad->current.speed = speed * 100;

  1405. 		spdcad->current.distance += distance;

  1406. 

  1407. 		sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)spdcad, PROFILE_SPDCAD);

  1408. 

  1409. 		spdcad->previous.cadenceTime = spdcad->current.cadenceTime;

  1410. 		spdcad->previous.cadence = spdcad->current.cadence;

  1411. 		spdcad->previous.cadenceCt = spdcad->current.cadenceCt;

  1412. 

  1413. 		spdcad->previous.speedTime = spdcad->current.speedTime;

  1414. 		spdcad->previous.speedCt = spdcad->current.speedCt;

  1415. 		spdcad->previous.speed = spdcad->current.speed;

  1416. 		spdcad->previous.distance = spdcad->current.distance;

  1417. 

  1418. 		//printf("payload_SPDCAD: speed: %.2f, cadence: %i, total distance: %.2f", spdcad->current.speed/100.0f, spdcad->current.cadence, spdcad->current.distance/1000.0f);

  1419. 	}

  1420. }

  1421. 

  1422. void AntPlus::payload_POWER(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)

  1423. {

  1424. 	//printf("payload_POWER: len:%i", dataLength);

  1425. 	payload_POWER_t *pwr = &payload.power;

  1426. /*

  1427. 	uint8_t eventCount = data[2];

  1428. 	uint8_t pedalPowerContribution = ((data[3] != 0xFF) && (data[3]&0x80)) ; // left/right is defined if NOT 0xFF (= no Pedal Power) AND BIT 7 is set

  1429. 	uint8_t pedalPower = (data[3]&0x7F); // right pedalPower % - stored in bit 0-6

  1430. 	uint8_t instantCadence = data[4];

  1431. 	uint16_t sumPower = data[5] + (data[6]<<8);

  1432. 	uint16_t instantPower = data[7] + (data[8]<<8);

  1433.         */

  1434. 	sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)pwr, PROFILE_POWER);

  1435. }

  1436. 

  1437. void AntPlus::payload_STRIDE(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)

  1438. {

  1439. 	//printf("payload_STRIDE: len:%i", dataLength);

  1440. 	payload_STRIDE_t *stride = &payload.stride;

  1441. 

  1442. 

  1443. 	int page = data[1];

  1444. 	if (page == 0){

  1445. 		stride->current.strides = data[7];

  1446. 		sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)stride, PROFILE_STRIDE);

  1447. 		stride->previous.strides = stride->current.strides;

  1448. 

  1449. 	}else if (page == 1){

  1450. 		stride->current.speed = ((float)(data[4]&0x0f) + (float)(data[5]/256.0f));

  1451. 		stride->current.cadence = ((float)data[3] + (float)((data[4] << 4) / 16.0f));

  1452. 

  1453. 		sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)stride, PROFILE_STRIDE);

  1454. 

  1455. 		stride->previous.speed = stride->current.speed;

  1456. 		stride->previous.cadence = stride->current.cadence;

  1457. 	}

  1458. 

  1459. }

  1460. 

  1461. void AntPlus::payload_SPEED(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)

  1462. {

  1463. 	//printf("payload_SPEED: len:%i", dataLength);

  1464. 	payload_SPEED_t *spd = &payload.spd;

  1465. 

  1466. 	spd->current.speedTime = data[5];

  1467. 	spd->current.speedTime |= (data[6] << 8);

  1468. 	spd->current.speedCt = data[7];

  1469. 	spd->current.speedCt |= (data[8] << 8);

  1470. 

  1471. 	spd->spdChange = (spd->current.speedTime != spd->previous.speedTime || spd->current.speedCt != spd->previous.speedCt);

  1472. 

  1473. 	if (spd->spdChange){

  1474. 		uint32_t speedRotationDelta = spd->current.speedCt - spd->previous.speedCt;	// number wheel revolutions

  1475. 		float speedTimeDelta = (float)(spd->current.speedTime - spd->previous.speedTime) / 1024.0f;	// time for above revolutions

  1476. 		if (!spd->wheelCircumference) spd->wheelCircumference = WHEEL_CIRCUMFERENCE;

  1477. 		float distance = (speedRotationDelta * (float)spd->wheelCircumference) / 1000.0f;		// calculated distance (meters) travelled as per above

  1478. 		float speed = (distance / (speedTimeDelta / 3600.0f)) / 1000.0f;		// its why we're here

  1479. 		spd->current.speed = speed * 100;

  1480. 		spd->current.distance += distance;

  1481. 

  1482. 		sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)spd, PROFILE_SPEED);

  1483. 

  1484. 		spd->previous.speedTime = spd->current.speedTime;

  1485. 		spd->previous.speedCt = spd->current.speedCt;

  1486. 		spd->previous.speed = spd->current.speed;

  1487. 		spd->previous.distance = spd->current.distance;

  1488. 	}

  1489. }

  1490. 

  1491. void AntPlus::payload_CADENCE(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)

  1492. {

  1493. 	//printf("payload_CADENCE: len:%i", dataLength);

  1494. 	payload_CADENCE_t *cad = &payload.cad;

  1495. 

  1496. 	cad->current.cadenceTime = data[5];

  1497. 	cad->current.cadenceTime |= (data[6] << 8);

  1498. 	cad->current.cadenceCt = data[7];

  1499. 	cad->current.cadenceCt |= (data[8] << 8);

  1500. 

  1501. 	cad->cadChange = (cad->current.cadenceTime != cad->previous.cadenceTime || cad->current.cadenceCt != cad->previous.cadenceCt);

  1502. 

  1503. 	if (cad->cadChange){

  1504. 		uint16_t cadence = (60 * (cad->current.cadenceCt - cad->previous.cadenceCt) * 1024) / (cad->current.cadenceTime - cad->previous.cadenceTime);

  1505. 		cad->current.cadence = cadence;

  1506. 

  1507. 		sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)cad, PROFILE_CADENCE);

  1508. 

  1509. 		cad->previous.cadenceTime = cad->current.cadenceTime;

  1510. 		cad->previous.cadence = cad->current.cadence;

  1511. 		cad->previous.cadenceCt = cad->current.cadenceCt;

  1512. 	}

  1513. }

  1514. 

  1515. 

  1516.