Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1821 - 1
void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
2
  va_list ap;
3
  uint16_t txd_bufferIndex = 0;
4
  uint8_t *currentBuffer;
5
  uint8_t currentBufferIndex;
6
  uint16_t lengthOfCurrentBuffer;
7
  uint8_t shift = 0;
8
 
9
  txd_buffer[txd_bufferIndex++] = '#';      // Start character
10
  txd_buffer[txd_bufferIndex++] = 'a' + addr;         // Address (a=0; b=1,...)
11
  txd_buffer[txd_bufferIndex++] = cmd;      // Command
12
 
13
  va_start(ap, numofbuffers);
14
 
15
  while(numofbuffers) {
16
    currentBuffer = va_arg(ap, uint8_t*);
17
    lengthOfCurrentBuffer = va_arg(ap, int);
18
    currentBufferIndex = 0;
19
    // Encode data: 3 bytes of data are encoded into 4 bytes,
20
    // where the 2 most significant bits are both 0.
21
    while(currentBufferIndex != lengthOfCurrentBuffer) {
22
      if (!shift) txd_buffer[txd_bufferIndex] = 0;
23
      txd_buffer[txd_bufferIndex]  |= currentBuffer[currentBufferIndex] >> (shift + 2);
24
      txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111;
25
      shift += 2;
26
      if (shift == 6) { shift=0; txd_bufferIndex++; }
27
      currentBufferIndex++;
28
    }
29
  }
30
  // If the number of data bytes was not divisible by 3, stuff
31
  //  with 0 pseudodata  until length is again divisible by 3.
32
  if (shift == 2) {
33
    // We need to stuff with zero bytes at the end.
34
    txd_buffer[txd_bufferIndex]  &= 0b00110000;
35
    txd_buffer[++txd_bufferIndex] = 0;
36
    shift = 4;
37
  }
38
  if (shift == 4) {
39
    // We need to stuff with zero bytes at the end.
40
    txd_buffer[txd_bufferIndex++] &= 0b00111100;
41
    txd_buffer[txd_bufferIndex]    = 0;
42
  }
43
  va_end(ap);
44
  AddCRC(pt); // add checksum after data block and initates the transmission
45
}