visus
/
teensy-usbhost
Suivre 1
Ajouter aux favoris 0
Bifurcation 0
Code Tickets 0 Demandes d'ajout 0 Versions 0 Wiki Activité
Vous ne pouvez pas sélectionner plus de 25 sujets Les noms de sujets doivent commencer par une lettre ou un nombre, peuvent contenir des tirets ('-') et peuvent comporter jusqu'à 35 caractères.
125 Révisions
1 Branche
Aborescence: 94b2decbca
Branches Tags
${ item.name }
Créer la branche ${ searchTerm }
de '94b2decbca'
${ noResults }
teensy-usbhost/antplus.cpp

1484 lines
42KB
Brut Annotations Historique 

  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. 		do_polling = false;

  105. 		return true;

  106. 	}

  107. 	return false;

  108. }

  109. 

  110. void AntPlus::disconnect()

  111. {

  112. 	updatetimer.stop();

  113. 	//txtimer.stop();

  114. }

  115. 

  116. 

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

  118. {

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

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

  121. }

  122. 

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

  124. {

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

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

  127. }

  128. 

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

  130. {

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

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

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

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

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

  136. 		rxlen = 0;

  137. 	} else {

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

  139. 		// TODO: should someday use EventResponder to call from yield()

  140. 	}

  141. }

  142. 

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

  144. {

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

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

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

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

  149. 	uint32_t tail = txtail;

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

  151. 	txtail = tail;

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

  153. 	txready = true;

  154. 	transmit();

  155. 	//txtimer.start(8000);

  156. 	// adjust tail...

  157. 	// start timer if more data to send

  158. }

  159. 

  160. 

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

  162. {

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

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

  165. 	//print_hexbytes(data, size);

  166. 	if (size > 64) return 0;

  167. 	uint32_t head = txhead;

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

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

  170. 	if (remain < size + 1) {

  171. 		// not enough space at end of buffer

  172. 		txbuffer[head] = 0xFF;

  173. 		head = 0;

  174. 	}

  175. 	uint32_t avail;

  176. 	do {

  177. 		uint32_t tail = txtail;

  178. 		if (head > tail) {

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

  180. 		} else {

  181. 			avail = tail - head;

  182. 		}

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

  184. 	txbuffer[head] = size;

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

  186. 	txhead = head + size;

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

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

  189. 	//print_hexbytes(txbuffer, 60);

  190. 	NVIC_DISABLE_IRQ(IRQ_USBHS);

  191. 	transmit();

  192. 	NVIC_ENABLE_IRQ(IRQ_USBHS);

  193. 	return size;

  194. }

  195. 

  196. void AntPlus::transmit()

  197. {

  198. 	if (!txready) return;

  199. 	uint32_t head = txhead;

  200. 	uint32_t tail = txtail;

  201. 	if (head == tail) {

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

  203. 		return; // no data to transit

  204. 	}

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

  206. 	uint32_t size = txbuffer[tail];

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

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

  209. 	if (size == 0xFF) {

  210. 		txtail = 0;

  211. 		tail = 0;

  212. 		size = txbuffer[0];

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

  214. 	}

  215. 	//txtail = tail + size;

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

  217. 	//txtimer.start(8000);

  218. 	txready = false;

  219. }

  220. 

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

  222. {

  223. 	if (whichTimer == &updatetimer) {

  224. 		updatetimer.start(250000);

  225. 		if (first_update) {

  226. 			ResetSystem();

  227. 			first_update = false;

  228. 		} else {

  229. 			do_polling = true;

  230. 		}

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

  232. 	}

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

  234. 		println("ant tx timer");

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

  236. 		txready = true;

  237. 		transmit();

  238. 	} */

  239. }

  240. 

  241. 

  242. void AntPlus::Task()

  243. {

  244. 	uint32_t len = rxlen;

  245. 	if (len) {

  246. 		handleMessages(rxpacket, len);

  247. 		NVIC_DISABLE_IRQ(IRQ_USBHS);

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

  249. 		rxlen = 0;

  250. 		NVIC_ENABLE_IRQ(IRQ_USBHS);

  251. 	}

  252. 	if (do_polling) {

  253. 		do_polling = false;

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

  255. 			TDCONFIG *cfg = &ant.dcfg[i];

  256. 			if (!(cfg->flags.profileValid)) continue;

  257. 			//printf("#### %i %i: %i %i %i ####", i, cfg->channel,

  258. 			// cfg->flags.channelStatus, cfg->flags.keyAccepted,

  259. 			// cfg->flags.chanIdOnce);

  260. 			if (cfg->flags.channelStatus) {

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

  262. 			} else {

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

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

  265. 				if (!cfg->flags.keyAccepted && !cfg->flags.chanIdOnce) {

  266. 					SetNetworkKey(cfg->networkNumber, getAntKey(ant.key));

  267. 				}

  268. 			}

  269. 		}

  270. 	}

  271. }

  272. 

  273. 

  274. 

  275. enum _akeys {

  276. 	KEY_ANTSPORT,

  277. 	KEY_SUUNTO,

  278. 	KEY_GARMIN,

  279. 	KEY_ANTPLUS,

  280. 	KEY_TOTAL,

  281. 	KEY_DEFAULT = KEY_ANTSPORT

  282. };

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

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

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

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

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

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

  289. };

  290. 

  291. 

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

  293. {

  294. 	uint8_t checksum = 0x00;

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

  296. 		checksum ^= buffer[i];

  297. 	return checksum;

  298. }

  299. 

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

  301. {

  302. 	// find and sync with input stream

  303. 	*pos = 0;

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

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

  306. 			return stream + *pos;

  307. 		(*pos)++;

  308. 	}

  309. 	return NULL;

  310. }

  311. 

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

  313. {

  314. 	// min message length is 5

  315. 	if (len < 5) return 0;

  316. 

  317. 	int crc = stream[STREAM_SYNC];

  318. 	crc ^= stream[STREAM_LENGTH];

  319. 	crc ^= stream[STREAM_MESSAGE];

  320. 	int mlen = 0;

  321. 

  322. 	do{

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

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

  325. 

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

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

  328. }

  329. 

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

  331. {

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

  333. 	return stream[STREAM_LENGTH] + 4;

  334. }

  335. 

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

  337. {

  338. 	int syncOffset = 0;

  339. 	//uint8_t buffer[ANTPLUS_MAXPACKETSIZE];

  340. 	uint8_t *stream = buffer;

  341. 

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

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

  344. 

  345. 	//int tBytes = ANTPLUS_MAXPACKETSIZE;

  346. 

  347. 	while (tBytes > 0){

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

  349. 		if (stream == NULL){

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

  351. 			return 0;

  352. 		}

  353. 		tBytes -= syncOffset;

  354. 

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

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

  357. 			return 0;

  358. 		}

  359. 

  360. 		//we have a valid message

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

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

  363. 			//return 0;

  364. 		//}

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

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

  367. 

  368. 		int len = msgGetLength(stream);

  369. 		stream += len;

  370. 		tBytes -= len;

  371. 	}

  372. 	return 1;

  373. }

  374. 

  375. 

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

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

  378. {

  379. 	callbackFunc = func;

  380. }

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

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

  383. {

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

  385. }

  386. 

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

  388. {

  389. 	cfg->flags.channelStatus = channelStatus;

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

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

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

  393. 		sendMessage(ANTP_MSG_CHANNELSTATUS, NULL, status);

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

  395. 	}

  396. }

  397. 

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

  399. 	const uint8_t *payload, const size_t dataLength)

  400. {

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

  402. 	//dump_hexbytes(payload, dataLength);

  403. 

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

  405. 

  406. 	switch (eventId){

  407. 	  case EVENT_RX_SEARCH_TIMEOUT:

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

  409. 	  	break;

  410. 

  411. 	  case EVENT_RX_FAIL:

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

  413. 	  	break;

  414. 

  415. 	  case EVENT_TX:

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

  417. 	  	break;

  418. 

  419. 	  case EVENT_RX_BROADCAST:

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

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

  422. 	  		cfg->flags.chanIdOnce = 1;

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

  424. 	  	}

  425. 		//dump_hexbytes(payload, dataLength);

  426. 		dispatchPayload(cfg, payload, dataLength);

  427. 		break;

  428. 	 }

  429. }

  430. 

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

  432. 	const uint8_t *payload, const size_t dataLength)

  433. {

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

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

  436. 

  437. 	switch (msgId){

  438. 	  case MESG_EVENT_ID:

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

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

  441. 	  	break;

  442. 

  443. 	  case MESG_NETWORK_KEY_ID:

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

  445. 	  	cfg->flags.keyAccepted = 1;

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

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

  448. 	  	else

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

  450. 	  	break;

  451. 

  452. 	  case MESG_ASSIGN_CHANNEL_ID:

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

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

  455. 	  	break;

  456. 

  457. 	  case MESG_CHANNEL_MESG_PERIOD_ID:

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

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

  460. 		break;

  461. 

  462. 	  case MESG_CHANNEL_SEARCH_TIMEOUT_ID:

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

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

  465. 	  	break;

  466. 

  467. 	  case MESG_CHANNEL_RADIO_FREQ_ID:

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

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

  470. 	  	break;

  471. 

  472. 	  case MESG_SEARCH_WAVEFORM_ID:

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

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

  475. 	  	break;

  476. 

  477. 	  case MESG_CHANNEL_ID_ID:

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

  479. 	  	OpenChannel(cfg->channel);

  480. 	  	break;

  481. 

  482. 	  case MESG_OPEN_CHANNEL_ID:

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

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

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

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

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

  488. 	  	break;

  489. 

  490.  	  case MESG_UNASSIGN_CHANNEL_ID:

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

  492. 		break;

  493. 

  494. 	  case MESG_CLOSE_CHANNEL_ID:

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

  496. 		cfg->flags.keyAccepted = 0;

  497. 		sendMessageChannelStatus(cfg, ANT_CHANNEL_STATUS_UNASSIGNED);

  498. 		break;

  499. 

  500.  	  case CHANNEL_IN_WRONG_STATE:

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

  502. 		break;

  503. 

  504.  	  case CHANNEL_NOT_OPENED:

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

  506. 		break;

  507. 

  508.  	  case CHANNEL_ID_NOT_SET: //??

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

  510. 		break;

  511. 

  512.  	  case CLOSE_ALL_CHANNELS: // Start RX Scan mode

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

  514. 		break;

  515. 

  516.  	  case TRANSFER_IN_PROGRESS: // TO ack message ID

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

  518. 		break;

  519. 

  520.  	  case TRANSFER_SEQUENCE_NUMBER_ERROR:

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

  522. 		break;

  523. 

  524.  	  case TRANSFER_IN_ERROR:

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

  526. 		break;

  527. 

  528.  	  case INVALID_MESSAGE:

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

  530. 		break;

  531. 

  532.  	  case INVALID_NETWORK_NUMBER:

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

  534. 		break;

  535. 

  536.  	  case INVALID_LIST_ID:

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

  538. 		break;

  539. 

  540.  	  case INVALID_SCAN_TX_CHANNEL:

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

  542. 		break;

  543. 

  544.  	  case INVALID_PARAMETER_PROVIDED:

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

  546.    		break;

  547. 

  548.  	  case EVENT_QUE_OVERFLOW:

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

  550. 		break;

  551. 

  552. 	  default:

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

  554. 		;

  555. 	};

  556. }

  557. 

  558. 

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

  560. 	const uint8_t *payload, const size_t dataLength)

  561. {

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

  563. 	//dump_hexbytes(payload, dataLength);

  564. 

  565. 	uint8_t chan = 0;

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

  567. 

  568. 	switch(msgId) {

  569. 	  case MESG_BROADCAST_DATA_ID:

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

  571. 		//dumpPayload(payload, dataLength);

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

  573. 	  	break;

  574. 

  575. 	  case MESG_STARTUP_MESG_ID:

  576. 	  	// reason == ANT_STARTUP_RESET_xxxx

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

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

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

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

  581. 		break;

  582. 

  583. 	  case MESG_RESPONSE_EVENT_ID:

  584. 	  	message_response(payload[STREAM_EVENT_CHANNEL_ID],

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

  586. 	  	break;

  587. 

  588. 	  case MESG_CHANNEL_STATUS_ID:

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

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

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

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

  593. 			payload[STREAM_CHANNELSTATUS_STATUS] & ANT_CHANNEL_STATUS_MASK);

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

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

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

  597. 	  	break;

  598. 

  599. 	  case MESG_CAPABILITIES_ID:

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

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

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

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

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

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

  606. 	  	break;

  607. 

  608. 	case MESG_CHANNEL_ID_ID:

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

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

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

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

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

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

  615. #if 0

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

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

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

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

  620.          		return;

  621.          	}

  622. #endif

  623. 		break;

  624. 

  625. 	case MESG_VERSION_ID:

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

  627. 		break;

  628. 	};

  629. }

  630. 

  631. 

  632. int AntPlus::ResetSystem()

  633. {

  634. 	//println("libantplus_ResetSystem");

  635. 	uint8_t msg[5];

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

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

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

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

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

  641. 	return write(msg, 5);

  642. }

  643. 

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

  645. {

  646. 	//println("libantplus_RequestMessage");

  647. 	uint8_t msg[6];

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

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

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

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

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

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

  654. 	return write(msg, 6);

  655. }

  656. 

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

  658. {

  659. 	uint8_t msg[13];

  660. 	msg[0] = MESG_TX_SYNC;

  661. 	msg[1] = 9;

  662. 	msg[2] = MESG_NETWORK_KEY_ID;

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

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

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

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

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

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

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

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

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

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

  673. 	return write(msg, 13);

  674. }

  675. 

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

  677. {

  678. 	uint8_t msg[6];

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

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

  681. 	msg[2] = MESG_CHANNEL_SEARCH_TIMEOUT_ID;

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

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

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

  685. 	return write(msg, 6);

  686. }

  687. 

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

  689. {

  690. 	uint8_t msg[7];

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

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

  693. 	msg[2] = MESG_CHANNEL_MESG_PERIOD_ID;

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

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

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

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

  698. 	return write(msg, 7);

  699. }

  700. 

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

  702. {

  703. 	uint8_t msg[6];

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

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

  706. 	msg[2] = MESG_CHANNEL_RADIO_FREQ_ID;

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

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

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

  710. 	return write(msg, 6);

  711. }

  712. 

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

  714. {

  715. 	uint8_t msg[7];

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

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

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

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

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

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

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

  723. 	return write(msg, 7);

  724. }

  725. 

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

  727. {

  728. 	uint8_t msg[5];

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

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

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

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

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

  734. 	return write(msg, 5);

  735. }

  736. 

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

  738. {

  739. 	uint8_t msg[5];

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

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

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

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

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

  745. 	return write(msg, 5);

  746. }

  747. 

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

  749. {

  750. 	uint8_t msg[7];

  751. 	msg[0] = MESG_TX_SYNC;

  752. 	msg[1] = 3;

  753. 	msg[2] = MESG_ASSIGN_CHANNEL_ID;

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

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

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

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

  758. 	return write(msg, 7);

  759. }

  760. 

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

  762. {

  763. 	uint8_t msg[9];

  764. 	msg[0] = MESG_TX_SYNC;

  765. 	msg[1] = 5;

  766. 	msg[2] = MESG_CHANNEL_ID_ID;

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

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

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

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

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

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

  773. 	return write(msg, 9);

  774. }

  775. 

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

  777. {

  778. 	uint8_t msg[13];

  779. 	msg[0] = MESG_TX_SYNC;

  780. 	msg[1] = 9;

  781. 	msg[2] = MESG_BURST_DATA_ID;

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

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

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

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

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

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

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

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

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

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

  792. 	return write(msg, 13);

  793. }

  794. 

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

  796. {

  797. 	int ret = 0;

  798. 	int seq = 0;

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

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

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

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

  803. 	}

  804. 	return ret;

  805. }

  806. 

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

  808. {

  809. 	uint8_t msg[13];

  810. 	msg[0] = MESG_TX_SYNC;

  811. 	msg[1] = 9;

  812. 	msg[2] = MESG_BROADCAST_DATA_ID;

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

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

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

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

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

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

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

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

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

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

  823. 	return write(msg, 13);

  824. }

  825. 

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

  827. {

  828. 	uint8_t msg[13];

  829. 	msg[0] = MESG_TX_SYNC;

  830. 	msg[1] = 9;

  831. 	msg[2] = MESG_ACKNOWLEDGED_DATA_ID;

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

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

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

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

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

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

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

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

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

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

  842. 	return write(msg, 13);

  843. }

  844. 

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

  846. {

  847. 	uint8_t msg[17];

  848. 	msg[0] = MESG_TX_SYNC;

  849. 	msg[1] = 13;

  850. 	msg[2] = MESG_EXT_ACKNOWLEDGED_DATA_ID;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  865. 	return write(msg, 17);

  866. }

  867. 

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

  869. {

  870. 	uint8_t msg[17];

  871. 	msg[0] = MESG_TX_SYNC;

  872. 	msg[1] = 13;

  873. 	msg[2] = MESG_EXT_BROADCAST_DATA_ID;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  888. 	return write(msg, 17);

  889. }

  890. 

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

  892. 	const int TranType, const uint8_t *data)

  893. {

  894. 	uint8_t msg[17];

  895. 	msg[0] = MESG_TX_SYNC;

  896. 	msg[1] = 13;

  897. 	msg[2] = MESG_EXT_BROADCAST_DATA_ID;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  912. 	return write(msg, 17);

  913. }

  914. 

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

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

  917. {

  918. 	int ret = 0;

  919. 	int seq = 0;

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

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

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

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

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

  925. 	}

  926. 	return ret;

  927. }

  928. 

  929. 

  930. 

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

  932. {

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

  934. 	cfg->deviceType = ANT_DEVICE_HRM;

  935. 	cfg->transType = ANT_TRANSMISSION_SLAVE;

  936. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  937. 	cfg->networkNumber = 0;

  938. 	cfg->channel = PROFILE_HRM;

  939. 	cfg->channelPeriod = ANT_PERIOD_HRM;

  940. 	cfg->RFFreq = ANT_FREQUENCY_SPORT;

  941. 	cfg->searchTimeout = 255;

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

  943. 	//cfg->keyIdx = KEY_ANTSPORT;

  944. 	cfg->flags.chanIdOnce = 0;

  945. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

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

  947. 	cfg->flags.keyAccepted = 0;

  948. 	cfg->flags.profileValid = 1;

  949. }

  950. 

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

  952. {

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

  954. 	cfg->deviceType = ANT_DEVICE_SPDCAD;

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

  956. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  957. 	cfg->networkNumber = 0;

  958. 	cfg->channel = PROFILE_SPDCAD;

  959. 	cfg->channelPeriod = ANT_PERIOD_SPDCAD;

  960. 	cfg->RFFreq = ANT_FREQUENCY_SPORT;

  961. 	cfg->searchTimeout = 255;

  962. 	cfg->searchWaveform = 0x53;

  963. 	//cfg->keyIdx = KEY_ANTSPORT;

  964. 	cfg->flags.chanIdOnce = 0;

  965. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

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

  967. 	cfg->flags.keyAccepted = 0;

  968. 	cfg->flags.profileValid = 1;

  969. }

  970. 

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

  972. {

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

  974. 	cfg->deviceType = ANT_DEVICE_POWER;

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

  976. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  977. 	cfg->networkNumber = 0;

  978. 	cfg->channel = PROFILE_POWER;

  979. 	cfg->channelPeriod = ANT_PERIOD_POWER;

  980. 	cfg->RFFreq = ANT_FREQUENCY_SPORT;

  981. 	cfg->searchTimeout = 255;

  982. 	cfg->searchWaveform = 0x53;

  983. 	//cfg->keyIdx = KEY_ANTSPORT;

  984. 	cfg->flags.chanIdOnce = 0;

  985. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

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

  987. 	cfg->flags.keyAccepted = 0;

  988. 	cfg->flags.profileValid = 1;

  989. }

  990. 

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

  992. {

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

  994. 	cfg->deviceType = ANT_DEVICE_STRIDE;

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

  996. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  997. 	cfg->networkNumber = 0;

  998. 	cfg->channel = PROFILE_STRIDE;

  999. 	cfg->channelPeriod = ANT_PERIOD_STRIDE;

  1000. 	cfg->RFFreq = ANT_FREQUENCY_STRIDE;

  1001. 	cfg->searchTimeout = 255;

  1002. 	cfg->searchWaveform = 0x53;

  1003. 	//cfg->keyIdx = KEY_ANTSPORT;

  1004. 	cfg->flags.chanIdOnce = 0;

  1005. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

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

  1007. 	cfg->flags.keyAccepted = 0;

  1008. 	cfg->flags.profileValid = 1;

  1009. }

  1010. 

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

  1012. {

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

  1014. 	cfg->deviceType = ANT_DEVICE_SPEED;

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

  1016. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  1017. 	cfg->networkNumber = 0;

  1018. 	cfg->channel = PROFILE_SPEED;

  1019. 	cfg->channelPeriod = ANT_PERIOD_SPEED;

  1020. 	cfg->RFFreq = ANT_FREQUENCY_SPORT;

  1021. 	cfg->searchTimeout = 255;

  1022. 	cfg->searchWaveform = 0x53;

  1023. 	//cfg->keyIdx = KEY_ANTSPORT;

  1024. 	cfg->flags.chanIdOnce = 0;

  1025. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

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

  1027. 	cfg->flags.keyAccepted = 0;

  1028. 	cfg->flags.profileValid = 1;

  1029. }

  1030. 

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

  1032. {

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

  1034. 	cfg->deviceType = ANT_DEVICE_CADENCE;

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

  1036. 	cfg->channelType = ANT_CHANNEL_TYPE_SLAVE;

  1037. 	cfg->networkNumber = 0;

  1038. 	cfg->channel = PROFILE_CADENCE;

  1039. 	cfg->channelPeriod = ANT_PERIOD_CADENCE;

  1040. 	cfg->RFFreq = ANT_FREQUENCY_SPORT;

  1041. 	cfg->searchTimeout = 255;

  1042. 	cfg->searchWaveform = 0x53;

  1043. 	//cfg->keyIdx = KEY_ANTSPORT;

  1044. 	cfg->flags.chanIdOnce = 0;

  1045. 	cfg->flags.channelStatus = ANT_CHANNEL_STATUS_UNASSIGNED;

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

  1047. 	cfg->flags.keyAccepted = 0;

  1048. 	cfg->flags.profileValid = 1;

  1049. }

  1050. 

  1051. #if 0

  1052. 

  1053. /*

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

  1055. {

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

  1057. 

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

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

  1060. 

  1061. 	return n;

  1062. }

  1063. 

  1064. */

  1065. int libantplus_Start ()

  1066. {

  1067. #if 0

  1068. 	uint8_t buffer[8];

  1069. 	factory_passkey(3825666043, buffer);

  1070. 	dump_hexbytes(buffer, 8);

  1071. #endif

  1072. 

  1073. 	int ct = 0;

  1074. 	uint32_t deviceId;

  1075. 

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

  1077. 		deviceId = 0;

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

  1079. 			continue;

  1080. 

  1081. 		ct++;

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

  1083. 

  1084. 		switch (i){

  1085. 		  case PROFILE_HRM:

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

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

  1088. 			break;

  1089. 		  case PROFILE_SPDCAD:

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

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

  1092. 			break;

  1093. 		  case PROFILE_POWER:

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

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

  1096. 			break;

  1097. 		  case PROFILE_STRIDE:

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

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

  1100. 			break;

  1101. 		  case PROFILE_SPEED:

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

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

  1104. 			break;

  1105. 		  case PROFILE_CADENCE:

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

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

  1108. 			break;

  1109. 		}

  1110. 	}

  1111. 

  1112. 	return ct;

  1113. }

  1114. 

  1115. 

  1116. TLIBANTPLUS *libantplus_Init (const uint8_t networkKey)

  1117. {

  1118. 	if (networkKey >= KEY_TOTAL){

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

  1120. 		//return NULL;

  1121. 		ant->key = KEY_DEFAULT;

  1122. 	}else{

  1123. 		ant->key = networkKey;

  1124. 	}

  1125. 

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

  1127. 	return ant;

  1128. }

  1129. #endif

  1130. 

  1131. 

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

  1133. {

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

  1135. 

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

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

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

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

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

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

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

  1143. 

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

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

  1146. }

  1147. 

  1148. /*

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

  1150. {

  1151. 	if (which < EVENTI_TOTAL) {

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

  1153. 

  1154. */

  1155. 

  1156. 

  1157. 

  1158. 

  1159. 

  1160. 

  1161. 

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

  1163. {

  1164. 	switch (cfg->channel) {

  1165. 	  case PROFILE_HRM:

  1166. 		payload_HRM(cfg, payload, len);

  1167. 		break;

  1168. 	  case PROFILE_SPDCAD:

  1169. 		payload_SPDCAD(cfg, payload, len);

  1170. 		break;

  1171. 	  case PROFILE_POWER:

  1172. 		payload_POWER(cfg, payload, len);

  1173. 		break;

  1174. 	  case PROFILE_STRIDE:

  1175. 		payload_STRIDE(cfg, payload, len);

  1176. 		break;

  1177. 	  case PROFILE_SPEED:

  1178. 		payload_SPEED(cfg, payload, len);

  1179. 		break;

  1180. 	  case PROFILE_CADENCE:

  1181. 		payload_CADENCE(cfg, payload, len);

  1182. 		break;

  1183. 	}

  1184. }

  1185. 

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

  1187. {

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

  1189. 	return antkeys[keyIdx];

  1190. }

  1191. 

  1192. 

  1193. #define WHEEL_CIRCUMFERENCE                     2122

  1194. 

  1195. 

  1196. typedef struct {

  1197.         struct {

  1198.                 uint16_t time;

  1199.                 uint16_t interval;

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

  1201.                 uint8_t sequence;

  1202.         }current;

  1203. 

  1204.         struct {

  1205.                 uint8_t bpm;

  1206.                 uint8_t sequence;

  1207.                 uint16_t time;

  1208.                 uint16_t interval;

  1209.         }previous;

  1210. }payload_HRM_t;

  1211. 

  1212. typedef struct {

  1213.         struct {

  1214.                 uint16_t cadenceTime;

  1215.                 uint16_t cadence;

  1216.                 uint16_t cadenceCt;

  1217.                 uint16_t speedTime;

  1218.                 uint16_t speed;

  1219.                 uint16_t speedCt;

  1220.                 uint32_t distance;

  1221.         }current;

  1222. 

  1223.         struct {

  1224.                 uint16_t cadenceTime;

  1225.                 uint16_t cadence;

  1226.                 uint16_t cadenceCt;

  1227.                 uint16_t speedTime;

  1228.                 uint16_t speed;

  1229.                 uint16_t speedCt;

  1230.                 uint32_t distance;

  1231.         }previous;

  1232. 

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

  1234.         uint8_t spdChange;

  1235.         uint8_t cadChange;

  1236. }payload_SPDCAD_t;

  1237. 

  1238. 

  1239. 

  1240. typedef struct {

  1241.         struct {

  1242.         	uint8_t sequence;

  1243.         	uint16_t pedalPowerContribution;

  1244.         	uint8_t pedalPower;

  1245.         	uint8_t instantCadence;

  1246.         	uint16_t sumPower;

  1247.         	uint16_t instantPower;

  1248.         } current;

  1249.         struct {

  1250.                 uint16_t stub;

  1251.         } previous;

  1252. }payload_POWER_t;

  1253. 

  1254. typedef struct {

  1255.         struct {

  1256.                 uint16_t speed;

  1257.                 uint16_t cadence;

  1258.                 uint8_t strides;

  1259.         }current;

  1260. 

  1261.         struct {

  1262.                 uint8_t strides;

  1263.                 uint16_t speed;

  1264.                 uint16_t cadence;

  1265.         }previous;

  1266. }payload_STRIDE_t;

  1267. 

  1268. typedef struct {

  1269.         struct {

  1270.                 uint16_t speedTime;

  1271.                 uint16_t speed;

  1272.                 uint16_t speedCt;

  1273.                 uint32_t distance;

  1274.         }current;

  1275. 

  1276.         struct {

  1277.                 uint16_t speedTime;

  1278.                 uint16_t speed;

  1279.                 uint16_t speedCt;

  1280.                 uint32_t distance;

  1281.         }previous;

  1282. 

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

  1284.         uint8_t spdChange;

  1285. }payload_SPEED_t;

  1286. 

  1287. typedef struct {

  1288.         struct {

  1289.                 uint16_t cadenceTime;

  1290.                 uint16_t cadence;

  1291.                 uint16_t cadenceCt;

  1292.         }current;

  1293. 

  1294.         struct {

  1295.                 uint16_t cadenceTime;

  1296.                 uint16_t cadence;

  1297.                 uint16_t cadenceCt;

  1298.         }previous;

  1299. 

  1300.         uint8_t cadChange;

  1301. }payload_CADENCE_t;

  1302. 

  1303. 

  1304. 

  1305. typedef struct {

  1306.         payload_HRM_t           hrm;

  1307.         payload_SPDCAD_t        spdcad;

  1308.         payload_POWER_t         power;

  1309.         payload_STRIDE_t        stride;

  1310.         payload_SPEED_t         spd;

  1311.         payload_CADENCE_t       cad;

  1312. }payload_genstorage_t;

  1313. 

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

  1315. 

  1316. 

  1317. 

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

  1319. {

  1320. 	payload_HRM_t *hrm = &payload.hrm;

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

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

  1323. 

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

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

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

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

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

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

  1330. 

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

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

  1333. 

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

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

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

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

  1338. 		}

  1339. 	}

  1340. 

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

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

  1343. }

  1344. 

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

  1346. {

  1347. 	payload_SPDCAD_t *spdcad = &payload.spdcad;

  1348. 

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

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

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

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

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

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

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

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

  1357. 

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

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

  1360. 

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

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

  1363. 		spdcad->current.cadence = cadence;

  1364. 

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

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

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

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

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

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

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

  1372. 

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

  1374. 

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

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

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

  1378. 

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

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

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

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

  1383. 

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

  1385. 	}

  1386. }

  1387. 

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

  1389. {

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

  1391. 	payload_POWER_t *pwr = &payload.power;

  1392. /*

  1393. 	uint8_t eventCount = data[2];

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

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

  1396. 	uint8_t instantCadence = data[4];

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

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

  1399.         */

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

  1401. }

  1402. 

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

  1404. {

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

  1406. 	payload_STRIDE_t *stride = &payload.stride;

  1407. 

  1408. 

  1409. 	int page = data[1];

  1410. 	if (page == 0){

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

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

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

  1414. 

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

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

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

  1418. 

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

  1420. 

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

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

  1423. 	}

  1424. 

  1425. }

  1426. 

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

  1428. {

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

  1430. 	payload_SPEED_t *spd = &payload.spd;

  1431. 

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

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

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

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

  1436. 

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

  1438. 

  1439. 	if (spd->spdChange){

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

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

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

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

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

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

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

  1447. 

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

  1449. 

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

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

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

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

  1454. 	}

  1455. }

  1456. 

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

  1458. {

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

  1460. 	payload_CADENCE_t *cad = &payload.cad;

  1461. 

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

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

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

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

  1466. 

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

  1468. 

  1469. 	if (cad->cadChange){

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

  1471. 		cad->current.cadence = cadence;

  1472. 

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

  1474. 

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

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

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

  1478. 	}

  1479. }

  1480. 

  1481. 

  1482.