ducky's telemetry library
telemetry.h@0:79b031fc31ac, 2015-03-31 (annotated)
- Committer:
- ikrase
- Date:
- Tue Mar 31 19:07:34 2015 +0000
- Revision:
- 0:79b031fc31ac
untested telemetry code
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ikrase | 0:79b031fc31ac | 1 | #ifndef _TELEMETRY_H_ |
ikrase | 0:79b031fc31ac | 2 | #define _TELEMETRY_H_ |
ikrase | 0:79b031fc31ac | 3 | |
ikrase | 0:79b031fc31ac | 4 | #include <stddef.h> |
ikrase | 0:79b031fc31ac | 5 | #include <stdint.h> |
ikrase | 0:79b031fc31ac | 6 | |
ikrase | 0:79b031fc31ac | 7 | namespace telemetry { |
ikrase | 0:79b031fc31ac | 8 | |
ikrase | 0:79b031fc31ac | 9 | #ifndef TELEMETRY_DATA_LIMIT |
ikrase | 0:79b031fc31ac | 10 | #define TELEMETRY_DATA_LIMIT 16 |
ikrase | 0:79b031fc31ac | 11 | #endif |
ikrase | 0:79b031fc31ac | 12 | |
ikrase | 0:79b031fc31ac | 13 | // Maximum number of DataInterface objects a Telemetry object can hold. |
ikrase | 0:79b031fc31ac | 14 | // Used for array sizing. |
ikrase | 0:79b031fc31ac | 15 | const size_t MAX_DATA_PER_TELEMETRY = TELEMETRY_DATA_LIMIT; |
ikrase | 0:79b031fc31ac | 16 | |
ikrase | 0:79b031fc31ac | 17 | // Maximum payload size for a received telemetry packet. |
ikrase | 0:79b031fc31ac | 18 | const size_t MAX_RECEIVE_PACKET_LENGTH = 255; |
ikrase | 0:79b031fc31ac | 19 | |
ikrase | 0:79b031fc31ac | 20 | // Various wire protocol constants. |
ikrase | 0:79b031fc31ac | 21 | const uint8_t SOF1 = 0x05; // start of frame byte 1 |
ikrase | 0:79b031fc31ac | 22 | const uint8_t SOF2 = 0x39; // start of frame byte 2 |
ikrase | 0:79b031fc31ac | 23 | const uint8_t SOF_SEQ[] = {0x05, 0x39}; |
ikrase | 0:79b031fc31ac | 24 | |
ikrase | 0:79b031fc31ac | 25 | const size_t LENGTH_SIZE = 2; |
ikrase | 0:79b031fc31ac | 26 | |
ikrase | 0:79b031fc31ac | 27 | // TODO: make these length independent |
ikrase | 0:79b031fc31ac | 28 | |
ikrase | 0:79b031fc31ac | 29 | const uint8_t OPCODE_HEADER = 0x81; |
ikrase | 0:79b031fc31ac | 30 | const uint8_t OPCODE_DATA = 0x01; |
ikrase | 0:79b031fc31ac | 31 | |
ikrase | 0:79b031fc31ac | 32 | const uint8_t DATAID_TERMINATOR = 0x00; |
ikrase | 0:79b031fc31ac | 33 | |
ikrase | 0:79b031fc31ac | 34 | const uint8_t DATATYPE_NUMERIC = 0x01; |
ikrase | 0:79b031fc31ac | 35 | const uint8_t DATATYPE_NUMERIC_ARRAY = 0x02; |
ikrase | 0:79b031fc31ac | 36 | |
ikrase | 0:79b031fc31ac | 37 | const uint8_t RECORDID_TERMINATOR = 0x00; |
ikrase | 0:79b031fc31ac | 38 | const uint8_t RECORDID_INTERNAL_NAME = 0x01; |
ikrase | 0:79b031fc31ac | 39 | const uint8_t RECORDID_DISPLAY_NAME = 0x02; |
ikrase | 0:79b031fc31ac | 40 | const uint8_t RECORDID_UNITS = 0x03; |
ikrase | 0:79b031fc31ac | 41 | |
ikrase | 0:79b031fc31ac | 42 | const uint8_t RECORDID_OVERRIDE_CTL = 0x08; |
ikrase | 0:79b031fc31ac | 43 | const uint8_t RECORDID_OVERRIDE_DATA = 0x08; |
ikrase | 0:79b031fc31ac | 44 | |
ikrase | 0:79b031fc31ac | 45 | const uint8_t RECORDID_NUMERIC_SUBTYPE = 0x40; |
ikrase | 0:79b031fc31ac | 46 | const uint8_t RECORDID_NUMERIC_LENGTH = 0x41; |
ikrase | 0:79b031fc31ac | 47 | const uint8_t RECORDID_NUMERIC_LIMITS = 0x42; |
ikrase | 0:79b031fc31ac | 48 | const uint8_t RECORDID_ARRAY_COUNT = 0x50; |
ikrase | 0:79b031fc31ac | 49 | |
ikrase | 0:79b031fc31ac | 50 | const uint8_t NUMERIC_SUBTYPE_UINT = 0x01; |
ikrase | 0:79b031fc31ac | 51 | const uint8_t NUMERIC_SUBTYPE_SINT = 0x02; |
ikrase | 0:79b031fc31ac | 52 | const uint8_t NUMERIC_SUBTYPE_FLOAT = 0x03; |
ikrase | 0:79b031fc31ac | 53 | |
ikrase | 0:79b031fc31ac | 54 | const uint32_t DECODER_TIMEOUT_MS = 100; |
ikrase | 0:79b031fc31ac | 55 | |
ikrase | 0:79b031fc31ac | 56 | // Hardware abstraction layer for the telemetry server. |
ikrase | 0:79b031fc31ac | 57 | class HalInterface { |
ikrase | 0:79b031fc31ac | 58 | public: |
ikrase | 0:79b031fc31ac | 59 | virtual ~HalInterface() {} |
ikrase | 0:79b031fc31ac | 60 | |
ikrase | 0:79b031fc31ac | 61 | // Write a byte to the transmit buffer. |
ikrase | 0:79b031fc31ac | 62 | virtual void transmit_byte(uint8_t data) = 0; |
ikrase | 0:79b031fc31ac | 63 | // Returns the number of bytes available in the receive buffer. |
ikrase | 0:79b031fc31ac | 64 | virtual size_t rx_available() = 0; |
ikrase | 0:79b031fc31ac | 65 | // Returns the next byte in the receive stream. rx_available must return > 0. |
ikrase | 0:79b031fc31ac | 66 | virtual uint8_t receive_byte() = 0; |
ikrase | 0:79b031fc31ac | 67 | |
ikrase | 0:79b031fc31ac | 68 | // TODO: more efficient block transmit operations? |
ikrase | 0:79b031fc31ac | 69 | |
ikrase | 0:79b031fc31ac | 70 | // Called on a telemetry error. |
ikrase | 0:79b031fc31ac | 71 | virtual void do_error(const char* message) = 0; |
ikrase | 0:79b031fc31ac | 72 | |
ikrase | 0:79b031fc31ac | 73 | // Return the current time in milliseconds. May overflow at any time. |
ikrase | 0:79b031fc31ac | 74 | virtual uint32_t get_time_ms() = 0; |
ikrase | 0:79b031fc31ac | 75 | }; |
ikrase | 0:79b031fc31ac | 76 | |
ikrase | 0:79b031fc31ac | 77 | // Abstract base class for building a packet to be transmitted. |
ikrase | 0:79b031fc31ac | 78 | // Implementation is unconstrained - writes may either be buffered or passed |
ikrase | 0:79b031fc31ac | 79 | // directly to the hardware transmit buffers. |
ikrase | 0:79b031fc31ac | 80 | class TransmitPacketInterface { |
ikrase | 0:79b031fc31ac | 81 | public: |
ikrase | 0:79b031fc31ac | 82 | virtual ~TransmitPacketInterface() {} |
ikrase | 0:79b031fc31ac | 83 | |
ikrase | 0:79b031fc31ac | 84 | // Writes a 8-bit unsigned integer to the packet stream. |
ikrase | 0:79b031fc31ac | 85 | virtual void write_byte(uint8_t data) = 0; |
ikrase | 0:79b031fc31ac | 86 | |
ikrase | 0:79b031fc31ac | 87 | // Writes a 8-bit unsigned integer to the packet stream. |
ikrase | 0:79b031fc31ac | 88 | virtual void write_uint8(uint8_t data) = 0; |
ikrase | 0:79b031fc31ac | 89 | // Writes a 16-bit unsigned integer to the packet stream. |
ikrase | 0:79b031fc31ac | 90 | virtual void write_uint16(uint16_t data) = 0; |
ikrase | 0:79b031fc31ac | 91 | // Writes a 32-bit unsigned integer to the packet stream. |
ikrase | 0:79b031fc31ac | 92 | virtual void write_uint32(uint32_t data) = 0; |
ikrase | 0:79b031fc31ac | 93 | // Writes a float to the packet stream. |
ikrase | 0:79b031fc31ac | 94 | virtual void write_float(float data) = 0; |
ikrase | 0:79b031fc31ac | 95 | |
ikrase | 0:79b031fc31ac | 96 | // Finish the packet and writes data to the transmit stream (if not already |
ikrase | 0:79b031fc31ac | 97 | // done). No more data may be written afterwards. |
ikrase | 0:79b031fc31ac | 98 | virtual void finish() = 0; |
ikrase | 0:79b031fc31ac | 99 | }; |
ikrase | 0:79b031fc31ac | 100 | |
ikrase | 0:79b031fc31ac | 101 | class ReceivePacketBuffer { |
ikrase | 0:79b031fc31ac | 102 | public: |
ikrase | 0:79b031fc31ac | 103 | ReceivePacketBuffer(HalInterface& hal); |
ikrase | 0:79b031fc31ac | 104 | |
ikrase | 0:79b031fc31ac | 105 | // Starts a new packet, resetting the packet length and read pointer. |
ikrase | 0:79b031fc31ac | 106 | void new_packet(); |
ikrase | 0:79b031fc31ac | 107 | |
ikrase | 0:79b031fc31ac | 108 | // Appends a new byte onto this packet, advancing the packet length |
ikrase | 0:79b031fc31ac | 109 | void add_byte(uint8_t byte); |
ikrase | 0:79b031fc31ac | 110 | |
ikrase | 0:79b031fc31ac | 111 | // Reads a 8-bit unsigned integer from the packet stream, advancing buffer. |
ikrase | 0:79b031fc31ac | 112 | uint8_t read_uint8(); |
ikrase | 0:79b031fc31ac | 113 | // Reads a 16-bit unsigned integer from the packet stream, advancing buffer. |
ikrase | 0:79b031fc31ac | 114 | uint16_t read_uint16(); |
ikrase | 0:79b031fc31ac | 115 | // Reads a 32-bit unsigned integer from the packet stream, advancing buffer. |
ikrase | 0:79b031fc31ac | 116 | uint32_t read_uint32(); |
ikrase | 0:79b031fc31ac | 117 | // Reads a float from the packet stream, advancing buffer. |
ikrase | 0:79b031fc31ac | 118 | float read_float(); |
ikrase | 0:79b031fc31ac | 119 | |
ikrase | 0:79b031fc31ac | 120 | protected: |
ikrase | 0:79b031fc31ac | 121 | HalInterface& hal; |
ikrase | 0:79b031fc31ac | 122 | |
ikrase | 0:79b031fc31ac | 123 | size_t packet_length; |
ikrase | 0:79b031fc31ac | 124 | size_t read_loc; |
ikrase | 0:79b031fc31ac | 125 | uint8_t data[MAX_RECEIVE_PACKET_LENGTH]; |
ikrase | 0:79b031fc31ac | 126 | }; |
ikrase | 0:79b031fc31ac | 127 | |
ikrase | 0:79b031fc31ac | 128 | // Abstract base class for telemetry data objects. |
ikrase | 0:79b031fc31ac | 129 | class Data { |
ikrase | 0:79b031fc31ac | 130 | public: |
ikrase | 0:79b031fc31ac | 131 | Data(const char* internal_name, const char* display_name, |
ikrase | 0:79b031fc31ac | 132 | const char* units): |
ikrase | 0:79b031fc31ac | 133 | internal_name(internal_name), |
ikrase | 0:79b031fc31ac | 134 | display_name(display_name), |
ikrase | 0:79b031fc31ac | 135 | units(units) {}; |
ikrase | 0:79b031fc31ac | 136 | |
ikrase | 0:79b031fc31ac | 137 | virtual ~Data() {} |
ikrase | 0:79b031fc31ac | 138 | |
ikrase | 0:79b031fc31ac | 139 | // Returns the data type code. |
ikrase | 0:79b031fc31ac | 140 | virtual uint8_t get_data_type() = 0; |
ikrase | 0:79b031fc31ac | 141 | |
ikrase | 0:79b031fc31ac | 142 | // Returns the length of the header KVRs, in bytes. Does not include the |
ikrase | 0:79b031fc31ac | 143 | // terminator header. |
ikrase | 0:79b031fc31ac | 144 | virtual size_t get_header_kvrs_length(); |
ikrase | 0:79b031fc31ac | 145 | // Writes the header KVRs to the transmit packet. Does not write the |
ikrase | 0:79b031fc31ac | 146 | // terminiator header. |
ikrase | 0:79b031fc31ac | 147 | virtual void write_header_kvrs(TransmitPacketInterface& packet); |
ikrase | 0:79b031fc31ac | 148 | |
ikrase | 0:79b031fc31ac | 149 | // Returns the length of the payload, in bytes. Should be "fast". |
ikrase | 0:79b031fc31ac | 150 | virtual size_t get_payload_length() = 0; |
ikrase | 0:79b031fc31ac | 151 | // Writes the payload to the transmit packet. Should be "fast". |
ikrase | 0:79b031fc31ac | 152 | virtual void write_payload(TransmitPacketInterface& packet) = 0; |
ikrase | 0:79b031fc31ac | 153 | |
ikrase | 0:79b031fc31ac | 154 | // Sets my value from the received packet, interpreting the current packet |
ikrase | 0:79b031fc31ac | 155 | // read position as my data type. |
ikrase | 0:79b031fc31ac | 156 | virtual void set_from_packet(ReceivePacketBuffer& packet) = 0; |
ikrase | 0:79b031fc31ac | 157 | |
ikrase | 0:79b031fc31ac | 158 | protected: |
ikrase | 0:79b031fc31ac | 159 | const char* internal_name; |
ikrase | 0:79b031fc31ac | 160 | const char* display_name; |
ikrase | 0:79b031fc31ac | 161 | const char* units; |
ikrase | 0:79b031fc31ac | 162 | }; |
ikrase | 0:79b031fc31ac | 163 | |
ikrase | 0:79b031fc31ac | 164 | // Telemetry Server object. |
ikrase | 0:79b031fc31ac | 165 | class Telemetry { |
ikrase | 0:79b031fc31ac | 166 | public: |
ikrase | 0:79b031fc31ac | 167 | Telemetry(HalInterface& hal) : |
ikrase | 0:79b031fc31ac | 168 | hal(hal), |
ikrase | 0:79b031fc31ac | 169 | data_count(0), |
ikrase | 0:79b031fc31ac | 170 | received_packet(ReceivePacketBuffer(hal)), |
ikrase | 0:79b031fc31ac | 171 | decoder_state(SOF), |
ikrase | 0:79b031fc31ac | 172 | decoder_pos(0), |
ikrase | 0:79b031fc31ac | 173 | packet_length(0), |
ikrase | 0:79b031fc31ac | 174 | decoder_last_received(false), |
ikrase | 0:79b031fc31ac | 175 | decoder_last_receive_ms(0), |
ikrase | 0:79b031fc31ac | 176 | header_transmitted(false), |
ikrase | 0:79b031fc31ac | 177 | packet_tx_sequence(0), |
ikrase | 0:79b031fc31ac | 178 | packet_rx_sequence(0) {}; |
ikrase | 0:79b031fc31ac | 179 | |
ikrase | 0:79b031fc31ac | 180 | // Associates a DataInterface with this object, returning the data ID. |
ikrase | 0:79b031fc31ac | 181 | size_t add_data(Data& new_data); |
ikrase | 0:79b031fc31ac | 182 | |
ikrase | 0:79b031fc31ac | 183 | // Marks a data ID as updated, to be transmitted in the next packet. |
ikrase | 0:79b031fc31ac | 184 | void mark_data_updated(size_t data_id); |
ikrase | 0:79b031fc31ac | 185 | |
ikrase | 0:79b031fc31ac | 186 | // Transmits header data. Must be called after all add_data calls are done |
ikrase | 0:79b031fc31ac | 187 | // and before and IO is done. |
ikrase | 0:79b031fc31ac | 188 | void transmit_header(); |
ikrase | 0:79b031fc31ac | 189 | |
ikrase | 0:79b031fc31ac | 190 | // Does IO, including transmitting telemetry packets. Should be called on |
ikrase | 0:79b031fc31ac | 191 | // a regular basis. Since this does IO, this may block depending on the HAL |
ikrase | 0:79b031fc31ac | 192 | // semantics. |
ikrase | 0:79b031fc31ac | 193 | void do_io(); |
ikrase | 0:79b031fc31ac | 194 | |
ikrase | 0:79b031fc31ac | 195 | // TODO: better docs defining in-band receive. |
ikrase | 0:79b031fc31ac | 196 | // Returns the number of bytes available in the receive stream. |
ikrase | 0:79b031fc31ac | 197 | size_t receive_available(); |
ikrase | 0:79b031fc31ac | 198 | // Returns the next byte in the receive stream. |
ikrase | 0:79b031fc31ac | 199 | uint8_t read_receive(); |
ikrase | 0:79b031fc31ac | 200 | |
ikrase | 0:79b031fc31ac | 201 | // Calls the HAL's error function if some condition is false. |
ikrase | 0:79b031fc31ac | 202 | void do_error(const char* message) { |
ikrase | 0:79b031fc31ac | 203 | hal.do_error(message); |
ikrase | 0:79b031fc31ac | 204 | } |
ikrase | 0:79b031fc31ac | 205 | |
ikrase | 0:79b031fc31ac | 206 | protected: |
ikrase | 0:79b031fc31ac | 207 | // Transmits any updated data. |
ikrase | 0:79b031fc31ac | 208 | void transmit_data(); |
ikrase | 0:79b031fc31ac | 209 | |
ikrase | 0:79b031fc31ac | 210 | // Handles received data, splitting regular UART data from in-band packet |
ikrase | 0:79b031fc31ac | 211 | // data and processing received telemetry packets. |
ikrase | 0:79b031fc31ac | 212 | void process_received_data(); |
ikrase | 0:79b031fc31ac | 213 | |
ikrase | 0:79b031fc31ac | 214 | // Handles a received packet in received_packet. |
ikrase | 0:79b031fc31ac | 215 | void process_received_packet(); |
ikrase | 0:79b031fc31ac | 216 | |
ikrase | 0:79b031fc31ac | 217 | HalInterface& hal; |
ikrase | 0:79b031fc31ac | 218 | |
ikrase | 0:79b031fc31ac | 219 | // Array of associated DataInterface objects. The index+1 is the |
ikrase | 0:79b031fc31ac | 220 | // DataInterface's data ID field. |
ikrase | 0:79b031fc31ac | 221 | Data* data[MAX_DATA_PER_TELEMETRY]; |
ikrase | 0:79b031fc31ac | 222 | // Whether each data has been updated or not. |
ikrase | 0:79b031fc31ac | 223 | bool data_updated[MAX_DATA_PER_TELEMETRY]; |
ikrase | 0:79b031fc31ac | 224 | // Count of associated DataInterface objects. |
ikrase | 0:79b031fc31ac | 225 | size_t data_count; |
ikrase | 0:79b031fc31ac | 226 | |
ikrase | 0:79b031fc31ac | 227 | // Buffer holding the receive packet being assembled / parsed. |
ikrase | 0:79b031fc31ac | 228 | ReceivePacketBuffer received_packet; |
ikrase | 0:79b031fc31ac | 229 | |
ikrase | 0:79b031fc31ac | 230 | enum DecoderState { |
ikrase | 0:79b031fc31ac | 231 | SOF, // reading start-of-frame sequence (or just non-telemetry data) |
ikrase | 0:79b031fc31ac | 232 | LENGTH, // reading packet length |
ikrase | 0:79b031fc31ac | 233 | DATA, // reading telemetry packet data |
ikrase | 0:79b031fc31ac | 234 | DATA_DESTUFF, // reading a stuffed byte |
ikrase | 0:79b031fc31ac | 235 | DATA_DESTUFF_END // last stuffed byte in a packet |
ikrase | 0:79b031fc31ac | 236 | } decoder_state; |
ikrase | 0:79b031fc31ac | 237 | |
ikrase | 0:79b031fc31ac | 238 | size_t decoder_pos; |
ikrase | 0:79b031fc31ac | 239 | size_t packet_length; |
ikrase | 0:79b031fc31ac | 240 | bool decoder_last_received; |
ikrase | 0:79b031fc31ac | 241 | uint32_t decoder_last_receive_ms; |
ikrase | 0:79b031fc31ac | 242 | |
ikrase | 0:79b031fc31ac | 243 | bool header_transmitted; |
ikrase | 0:79b031fc31ac | 244 | |
ikrase | 0:79b031fc31ac | 245 | // Sequence number of the next packet to be transmitted. |
ikrase | 0:79b031fc31ac | 246 | uint8_t packet_tx_sequence; |
ikrase | 0:79b031fc31ac | 247 | uint8_t packet_rx_sequence; // TODO use this somewhere |
ikrase | 0:79b031fc31ac | 248 | }; |
ikrase | 0:79b031fc31ac | 249 | |
ikrase | 0:79b031fc31ac | 250 | // A telemetry packet with a length known before data is written to it. |
ikrase | 0:79b031fc31ac | 251 | // Data is written directly to the hardware transmit buffers without packet |
ikrase | 0:79b031fc31ac | 252 | // buffering. Assumes transmit buffers won't fill up. |
ikrase | 0:79b031fc31ac | 253 | class FixedLengthTransmitPacket : public TransmitPacketInterface { |
ikrase | 0:79b031fc31ac | 254 | public: |
ikrase | 0:79b031fc31ac | 255 | FixedLengthTransmitPacket(HalInterface& hal, size_t length); |
ikrase | 0:79b031fc31ac | 256 | |
ikrase | 0:79b031fc31ac | 257 | virtual void write_byte(uint8_t data); |
ikrase | 0:79b031fc31ac | 258 | |
ikrase | 0:79b031fc31ac | 259 | virtual void write_uint8(uint8_t data); |
ikrase | 0:79b031fc31ac | 260 | virtual void write_uint16(uint16_t data); |
ikrase | 0:79b031fc31ac | 261 | virtual void write_uint32(uint32_t data); |
ikrase | 0:79b031fc31ac | 262 | virtual void write_float(float data); |
ikrase | 0:79b031fc31ac | 263 | |
ikrase | 0:79b031fc31ac | 264 | virtual void finish(); |
ikrase | 0:79b031fc31ac | 265 | |
ikrase | 0:79b031fc31ac | 266 | protected: |
ikrase | 0:79b031fc31ac | 267 | HalInterface& hal; |
ikrase | 0:79b031fc31ac | 268 | |
ikrase | 0:79b031fc31ac | 269 | // Predetermined length, in bytes, of this packet's payload, for sanity check. |
ikrase | 0:79b031fc31ac | 270 | size_t length; |
ikrase | 0:79b031fc31ac | 271 | |
ikrase | 0:79b031fc31ac | 272 | // Current length, in bytes, of this packet's payload. |
ikrase | 0:79b031fc31ac | 273 | size_t count; |
ikrase | 0:79b031fc31ac | 274 | |
ikrase | 0:79b031fc31ac | 275 | // Is the packet valid? |
ikrase | 0:79b031fc31ac | 276 | bool valid; |
ikrase | 0:79b031fc31ac | 277 | }; |
ikrase | 0:79b031fc31ac | 278 | |
ikrase | 0:79b031fc31ac | 279 | template <typename T> |
ikrase | 0:79b031fc31ac | 280 | class Numeric : public Data { |
ikrase | 0:79b031fc31ac | 281 | public: |
ikrase | 0:79b031fc31ac | 282 | Numeric(Telemetry& telemetry_container, |
ikrase | 0:79b031fc31ac | 283 | const char* internal_name, const char* display_name, |
ikrase | 0:79b031fc31ac | 284 | const char* units, T init_value): |
ikrase | 0:79b031fc31ac | 285 | Data(internal_name, display_name, units), |
ikrase | 0:79b031fc31ac | 286 | telemetry_container(telemetry_container), |
ikrase | 0:79b031fc31ac | 287 | value(init_value), min_val(init_value), max_val(init_value), |
ikrase | 0:79b031fc31ac | 288 | frozen(false) { |
ikrase | 0:79b031fc31ac | 289 | data_id = telemetry_container.add_data(*this); |
ikrase | 0:79b031fc31ac | 290 | } |
ikrase | 0:79b031fc31ac | 291 | |
ikrase | 0:79b031fc31ac | 292 | T operator = (T b) { |
ikrase | 0:79b031fc31ac | 293 | if (!frozen) { |
ikrase | 0:79b031fc31ac | 294 | value = b; |
ikrase | 0:79b031fc31ac | 295 | telemetry_container.mark_data_updated(data_id); |
ikrase | 0:79b031fc31ac | 296 | } |
ikrase | 0:79b031fc31ac | 297 | return value; |
ikrase | 0:79b031fc31ac | 298 | } |
ikrase | 0:79b031fc31ac | 299 | |
ikrase | 0:79b031fc31ac | 300 | operator T() { |
ikrase | 0:79b031fc31ac | 301 | return value; |
ikrase | 0:79b031fc31ac | 302 | } |
ikrase | 0:79b031fc31ac | 303 | |
ikrase | 0:79b031fc31ac | 304 | Numeric<T>& set_limits(T min, T max) { |
ikrase | 0:79b031fc31ac | 305 | min_val = min; |
ikrase | 0:79b031fc31ac | 306 | max_val = max; |
ikrase | 0:79b031fc31ac | 307 | return *this; |
ikrase | 0:79b031fc31ac | 308 | } |
ikrase | 0:79b031fc31ac | 309 | |
ikrase | 0:79b031fc31ac | 310 | virtual uint8_t get_data_type() { return DATATYPE_NUMERIC; } |
ikrase | 0:79b031fc31ac | 311 | |
ikrase | 0:79b031fc31ac | 312 | virtual size_t get_header_kvrs_length() { |
ikrase | 0:79b031fc31ac | 313 | return Data::get_header_kvrs_length() |
ikrase | 0:79b031fc31ac | 314 | + 1 + 1 // subtype |
ikrase | 0:79b031fc31ac | 315 | + 1 + 1 // data length |
ikrase | 0:79b031fc31ac | 316 | + 1 + sizeof(value) + sizeof(value); // limits |
ikrase | 0:79b031fc31ac | 317 | } |
ikrase | 0:79b031fc31ac | 318 | |
ikrase | 0:79b031fc31ac | 319 | virtual void write_header_kvrs(TransmitPacketInterface& packet) { |
ikrase | 0:79b031fc31ac | 320 | Data::write_header_kvrs(packet); |
ikrase | 0:79b031fc31ac | 321 | packet.write_uint8(RECORDID_NUMERIC_SUBTYPE); |
ikrase | 0:79b031fc31ac | 322 | packet.write_uint8(get_subtype()); |
ikrase | 0:79b031fc31ac | 323 | packet.write_uint8(RECORDID_NUMERIC_LENGTH); |
ikrase | 0:79b031fc31ac | 324 | packet.write_uint8(sizeof(value)); |
ikrase | 0:79b031fc31ac | 325 | packet.write_uint8(RECORDID_NUMERIC_LIMITS); |
ikrase | 0:79b031fc31ac | 326 | serialize_data(min_val, packet); |
ikrase | 0:79b031fc31ac | 327 | serialize_data(max_val, packet); |
ikrase | 0:79b031fc31ac | 328 | } |
ikrase | 0:79b031fc31ac | 329 | |
ikrase | 0:79b031fc31ac | 330 | uint8_t get_subtype(); |
ikrase | 0:79b031fc31ac | 331 | |
ikrase | 0:79b031fc31ac | 332 | virtual size_t get_payload_length() { return sizeof(value); } |
ikrase | 0:79b031fc31ac | 333 | virtual void write_payload(TransmitPacketInterface& packet) { serialize_data(value, packet); } |
ikrase | 0:79b031fc31ac | 334 | virtual void set_from_packet(ReceivePacketBuffer& packet) { |
ikrase | 0:79b031fc31ac | 335 | value = deserialize_data(packet); |
ikrase | 0:79b031fc31ac | 336 | telemetry_container.mark_data_updated(data_id); } |
ikrase | 0:79b031fc31ac | 337 | |
ikrase | 0:79b031fc31ac | 338 | void serialize_data(T data, TransmitPacketInterface& packet); |
ikrase | 0:79b031fc31ac | 339 | T deserialize_data(ReceivePacketBuffer& packet); |
ikrase | 0:79b031fc31ac | 340 | |
ikrase | 0:79b031fc31ac | 341 | protected: |
ikrase | 0:79b031fc31ac | 342 | Telemetry& telemetry_container; |
ikrase | 0:79b031fc31ac | 343 | size_t data_id; |
ikrase | 0:79b031fc31ac | 344 | T value; |
ikrase | 0:79b031fc31ac | 345 | T min_val, max_val; |
ikrase | 0:79b031fc31ac | 346 | bool frozen; |
ikrase | 0:79b031fc31ac | 347 | }; |
ikrase | 0:79b031fc31ac | 348 | |
ikrase | 0:79b031fc31ac | 349 | template <typename T, uint32_t array_count> |
ikrase | 0:79b031fc31ac | 350 | class NumericArrayAccessor; |
ikrase | 0:79b031fc31ac | 351 | |
ikrase | 0:79b031fc31ac | 352 | // TODO: fix this partial specialization inheritance nightmare |
ikrase | 0:79b031fc31ac | 353 | template <typename T, uint32_t array_count> |
ikrase | 0:79b031fc31ac | 354 | class NumericArrayBase : public Data { |
ikrase | 0:79b031fc31ac | 355 | friend class NumericArrayAccessor<T, array_count>; |
ikrase | 0:79b031fc31ac | 356 | public: |
ikrase | 0:79b031fc31ac | 357 | NumericArrayBase(Telemetry& telemetry_container, |
ikrase | 0:79b031fc31ac | 358 | const char* internal_name, const char* display_name, |
ikrase | 0:79b031fc31ac | 359 | const char* units, T elem_init_value): |
ikrase | 0:79b031fc31ac | 360 | Data(internal_name, display_name, units), |
ikrase | 0:79b031fc31ac | 361 | telemetry_container(telemetry_container), |
ikrase | 0:79b031fc31ac | 362 | min_val(elem_init_value), max_val(elem_init_value), |
ikrase | 0:79b031fc31ac | 363 | frozen(false) { |
ikrase | 0:79b031fc31ac | 364 | for (size_t i=0; i<array_count; i++) { |
ikrase | 0:79b031fc31ac | 365 | value[i] = elem_init_value; |
ikrase | 0:79b031fc31ac | 366 | } |
ikrase | 0:79b031fc31ac | 367 | data_id = telemetry_container.add_data(*this); |
ikrase | 0:79b031fc31ac | 368 | } |
ikrase | 0:79b031fc31ac | 369 | |
ikrase | 0:79b031fc31ac | 370 | NumericArrayAccessor<T, array_count> operator[] (const int index) { |
ikrase | 0:79b031fc31ac | 371 | // TODO: add bounds checking here |
ikrase | 0:79b031fc31ac | 372 | // TODO: add "frozen" check |
ikrase | 0:79b031fc31ac | 373 | return NumericArrayAccessor<T, array_count>(*this, index); |
ikrase | 0:79b031fc31ac | 374 | } |
ikrase | 0:79b031fc31ac | 375 | |
ikrase | 0:79b031fc31ac | 376 | NumericArrayBase<T, array_count>& set_limits(T min, T max) { |
ikrase | 0:79b031fc31ac | 377 | min_val = min; |
ikrase | 0:79b031fc31ac | 378 | max_val = max; |
ikrase | 0:79b031fc31ac | 379 | return *this; |
ikrase | 0:79b031fc31ac | 380 | } |
ikrase | 0:79b031fc31ac | 381 | |
ikrase | 0:79b031fc31ac | 382 | virtual uint8_t get_data_type() { return DATATYPE_NUMERIC_ARRAY; } |
ikrase | 0:79b031fc31ac | 383 | |
ikrase | 0:79b031fc31ac | 384 | virtual size_t get_header_kvrs_length() { |
ikrase | 0:79b031fc31ac | 385 | return Data::get_header_kvrs_length() |
ikrase | 0:79b031fc31ac | 386 | + 1 + 1 // subtype |
ikrase | 0:79b031fc31ac | 387 | + 1 + 1 // data length |
ikrase | 0:79b031fc31ac | 388 | + 1 + 4 // array length |
ikrase | 0:79b031fc31ac | 389 | + 1 + sizeof(value[0]) + sizeof(value[0]); // limits |
ikrase | 0:79b031fc31ac | 390 | } |
ikrase | 0:79b031fc31ac | 391 | |
ikrase | 0:79b031fc31ac | 392 | virtual void write_header_kvrs(TransmitPacketInterface& packet) { |
ikrase | 0:79b031fc31ac | 393 | Data::write_header_kvrs(packet); |
ikrase | 0:79b031fc31ac | 394 | packet.write_uint8(RECORDID_NUMERIC_SUBTYPE); |
ikrase | 0:79b031fc31ac | 395 | packet.write_uint8(get_subtype()); |
ikrase | 0:79b031fc31ac | 396 | packet.write_uint8(RECORDID_NUMERIC_LENGTH); |
ikrase | 0:79b031fc31ac | 397 | packet.write_uint8(sizeof(value[0])); |
ikrase | 0:79b031fc31ac | 398 | packet.write_uint8(RECORDID_ARRAY_COUNT); |
ikrase | 0:79b031fc31ac | 399 | packet.write_uint32(array_count); |
ikrase | 0:79b031fc31ac | 400 | packet.write_uint8(RECORDID_NUMERIC_LIMITS); |
ikrase | 0:79b031fc31ac | 401 | serialize_data(min_val, packet); |
ikrase | 0:79b031fc31ac | 402 | serialize_data(max_val, packet); |
ikrase | 0:79b031fc31ac | 403 | } |
ikrase | 0:79b031fc31ac | 404 | |
ikrase | 0:79b031fc31ac | 405 | virtual uint8_t get_subtype() = 0; |
ikrase | 0:79b031fc31ac | 406 | |
ikrase | 0:79b031fc31ac | 407 | virtual size_t get_payload_length() { return sizeof(value); } |
ikrase | 0:79b031fc31ac | 408 | virtual void write_payload(TransmitPacketInterface& packet) { |
ikrase | 0:79b031fc31ac | 409 | for (size_t i=0; i<array_count; i++) { serialize_data(this->value[i], packet); } } |
ikrase | 0:79b031fc31ac | 410 | virtual void set_from_packet(ReceivePacketBuffer& packet) { |
ikrase | 0:79b031fc31ac | 411 | for (size_t i=0; i<array_count; i++) { value[i] = deserialize_data(packet); } |
ikrase | 0:79b031fc31ac | 412 | telemetry_container.mark_data_updated(data_id); } |
ikrase | 0:79b031fc31ac | 413 | |
ikrase | 0:79b031fc31ac | 414 | virtual void serialize_data(T data, TransmitPacketInterface& packet) = 0; |
ikrase | 0:79b031fc31ac | 415 | virtual T deserialize_data(ReceivePacketBuffer& packet) = 0; |
ikrase | 0:79b031fc31ac | 416 | |
ikrase | 0:79b031fc31ac | 417 | protected: |
ikrase | 0:79b031fc31ac | 418 | Telemetry& telemetry_container; |
ikrase | 0:79b031fc31ac | 419 | size_t data_id; |
ikrase | 0:79b031fc31ac | 420 | T value[array_count]; |
ikrase | 0:79b031fc31ac | 421 | T min_val, max_val; |
ikrase | 0:79b031fc31ac | 422 | bool frozen; |
ikrase | 0:79b031fc31ac | 423 | }; |
ikrase | 0:79b031fc31ac | 424 | |
ikrase | 0:79b031fc31ac | 425 | template <typename T, uint32_t array_count> |
ikrase | 0:79b031fc31ac | 426 | class NumericArrayAccessor { |
ikrase | 0:79b031fc31ac | 427 | public: |
ikrase | 0:79b031fc31ac | 428 | NumericArrayAccessor(NumericArrayBase<T, array_count>& container, size_t index) : |
ikrase | 0:79b031fc31ac | 429 | container(container), index(index) { } |
ikrase | 0:79b031fc31ac | 430 | |
ikrase | 0:79b031fc31ac | 431 | T operator = (T b) { |
ikrase | 0:79b031fc31ac | 432 | if (!container.frozen) { |
ikrase | 0:79b031fc31ac | 433 | container.value[index] = b; |
ikrase | 0:79b031fc31ac | 434 | container.telemetry_container.mark_data_updated(container.data_id); |
ikrase | 0:79b031fc31ac | 435 | } |
ikrase | 0:79b031fc31ac | 436 | return container.value[index]; |
ikrase | 0:79b031fc31ac | 437 | } |
ikrase | 0:79b031fc31ac | 438 | |
ikrase | 0:79b031fc31ac | 439 | operator T() { |
ikrase | 0:79b031fc31ac | 440 | return container.value[index]; |
ikrase | 0:79b031fc31ac | 441 | } |
ikrase | 0:79b031fc31ac | 442 | |
ikrase | 0:79b031fc31ac | 443 | protected: |
ikrase | 0:79b031fc31ac | 444 | NumericArrayBase<T, array_count>& container; |
ikrase | 0:79b031fc31ac | 445 | size_t index; |
ikrase | 0:79b031fc31ac | 446 | }; |
ikrase | 0:79b031fc31ac | 447 | |
ikrase | 0:79b031fc31ac | 448 | template <typename T, uint32_t array_count> |
ikrase | 0:79b031fc31ac | 449 | class NumericArray : public NumericArrayBase<T, array_count> { |
ikrase | 0:79b031fc31ac | 450 | NumericArray(Telemetry& telemetry_container, |
ikrase | 0:79b031fc31ac | 451 | const char* internal_name, const char* display_name, |
ikrase | 0:79b031fc31ac | 452 | const char* units, T elem_init_value); |
ikrase | 0:79b031fc31ac | 453 | virtual uint8_t get_subtype(); |
ikrase | 0:79b031fc31ac | 454 | virtual void write_payload(TransmitPacketInterface& packet); |
ikrase | 0:79b031fc31ac | 455 | virtual T deserialize_data(ReceivePacketBuffer& packet); |
ikrase | 0:79b031fc31ac | 456 | }; |
ikrase | 0:79b031fc31ac | 457 | |
ikrase | 0:79b031fc31ac | 458 | template <uint32_t array_count> |
ikrase | 0:79b031fc31ac | 459 | class NumericArray<uint8_t, array_count> : public NumericArrayBase<uint8_t, array_count> { |
ikrase | 0:79b031fc31ac | 460 | public: |
ikrase | 0:79b031fc31ac | 461 | NumericArray(Telemetry& telemetry_container, |
ikrase | 0:79b031fc31ac | 462 | const char* internal_name, const char* display_name, |
ikrase | 0:79b031fc31ac | 463 | const char* units, uint8_t elem_init_value): |
ikrase | 0:79b031fc31ac | 464 | NumericArrayBase<uint8_t, array_count>( |
ikrase | 0:79b031fc31ac | 465 | telemetry_container, internal_name, display_name, |
ikrase | 0:79b031fc31ac | 466 | units, elem_init_value) {}; |
ikrase | 0:79b031fc31ac | 467 | virtual uint8_t get_subtype() {return NUMERIC_SUBTYPE_UINT; } |
ikrase | 0:79b031fc31ac | 468 | virtual void serialize_data(uint8_t data, TransmitPacketInterface& packet) { |
ikrase | 0:79b031fc31ac | 469 | packet.write_uint8(data); } |
ikrase | 0:79b031fc31ac | 470 | virtual uint8_t deserialize_data(ReceivePacketBuffer& packet) { |
ikrase | 0:79b031fc31ac | 471 | return packet.read_uint8(); } |
ikrase | 0:79b031fc31ac | 472 | }; |
ikrase | 0:79b031fc31ac | 473 | |
ikrase | 0:79b031fc31ac | 474 | template <uint32_t array_count> |
ikrase | 0:79b031fc31ac | 475 | class NumericArray<uint16_t, array_count> : public NumericArrayBase<uint16_t, array_count> { |
ikrase | 0:79b031fc31ac | 476 | public: |
ikrase | 0:79b031fc31ac | 477 | NumericArray(Telemetry& telemetry_container, |
ikrase | 0:79b031fc31ac | 478 | const char* internal_name, const char* display_name, |
ikrase | 0:79b031fc31ac | 479 | const char* units, uint16_t elem_init_value): |
ikrase | 0:79b031fc31ac | 480 | NumericArrayBase<uint16_t, array_count>( |
ikrase | 0:79b031fc31ac | 481 | telemetry_container, internal_name, display_name, |
ikrase | 0:79b031fc31ac | 482 | units, elem_init_value) {}; |
ikrase | 0:79b031fc31ac | 483 | virtual uint8_t get_subtype() {return NUMERIC_SUBTYPE_UINT; } |
ikrase | 0:79b031fc31ac | 484 | virtual void serialize_data(uint16_t data, TransmitPacketInterface& packet) { |
ikrase | 0:79b031fc31ac | 485 | packet.write_uint16(data); } |
ikrase | 0:79b031fc31ac | 486 | virtual uint16_t deserialize_data(ReceivePacketBuffer& packet) { |
ikrase | 0:79b031fc31ac | 487 | return packet.read_uint16(); } |
ikrase | 0:79b031fc31ac | 488 | }; |
ikrase | 0:79b031fc31ac | 489 | |
ikrase | 0:79b031fc31ac | 490 | template <uint32_t array_count> |
ikrase | 0:79b031fc31ac | 491 | class NumericArray<uint32_t, array_count> : public NumericArrayBase<uint32_t, array_count> { |
ikrase | 0:79b031fc31ac | 492 | public: |
ikrase | 0:79b031fc31ac | 493 | NumericArray(Telemetry& telemetry_container, |
ikrase | 0:79b031fc31ac | 494 | const char* internal_name, const char* display_name, |
ikrase | 0:79b031fc31ac | 495 | const char* units, uint32_t elem_init_value): |
ikrase | 0:79b031fc31ac | 496 | NumericArrayBase<uint32_t, array_count>( |
ikrase | 0:79b031fc31ac | 497 | telemetry_container, internal_name, display_name, |
ikrase | 0:79b031fc31ac | 498 | units, elem_init_value) {}; |
ikrase | 0:79b031fc31ac | 499 | virtual uint8_t get_subtype() {return NUMERIC_SUBTYPE_UINT; } |
ikrase | 0:79b031fc31ac | 500 | virtual void serialize_data(uint32_t data, TransmitPacketInterface& packet) { |
ikrase | 0:79b031fc31ac | 501 | packet.write_uint32(data); } |
ikrase | 0:79b031fc31ac | 502 | virtual uint32_t deserialize_data(ReceivePacketBuffer& packet) { |
ikrase | 0:79b031fc31ac | 503 | return packet.read_uint32(); } |
ikrase | 0:79b031fc31ac | 504 | }; |
ikrase | 0:79b031fc31ac | 505 | |
ikrase | 0:79b031fc31ac | 506 | template <uint32_t array_count> |
ikrase | 0:79b031fc31ac | 507 | class NumericArray<float, array_count> : public NumericArrayBase<float, array_count> { |
ikrase | 0:79b031fc31ac | 508 | public: |
ikrase | 0:79b031fc31ac | 509 | NumericArray(Telemetry& telemetry_container, |
ikrase | 0:79b031fc31ac | 510 | const char* internal_name, const char* display_name, |
ikrase | 0:79b031fc31ac | 511 | const char* units, float elem_init_value): |
ikrase | 0:79b031fc31ac | 512 | NumericArrayBase<float, array_count>( |
ikrase | 0:79b031fc31ac | 513 | telemetry_container, internal_name, display_name, |
ikrase | 0:79b031fc31ac | 514 | units, elem_init_value) {}; |
ikrase | 0:79b031fc31ac | 515 | virtual uint8_t get_subtype() {return NUMERIC_SUBTYPE_FLOAT; } |
ikrase | 0:79b031fc31ac | 516 | virtual void serialize_data(float data, TransmitPacketInterface& packet) { |
ikrase | 0:79b031fc31ac | 517 | packet.write_float(data); } |
ikrase | 0:79b031fc31ac | 518 | virtual float deserialize_data(ReceivePacketBuffer& packet) { |
ikrase | 0:79b031fc31ac | 519 | return packet.read_float(); } |
ikrase | 0:79b031fc31ac | 520 | }; |
ikrase | 0:79b031fc31ac | 521 | |
ikrase | 0:79b031fc31ac | 522 | } |
ikrase | 0:79b031fc31ac | 523 | |
ikrase | 0:79b031fc31ac | 524 | #endif |