Revision 0:aca5a32d2759, committed 2016-03-18

Comitter:

vsutardja

Date:

Fri Mar 18 22:33:32 2016 +0000

Commit message:

init

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/packet.cpp	Fri Mar 18 22:33:32 2016 +0000
@@ -0,0 +1,173 @@
+/**
+ * Transmit and receive packet interfaces
+ */
+
+#include "telemetry.h"
+
+namespace telemetry {
+
+namespace internal {
+  template<> void pkt_write<uint8_t>(TransmitPacket& interface, uint8_t data) {
+    interface.write_uint8(data);
+  }
+  template<> void pkt_write<uint16_t>(TransmitPacket& interface, uint16_t data) {
+    interface.write_uint16(data);
+  }
+  template<> void pkt_write<uint32_t>(TransmitPacket& interface, uint32_t data) {
+    interface.write_uint32(data);
+  }
+  template<> void pkt_write<float>(TransmitPacket& interface, float data) {
+    interface.write_float(data);
+  }
+
+  template<> uint8_t buf_read<uint8_t>(ReceivePacketBuffer& buffer) {
+    return buffer.read_float();
+  }
+  template<> uint16_t buf_read<uint16_t>(ReceivePacketBuffer& buffer) {
+    return buffer.read_uint16();
+  }
+  template<> uint32_t buf_read<uint32_t>(ReceivePacketBuffer& buffer) {
+    return buffer.read_uint32();
+  }
+  template<> float buf_read<float>(ReceivePacketBuffer& buffer) {
+    return buffer.read_float();
+  }
+}
+
+FixedLengthTransmitPacket::FixedLengthTransmitPacket(HalInterface& hal,
+    size_t length) :
+        hal(hal),
+        length(length),
+        count(0) {
+  for (int i=0; i<protocol::SOF_LENGTH; i++) {
+    hal.transmit_byte(protocol::SOF_SEQ[i]);
+  }
+
+  hal.transmit_byte((length >> 8) & 0xff);
+  hal.transmit_byte((length >> 0) & 0xff);
+
+  valid = true;
+}
+
+void FixedLengthTransmitPacket::write_byte(uint8_t data) {
+  if (!valid) {
+    hal.do_error("Writing to invalid packet");
+    return;
+  } else if (count + 1 > length) {
+    hal.do_error("Writing over packet length");
+    return;
+  }
+  hal.transmit_byte(data);
+#if SOF_LENGTH > 2
+#error "Byte stuffing algorithm does not work for SOF_LENGTH > 2"
+#endif
+  if (data == protocol::SOF_SEQ[0]) {
+    hal.transmit_byte(protocol::SOF_SEQ0_STUFF);
+  }
+  count++;
+}
+
+void FixedLengthTransmitPacket::write_uint8(uint8_t data) {
+  write_byte(data);
+}
+
+void FixedLengthTransmitPacket::write_uint16(uint16_t data) {
+  write_byte((data >> 8) & 0xff);
+  write_byte((data >> 0) & 0xff);
+}
+
+void FixedLengthTransmitPacket::write_uint32(uint32_t data) {
+  write_byte((data >> 24) & 0xff);
+  write_byte((data >> 16) & 0xff);
+  write_byte((data >> 8) & 0xff);
+  write_byte((data >> 0) & 0xff);
+}
+
+void FixedLengthTransmitPacket::write_float(float data) {
+  // TODO: THIS IS ENDIANNESS DEPENDENT, ABSTRACT INTO HAL?
+  uint8_t *float_array = (uint8_t*) &data;
+  write_byte(float_array[3]);
+  write_byte(float_array[2]);
+  write_byte(float_array[1]);
+  write_byte(float_array[0]);
+}
+
+void FixedLengthTransmitPacket::finish() {
+  if (!valid) {
+    hal.do_error("Finish invalid packet");
+    return;
+  } else if (count != length) {
+    hal.do_error("TX packet under length");
+    return;
+  }
+
+  // TODO: add CRC check here
+}
+
+ReceivePacketBuffer::ReceivePacketBuffer(HalInterface& hal) :
+    hal(hal) {
+  new_packet();
+}
+
+void ReceivePacketBuffer::new_packet() {
+  packet_length = 0;
+  read_loc = 0;
+}
+
+void ReceivePacketBuffer::add_byte(uint8_t byte) {
+  if (packet_length >= MAX_RECEIVE_PACKET_LENGTH) {
+    hal.do_error("RX packet over length");
+    return;
+  }
+
+  data[packet_length] = byte;
+  packet_length++;
+}
+
+uint8_t ReceivePacketBuffer::read_uint8() {
+  if (read_loc + 1 > packet_length) {
+    hal.do_error("Read uint8 over length");
+    return 0;
+  }
+  read_loc += 1;
+  return data[read_loc - 1];
+}
+
+uint16_t ReceivePacketBuffer::read_uint16() {
+  if (read_loc + 2 > packet_length) {
+    hal.do_error("Read uint16 over length");
+    return 0;
+  }
+  read_loc += 2;
+  return ((uint16_t)data[read_loc - 2] << 8)
+       | ((uint16_t)data[read_loc - 1] << 0);
+}
+
+uint32_t ReceivePacketBuffer::read_uint32() {
+  if (read_loc + 4 > packet_length) {
+    hal.do_error("Read uint32 over length");
+    return 0;
+  }
+  read_loc += 4;
+  return ((uint32_t)data[read_loc - 4] << 24)
+       | ((uint32_t)data[read_loc - 3] << 16)
+       | ((uint32_t)data[read_loc - 2] << 8)
+       | ((uint32_t)data[read_loc - 1] << 0);
+}
+
+float ReceivePacketBuffer::read_float() {
+  if (read_loc + 4 > packet_length) {
+    hal.do_error("Read float over length");
+    return 0;
+  }
+  read_loc += 4;
+  float out = 0;
+  uint8_t* out_array = (uint8_t*)&out;
+  out_array[0] = data[read_loc - 1];
+  out_array[1] = data[read_loc - 2];
+  out_array[2] = data[read_loc - 3];
+  out_array[3] = data[read_loc - 4];
+  return out;
+}
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/packet.h	Fri Mar 18 22:33:32 2016 +0000
@@ -0,0 +1,102 @@
+#ifndef _PACKET_H_
+#define _PACKET_H_
+
+namespace telemetry {
+class TransmitPacket;
+class ReceivePacketBuffer;
+
+namespace internal {
+  template<typename T> void pkt_write(TransmitPacket& interface, T data);
+  template<typename T> T buf_read(ReceivePacketBuffer& buffer);
+}
+
+// Abstract base class for building a packet to be transmitted.
+// Implementation is unconstrained - writes may either be buffered or passed
+// directly to the hardware transmit buffers.
+class TransmitPacket {
+public:
+  virtual ~TransmitPacket() {}
+
+  // Writes a 8-bit unsigned integer to the packet stream.
+  virtual void write_uint8(uint8_t data) = 0;
+  // Writes a 16-bit unsigned integer to the packet stream.
+  virtual void write_uint16(uint16_t data) = 0;
+  // Writes a 32-bit unsigned integer to the packet stream.
+  virtual void write_uint32(uint32_t data) = 0;
+  // Writes a float to the packet stream.
+  virtual void write_float(float data) = 0;
+
+  // Generic templated write operations.
+  template<typename T> void write(T data) {
+    internal::pkt_write<T>(*this, data);
+  }
+
+  // Finish the packet and writes data to the transmit stream (if not already
+  // done). No more data may be written afterwards.
+  virtual void finish() = 0;
+};
+
+class ReceivePacketBuffer {
+public:
+  ReceivePacketBuffer(HalInterface& hal);
+
+  // Starts a new packet, resetting the packet length and read pointer.
+  void new_packet();
+
+  // Appends a new byte onto this packet, advancing the packet length
+  void add_byte(uint8_t byte);
+
+  // Reads a 8-bit unsigned integer from the packet stream, advancing buffer.
+  uint8_t read_uint8();
+  // Reads a 16-bit unsigned integer from the packet stream, advancing buffer.
+  uint16_t read_uint16();
+  // Reads a 32-bit unsigned integer from the packet stream, advancing buffer.
+  uint32_t read_uint32();
+  // Reads a float from the packet stream, advancing buffer.
+  float read_float();
+
+  // Generic templated write operations.
+  template<typename T> T read() {
+	return internal::buf_read<T>(*this);
+  }
+
+protected:
+  HalInterface& hal;
+
+  size_t packet_length;
+  size_t read_loc;
+  uint8_t data[MAX_RECEIVE_PACKET_LENGTH];
+};
+
+// A telemetry packet with a length known before data is written to it.
+// Data is written directly to the hardware transmit buffers without packet
+// buffering. Assumes transmit buffers won't fill up.
+class FixedLengthTransmitPacket : public TransmitPacket {
+public:
+  FixedLengthTransmitPacket(HalInterface& hal, size_t length);
+
+  void write_byte(uint8_t data);
+
+  void write_uint8(uint8_t data);
+  void write_uint16(uint16_t data);
+  void write_uint32(uint32_t data);
+  void write_float(float data);
+
+  virtual void finish();
+
+protected:
+  HalInterface& hal;
+
+  // Predetermined length, in bytes, of this packet's payload, for sanity check.
+  size_t length;
+
+  // Current length, in bytes, of this packet's payload.
+  size_t count;
+
+  // Is the packet valid?
+  bool valid;
+};
+
+}
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/protocol.cpp	Fri Mar 18 22:33:32 2016 +0000
@@ -0,0 +1,37 @@
+#include "telemetry.h"
+
+namespace telemetry {
+
+namespace protocol {
+
+template<> uint8_t numeric_subtype<uint8_t>() {
+  return NUMERIC_SUBTYPE_UINT;
+}
+template<> uint8_t numeric_subtype<uint16_t>() {
+  return NUMERIC_SUBTYPE_UINT;
+}
+template<> uint8_t numeric_subtype<uint32_t>() {
+  return NUMERIC_SUBTYPE_UINT;
+}
+
+template<> uint8_t numeric_subtype<int8_t>() {
+  return NUMERIC_SUBTYPE_SINT;
+}
+template<> uint8_t numeric_subtype<int16_t>() {
+  return NUMERIC_SUBTYPE_SINT;
+}
+template<> uint8_t numeric_subtype<int32_t>() {
+  return NUMERIC_SUBTYPE_SINT;
+}
+
+template<> uint8_t numeric_subtype<float>() {
+  return NUMERIC_SUBTYPE_FLOAT;
+}
+
+template<> uint8_t numeric_subtype<double>() {
+  return NUMERIC_SUBTYPE_FLOAT;
+}
+
+}
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/protocol.h	Fri Mar 18 22:33:32 2016 +0000
@@ -0,0 +1,55 @@
+/**
+ * Telemetry wire protocol constants.
+ */
+
+#ifndef _PROTOCOL_H_
+#define _PROTOCOL_H_
+
+namespace telemetry {
+
+namespace protocol {
+
+// Start of frame sequence.
+const uint8_t SOF_SEQ[] = {0x05, 0x39};
+const uint8_t SOF_LENGTH = sizeof(SOF_SEQ) / sizeof(SOF_SEQ[0]);
+// A dummy byte to "stuff" when the start of frame shows up in the data.
+const uint8_t SOF_SEQ0_STUFF = 0x00;
+
+const size_t LENGTH_SIZE = 2;
+
+// TODO: make these length independent
+
+const uint8_t OPCODE_HEADER = 0x81;
+const uint8_t OPCODE_DATA = 0x01;
+
+const uint8_t DATAID_TERMINATOR = 0x00;
+
+const uint8_t DATATYPE_NUMERIC = 0x01;
+const uint8_t DATATYPE_NUMERIC_ARRAY = 0x02;
+
+const uint8_t RECORDID_TERMINATOR = 0x00;
+const uint8_t RECORDID_INTERNAL_NAME = 0x01;
+const uint8_t RECORDID_DISPLAY_NAME = 0x02;
+const uint8_t RECORDID_UNITS = 0x03;
+
+const uint8_t RECORDID_OVERRIDE_CTL = 0x08;
+const uint8_t RECORDID_OVERRIDE_DATA = 0x08;
+
+const uint8_t RECORDID_NUMERIC_SUBTYPE = 0x40;
+const uint8_t RECORDID_NUMERIC_LENGTH = 0x41;
+const uint8_t RECORDID_NUMERIC_LIMITS = 0x42;
+const uint8_t RECORDID_ARRAY_COUNT = 0x50;
+
+const uint8_t NUMERIC_SUBTYPE_UINT = 0x01;
+const uint8_t NUMERIC_SUBTYPE_SINT = 0x02;
+const uint8_t NUMERIC_SUBTYPE_FLOAT = 0x03;
+
+/**
+ * Returns the subtype field value for a numeric recordid.
+ */
+template<typename T> uint8_t numeric_subtype();
+}
+
+}
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/queue.h	Fri Mar 18 22:33:32 2016 +0000
@@ -0,0 +1,103 @@
+#include <stddef.h>
+#include <stdint.h>
+
+#ifndef _QUEUE_H_
+#define _QUEUE_H_
+
+namespace telemetry {
+/**
+ * Statically allocated, lock-free queue.
+ * Thread-safe if used in single-producer single-consumer mode.
+ */
+template <typename T, size_t N> class Queue {
+public:
+  Queue() : read_ptr(values), write_ptr(values), begin(values), last(values + N) {}
+
+  // Return true if the queue is full (enqueue will return false).
+  bool full() const {
+    if (read_ptr == begin) {
+      // Read pointer at beginning, need to wrap around check.
+      if (write_ptr == last) {
+        return true;
+      } else {
+        return false;
+      }
+    } else {
+      if (write_ptr == (read_ptr - 1)) {
+        return true;
+      } else {
+        return false;
+      }
+    }
+  }
+
+  // Return true if the queue is empty (dequeue will return false).
+  bool empty() const {
+    return (read_ptr == write_ptr);
+  }
+
+  /**
+   * Puts a new value to the tail of the queue. Returns true if successful,
+   * false if not.
+   */
+  bool enqueue(const T& value) {
+    if (full()) {
+      return false;
+    }
+
+    //memcpy((void*)write_ptr, &value, sizeof(T));  // Make it array-compatible.
+    *write_ptr = value;
+
+    if (write_ptr == last) {
+      write_ptr = begin;
+    } else {
+      write_ptr++;
+    }
+
+    return true;
+  }
+
+  /**
+   * Assigns output to the last element in the queue.
+   */
+  bool dequeue(T* output) {
+    if (empty()) {
+      return false;
+    }
+
+    //memcpy(output, (void*)read_ptr, sizeof(T));  // Make it array-compatible.
+    *output = *read_ptr;
+
+    if (read_ptr == last) {
+      read_ptr = begin;
+    } else {
+      read_ptr++;
+    }
+
+    return true;
+  }
+
+protected:
+  // Lots of volatiles to prevent compiler reordering which could corrupt data
+  // when accessed by multiple threads. Yes, it's completely overkill, but
+  // memory fences aren't in earlier C++ versions.
+  volatile T values[N+1];
+
+  // Read pointer, points to next element to be returned by dequeue.
+  // Queue is empty if this equals write_ptr. Must never be incremented
+  // past write_ptr.
+  volatile T* volatile read_ptr;
+  // Write pointer, points to next location to be written by enqueue.
+  // Must never be incremented to read_ptr. Queue is full when this is one
+  // less than read_ptr.
+  volatile T* volatile write_ptr;
+
+  // Pointer to beginning of array, cleaner than using values directly.
+  volatile T* const begin;
+  // Pointer to one past the last element of the array.
+  volatile T* const last;
+};
+
+}
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/telemetry-data.cpp	Fri Mar 18 22:33:32 2016 +0000
@@ -0,0 +1,37 @@
+/*
+ * telemetry-data.cpp
+ *
+ *  Created on: Mar 2, 2015
+ *      Author: Ducky
+ *
+ * Implementation for Telemetry Data classes.
+ */
+#include "telemetry.h"
+#include <string.h>
+
+namespace telemetry {
+void packet_write_string(TransmitPacket& packet, const char* str) {
+  // TODO: move into HAL for higher performance?
+  while (*str != '\0') {
+    packet.write_uint8(*str);
+    str++;
+  }
+  packet.write_uint8('\0');
+}
+
+size_t Data::get_header_kvrs_length() {
+  return 1 + strlen(internal_name) + 1
+      + 1 + strlen(display_name) + 1
+      + 1 + strlen(units) + 1;
+}
+
+void Data::write_header_kvrs(TransmitPacket& packet) {
+  packet.write_uint8(protocol::RECORDID_INTERNAL_NAME);
+  packet_write_string(packet, internal_name);
+  packet.write_uint8(protocol::RECORDID_DISPLAY_NAME);
+  packet_write_string(packet, display_name);
+  packet.write_uint8(protocol::RECORDID_UNITS);
+  packet_write_string(packet, units);
+}
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/telemetry-hal.h	Fri Mar 18 22:33:32 2016 +0000
@@ -0,0 +1,29 @@
+#ifndef _TELEMETRY_HAL_H_
+#define _TELEMETRY_HAL_H_
+
+namespace telemetry {
+
+// Hardware abstraction layer for the telemetry server.
+class HalInterface {
+public:
+  virtual ~HalInterface() {}
+
+  // Write a byte to the transmit buffer.
+  virtual void transmit_byte(uint8_t data) = 0;
+  // Returns the number of bytes available in the receive buffer.
+  virtual size_t rx_available() = 0;
+  // Returns the next byte in the receive stream. rx_available must return > 0.
+  virtual uint8_t receive_byte() = 0;
+
+  // TODO: more efficient block transmit operations?
+
+  // Called on a telemetry error.
+  virtual void do_error(const char* message) = 0;
+
+  // Return the current time in milliseconds. May overflow at any time.
+  virtual uint32_t get_time_ms() = 0;
+};
+
+}
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/telemetry-mbed-hal.cpp	Fri Mar 18 22:33:32 2016 +0000
@@ -0,0 +1,53 @@
+/*
+ * telemetry-mbedo-hal.cpp
+ *
+ *  Created on: Mar 4, 2015
+ *      Author: Ducky
+ *
+ * Telemetry HAL for Serial on mBed.
+ */
+
+#include "telemetry.h"
+
+#ifdef TELEMETRY_HAL_MBED
+
+namespace telemetry {
+
+void MbedHal::transmit_byte(uint8_t data) {
+  // TODO: optimize with DMA
+  if (serial != NULL) {
+    serial->putc(data);
+  }
+}
+
+size_t MbedHal::rx_available() {
+  if (serial != NULL) {
+    return serial->rxBufferGetCount();
+  } else {
+	return 0;
+  }
+
+}
+
+uint8_t MbedHal::receive_byte() {
+  if (serial != NULL) {
+    return serial->getc();
+  } else {
+	return 0;
+  }
+}
+
+void MbedHal::do_error(const char* msg) {
+  if (serial != NULL) {
+    serial->puts(msg);
+    serial->puts("\r\n");
+  }
+}
+
+uint32_t MbedHal::get_time_ms() {
+  return timer.read_ms();
+}
+
+}
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/telemetry-mbed-hal.h	Fri Mar 18 22:33:32 2016 +0000
@@ -0,0 +1,48 @@
+/**
+ * HAL header for mbed platforms. DO NOT INCLUDE THIS FILE DIRECTLY.
+ * Use the automatic platform detection in telemetry.h instead.
+ */
+
+#include "mbed.h"
+#include "MODSERIAL.h"
+
+#include "telemetry-hal.h"
+
+#ifndef _TELEMETRY_MBED_HAL_
+#define _TELEMETRY_MBED_HAL_
+#define TELEMETRY_HAL
+#define TELEMETRY_HAL_MBED
+
+namespace telemetry {
+
+class MbedHal : public HalInterface {
+public:
+  MbedHal(MODSERIAL& serial_in) :
+    serial(&serial_in) {
+	  timer.start();
+  }
+  MbedHal() :
+      serial(NULL) {
+  	  timer.start();
+  }
+
+  void set_serial(MODSERIAL& serial_new) {
+	serial = &serial_new;
+  }
+
+  virtual void transmit_byte(uint8_t data);
+  virtual size_t rx_available();
+  virtual uint8_t receive_byte();
+
+  virtual void do_error(const char* message);
+
+  virtual uint32_t get_time_ms();
+
+protected:
+  MODSERIAL* serial;
+  Timer timer;
+};
+
+}
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/telemetry.cpp	Fri Mar 18 22:33:32 2016 +0000
@@ -0,0 +1,209 @@
+/*
+ * telemetry.cpp
+ *
+ *  Created on: Mar 2, 2015
+ *      Author: Ducky
+ *
+ * Implementation for the base Telemetry class.
+ */
+
+#include "telemetry.h"
+
+namespace telemetry {
+
+size_t Telemetry::add_data(Data& new_data) {
+  if (data_count >= MAX_DATA_PER_TELEMETRY) {
+    do_error("MAX_DATA_PER_TELEMETRY limit reached.");
+    return 0;
+  }
+  if (header_transmitted) {
+    do_error("Cannot add new data after header transmitted.");
+    return 0;
+  }
+  data[data_count] = &new_data;
+  data_updated[data_count] = true;
+  data_count++;
+  return data_count - 1;
+}
+
+void Telemetry::mark_data_updated(size_t data_id) {
+  data_updated[data_id] = true;
+}
+
+void Telemetry::transmit_header() {
+  if (header_transmitted) {
+    do_error("Cannot retransmit header.");
+    return;
+  }
+
+  size_t packet_legnth = 2; // opcode + sequence
+  for (int data_idx = 0; data_idx < data_count; data_idx++) {
+    packet_legnth += 2; // data ID, data type
+    packet_legnth += data[data_idx]->get_header_kvrs_length();
+    packet_legnth += 1; // terminator record id
+  }
+  packet_legnth++;  // terminator "record"
+
+  FixedLengthTransmitPacket packet(hal, packet_legnth);
+
+  packet.write_uint8(protocol::OPCODE_HEADER);
+  packet.write_uint8(packet_tx_sequence);
+  for (int data_idx = 0; data_idx < data_count; data_idx++) {
+    packet.write_uint8(data_idx+1);
+    packet.write_uint8(data[data_idx]->get_data_type());
+    data[data_idx]->write_header_kvrs(packet);
+    packet.write_uint8(protocol::RECORDID_TERMINATOR);
+  }
+  packet.write_uint8(protocol::DATAID_TERMINATOR);
+
+  packet.finish();
+
+  packet_tx_sequence++;
+  header_transmitted = true;
+}
+
+void Telemetry::do_io() {
+  transmit_data();
+  process_received_data();
+}
+
+void Telemetry::transmit_data() {
+  if (!header_transmitted) {
+    do_error("Must transmit header before transmitting data.");
+    return;
+  }
+
+  // Keep a local copy to make it more thread-safe
+  bool data_updated_local[MAX_DATA_PER_TELEMETRY];
+
+  size_t packet_legnth = 2; // opcode + sequence
+  for (int data_idx = 0; data_idx < data_count; data_idx++) {
+    data_updated_local[data_idx] = data_updated[data_idx];
+    data_updated[data_idx] = 0;
+    if (data_updated_local[data_idx]) {
+      packet_legnth += 1; // data ID
+      packet_legnth += data[data_idx]->get_payload_length();
+    }
+  }
+  packet_legnth++;  // terminator "record"
+
+  FixedLengthTransmitPacket packet(hal, packet_legnth);
+
+  packet.write_uint8(protocol::OPCODE_DATA);
+  packet.write_uint8(packet_tx_sequence);
+  for (int data_idx = 0; data_idx < data_count; data_idx++) {
+    if (data_updated_local[data_idx]) {
+      packet.write_uint8(data_idx+1);
+      data[data_idx]->write_payload(packet);
+    }
+  }
+  packet.write_uint8(protocol::DATAID_TERMINATOR);
+
+  packet.finish();
+
+  packet_tx_sequence++;
+}
+
+void Telemetry::process_received_data() {
+  uint32_t current_time = hal.get_time_ms();
+
+  if (decoder_last_receive_ms <= current_time) {
+    if (!decoder_last_received && decoder_state != SOF && decoder_pos != 0
+        && (decoder_last_receive_ms - current_time > DECODER_TIMEOUT_MS)) {
+      decoder_pos = 0;
+      packet_length = 0;
+      decoder_state = SOF;
+      hal.do_error("RX timeout");
+    }
+  } else {
+    // timer overflowed, do nothing
+  }
+  decoder_last_receive_ms = current_time;
+
+  decoder_last_received = false;
+  while (hal.rx_available()) {
+    decoder_last_received = true;
+
+    uint8_t rx_byte = hal.receive_byte();
+
+    if (decoder_state == SOF) {
+      if (rx_byte == protocol::SOF_SEQ[decoder_pos]) {
+        decoder_pos++;
+        if (decoder_pos >= protocol::SOF_LENGTH) {
+          decoder_pos = 0;
+          packet_length = 0;
+          decoder_state = LENGTH;
+        }
+      } else {
+        if (decoder_pos > 0) {
+          // Pass through any partial SOF sequence.
+          for (uint8_t i=0; i<decoder_pos; i++) {
+            rx_buffer.enqueue(protocol::SOF_SEQ[i]);
+          }
+        }
+        decoder_pos = 0;
+        rx_buffer.enqueue(rx_byte);
+      }
+    } else if (decoder_state == LENGTH) {
+      packet_length = (packet_length << 8) | rx_byte;
+      decoder_pos++;
+      if (decoder_pos >= protocol::LENGTH_SIZE) {
+        decoder_pos = 0;
+        decoder_state = DATA;
+      }
+    } else if (decoder_state == DATA) {
+      received_packet.add_byte(rx_byte);
+      decoder_pos++;
+      if (decoder_pos >= packet_length) {
+        process_received_packet();
+
+        decoder_pos = 0;
+        if (rx_byte == protocol::SOF_SEQ[0]) {
+          decoder_state = DATA_DESTUFF_END;
+        } else {
+          decoder_state = SOF;
+        }
+      } else {
+        if (rx_byte == protocol::SOF_SEQ[0]) {
+          decoder_state = DATA_DESTUFF;
+        }
+      }
+    } else if (decoder_state == DATA_DESTUFF) {
+      decoder_state = DATA;
+    } else if (decoder_state == DATA_DESTUFF_END) {
+      decoder_state = SOF;
+    }
+  }
+}
+
+void Telemetry::process_received_packet() {
+  uint8_t opcode = received_packet.read_uint8();
+  if (opcode == protocol::OPCODE_DATA) {
+    uint8_t data_id = received_packet.read_uint8();
+    while (data_id != protocol::DATAID_TERMINATOR) {
+      if (data_id < data_count + 1) {
+        data[data_id - 1]->set_from_packet(received_packet);
+      } else {
+        hal.do_error("Unknown data ID");
+      }
+      data_id = received_packet.read_uint8();
+    }
+  } else {
+    hal.do_error("Unknown opcode");
+  }
+}
+
+bool Telemetry::receive_available() {
+  return !rx_buffer.empty();
+}
+
+uint8_t Telemetry::read_receive() {
+  uint8_t rx_data = '#';
+  if (rx_buffer.dequeue(&rx_data)) {
+    return rx_data;
+  } else {
+    return 255;
+  }
+}
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/telemetry.h	Fri Mar 18 22:33:32 2016 +0000
@@ -0,0 +1,343 @@
+#ifndef _TELEMETRY_H_
+#define _TELEMETRY_H_
+
+#include <stddef.h>
+#include <stdint.h>
+
+// Maximum number of Data objects a Telemetry object can hold.
+// Default given here, but can be redefined with a compiler define.
+#ifndef TELEMETRY_DATA_LIMIT
+#define TELEMETRY_DATA_LIMIT 16
+#endif
+
+#ifndef TELEMETRY_SERIAL_RX_BUFFER_SIZE
+#define TELEMETRY_SERIAL_RX_BUFFER_SIZE 256
+#endif
+
+namespace telemetry {
+// Maximum number of Data objects a Telemetry object can hold.
+// Used for array sizing.
+const size_t MAX_DATA_PER_TELEMETRY = TELEMETRY_DATA_LIMIT;
+
+// Maximum payload size for a received telemetry packet.
+const size_t MAX_RECEIVE_PACKET_LENGTH = 255;
+
+// Time after which a partially received packet is discarded.
+const uint32_t DECODER_TIMEOUT_MS = 100;
+
+// Buffer size for received non-telemetry data.
+const size_t SERIAL_RX_BUFFER_SIZE = TELEMETRY_SERIAL_RX_BUFFER_SIZE;
+}
+
+#ifdef ARDUINO
+  #ifdef TELEMETRY_HAL
+    #error "Multiple telemetry HALs defined"
+  #endif
+  #include "telemetry-arduino-hal.h"
+#endif
+
+#if defined(__MBED__)
+  #ifdef TELEMETRY_HAL
+    #error "Multiple telemetry HALs defined"
+  #endif
+  #include "telemetry-mbed-hal.h"
+#endif
+
+#ifndef TELEMETRY_HAL
+  #error "No telemetry HAL defined"
+#endif
+
+#include "protocol.h"
+#include "packet.h"
+#include "queue.h"
+
+namespace telemetry {
+// Abstract base class for telemetry data objects.
+class Data {
+public:
+  Data(const char* internal_name, const char* display_name,
+      const char* units):
+      internal_name(internal_name),
+      display_name(display_name),
+      units(units) {};
+
+  virtual ~Data() {}
+
+  // Returns the data type code.
+  virtual uint8_t get_data_type() = 0;
+
+  // Returns the length of the header KVRs, in bytes. Does not include the
+  // terminator header.
+  virtual size_t get_header_kvrs_length();
+  // Writes the header KVRs to the transmit packet. Does not write the
+  // terminiator header.
+  virtual void write_header_kvrs(TransmitPacket& packet);
+
+  // Returns the length of the payload, in bytes. Should be "fast".
+  virtual size_t get_payload_length() = 0;
+  // Writes the payload to the transmit packet. Should be "fast".
+  virtual void write_payload(TransmitPacket& packet) = 0;
+
+  // Sets my value from the received packet, interpreting the current packet
+  // read position as my data type.
+  virtual void set_from_packet(ReceivePacketBuffer& packet) = 0;
+
+protected:
+  const char* internal_name;
+  const char* display_name;
+  const char* units;
+};
+
+// Telemetry Server object.
+class Telemetry {
+public:
+  Telemetry(HalInterface& hal) :
+    hal(hal),
+    data_count(0),
+    received_packet(ReceivePacketBuffer(hal)),
+    decoder_state(SOF),
+    decoder_pos(0),
+    packet_length(0),
+	  decoder_last_received(false),
+	  decoder_last_receive_ms(0),
+    header_transmitted(false),
+    packet_tx_sequence(0),
+    packet_rx_sequence(0) {};
+
+  // Associates a DataInterface with this object, returning the data ID.
+  size_t add_data(Data& new_data);
+
+  // Marks a data ID as updated, to be transmitted in the next packet.
+  void mark_data_updated(size_t data_id);
+
+  // Transmits header data. Must be called after all add_data calls are done
+  // and before and IO is done.
+  void transmit_header();
+
+  // Does IO, including transmitting telemetry packets. Should be called on
+  // a regular basis. Since this does IO, this may block depending on the HAL
+  // semantics.
+  void do_io();
+
+  // TODO: better docs defining in-band receive.
+  // Returns whether or not read_receive will return valid data.
+  bool receive_available();
+  // Returns the next byte in the receive stream.
+  uint8_t read_receive();
+
+  // Calls the HAL's error function if some condition is false.
+  void do_error(const char* message) {
+    hal.do_error(message);
+  }
+
+protected:
+  // Transmits any updated data.
+  void transmit_data();
+
+  // Handles received data, splitting regular UART data from in-band packet
+  // data and processing received telemetry packets.
+  void process_received_data();
+
+  // Handles a received packet in received_packet.
+  void process_received_packet();
+
+  HalInterface& hal;
+
+  // Array of associated DataInterface objects. The index+1 is the
+  // DataInterface's data ID field.
+  Data* data[MAX_DATA_PER_TELEMETRY];
+  // Whether each data has been updated or not.
+  bool data_updated[MAX_DATA_PER_TELEMETRY];
+  // Count of associated DataInterface objects.
+  size_t data_count;
+
+  // Buffer holding the receive packet being assembled / parsed.
+  ReceivePacketBuffer received_packet;
+
+  enum DecoderState {
+    SOF,    // reading start-of-frame sequence (or just non-telemetry data)
+    LENGTH, // reading packet length
+    DATA,   // reading telemetry packet data
+    DATA_DESTUFF,     // reading a stuffed byte
+    DATA_DESTUFF_END  // last stuffed byte in a packet
+  } decoder_state;
+
+  size_t decoder_pos;
+  size_t packet_length;
+  bool decoder_last_received;
+  uint32_t decoder_last_receive_ms;
+
+  Queue<uint8_t, SERIAL_RX_BUFFER_SIZE> rx_buffer;
+
+  bool header_transmitted;
+
+  // Sequence number of the next packet to be transmitted.
+  uint8_t packet_tx_sequence;
+  uint8_t packet_rx_sequence; // TODO use this somewhere
+};
+
+template <typename T>
+class Numeric : public Data {
+public:
+  Numeric(Telemetry& telemetry_container,
+      const char* internal_name, const char* display_name,
+      const char* units, T init_value):
+      Data(internal_name, display_name, units),
+      telemetry_container(telemetry_container),
+      value(init_value), min_val(init_value), max_val(init_value) {
+    data_id = telemetry_container.add_data(*this);
+  }
+
+  T operator = (T b) {
+    value = b;
+    telemetry_container.mark_data_updated(data_id);
+    return b;
+  }
+
+  operator T() {
+    return value;
+  }
+
+  Numeric<T>& set_limits(T min, T max) {
+    min_val = min;
+    max_val = max;
+    return *this;
+  }
+
+  uint8_t get_data_type() { return protocol::DATATYPE_NUMERIC; }
+
+  size_t get_header_kvrs_length() {
+    return Data::get_header_kvrs_length()
+        + 1 + 1   // subtype
+        + 1 + 1   // data length
+        + 1 + sizeof(value) + sizeof(value);  // limits
+  }
+
+  void write_header_kvrs(TransmitPacket& packet) {
+    Data::write_header_kvrs(packet);
+    packet.write_uint8(protocol::RECORDID_NUMERIC_SUBTYPE);
+    packet.write_uint8(protocol::numeric_subtype<T>());
+    packet.write_uint8(protocol::RECORDID_NUMERIC_LENGTH);
+    packet.write_uint8(sizeof(value));
+    packet.write_uint8(protocol::RECORDID_NUMERIC_LIMITS);
+    serialize_data(min_val, packet);
+    serialize_data(max_val, packet);
+  }
+
+  size_t get_payload_length() { return sizeof(value); }
+  void write_payload(TransmitPacket& packet) { serialize_data(value, packet); }
+  void set_from_packet(ReceivePacketBuffer& packet) {
+    value = deserialize_data(packet);
+    telemetry_container.mark_data_updated(data_id); }
+
+  void serialize_data(T value, TransmitPacket& packet) {
+    packet.write<T>(value);
+  }
+  T deserialize_data(ReceivePacketBuffer& packet) {
+    return packet.read<T>();
+  }
+
+
+protected:
+  Telemetry& telemetry_container;
+  size_t data_id;
+  T value;
+  T min_val, max_val;
+};
+
+template <typename T, uint32_t array_count>
+class NumericArrayAccessor;
+
+template <typename T, uint32_t array_count>
+class NumericArray : public Data {
+  friend class NumericArrayAccessor<T, array_count>;
+public:
+  NumericArray(Telemetry& telemetry_container,
+      const char* internal_name, const char* display_name,
+      const char* units, T elem_init_value):
+      Data(internal_name, display_name, units),
+      telemetry_container(telemetry_container),
+      min_val(elem_init_value), max_val(elem_init_value) {
+    for (size_t i=0; i<array_count; i++) {
+      value[i] = elem_init_value;
+    }
+    data_id = telemetry_container.add_data(*this);
+  }
+
+  NumericArrayAccessor<T, array_count> operator[] (const int index) {
+    // TODO: add bounds checking here?
+    return NumericArrayAccessor<T, array_count>(*this, index);
+  }
+
+  NumericArray<T, array_count>& set_limits(T min, T max) {
+    min_val = min;
+    max_val = max;
+    return *this;
+  }
+
+  uint8_t get_data_type() { return protocol::DATATYPE_NUMERIC_ARRAY; }
+
+  size_t get_header_kvrs_length() {
+    return Data::get_header_kvrs_length()
+        + 1 + 1   // subtype
+        + 1 + 1   // data length
+        + 1 + 4   // array length
+        + 1 + sizeof(value[0]) + sizeof(value[0]);  // limits
+  }
+
+  void write_header_kvrs(TransmitPacket& packet) {
+    Data::write_header_kvrs(packet);
+    packet.write_uint8(protocol::RECORDID_NUMERIC_SUBTYPE);
+    packet.write_uint8(protocol::numeric_subtype<T>());
+    packet.write_uint8(protocol::RECORDID_NUMERIC_LENGTH);
+    packet.write_uint8(sizeof(value[0]));
+    packet.write_uint8(protocol::RECORDID_ARRAY_COUNT);
+    packet.write_uint32(array_count);
+    packet.write_uint8(protocol::RECORDID_NUMERIC_LIMITS);
+    serialize_data(min_val, packet);
+    serialize_data(max_val, packet);
+  }
+
+  size_t get_payload_length() { return sizeof(value); }
+  void write_payload(TransmitPacket& packet) {
+    for (size_t i=0; i<array_count; i++) { serialize_data(this->value[i], packet); } }
+  void set_from_packet(ReceivePacketBuffer& packet) {
+    for (size_t i=0; i<array_count; i++) { value[i] = deserialize_data(packet); }
+    telemetry_container.mark_data_updated(data_id); }
+
+  void serialize_data(T data, TransmitPacket& packet) {
+    packet.write<T>(data); }
+  T deserialize_data(ReceivePacketBuffer& packet) {
+    return packet.read<T>(); }
+
+protected:
+  Telemetry& telemetry_container;
+  size_t data_id;
+  T value[array_count];
+  T min_val, max_val;
+};
+
+template <typename T, uint32_t array_count>
+class NumericArrayAccessor {
+public:
+  NumericArrayAccessor(NumericArray<T, array_count>& container, size_t index) :
+    container(container), index(index) { }
+
+  T operator = (T b) {
+    container.value[index] = b;
+    container.telemetry_container.mark_data_updated(container.data_id);
+    return b;
+  }
+
+  operator T() {
+    return container.value[index];
+  }
+
+protected:
+  NumericArray<T, array_count>& container;
+  size_t index;
+};
+
+}
+
+#endif