visus
/
teensy-usbhost
Seguir 1
Destacar 0
Fork 0
Código Incidencias 0 Pull Requests 0 Lanzamientos 0 Wiki Actividad
No puede seleccionar más de 25 temas Los temas deben comenzar con una letra o número, pueden incluir guiones ('-') y pueden tener hasta 35 caracteres de largo.
122 Commits
1 Rama
Árbol: 57ca257e9b
Ramas Etiquetas
${ item.name }
Crear rama ${ searchTerm }
desde '57ca257e9b'
${ noResults }
teensy-usbhost/antplus.cpp

1517 líneas
43KB
Original Blame Histórico 

  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. 	driver_ready_for_device(this);

  57. 	callbackFunc = NULL;

  58. }

  59. 

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

  61. {

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

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

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

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

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

  67. 	rxpipe = txpipe = NULL;

  68. 	const uint8_t *p = descriptors;

  69. 	const uint8_t *end = p + len;

  70. 	int descriptorLength = p[0];

  71. 	int descriptorType = p[1];

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

  73. 	p += descriptorLength;

  74. 	while (p < end) {

  75. 		descriptorLength = p[0];

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

  77. 		descriptorType = p[1];

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

  79. 			uint8_t epAddr = p[2];

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

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

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

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

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

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

  86. 			}

  87. 		}

  88. 		p += descriptorLength;

  89. 	}

  90. 	if (rxpipe && txpipe) {

  91. 		rxpipe->callback_function = rx_callback;

  92. 		txpipe->callback_function = tx_callback;

  93. 		txhead = 0;

  94. 		txtail = 0;

  95. 		//rxhead = 0;

  96. 		//rxtail = 0;

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

  98. 		first_update = true;

  99. 		txready = true;

  100. 		updatetimer.start(500000);

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

  102. 		rxlen = 0;

  103. 		return true;

  104. 	}

  105. 	return false;

  106. }

  107. 

  108. void AntPlus::disconnect()

  109. {

  110. 	updatetimer.stop();

  111. 	//txtimer.stop();

  112. }

  113. 

  114. 

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

  116. {

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

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

  119. }

  120. 

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

  122. {

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

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

  125. }

  126. 

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

  128. {

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

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

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

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

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

  134. 		rxlen = 0;

  135. 	} else {

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

  137. 	}

  138. }

  139. 

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

  141. {

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

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

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

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

  146. 	uint32_t tail = txtail;

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

  148. 	txtail = tail;

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

  150. 	txready = true;

  151. 	transmit();

  152. 	//txtimer.start(8000);

  153. 	// adjust tail...

  154. 	// start timer if more data to send

  155. }

  156. 

  157. 

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

  159. {

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

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

  162. 	//print_hexbytes(data, size);

  163. 	if (size > 64) return 0;

  164. 	uint32_t head = txhead;

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

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

  167. 	if (remain < size + 1) {

  168. 		// not enough space at end of buffer

  169. 		txbuffer[head] = 0xFF;

  170. 		head = 0;

  171. 	}

  172. 	uint32_t avail;

  173. 	do {

  174. 		uint32_t tail = txtail;

  175. 		if (head > tail) {

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

  177. 		} else {

  178. 			avail = tail - head;

  179. 		}

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

  181. 	txbuffer[head] = size;

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

  183. 	txhead = head + size;

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

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

  186. 	//print_hexbytes(txbuffer, 60);

  187. 	NVIC_DISABLE_IRQ(IRQ_USBHS);

  188. 	transmit();

  189. 	NVIC_ENABLE_IRQ(IRQ_USBHS);

  190. 	return size;

  191. }

  192. 

  193. void AntPlus::transmit()

  194. {

  195. 	if (!txready) return;

  196. 	uint32_t head = txhead;

  197. 	uint32_t tail = txtail;

  198. 	if (head == tail) {

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

  200. 		return; // no data to transit

  201. 	}

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

  203. 	uint32_t size = txbuffer[tail];

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

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

  206. 	if (size == 0xFF) {

  207. 		txtail = 0;

  208. 		tail = 0;

  209. 		size = txbuffer[0];

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

  211. 	}

  212. 	//txtail = tail + size;

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

  214. 	//txtimer.start(8000);

  215. 	txready = false;

  216. }

  217. 

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

  219. {

  220. 	if (whichTimer == &updatetimer) {

  221. 		updatetimer.start(250000);

  222. 		if (first_update) {

  223. 			ResetSystem();

  224. 			first_update = false;

  225. 		}

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

  227. 	}

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

  229. 		println("ant tx timer");

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

  231. 		txready = true;

  232. 		transmit();

  233. 	} */

  234. }

  235. 

  236. 

  237. void AntPlus::Task()

  238. {

  239. 	uint32_t len = rxlen;

  240. 	if (len) {

  241. 		handleMessages(rxpacket, len);

  242. 		NVIC_DISABLE_IRQ(IRQ_USBHS);

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

  244. 		rxlen = 0;

  245. 		NVIC_ENABLE_IRQ(IRQ_USBHS);

  246. 	}

  247. 

  248. 

  249. 

  250. }

  251. 

  252. 

  253. 

  254. 

  255. 

  256. 

  257. enum _akeys {

  258. 	KEY_ANTSPORT,

  259. 	KEY_SUUNTO,

  260. 	KEY_GARMIN,

  261. 	KEY_ANTPLUS,

  262. 	KEY_TOTAL,

  263. 	KEY_DEFAULT = KEY_ANTSPORT

  264. };

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

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

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

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

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

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

  271. };

  272. 

  273. 

  274. 

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

  276. {

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

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

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

  280. }*/

  281. 

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

  283. {

  284. 	uint8_t checksum = 0x00;

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

  286. 		checksum ^= buffer[i];

  287. 	return checksum;

  288. }

  289. 

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

  291. {

  292. 	// find and sync with input stream

  293. 	*pos = 0;

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

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

  296. 			return stream + *pos;

  297. 		(*pos)++;

  298. 	}

  299. 	return NULL;

  300. }

  301. 

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

  303. {

  304. 	// min message length is 5

  305. 	if (len < 5) return 0;

  306. 

  307. 	int crc = stream[STREAM_SYNC];

  308. 	crc ^= stream[STREAM_LENGTH];

  309. 	crc ^= stream[STREAM_MESSAGE];

  310. 	int mlen = 0;

  311. 

  312. 	do{

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

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

  315. 

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

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

  318. }

  319. 

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

  321. {

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

  323. 	return stream[STREAM_LENGTH] + 4;

  324. }

  325. 

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

  327. {

  328. 	int syncOffset = 0;

  329. 	//uint8_t buffer[ANTPLUS_MAXPACKETSIZE];

  330. 	uint8_t *stream = buffer;

  331. 

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

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

  334. 

  335. 	//int tBytes = ANTPLUS_MAXPACKETSIZE;

  336. 

  337. 	while (tBytes > 0){

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

  339. 		if (stream == NULL){

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

  341. 			return 0;

  342. 		}

  343. 		tBytes -= syncOffset;

  344. 

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

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

  347. 			return 0;

  348. 		}

  349. 

  350. 		//we have a valid message

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

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

  353. 			//return 0;

  354. 		//}

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

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

  357. 

  358. 

  359. 

  360. 		int len = msgGetLength(stream);

  361. 		stream += len;

  362. 		tBytes -= len;

  363. 	}

  364. 	return 1;

  365. }

  366. 

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

  368. {

  369. 	if (which < EVENTI_TOTAL) {

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

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

  372. 		return 1;

  373. 	}

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

  375. 	return 0;

  376. }*/

  377. 

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

  379. {

  380. 	if (profile < PROFILE_TOTAL) {

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

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

  383. 		return 1;

  384. 	}

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

  386. 	return 0;

  387. }*/

  388. 

  389. 

  390. 

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

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

  393. {

  394. 	callbackFunc = func;

  395. }

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

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

  398. {

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

  400. }

  401. 

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

  403. {

  404. 	cfg->flags.channelStatus = channelStatus;

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

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

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

  408. 		sendMessage(ANTP_MSG_CHANNELSTATUS, NULL, status);

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

  410. 	}

  411. }

  412. 

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

  414. 	const uint8_t *payload, const size_t dataLength)

  415. {

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

  417. 	//dump_hexbytes(payload, dataLength);

  418. 

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

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

  421. 

  422. 	switch (eventId){

  423. 	  case EVENT_RX_SEARCH_TIMEOUT:

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

  425. 	  	break;

  426. 

  427. 	  case EVENT_RX_FAIL:

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

  429. 	  	break;

  430. 

  431. 	  case EVENT_TX:

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

  433. 	  	break;

  434. 

  435. 	  case EVENT_RX_BROADCAST:

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

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

  438. 	  		cfg->flags.chanIdOnce = 1;

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

  440. 	  	}

  441. 		//dump_hexbytes(payload, dataLength);

  442. 		dispatchPayload(cfg, payload, dataLength);

  443. 		break;

  444. 	 }

  445. }

  446. 

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

  448. 	const uint8_t *payload, const size_t dataLength)

  449. {

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

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

  452. 

  453. 	switch (msgId){

  454. 	  case MESG_EVENT_ID:

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

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

  457. 	  	break;

  458. 

  459. 	  case MESG_NETWORK_KEY_ID:

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

  461. 	  	cfg->flags.keyAccepted = 1;

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

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

  464. 	  	else

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

  466. 	  	break;

  467. 

  468. 	  case MESG_ASSIGN_CHANNEL_ID:

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

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

  471. 	  	break;

  472. 

  473. 	  case MESG_CHANNEL_MESG_PERIOD_ID:

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

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

  476. 		break;

  477. 

  478. 	  case MESG_CHANNEL_SEARCH_TIMEOUT_ID:

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

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

  481. 	  	break;

  482. 

  483. 	  case MESG_CHANNEL_RADIO_FREQ_ID:

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

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

  486. 	  	break;

  487. 

  488. 	  case MESG_SEARCH_WAVEFORM_ID:

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

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

  491. 	  	break;

  492. 

  493. 	  case MESG_CHANNEL_ID_ID:

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

  495. 	  	OpenChannel(cfg->channel);

  496. 	  	break;

  497. 

  498. 	  case MESG_OPEN_CHANNEL_ID:

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

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

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

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

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

  504. 	  	break;

  505. 

  506.  	  case MESG_UNASSIGN_CHANNEL_ID:

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

  508. 		break;

  509. 

  510. 	  case MESG_CLOSE_CHANNEL_ID:

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

  512. 		cfg->flags.keyAccepted = 0;

  513. 		sendMessageChannelStatus(cfg, ANT_CHANNEL_STATUS_UNASSIGNED);

  514. 		break;

  515. 

  516.  	  case CHANNEL_IN_WRONG_STATE:

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

  518. 		break;

  519. 

  520.  	  case CHANNEL_NOT_OPENED:

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

  522. 		break;

  523. 

  524.  	  case CHANNEL_ID_NOT_SET: //??

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

  526. 		break;

  527. 

  528.  	  case CLOSE_ALL_CHANNELS: // Start RX Scan mode

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

  530. 		break;

  531. 

  532.  	  case TRANSFER_IN_PROGRESS: // TO ack message ID

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

  534. 		break;

  535. 

  536.  	  case TRANSFER_SEQUENCE_NUMBER_ERROR:

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

  538. 		break;

  539. 

  540.  	  case TRANSFER_IN_ERROR:

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

  542. 		break;

  543. 

  544.  	  case INVALID_MESSAGE:

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

  546. 		break;

  547. 

  548.  	  case INVALID_NETWORK_NUMBER:

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

  550. 		break;

  551. 

  552.  	  case INVALID_LIST_ID:

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

  554. 		break;

  555. 

  556.  	  case INVALID_SCAN_TX_CHANNEL:

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

  558. 		break;

  559. 

  560.  	  case INVALID_PARAMETER_PROVIDED:

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

  562.    		break;

  563. 

  564.  	  case EVENT_QUE_OVERFLOW:

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

  566. 		break;

  567. 

  568. 	  default:

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

  570. 		;

  571. 	};

  572. }

  573. 

  574. 

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

  576. 	const uint8_t *payload, const size_t dataLength)

  577. {

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

  579. 	//dump_hexbytes(payload, dataLength);

  580. 

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

  582. 	uint8_t chan = 0;

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

  584. 

  585. 	switch(msgId) {

  586. 	  case MESG_BROADCAST_DATA_ID:

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

  588. 		//dumpPayload(payload, dataLength);

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

  590. 	  	break;

  591. 

  592. 	  case MESG_STARTUP_MESG_ID:

  593. 	  	// reason == ANT_STARTUP_RESET_xxxx

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

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

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

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

  598. 		break;

  599. 

  600. 	  case MESG_RESPONSE_EVENT_ID:

  601. 	  	message_response(payload[STREAM_EVENT_CHANNEL_ID],

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

  603. 	  	break;

  604. 

  605. 	  case MESG_CHANNEL_STATUS_ID:

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

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

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

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

  610. 			payload[STREAM_CHANNELSTATUS_STATUS] & ANT_CHANNEL_STATUS_MASK);

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

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

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

  614. 	  	break;

  615. 

  616. 	  case MESG_CAPABILITIES_ID:

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

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

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

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

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

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

  623. 	  	break;

  624. 

  625. 	case MESG_CHANNEL_ID_ID:

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

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

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

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

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

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

  632. #if 0

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

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

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

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

  637.          		return;

  638.          	}

  639. #endif

  640. 		break;

  641. 

  642. 	case MESG_VERSION_ID:

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

  644. 		break;

  645. 	};

  646. }

  647. 

  648. 

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

  650. //{

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

  652. //}

  653. 

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

  655. //{

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

  657. //}

  658. 

  659. int AntPlus::ResetSystem()

  660. {

  661. 	//println("libantplus_ResetSystem");

  662. 	uint8_t msg[5];

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

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

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

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

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

  668. 	return write(msg, 5);

  669. }

  670. 

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

  672. {

  673. 	//println("libantplus_RequestMessage");

  674. 	uint8_t msg[6];

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

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

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

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

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

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

  681. 	return write(msg, 6);

  682. }

  683. 

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

  685. {

  686. 	uint8_t msg[13];

  687. 	msg[0] = MESG_TX_SYNC;

  688. 	msg[1] = 9;

  689. 	msg[2] = MESG_NETWORK_KEY_ID;

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

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

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

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

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

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

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

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

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

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

  700. 	return write(msg, 13);

  701. }

  702. 

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

  704. {

  705. 	uint8_t msg[6];

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

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

  708. 	msg[2] = MESG_CHANNEL_SEARCH_TIMEOUT_ID;

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

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

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

  712. 	return write(msg, 6);

  713. }

  714. 

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

  716. {

  717. 	uint8_t msg[7];

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

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

  720. 	msg[2] = MESG_CHANNEL_MESG_PERIOD_ID;

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

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

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

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

  725. 	return write(msg, 7);

  726. }

  727. 

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

  729. {

  730. 	uint8_t msg[6];

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

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

  733. 	msg[2] = MESG_CHANNEL_RADIO_FREQ_ID;

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

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

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

  737. 	return write(msg, 6);

  738. }

  739. 

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

  741. {

  742. 	uint8_t msg[7];

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

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

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

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

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

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

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

  750. 	return write(msg, 7);

  751. }

  752. 

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

  754. {

  755. 	uint8_t msg[5];

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

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

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

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

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

  761. 	return write(msg, 5);

  762. }

  763. 

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

  765. {

  766. 	uint8_t msg[5];

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

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

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

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

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

  772. 	return write(msg, 5);

  773. }

  774. 

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

  776. {

  777. 	uint8_t msg[7];

  778. 	msg[0] = MESG_TX_SYNC;

  779. 	msg[1] = 3;

  780. 	msg[2] = MESG_ASSIGN_CHANNEL_ID;

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

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

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

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

  785. 	return write(msg, 7);

  786. }

  787. 

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

  789. {

  790. 	uint8_t msg[9];

  791. 	msg[0] = MESG_TX_SYNC;

  792. 	msg[1] = 5;

  793. 	msg[2] = MESG_CHANNEL_ID_ID;

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

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

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

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

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

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

  800. 	return write(msg, 9);

  801. }

  802. 

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

  804. {

  805. 	uint8_t msg[13];

  806. 	msg[0] = MESG_TX_SYNC;

  807. 	msg[1] = 9;

  808. 	msg[2] = MESG_BURST_DATA_ID;

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

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

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

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

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

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

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

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

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

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

  819. 	return write(msg, 13);

  820. }

  821. 

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

  823. {

  824. 	int ret = 0;

  825. 	int seq = 0;

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

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

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

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

  830. 	}

  831. 	return ret;

  832. }

  833. 

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

  835. {

  836. 	uint8_t msg[13];

  837. 	msg[0] = MESG_TX_SYNC;

  838. 	msg[1] = 9;

  839. 	msg[2] = MESG_BROADCAST_DATA_ID;

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

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

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

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

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

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

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

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

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

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

  850. 	return write(msg, 13);

  851. }

  852. 

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

  854. {

  855. 	uint8_t msg[13];

  856. 	msg[0] = MESG_TX_SYNC;

  857. 	msg[1] = 9;

  858. 	msg[2] = MESG_ACKNOWLEDGED_DATA_ID;

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

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

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

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

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

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

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

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

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

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

  869. 	return write(msg, 13);

  870. }

  871. 

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

  873. {

  874. 	uint8_t msg[17];

  875. 	msg[0] = MESG_TX_SYNC;

  876. 	msg[1] = 13;

  877. 	msg[2] = MESG_EXT_ACKNOWLEDGED_DATA_ID;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  892. 	return write(msg, 17);

  893. }

  894. 

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

  896. {

  897. 	uint8_t msg[17];

  898. 	msg[0] = MESG_TX_SYNC;

  899. 	msg[1] = 13;

  900. 	msg[2] = MESG_EXT_BROADCAST_DATA_ID;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  915. 	return write(msg, 17);

  916. }

  917. 

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

  919. 	const int TranType, const uint8_t *data)

  920. {

  921. 	uint8_t msg[17];

  922. 	msg[0] = MESG_TX_SYNC;

  923. 	msg[1] = 13;

  924. 	msg[2] = MESG_EXT_BROADCAST_DATA_ID;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  939. 	return write(msg, 17);

  940. }

  941. 

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

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

  944. {

  945. 	int ret = 0;

  946. 	int seq = 0;

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

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

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

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

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

  952. 	}

  953. 	return ret;

  954. }

  955. 

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

  957. //{

  958. 	//return antplus_getAntKey(keyIdx);

  959. //}

  960. 

  961. 

  962. 

  963. 

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

  965. {

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

  967. 	cfg->deviceType = ANT_DEVICE_HRM;

  968. 	cfg->transType = ANT_TRANSMISSION_SLAVE;

  969. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  970. 	cfg->networkNumber = 0;

  971. 	cfg->channel = PROFILE_HRM;

  972. 	cfg->channelPeriod = ANT_PERIOD_HRM;

  973. 	cfg->RFFreq = ANT_FREQUENCY_SPORT;

  974. 	cfg->searchTimeout = 255;

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

  976. 	//cfg->keyIdx = KEY_ANTSPORT;

  977. 	cfg->flags.chanIdOnce = 0;

  978. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

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

  980. 	cfg->flags.keyAccepted = 0;

  981. 	cfg->flags.profileValid = 1;

  982. }

  983. 

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

  985. {

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

  987. 	cfg->deviceType = ANT_DEVICE_SPDCAD;

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

  989. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  990. 	cfg->networkNumber = 0;

  991. 	cfg->channel = PROFILE_SPDCAD;

  992. 	cfg->channelPeriod = ANT_PERIOD_SPDCAD;

  993. 	cfg->RFFreq = ANT_FREQUENCY_SPORT;

  994. 	cfg->searchTimeout = 255;

  995. 	cfg->searchWaveform = 0x53;

  996. 	//cfg->keyIdx = KEY_ANTSPORT;

  997. 	cfg->flags.chanIdOnce = 0;

  998. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

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

  1000. 	cfg->flags.keyAccepted = 0;

  1001. 	cfg->flags.profileValid = 1;

  1002. }

  1003. 

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

  1005. {

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

  1007. 	cfg->deviceType = ANT_DEVICE_POWER;

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

  1009. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  1010. 	cfg->networkNumber = 0;

  1011. 	cfg->channel = PROFILE_POWER;

  1012. 	cfg->channelPeriod = ANT_PERIOD_POWER;

  1013. 	cfg->RFFreq = ANT_FREQUENCY_SPORT;

  1014. 	cfg->searchTimeout = 255;

  1015. 	cfg->searchWaveform = 0x53;

  1016. 	//cfg->keyIdx = KEY_ANTSPORT;

  1017. 	cfg->flags.chanIdOnce = 0;

  1018. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

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

  1020. 	cfg->flags.keyAccepted = 0;

  1021. 	cfg->flags.profileValid = 1;

  1022. }

  1023. 

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

  1025. {

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

  1027. 	cfg->deviceType = ANT_DEVICE_STRIDE;

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

  1029. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  1030. 	cfg->networkNumber = 0;

  1031. 	cfg->channel = PROFILE_STRIDE;

  1032. 	cfg->channelPeriod = ANT_PERIOD_STRIDE;

  1033. 	cfg->RFFreq = ANT_FREQUENCY_STRIDE;

  1034. 	cfg->searchTimeout = 255;

  1035. 	cfg->searchWaveform = 0x53;

  1036. 	//cfg->keyIdx = KEY_ANTSPORT;

  1037. 	cfg->flags.chanIdOnce = 0;

  1038. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

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

  1040. 	cfg->flags.keyAccepted = 0;

  1041. 	cfg->flags.profileValid = 1;

  1042. }

  1043. 

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

  1045. {

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

  1047. 	cfg->deviceType = ANT_DEVICE_SPEED;

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

  1049. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  1050. 	cfg->networkNumber = 0;

  1051. 	cfg->channel = PROFILE_SPEED;

  1052. 	cfg->channelPeriod = ANT_PERIOD_SPEED;

  1053. 	cfg->RFFreq = ANT_FREQUENCY_SPORT;

  1054. 	cfg->searchTimeout = 255;

  1055. 	cfg->searchWaveform = 0x53;

  1056. 	//cfg->keyIdx = KEY_ANTSPORT;

  1057. 	cfg->flags.chanIdOnce = 0;

  1058. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

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

  1060. 	cfg->flags.keyAccepted = 0;

  1061. 	cfg->flags.profileValid = 1;

  1062. }

  1063. 

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

  1065. {

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

  1067. 	cfg->deviceType = ANT_DEVICE_CADENCE;

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

  1069. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  1070. 	cfg->networkNumber = 0;

  1071. 	cfg->channel = PROFILE_CADENCE;

  1072. 	cfg->channelPeriod = ANT_PERIOD_CADENCE;

  1073. 	cfg->RFFreq = ANT_FREQUENCY_SPORT;

  1074. 	cfg->searchTimeout = 255;

  1075. 	cfg->searchWaveform = 0x53;

  1076. 	//cfg->keyIdx = KEY_ANTSPORT;

  1077. 	cfg->flags.chanIdOnce = 0;

  1078. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

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

  1080. 	cfg->flags.keyAccepted = 0;

  1081. 	cfg->flags.profileValid = 1;

  1082. }

  1083. 

  1084. #if 0

  1085. 

  1086. /*

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

  1088. {

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

  1090. 

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

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

  1093. 

  1094. 	return n;

  1095. }

  1096. 

  1097. */

  1098. int libantplus_Start ()

  1099. {

  1100. #if 0

  1101. 	uint8_t buffer[8];

  1102. 	factory_passkey(3825666043, buffer);

  1103. 	dump_hexbytes(buffer, 8);

  1104. #endif

  1105. 

  1106. 	int ct = 0;

  1107. 	uint32_t deviceId;

  1108. 

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

  1110. 		deviceId = 0;

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

  1112. 			continue;

  1113. 

  1114. 		ct++;

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

  1116. 

  1117. 		switch (i){

  1118. 		  case PROFILE_HRM:

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

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

  1121. 			break;

  1122. 		  case PROFILE_SPDCAD:

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

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

  1125. 			break;

  1126. 		  case PROFILE_POWER:

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

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

  1129. 			break;

  1130. 		  case PROFILE_STRIDE:

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

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

  1133. 			break;

  1134. 		  case PROFILE_SPEED:

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

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

  1137. 			break;

  1138. 		  case PROFILE_CADENCE:

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

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

  1141. 			break;

  1142. 		}

  1143. 	}

  1144. 

  1145. 	return ct;

  1146. }

  1147. 

  1148. 

  1149. TLIBANTPLUS *libantplus_Init (const uint8_t networkKey)

  1150. {

  1151. 	if (networkKey >= KEY_TOTAL){

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

  1153. 		//return NULL;

  1154. 		ant->key = KEY_DEFAULT;

  1155. 	}else{

  1156. 		ant->key = networkKey;

  1157. 	}

  1158. 

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

  1160. 	return ant;

  1161. }

  1162. #endif

  1163. 

  1164. 

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

  1166. {

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

  1168. 

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

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

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

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

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

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

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

  1176. 

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

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

  1179. }

  1180. 

  1181. /*

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

  1183. {

  1184. 	if (which < EVENTI_TOTAL) {

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

  1186. 

  1187. */

  1188. 

  1189. 

  1190. 

  1191. 

  1192. 

  1193. 

  1194. 

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

  1196. {

  1197. 	switch (cfg->channel) {

  1198. 	  case PROFILE_HRM:

  1199. 		payload_HRM(cfg, payload, len);

  1200. 		break;

  1201. 	  case PROFILE_SPDCAD:

  1202. 		payload_SPDCAD(cfg, payload, len);

  1203. 		break;

  1204. 	  case PROFILE_POWER:

  1205. 		payload_POWER(cfg, payload, len);

  1206. 		break;

  1207. 	  case PROFILE_STRIDE:

  1208. 		payload_STRIDE(cfg, payload, len);

  1209. 		break;

  1210. 	  case PROFILE_SPEED:

  1211. 		payload_SPEED(cfg, payload, len);

  1212. 		break;

  1213. 	  case PROFILE_CADENCE:

  1214. 		payload_CADENCE(cfg, payload, len);

  1215. 		break;

  1216. 	}

  1217. }

  1218. 

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

  1220. {

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

  1222. 	return antkeys[keyIdx];

  1223. }

  1224. 

  1225. 

  1226. #define WHEEL_CIRCUMFERENCE                     2122

  1227. 

  1228. 

  1229. typedef struct {

  1230.         struct {

  1231.                 uint16_t time;

  1232.                 uint16_t interval;

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

  1234.                 uint8_t sequence;

  1235.         }current;

  1236. 

  1237.         struct {

  1238.                 uint8_t bpm;

  1239.                 uint8_t sequence;

  1240.                 uint16_t time;

  1241.                 uint16_t interval;

  1242.         }previous;

  1243. }payload_HRM_t;

  1244. 

  1245. typedef struct {

  1246.         struct {

  1247.                 uint16_t cadenceTime;

  1248.                 uint16_t cadence;

  1249.                 uint16_t cadenceCt;

  1250.                 uint16_t speedTime;

  1251.                 uint16_t speed;

  1252.                 uint16_t speedCt;

  1253.                 uint32_t distance;

  1254.         }current;

  1255. 

  1256.         struct {

  1257.                 uint16_t cadenceTime;

  1258.                 uint16_t cadence;

  1259.                 uint16_t cadenceCt;

  1260.                 uint16_t speedTime;

  1261.                 uint16_t speed;

  1262.                 uint16_t speedCt;

  1263.                 uint32_t distance;

  1264.         }previous;

  1265. 

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

  1267.         uint8_t spdChange;

  1268.         uint8_t cadChange;

  1269. }payload_SPDCAD_t;

  1270. 

  1271. 

  1272. 

  1273. typedef struct {

  1274.         struct {

  1275.         	uint8_t sequence;

  1276.         	uint16_t pedalPowerContribution;

  1277.         	uint8_t pedalPower;

  1278.         	uint8_t instantCadence;

  1279.         	uint16_t sumPower;

  1280.         	uint16_t instantPower;

  1281.         } current;

  1282.         struct {

  1283.                 uint16_t stub;

  1284.         } previous;

  1285. }payload_POWER_t;

  1286. 

  1287. typedef struct {

  1288.         struct {

  1289.                 uint16_t speed;

  1290.                 uint16_t cadence;

  1291.                 uint8_t strides;

  1292.         }current;

  1293. 

  1294.         struct {

  1295.                 uint8_t strides;

  1296.                 uint16_t speed;

  1297.                 uint16_t cadence;

  1298.         }previous;

  1299. }payload_STRIDE_t;

  1300. 

  1301. typedef struct {

  1302.         struct {

  1303.                 uint16_t speedTime;

  1304.                 uint16_t speed;

  1305.                 uint16_t speedCt;

  1306.                 uint32_t distance;

  1307.         }current;

  1308. 

  1309.         struct {

  1310.                 uint16_t speedTime;

  1311.                 uint16_t speed;

  1312.                 uint16_t speedCt;

  1313.                 uint32_t distance;

  1314.         }previous;

  1315. 

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

  1317.         uint8_t spdChange;

  1318. }payload_SPEED_t;

  1319. 

  1320. typedef struct {

  1321.         struct {

  1322.                 uint16_t cadenceTime;

  1323.                 uint16_t cadence;

  1324.                 uint16_t cadenceCt;

  1325.         }current;

  1326. 

  1327.         struct {

  1328.                 uint16_t cadenceTime;

  1329.                 uint16_t cadence;

  1330.                 uint16_t cadenceCt;

  1331.         }previous;

  1332. 

  1333.         uint8_t cadChange;

  1334. }payload_CADENCE_t;

  1335. 

  1336. 

  1337. 

  1338. typedef struct {

  1339.         payload_HRM_t           hrm;

  1340.         payload_SPDCAD_t        spdcad;

  1341.         payload_POWER_t         power;

  1342.         payload_STRIDE_t        stride;

  1343.         payload_SPEED_t         spd;

  1344.         payload_CADENCE_t       cad;

  1345. }payload_genstorage_t;

  1346. 

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

  1348. 

  1349. 

  1350. 

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

  1352. {

  1353. 	payload_HRM_t *hrm = &payload.hrm;

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

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

  1356. 

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

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

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

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

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

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

  1363. 

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

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

  1366. 

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

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

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

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

  1371. 		}

  1372. 	}

  1373. 

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

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

  1376. }

  1377. 

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

  1379. {

  1380. 	payload_SPDCAD_t *spdcad = &payload.spdcad;

  1381. 

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

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

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

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

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

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

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

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

  1390. 

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

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

  1393. 

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

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

  1396. 		spdcad->current.cadence = cadence;

  1397. 

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

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

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

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

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

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

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

  1405. 

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

  1407. 

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

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

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

  1411. 

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

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

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

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

  1416. 

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

  1418. 	}

  1419. }

  1420. 

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

  1422. {

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

  1424. 	payload_POWER_t *pwr = &payload.power;

  1425. /*

  1426. 	uint8_t eventCount = data[2];

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

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

  1429. 	uint8_t instantCadence = data[4];

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

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

  1432.         */

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

  1434. }

  1435. 

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

  1437. {

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

  1439. 	payload_STRIDE_t *stride = &payload.stride;

  1440. 

  1441. 

  1442. 	int page = data[1];

  1443. 	if (page == 0){

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

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

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

  1447. 

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

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

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

  1451. 

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

  1453. 

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

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

  1456. 	}

  1457. 

  1458. }

  1459. 

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

  1461. {

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

  1463. 	payload_SPEED_t *spd = &payload.spd;

  1464. 

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

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

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

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

  1469. 

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

  1471. 

  1472. 	if (spd->spdChange){

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

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

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

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

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

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

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

  1480. 

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

  1482. 

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

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

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

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

  1487. 	}

  1488. }

  1489. 

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

  1491. {

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

  1493. 	payload_CADENCE_t *cad = &payload.cad;

  1494. 

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

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

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

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

  1499. 

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

  1501. 

  1502. 	if (cad->cadChange){

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

  1504. 		cad->current.cadence = cadence;

  1505. 

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

  1507. 

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

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

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

  1511. 	}

  1512. }

  1513. 

  1514. 

  1515.