Subversion Repositories FlightCtrl

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
}