Exemple #1
0
 virtual void DataReceived(const void *data, size_t length) {
   mutex.Lock();
   buffer.Shift();
   auto range = buffer.Write();
   if (range.size < length)
     length = range.size;
   memcpy(range.data, data, length);
   buffer.Append(length);
   mutex.Unlock();
   SendNotification();
 }
Exemple #2
0
bool
KRT2Device::DataReceived(const void *_data, size_t length,
                         struct NMEAInfo &info)
{
  assert(_data != nullptr);
  assert(length > 0);

  const uint8_t *data = (const uint8_t *)_data;
  const uint8_t *end = data + length;

  do {
    // Append new data to the buffer, as much as fits in there
    auto range = rx_buf.Write();
    if (rx_buf.IsFull()) {
      // Overflow: reset buffer to recover quickly
      rx_buf.Clear();
      expected_msg_length = 0;
      continue;
    }
    size_t nbytes = std::min(range.size, size_t(end - data));
    memcpy(range.data, data, nbytes);
    data += nbytes;
    rx_buf.Append(nbytes);

    for (;;) {
      // Read data from buffer to handle the messages
      range = rx_buf.Read();
      if (range.IsEmpty())
        break;

      if (range.size < expected_msg_length)
        break;

      expected_msg_length = ExpectedMsgLength(range.data, range.size);

      if (range.size >= expected_msg_length) {
        switch (*(const uint8_t *) range.data) {
          case ACK:
          case NAK:
            // Received a response to a normal command (STX)
            response_mutex.Lock();
            response = *(const uint8_t *) range.data;
            // Signal the response to the TX thread
            rx_cond.signal();
            response_mutex.Unlock();
            break;
          default:
            // Received a command from the radio -> ignore it
            break;
        }
        // Message handled -> remove message
        rx_buf.Consume(expected_msg_length);
        expected_msg_length = 0;
        // Received something from the radio -> the connection is alive
        info.alive.Update(info.clock);
      }
    }
  } while (data < end);

  return true;
}