telemetry
Dependents: Everything Sequential_Timing FixedPWM FixedPWMWill
Diff: telemetry.h
- Revision:
- 0:aca5a32d2759
diff -r 000000000000 -r aca5a32d2759 telemetry.h --- /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