Last active
August 29, 2015 14:28
-
-
Save KivApple/907bb1a4cd235f1b9f25 to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| typedef struct RadioChannel { | |
| size_t curRxPacketChunkCount; | |
| RadioChannelPacket *curRxPacket; | |
| mailbox_t rxMailbox; | |
| msg_t rxMailboxBuffer[RADIO_CHANNEL_MAILBOX_SIZE]; | |
| mailbox_t txMailbox; | |
| msg_t txMailboxBuffer[RADIO_CHANNEL_MAILBOX_SIZE]; | |
| } RadioChannel; | |
| typedef struct RadioChannelChunk { | |
| uint8_t current; | |
| uint8_t total; | |
| uint8_t payload[0]; | |
| } __attribute__((packed)) RadioChannelChunk; | |
| typedef struct RadioChannelPacket { | |
| uint16_t size; | |
| uint8_t payload[0]; | |
| } __attribute__((packed)) RadioChannelPacket; | |
| static THD_FUNCTION(radioChannelRxThreadFunc, arg) { | |
| RadioChannel *channel = (RadioChannel*)arg; | |
| chRegSetThreadName("radioChannelRx"); | |
| size_t maxChunkPayloadSize = NRF24L01_MAX_PAYLOAD_SIZE - sizeof(RadioChannelChunk); | |
| while (TRUE) { | |
| NRF24L01_Packet *chunkPacket; | |
| if (NRF24L01_recvPacket(&chunkPacket, TIME_INFINITE) != MSG_OK) continue; | |
| if (chunkPacket->size >= sizeof(RadioChannelChunk)) { | |
| RadioChannelChunk *chunk = (RadioChannelChunk*)(chunkPacket->data); | |
| if (chunk->current == 0) { // Start of new packet | |
| if (channel->curRxPacket) { | |
| chHeapFree(channel->curRxPacket); | |
| } | |
| RadioChannelPacket *packet = (RadioChannelPacket*)chunk->payload; | |
| if ((packet->size) && (packet->size <= (maxChunkPayloadSize * chunk->total))) { | |
| channel->curRxPacket = chHeapAlloc(NULL, maxChunkPayloadSize * chunk->total); | |
| } else { | |
| channel->curRxPacket = NULL; | |
| } | |
| channel->curRxPacketChunkCount = chunk->total; | |
| } | |
| if (channel->curRxPacket) { | |
| if ((chunk->total == channel->curRxPacketChunkCount) && (chunk->current < chunk->total)) { | |
| memmove((uint8_t*)channel->curRxPacket + maxChunkPayloadSize * chunk->current, chunk->payload, | |
| chunkPacket->size - sizeof(RadioChannelChunk)); | |
| if (chunk->current == (chunk->total - 1)) { | |
| if (chMBPost(&channel->rxMailbox, (msg_t)channel->curRxPacket, TIME_IMMEDIATE) != MSG_OK) { | |
| chHeapFree(channel->curRxPacket); | |
| } | |
| channel->curRxPacket = NULL; | |
| } | |
| } | |
| } | |
| } | |
| chHeapFree(chunkPacket); | |
| } | |
| } | |
| static THD_FUNCTION(radioChannelTxThreadFunc, arg) { | |
| RadioChannel *channel = (RadioChannel*)arg; | |
| chRegSetThreadName("radioChannelTx"); | |
| size_t maxChunkPayloadSize = NRF24L01_MAX_PAYLOAD_SIZE - sizeof(RadioChannelChunk); | |
| while (TRUE) { | |
| RadioChannelPacket *packet; | |
| if (chMBFetch(&channel->txMailbox, (msg_t*)&packet, TIME_INFINITE) != MSG_OK) continue; | |
| uint8_t *bytes = (uint8_t*)packet; | |
| size_t offset = 0, current = 0, total = (packet->size + (maxChunkPayloadSize - 1)) / maxChunkPayloadSize; | |
| while (offset < packet->size) { | |
| NRF24L01_Packet *chunkPacket = chHeapAlloc(NULL, sizeof(NRF24L01_Packet)); | |
| RadioChannelChunk *chunk = (RadioChannelChunk*)chunkPacket->data; | |
| chunk->current = current; | |
| chunk->total = total; | |
| uint8_t len = MIN(maxChunkPayloadSize, packet->size - offset); | |
| memmove(chunk->payload, bytes + offset, len); | |
| chunkPacket->size = len + sizeof(RadioChannelChunk); | |
| chunkPacket->addr = 0xE7E7E7E7E7; | |
| chunkPacket->autoAck = FALSE; | |
| NRF24L01_sendPacket(chunkPacket, TIME_INFINITE); | |
| offset += maxChunkPayloadSize; | |
| current++; | |
| } | |
| chHeapFree(packet); | |
| } | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment