| 0,0 → 1,45 |
|---|
| void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
| va_list ap; |
| uint16_t txd_bufferIndex = 0; |
| uint8_t *currentBuffer; |
| uint8_t currentBufferIndex; |
| uint16_t lengthOfCurrentBuffer; |
| uint8_t shift = 0; |
| |
| txd_buffer[txd_bufferIndex++] = '#'; // Start character |
| txd_buffer[txd_bufferIndex++] = 'a' + addr; // Address (a=0; b=1,...) |
| txd_buffer[txd_bufferIndex++] = cmd; // Command |
| |
| va_start(ap, numofbuffers); |
| |
| while(numofbuffers) { |
| currentBuffer = va_arg(ap, uint8_t*); |
| lengthOfCurrentBuffer = va_arg(ap, int); |
| currentBufferIndex = 0; |
| // Encode data: 3 bytes of data are encoded into 4 bytes, |
| // where the 2 most significant bits are both 0. |
| while(currentBufferIndex != lengthOfCurrentBuffer) { |
| if (!shift) txd_buffer[txd_bufferIndex] = 0; |
| txd_buffer[txd_bufferIndex] |= currentBuffer[currentBufferIndex] >> (shift + 2); |
| txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111; |
| shift += 2; |
| if (shift == 6) { shift=0; txd_bufferIndex++; } |
| currentBufferIndex++; |
| } |
| } |
| // If the number of data bytes was not divisible by 3, stuff |
| // with 0 pseudodata until length is again divisible by 3. |
| if (shift == 2) { |
| // We need to stuff with zero bytes at the end. |
| txd_buffer[txd_bufferIndex] &= 0b00110000; |
| txd_buffer[++txd_bufferIndex] = 0; |
| shift = 4; |
| } |
| if (shift == 4) { |
| // We need to stuff with zero bytes at the end. |
| txd_buffer[txd_bufferIndex++] &= 0b00111100; |
| txd_buffer[txd_bufferIndex] = 0; |
| } |
| va_end(ap); |
| AddCRC(pt); // add checksum after data block and initates the transmission |
| } |