telemetry.h

00001 #ifndef _TELEMETRY_H_
00002 #define _TELEMETRY_H_
00003 
00004 #include <stddef.h>
00005 #include <stdint.h>
00006 
00007 namespace telemetry {
00008 
00009 #ifndef TELEMETRY_DATA_LIMIT
00010 #define TELEMETRY_DATA_LIMIT 16
00011 #endif
00012 
00013 // Maximum number of DataInterface objects a Telemetry object can hold.
00014 // Used for array sizing.
00015 const size_t MAX_DATA_PER_TELEMETRY = TELEMETRY_DATA_LIMIT;
00016 
00017 // Maximum payload size for a received telemetry packet.
00018 const size_t MAX_RECEIVE_PACKET_LENGTH = 255;
00019 
00020 // Various wire protocol constants.
00021 const uint8_t SOF1 = 0x05;  // start of frame byte 1
00022 const uint8_t SOF2 = 0x39;  // start of frame byte 2
00023 const uint8_t SOF_SEQ[] = {0x05, 0x39};
00024 
00025 const size_t LENGTH_SIZE = 2;
00026 
00027 // TODO: make these length independent
00028 
00029 const uint8_t OPCODE_HEADER = 0x81;
00030 const uint8_t OPCODE_DATA = 0x01;
00031 
00032 const uint8_t DATAID_TERMINATOR = 0x00;
00033 
00034 const uint8_t DATATYPE_NUMERIC = 0x01;
00035 const uint8_t DATATYPE_NUMERIC_ARRAY = 0x02;
00036 
00037 const uint8_t RECORDID_TERMINATOR = 0x00;
00038 const uint8_t RECORDID_INTERNAL_NAME = 0x01;
00039 const uint8_t RECORDID_DISPLAY_NAME = 0x02;
00040 const uint8_t RECORDID_UNITS = 0x03;
00041 
00042 const uint8_t RECORDID_OVERRIDE_CTL = 0x08;
00043 const uint8_t RECORDID_OVERRIDE_DATA = 0x08;
00044 
00045 const uint8_t RECORDID_NUMERIC_SUBTYPE = 0x40;
00046 const uint8_t RECORDID_NUMERIC_LENGTH = 0x41;
00047 const uint8_t RECORDID_NUMERIC_LIMITS = 0x42;
00048 const uint8_t RECORDID_ARRAY_COUNT = 0x50;
00049 
00050 const uint8_t NUMERIC_SUBTYPE_UINT = 0x01;
00051 const uint8_t NUMERIC_SUBTYPE_SINT = 0x02;
00052 const uint8_t NUMERIC_SUBTYPE_FLOAT = 0x03;
00053 
00054 const uint32_t DECODER_TIMEOUT_MS = 100;
00055 
00056 // Hardware abstraction layer for the telemetry server.
00057 class HalInterface {
00058 public:
00059   virtual ~HalInterface() {}
00060 
00061   // Write a byte to the transmit buffer.
00062   virtual void transmit_byte(uint8_t data) = 0;
00063   // Returns the number of bytes available in the receive buffer.
00064   virtual size_t rx_available() = 0;
00065   // Returns the next byte in the receive stream. rx_available must return > 0.
00066   virtual uint8_t receive_byte() = 0;
00067 
00068   // TODO: more efficient block transmit operations?
00069 
00070   // Called on a telemetry error.
00071   virtual void do_error(const char* message) = 0;
00072 
00073   // Return the current time in milliseconds. May overflow at any time.
00074   virtual uint32_t get_time_ms() = 0;
00075 };
00076 
00077 // Abstract base class for building a packet to be transmitted.
00078 // Implementation is unconstrained - writes may either be buffered or passed
00079 // directly to the hardware transmit buffers.
00080 class TransmitPacketInterface {
00081 public:
00082   virtual ~TransmitPacketInterface() {}
00083 
00084   // Writes a 8-bit unsigned integer to the packet stream.
00085   virtual void write_byte(uint8_t data) = 0;
00086     
00087   // Writes a 8-bit unsigned integer to the packet stream.
00088   virtual void write_uint8(uint8_t data) = 0;
00089   // Writes a 16-bit unsigned integer to the packet stream.
00090   virtual void write_uint16(uint16_t data) = 0;
00091   // Writes a 32-bit unsigned integer to the packet stream.
00092   virtual void write_uint32(uint32_t data) = 0;
00093   // Writes a float to the packet stream.
00094   virtual void write_float(float data) = 0;
00095 
00096   // Finish the packet and writes data to the transmit stream (if not already
00097   // done). No more data may be written afterwards.
00098   virtual void finish() = 0;
00099 };
00100 
00101 class ReceivePacketBuffer {
00102 public:
00103   ReceivePacketBuffer(HalInterface& hal);
00104 
00105   // Starts a new packet, resetting the packet length and read pointer.
00106   void new_packet();
00107 
00108   // Appends a new byte onto this packet, advancing the packet length
00109   void add_byte(uint8_t byte);
00110 
00111   // Reads a 8-bit unsigned integer from the packet stream, advancing buffer.
00112   uint8_t read_uint8();
00113   // Reads a 16-bit unsigned integer from the packet stream, advancing buffer.
00114   uint16_t read_uint16();
00115   // Reads a 32-bit unsigned integer from the packet stream, advancing buffer.
00116   uint32_t read_uint32();
00117   // Reads a float from the packet stream, advancing buffer.
00118   float read_float();
00119 
00120 protected:
00121   HalInterface& hal;
00122 
00123   size_t packet_length;
00124   size_t read_loc;
00125   uint8_t data[MAX_RECEIVE_PACKET_LENGTH];
00126 };
00127 
00128 // Abstract base class for telemetry data objects.
00129 class Data {
00130 public:
00131   Data(const char* internal_name, const char* display_name,
00132       const char* units):
00133       internal_name(internal_name),
00134       display_name(display_name),
00135       units(units) {};
00136 
00137   virtual ~Data() {}
00138 
00139   // Returns the data type code.
00140   virtual uint8_t get_data_type() = 0;
00141 
00142   // Returns the length of the header KVRs, in bytes. Does not include the
00143   // terminator header.
00144   virtual size_t get_header_kvrs_length();
00145   // Writes the header KVRs to the transmit packet. Does not write the
00146   // terminiator header.
00147   virtual void write_header_kvrs(TransmitPacketInterface& packet);
00148 
00149   // Returns the length of the payload, in bytes. Should be "fast".
00150   virtual size_t get_payload_length() = 0;
00151   // Writes the payload to the transmit packet. Should be "fast".
00152   virtual void write_payload(TransmitPacketInterface& packet) = 0;
00153 
00154   // Sets my value from the received packet, interpreting the current packet
00155   // read position as my data type.
00156   virtual void set_from_packet(ReceivePacketBuffer& packet) = 0;
00157 
00158 protected:
00159   const char* internal_name;
00160   const char* display_name;
00161   const char* units;
00162 };
00163 
00164 // Telemetry Server object.
00165 class Telemetry {
00166 public:
00167   Telemetry(HalInterface& hal) :
00168     hal(hal),
00169     data_count(0),
00170     received_packet(ReceivePacketBuffer(hal)),
00171     decoder_state(SOF),
00172     decoder_pos(0),
00173     packet_length(0),
00174     decoder_last_received(false),
00175     decoder_last_receive_ms(0),
00176     header_transmitted(false),
00177     packet_tx_sequence(0),
00178     packet_rx_sequence(0) {};
00179 
00180   // Associates a DataInterface with this object, returning the data ID.
00181   size_t add_data(Data& new_data);
00182 
00183   // Marks a data ID as updated, to be transmitted in the next packet.
00184   void mark_data_updated(size_t data_id);
00185 
00186   // Transmits header data. Must be called after all add_data calls are done
00187   // and before and IO is done.
00188   void transmit_header();
00189 
00190   // Does IO, including transmitting telemetry packets. Should be called on
00191   // a regular basis. Since this does IO, this may block depending on the HAL
00192   // semantics.
00193   void do_io();
00194 
00195   // TODO: better docs defining in-band receive.
00196   // Returns the number of bytes available in the receive stream.
00197   size_t receive_available();
00198   // Returns the next byte in the receive stream.
00199   uint8_t read_receive();
00200 
00201   // Calls the HAL's error function if some condition is false.
00202   void do_error(const char* message) {
00203     hal.do_error(message);
00204   }
00205 
00206 protected:
00207   // Transmits any updated data.
00208   void transmit_data();
00209 
00210   // Handles received data, splitting regular UART data from in-band packet
00211   // data and processing received telemetry packets.
00212   void process_received_data();
00213 
00214   // Handles a received packet in received_packet.
00215   void process_received_packet();
00216 
00217   HalInterface& hal;
00218 
00219   // Array of associated DataInterface objects. The index+1 is the
00220   // DataInterface's data ID field.
00221   Data* data[MAX_DATA_PER_TELEMETRY];
00222   // Whether each data has been updated or not.
00223   bool data_updated[MAX_DATA_PER_TELEMETRY];
00224   // Count of associated DataInterface objects.
00225   size_t data_count;
00226 
00227   // Buffer holding the receive packet being assembled / parsed.
00228   ReceivePacketBuffer received_packet;
00229 
00230   enum DecoderState {
00231     SOF,    // reading start-of-frame sequence (or just non-telemetry data)
00232     LENGTH, // reading packet length
00233     DATA,   // reading telemetry packet data
00234     DATA_DESTUFF,     // reading a stuffed byte
00235     DATA_DESTUFF_END  // last stuffed byte in a packet
00236   } decoder_state;
00237 
00238   size_t decoder_pos;
00239   size_t packet_length;
00240   bool decoder_last_received;
00241   uint32_t decoder_last_receive_ms;
00242 
00243   bool header_transmitted;
00244 
00245   // Sequence number of the next packet to be transmitted.
00246   uint8_t packet_tx_sequence;
00247   uint8_t packet_rx_sequence; // TODO use this somewhere
00248 };
00249 
00250 // A telemetry packet with a length known before data is written to it.
00251 // Data is written directly to the hardware transmit buffers without packet
00252 // buffering. Assumes transmit buffers won't fill up.
00253 class FixedLengthTransmitPacket : public TransmitPacketInterface {
00254 public:
00255   FixedLengthTransmitPacket(HalInterface& hal, size_t length);
00256 
00257   virtual void write_byte(uint8_t data);
00258 
00259   virtual void write_uint8(uint8_t data);
00260   virtual void write_uint16(uint16_t data);
00261   virtual void write_uint32(uint32_t data);
00262   virtual void write_float(float data);
00263 
00264   virtual void finish();
00265 
00266 protected:
00267   HalInterface& hal;
00268 
00269   // Predetermined length, in bytes, of this packet's payload, for sanity check.
00270   size_t length;
00271 
00272   // Current length, in bytes, of this packet's payload.
00273   size_t count;
00274 
00275   // Is the packet valid?
00276   bool valid;
00277 };
00278 
00279 template <typename T> 
00280 class Numeric : public Data {
00281 public:
00282   Numeric(Telemetry& telemetry_container, 
00283       const char* internal_name, const char* display_name,
00284       const char* units, T init_value):
00285       Data(internal_name, display_name, units),
00286       telemetry_container(telemetry_container),
00287       value(init_value), min_val(init_value), max_val(init_value),
00288       frozen(false) {
00289     data_id = telemetry_container.add_data(*this);
00290   }
00291 
00292   T operator = (T b) {
00293     if (!frozen) {
00294       value = b;
00295       telemetry_container.mark_data_updated(data_id);
00296     }
00297     return value;
00298   }
00299 
00300   operator T() {
00301     return value;
00302   }
00303   
00304   Numeric<T>& set_limits(T min, T max) {
00305     min_val = min;
00306     max_val = max;
00307     return *this;
00308   }
00309   
00310   virtual uint8_t get_data_type() { return DATATYPE_NUMERIC; }
00311 
00312   virtual size_t get_header_kvrs_length() {
00313     return Data::get_header_kvrs_length()
00314         + 1 + 1   // subtype
00315         + 1 + 1   // data length
00316         + 1 + sizeof(value) + sizeof(value);  // limits
00317   }
00318 
00319   virtual void write_header_kvrs(TransmitPacketInterface& packet) {
00320     Data::write_header_kvrs(packet);
00321     packet.write_uint8(RECORDID_NUMERIC_SUBTYPE);
00322     packet.write_uint8(get_subtype());
00323     packet.write_uint8(RECORDID_NUMERIC_LENGTH);
00324     packet.write_uint8(sizeof(value));
00325     packet.write_uint8(RECORDID_NUMERIC_LIMITS);
00326     serialize_data(min_val, packet);
00327     serialize_data(max_val, packet);
00328   }
00329 
00330   uint8_t get_subtype();
00331 
00332   virtual size_t get_payload_length() { return sizeof(value); }
00333   virtual void write_payload(TransmitPacketInterface& packet) { serialize_data(value, packet); }
00334   virtual void set_from_packet(ReceivePacketBuffer& packet) {
00335     value = deserialize_data(packet);
00336     telemetry_container.mark_data_updated(data_id); }
00337   
00338   void serialize_data(T data, TransmitPacketInterface& packet);
00339   T deserialize_data(ReceivePacketBuffer& packet);
00340 
00341 protected:
00342   Telemetry& telemetry_container;
00343   size_t data_id;
00344   T value;
00345   T min_val, max_val;
00346   bool frozen;
00347 };
00348 
00349 template <typename T, uint32_t array_count>
00350 class NumericArrayAccessor;
00351 
00352 // TODO: fix this partial specialization inheritance nightmare
00353 template <typename T, uint32_t array_count> 
00354 class NumericArrayBase : public Data {
00355   friend class NumericArrayAccessor<T, array_count>;
00356 public:
00357   NumericArrayBase(Telemetry& telemetry_container, 
00358       const char* internal_name, const char* display_name,
00359       const char* units, T elem_init_value):
00360       Data(internal_name, display_name, units),
00361       telemetry_container(telemetry_container),
00362       min_val(elem_init_value), max_val(elem_init_value),
00363       frozen(false) {
00364     for (size_t i=0; i<array_count; i++) {
00365       value[i] = elem_init_value;
00366     }
00367     data_id = telemetry_container.add_data(*this);
00368   }
00369 
00370   NumericArrayAccessor<T, array_count> operator[] (const int index) {
00371     // TODO: add bounds checking here
00372     // TODO: add "frozen" check
00373     return NumericArrayAccessor<T, array_count>(*this, index);
00374   }
00375 
00376   NumericArrayBase<T, array_count>& set_limits(T min, T max) {
00377     min_val = min;
00378     max_val = max;
00379     return *this;
00380   }
00381   
00382   virtual uint8_t get_data_type() { return DATATYPE_NUMERIC_ARRAY; }
00383 
00384   virtual size_t get_header_kvrs_length() {
00385     return Data::get_header_kvrs_length()
00386         + 1 + 1   // subtype
00387         + 1 + 1   // data length
00388         + 1 + 4   // array length
00389         + 1 + sizeof(value[0]) + sizeof(value[0]);  // limits
00390   }
00391 
00392   virtual void write_header_kvrs(TransmitPacketInterface& packet) {
00393     Data::write_header_kvrs(packet);
00394     packet.write_uint8(RECORDID_NUMERIC_SUBTYPE);
00395     packet.write_uint8(get_subtype());
00396     packet.write_uint8(RECORDID_NUMERIC_LENGTH);
00397     packet.write_uint8(sizeof(value[0]));
00398     packet.write_uint8(RECORDID_ARRAY_COUNT);
00399     packet.write_uint32(array_count);
00400     packet.write_uint8(RECORDID_NUMERIC_LIMITS);
00401     serialize_data(min_val, packet);
00402     serialize_data(max_val, packet);
00403   }
00404 
00405   virtual uint8_t get_subtype() = 0;
00406 
00407   virtual size_t get_payload_length() { return sizeof(value); }
00408   virtual void write_payload(TransmitPacketInterface& packet) {
00409     for (size_t i=0; i<array_count; i++) { serialize_data(this->value[i], packet); } }
00410   virtual void set_from_packet(ReceivePacketBuffer& packet) {
00411     for (size_t i=0; i<array_count; i++) { value[i] = deserialize_data(packet); }
00412     telemetry_container.mark_data_updated(data_id); }
00413 
00414   virtual void serialize_data(T data, TransmitPacketInterface& packet) = 0;
00415   virtual T deserialize_data(ReceivePacketBuffer& packet) = 0;
00416   
00417 protected:
00418   Telemetry& telemetry_container;
00419   size_t data_id;
00420   T value[array_count];
00421   T min_val, max_val;
00422   bool frozen;
00423 };
00424 
00425 template <typename T, uint32_t array_count>
00426 class NumericArrayAccessor {
00427 public:
00428   NumericArrayAccessor(NumericArrayBase<T, array_count>& container, size_t index) :
00429     container(container), index(index) { }
00430 
00431   T operator = (T b) {
00432     if (!container.frozen) {
00433       container.value[index] = b;
00434       container.telemetry_container.mark_data_updated(container.data_id);
00435     }
00436     return container.value[index];
00437   }
00438 
00439   operator T() {
00440     return container.value[index];
00441   }
00442 
00443 protected:
00444   NumericArrayBase<T, array_count>& container;
00445   size_t index;
00446 };
00447 
00448 template <typename T, uint32_t array_count> 
00449 class NumericArray : public NumericArrayBase<T, array_count> {
00450   NumericArray(Telemetry& telemetry_container, 
00451       const char* internal_name, const char* display_name,
00452       const char* units, T elem_init_value);
00453   virtual uint8_t get_subtype();
00454   virtual void write_payload(TransmitPacketInterface& packet);
00455   virtual T deserialize_data(ReceivePacketBuffer& packet);
00456 };
00457 
00458 template <uint32_t array_count> 
00459 class NumericArray<uint8_t, array_count> : public NumericArrayBase<uint8_t, array_count> {
00460 public:
00461   NumericArray(Telemetry& telemetry_container, 
00462       const char* internal_name, const char* display_name,
00463       const char* units, uint8_t elem_init_value):
00464       NumericArrayBase<uint8_t, array_count>(
00465           telemetry_container, internal_name, display_name,
00466           units, elem_init_value) {};
00467   virtual uint8_t get_subtype() {return NUMERIC_SUBTYPE_UINT; }
00468   virtual void serialize_data(uint8_t data, TransmitPacketInterface& packet) {
00469     packet.write_uint8(data); }
00470   virtual uint8_t deserialize_data(ReceivePacketBuffer& packet) {
00471     return packet.read_uint8(); }
00472 };
00473 
00474 template <uint32_t array_count> 
00475 class NumericArray<uint16_t, array_count> : public NumericArrayBase<uint16_t, array_count> {
00476 public:
00477   NumericArray(Telemetry& telemetry_container, 
00478       const char* internal_name, const char* display_name,
00479       const char* units, uint16_t elem_init_value):
00480       NumericArrayBase<uint16_t, array_count>(
00481           telemetry_container, internal_name, display_name,
00482           units, elem_init_value) {};
00483   virtual uint8_t get_subtype() {return NUMERIC_SUBTYPE_UINT; }
00484   virtual void serialize_data(uint16_t data, TransmitPacketInterface& packet) {
00485     packet.write_uint16(data); }
00486   virtual uint16_t deserialize_data(ReceivePacketBuffer& packet) {
00487     return packet.read_uint16(); }
00488 };
00489 
00490 template <uint32_t array_count> 
00491 class NumericArray<uint32_t, array_count> : public NumericArrayBase<uint32_t, array_count> {
00492 public:
00493   NumericArray(Telemetry& telemetry_container, 
00494       const char* internal_name, const char* display_name,
00495       const char* units, uint32_t elem_init_value):
00496       NumericArrayBase<uint32_t, array_count>(
00497           telemetry_container, internal_name, display_name,
00498           units, elem_init_value) {};
00499   virtual uint8_t get_subtype() {return NUMERIC_SUBTYPE_UINT; }
00500   virtual void serialize_data(uint32_t data, TransmitPacketInterface& packet) {
00501     packet.write_uint32(data); }
00502   virtual uint32_t deserialize_data(ReceivePacketBuffer& packet) {
00503     return packet.read_uint32(); }
00504 };
00505 
00506 template <uint32_t array_count> 
00507 class NumericArray<float, array_count> : public NumericArrayBase<float, array_count> {
00508 public:
00509   NumericArray(Telemetry& telemetry_container, 
00510       const char* internal_name, const char* display_name,
00511       const char* units, float elem_init_value):
00512       NumericArrayBase<float, array_count>(
00513           telemetry_container, internal_name, display_name,
00514           units, elem_init_value) {};
00515   virtual uint8_t get_subtype() {return NUMERIC_SUBTYPE_FLOAT; }
00516   virtual void serialize_data(float data, TransmitPacketInterface& packet) {
00517     packet.write_float(data); }
00518   virtual float deserialize_data(ReceivePacketBuffer& packet) {
00519     return packet.read_float(); }
00520 };
00521 
00522 }
00523 
00524 #endif