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 |
} |