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

}