Daniel Lee / telemetry

ducky's telemetry library

telemetry.cpp@0:80dd1516ad46, 2015-03-18 (annotated)

Committer:
dnleek
Date:
Wed Mar 18 07:59:36 2015 +0000
Revision:
0:80dd1516ad46
Telemetry library from ducky

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dnleek 0:80dd1516ad46 1 /*
dnleek 0:80dd1516ad46 2 * telemetry.cpp
dnleek 0:80dd1516ad46 3 *
dnleek 0:80dd1516ad46 4 * Created on: Mar 2, 2015
dnleek 0:80dd1516ad46 5 * Author: Ducky
dnleek 0:80dd1516ad46 6 *
dnleek 0:80dd1516ad46 7 * Implementation for the base Telemetry class.
dnleek 0:80dd1516ad46 8 */
dnleek 0:80dd1516ad46 9
dnleek 0:80dd1516ad46 10 #include <telemetry.h>
dnleek 0:80dd1516ad46 11
dnleek 0:80dd1516ad46 12 namespace telemetry {
dnleek 0:80dd1516ad46 13
dnleek 0:80dd1516ad46 14 size_t Telemetry::add_data(Data& new_data) {
dnleek 0:80dd1516ad46 15 if (data_count >= MAX_DATA_PER_TELEMETRY) {
dnleek 0:80dd1516ad46 16 do_error("MAX_DATA_PER_TELEMETRY limit reached.");
dnleek 0:80dd1516ad46 17 return 0;
dnleek 0:80dd1516ad46 18 }
dnleek 0:80dd1516ad46 19 if (header_transmitted) {
dnleek 0:80dd1516ad46 20 do_error("Cannot add new data after header transmitted.");
dnleek 0:80dd1516ad46 21 return 0;
dnleek 0:80dd1516ad46 22 }
dnleek 0:80dd1516ad46 23 data[data_count] = &new_data;
dnleek 0:80dd1516ad46 24 data_updated[data_count] = true;
dnleek 0:80dd1516ad46 25 data_count++;
dnleek 0:80dd1516ad46 26 return data_count - 1;
dnleek 0:80dd1516ad46 27 }
dnleek 0:80dd1516ad46 28
dnleek 0:80dd1516ad46 29 void Telemetry::mark_data_updated(size_t data_id) {
dnleek 0:80dd1516ad46 30 data_updated[data_id] = true;
dnleek 0:80dd1516ad46 31 }
dnleek 0:80dd1516ad46 32
dnleek 0:80dd1516ad46 33 void Telemetry::transmit_header() {
dnleek 0:80dd1516ad46 34 if (header_transmitted) {
dnleek 0:80dd1516ad46 35 do_error("Cannot retransmit header.");
dnleek 0:80dd1516ad46 36 return;
dnleek 0:80dd1516ad46 37 }
dnleek 0:80dd1516ad46 38
dnleek 0:80dd1516ad46 39 size_t packet_legnth = 2; // opcode + sequence
dnleek 0:80dd1516ad46 40 for (int data_idx = 0; data_idx < data_count; data_idx++) {
dnleek 0:80dd1516ad46 41 packet_legnth += 2; // data ID, data type
dnleek 0:80dd1516ad46 42 packet_legnth += data[data_idx]->get_header_kvrs_length();
dnleek 0:80dd1516ad46 43 packet_legnth += 1; // terminator record id
dnleek 0:80dd1516ad46 44 }
dnleek 0:80dd1516ad46 45 packet_legnth++; // terminator "record"
dnleek 0:80dd1516ad46 46
dnleek 0:80dd1516ad46 47 FixedLengthTransmitPacket packet(hal, packet_legnth);
dnleek 0:80dd1516ad46 48
dnleek 0:80dd1516ad46 49 packet.write_uint8(OPCODE_HEADER);
dnleek 0:80dd1516ad46 50 packet.write_uint8(packet_tx_sequence);
dnleek 0:80dd1516ad46 51 for (int data_idx = 0; data_idx < data_count; data_idx++) {
dnleek 0:80dd1516ad46 52 packet.write_uint8(data_idx+1);
dnleek 0:80dd1516ad46 53 packet.write_uint8(data[data_idx]->get_data_type());
dnleek 0:80dd1516ad46 54 data[data_idx]->write_header_kvrs(packet);
dnleek 0:80dd1516ad46 55 packet.write_uint8(RECORDID_TERMINATOR);
dnleek 0:80dd1516ad46 56 }
dnleek 0:80dd1516ad46 57 packet.write_uint8(DATAID_TERMINATOR);
dnleek 0:80dd1516ad46 58
dnleek 0:80dd1516ad46 59 packet.finish();
dnleek 0:80dd1516ad46 60
dnleek 0:80dd1516ad46 61 packet_tx_sequence++;
dnleek 0:80dd1516ad46 62 header_transmitted = true;
dnleek 0:80dd1516ad46 63 }
dnleek 0:80dd1516ad46 64
dnleek 0:80dd1516ad46 65 void Telemetry::do_io() {
dnleek 0:80dd1516ad46 66 transmit_data();
dnleek 0:80dd1516ad46 67 process_received_data();
dnleek 0:80dd1516ad46 68 }
dnleek 0:80dd1516ad46 69
dnleek 0:80dd1516ad46 70 void Telemetry::transmit_data() {
dnleek 0:80dd1516ad46 71 if (!header_transmitted) {
dnleek 0:80dd1516ad46 72 do_error("Must transmit header before transmitting data.");
dnleek 0:80dd1516ad46 73 return;
dnleek 0:80dd1516ad46 74 }
dnleek 0:80dd1516ad46 75
dnleek 0:80dd1516ad46 76 // Keep a local copy to make it more thread-safe
dnleek 0:80dd1516ad46 77 bool data_updated_local[MAX_DATA_PER_TELEMETRY];
dnleek 0:80dd1516ad46 78
dnleek 0:80dd1516ad46 79 size_t packet_legnth = 2; // opcode + sequence
dnleek 0:80dd1516ad46 80 for (int data_idx = 0; data_idx < data_count; data_idx++) {
dnleek 0:80dd1516ad46 81 data_updated_local[data_idx] = data_updated[data_idx];
dnleek 0:80dd1516ad46 82 data_updated[data_idx] = 0;
dnleek 0:80dd1516ad46 83 if (data_updated_local[data_idx]) {
dnleek 0:80dd1516ad46 84 packet_legnth += 1; // data ID
dnleek 0:80dd1516ad46 85 packet_legnth += data[data_idx]->get_payload_length();
dnleek 0:80dd1516ad46 86 }
dnleek 0:80dd1516ad46 87 }
dnleek 0:80dd1516ad46 88 packet_legnth++; // terminator "record"
dnleek 0:80dd1516ad46 89
dnleek 0:80dd1516ad46 90 FixedLengthTransmitPacket packet(hal, packet_legnth);
dnleek 0:80dd1516ad46 91
dnleek 0:80dd1516ad46 92 packet.write_uint8(OPCODE_DATA);
dnleek 0:80dd1516ad46 93 packet.write_uint8(packet_tx_sequence);
dnleek 0:80dd1516ad46 94 for (int data_idx = 0; data_idx < data_count; data_idx++) {
dnleek 0:80dd1516ad46 95 if (data_updated_local[data_idx]) {
dnleek 0:80dd1516ad46 96 packet.write_uint8(data_idx+1);
dnleek 0:80dd1516ad46 97 data[data_idx]->write_payload(packet);
dnleek 0:80dd1516ad46 98 }
dnleek 0:80dd1516ad46 99 }
dnleek 0:80dd1516ad46 100 packet.write_uint8(DATAID_TERMINATOR);
dnleek 0:80dd1516ad46 101
dnleek 0:80dd1516ad46 102 packet.finish();
dnleek 0:80dd1516ad46 103
dnleek 0:80dd1516ad46 104 packet_tx_sequence++;
dnleek 0:80dd1516ad46 105 }
dnleek 0:80dd1516ad46 106
dnleek 0:80dd1516ad46 107 void Telemetry::process_received_data() {
dnleek 0:80dd1516ad46 108 uint32_t current_time = hal.get_time_ms();
dnleek 0:80dd1516ad46 109
dnleek 0:80dd1516ad46 110 if (decoder_last_receive_ms <= current_time) {
dnleek 0:80dd1516ad46 111 if (!decoder_last_received && decoder_state != SOF && decoder_pos != 0
dnleek 0:80dd1516ad46 112 && (decoder_last_receive_ms - current_time > DECODER_TIMEOUT_MS)) {
dnleek 0:80dd1516ad46 113 decoder_pos = 0;
dnleek 0:80dd1516ad46 114 packet_length = 0;
dnleek 0:80dd1516ad46 115 decoder_state = SOF;
dnleek 0:80dd1516ad46 116 hal.do_error("RX timeout");
dnleek 0:80dd1516ad46 117 }
dnleek 0:80dd1516ad46 118 } else {
dnleek 0:80dd1516ad46 119 // timer overflowed, do nothing
dnleek 0:80dd1516ad46 120 }
dnleek 0:80dd1516ad46 121 decoder_last_receive_ms = current_time;
dnleek 0:80dd1516ad46 122
dnleek 0:80dd1516ad46 123 decoder_last_received = false;
dnleek 0:80dd1516ad46 124 while (hal.rx_available()) {
dnleek 0:80dd1516ad46 125 decoder_last_received = true;
dnleek 0:80dd1516ad46 126
dnleek 0:80dd1516ad46 127 uint8_t rx_byte = hal.receive_byte();
dnleek 0:80dd1516ad46 128
dnleek 0:80dd1516ad46 129 if (decoder_state == SOF) {
dnleek 0:80dd1516ad46 130 if (rx_byte == SOF_SEQ[decoder_pos]) {
dnleek 0:80dd1516ad46 131 decoder_pos++;
dnleek 0:80dd1516ad46 132 if (decoder_pos >= (sizeof(SOF_SEQ) / sizeof(SOF_SEQ[0]))) {
dnleek 0:80dd1516ad46 133 decoder_pos = 0;
dnleek 0:80dd1516ad46 134 packet_length = 0;
dnleek 0:80dd1516ad46 135 decoder_state = LENGTH;
dnleek 0:80dd1516ad46 136 }
dnleek 0:80dd1516ad46 137 } else {
dnleek 0:80dd1516ad46 138 decoder_pos = 0;
dnleek 0:80dd1516ad46 139 // TODO: pass rest of data through
dnleek 0:80dd1516ad46 140 }
dnleek 0:80dd1516ad46 141 } else if (decoder_state == LENGTH) {
dnleek 0:80dd1516ad46 142 packet_length = (packet_length << 8) | rx_byte;
dnleek 0:80dd1516ad46 143 decoder_pos++;
dnleek 0:80dd1516ad46 144 if (decoder_pos >= LENGTH_SIZE) {
dnleek 0:80dd1516ad46 145 decoder_pos = 0;
dnleek 0:80dd1516ad46 146 decoder_state = DATA;
dnleek 0:80dd1516ad46 147 }
dnleek 0:80dd1516ad46 148 } else if (decoder_state == DATA) {
dnleek 0:80dd1516ad46 149 received_packet.add_byte(rx_byte);
dnleek 0:80dd1516ad46 150 decoder_pos++;
dnleek 0:80dd1516ad46 151 if (decoder_pos >= packet_length) {
dnleek 0:80dd1516ad46 152 process_received_packet();
dnleek 0:80dd1516ad46 153
dnleek 0:80dd1516ad46 154 decoder_pos = 0;
dnleek 0:80dd1516ad46 155 if (rx_byte == SOF_SEQ[0]) {
dnleek 0:80dd1516ad46 156 decoder_state = DATA_DESTUFF_END;
dnleek 0:80dd1516ad46 157 } else {
dnleek 0:80dd1516ad46 158 decoder_state = SOF;
dnleek 0:80dd1516ad46 159 }
dnleek 0:80dd1516ad46 160 } else {
dnleek 0:80dd1516ad46 161 if (rx_byte == SOF_SEQ[0]) {
dnleek 0:80dd1516ad46 162 decoder_state = DATA_DESTUFF;
dnleek 0:80dd1516ad46 163 }
dnleek 0:80dd1516ad46 164 }
dnleek 0:80dd1516ad46 165 } else if (decoder_state == DATA_DESTUFF) {
dnleek 0:80dd1516ad46 166 decoder_state = DATA;
dnleek 0:80dd1516ad46 167 } else if (decoder_state == DATA_DESTUFF_END) {
dnleek 0:80dd1516ad46 168 decoder_state = SOF;
dnleek 0:80dd1516ad46 169 }
dnleek 0:80dd1516ad46 170 }
dnleek 0:80dd1516ad46 171 }
dnleek 0:80dd1516ad46 172
dnleek 0:80dd1516ad46 173 void Telemetry::process_received_packet() {
dnleek 0:80dd1516ad46 174 uint8_t opcode = received_packet.read_uint8();
dnleek 0:80dd1516ad46 175 if (opcode == OPCODE_DATA) {
dnleek 0:80dd1516ad46 176 uint8_t data_id = received_packet.read_uint8();
dnleek 0:80dd1516ad46 177 while (data_id != DATAID_TERMINATOR) {
dnleek 0:80dd1516ad46 178 if (data_id < data_count + 1) {
dnleek 0:80dd1516ad46 179 data[data_id - 1]->set_from_packet(received_packet);
dnleek 0:80dd1516ad46 180 } else {
dnleek 0:80dd1516ad46 181 hal.do_error("Unknown data ID");
dnleek 0:80dd1516ad46 182 }
dnleek 0:80dd1516ad46 183 data_id = received_packet.read_uint8();
dnleek 0:80dd1516ad46 184 }
dnleek 0:80dd1516ad46 185 } else {
dnleek 0:80dd1516ad46 186 hal.do_error("Unknown opcode");
dnleek 0:80dd1516ad46 187 }
dnleek 0:80dd1516ad46 188 }
dnleek 0:80dd1516ad46 189
dnleek 0:80dd1516ad46 190 size_t Telemetry::receive_available() {
dnleek 0:80dd1516ad46 191 // TODO: implement me
dnleek 0:80dd1516ad46 192 return 0;
dnleek 0:80dd1516ad46 193 }
dnleek 0:80dd1516ad46 194
dnleek 0:80dd1516ad46 195 uint8_t read_receive() {
dnleek 0:80dd1516ad46 196 // TODO: implement me
dnleek 0:80dd1516ad46 197 return 0;
dnleek 0:80dd1516ad46 198 }
dnleek 0:80dd1516ad46 199
dnleek 0:80dd1516ad46 200 FixedLengthTransmitPacket::FixedLengthTransmitPacket(HalInterface& hal,
dnleek 0:80dd1516ad46 201 size_t length) :
dnleek 0:80dd1516ad46 202 hal(hal),
dnleek 0:80dd1516ad46 203 length(length),
dnleek 0:80dd1516ad46 204 count(0) {
dnleek 0:80dd1516ad46 205 hal.transmit_byte(SOF1);
dnleek 0:80dd1516ad46 206 hal.transmit_byte(SOF2);
dnleek 0:80dd1516ad46 207
dnleek 0:80dd1516ad46 208 hal.transmit_byte((length >> 8) & 0xff);
dnleek 0:80dd1516ad46 209 hal.transmit_byte((length >> 0) & 0xff);
dnleek 0:80dd1516ad46 210
dnleek 0:80dd1516ad46 211 valid = true;
dnleek 0:80dd1516ad46 212 }
dnleek 0:80dd1516ad46 213
dnleek 0:80dd1516ad46 214 void FixedLengthTransmitPacket::write_byte(uint8_t data) {
dnleek 0:80dd1516ad46 215 if (!valid) {
dnleek 0:80dd1516ad46 216 hal.do_error("Writing to invalid packet");
dnleek 0:80dd1516ad46 217 return;
dnleek 0:80dd1516ad46 218 } else if (count + 1 > length) {
dnleek 0:80dd1516ad46 219 hal.do_error("Writing over packet length");
dnleek 0:80dd1516ad46 220 return;
dnleek 0:80dd1516ad46 221 }
dnleek 0:80dd1516ad46 222 hal.transmit_byte(data);
dnleek 0:80dd1516ad46 223 if (data == SOF1) {
dnleek 0:80dd1516ad46 224 hal.transmit_byte(0x00); // TODO: proper abstraction and magic numbers
dnleek 0:80dd1516ad46 225 }
dnleek 0:80dd1516ad46 226 count++;
dnleek 0:80dd1516ad46 227 }
dnleek 0:80dd1516ad46 228
dnleek 0:80dd1516ad46 229 void FixedLengthTransmitPacket::write_uint8(uint8_t data) {
dnleek 0:80dd1516ad46 230 write_byte(data);
dnleek 0:80dd1516ad46 231 }
dnleek 0:80dd1516ad46 232
dnleek 0:80dd1516ad46 233 void FixedLengthTransmitPacket::write_uint16(uint16_t data) {
dnleek 0:80dd1516ad46 234 write_byte((data >> 8) & 0xff);
dnleek 0:80dd1516ad46 235 write_byte((data >> 0) & 0xff);
dnleek 0:80dd1516ad46 236 }
dnleek 0:80dd1516ad46 237
dnleek 0:80dd1516ad46 238 void FixedLengthTransmitPacket::write_uint32(uint32_t data) {
dnleek 0:80dd1516ad46 239 write_byte((data >> 24) & 0xff);
dnleek 0:80dd1516ad46 240 write_byte((data >> 16) & 0xff);
dnleek 0:80dd1516ad46 241 write_byte((data >> 8) & 0xff);
dnleek 0:80dd1516ad46 242 write_byte((data >> 0) & 0xff);
dnleek 0:80dd1516ad46 243 }
dnleek 0:80dd1516ad46 244
dnleek 0:80dd1516ad46 245 void FixedLengthTransmitPacket::write_float(float data) {
dnleek 0:80dd1516ad46 246 // TODO: THIS IS ENDIANNESS DEPENDENT, ABSTRACT INTO HAL?
dnleek 0:80dd1516ad46 247 uint8_t *float_array = (uint8_t*) &data;
dnleek 0:80dd1516ad46 248 write_byte(float_array[3]);
dnleek 0:80dd1516ad46 249 write_byte(float_array[2]);
dnleek 0:80dd1516ad46 250 write_byte(float_array[1]);
dnleek 0:80dd1516ad46 251 write_byte(float_array[0]);
dnleek 0:80dd1516ad46 252 }
dnleek 0:80dd1516ad46 253
dnleek 0:80dd1516ad46 254 void FixedLengthTransmitPacket::finish() {
dnleek 0:80dd1516ad46 255 if (!valid) {
dnleek 0:80dd1516ad46 256 hal.do_error("Finish invalid packet");
dnleek 0:80dd1516ad46 257 return;
dnleek 0:80dd1516ad46 258 } else if (count != length) {
dnleek 0:80dd1516ad46 259 hal.do_error("TX packet under length");
dnleek 0:80dd1516ad46 260 return;
dnleek 0:80dd1516ad46 261 }
dnleek 0:80dd1516ad46 262
dnleek 0:80dd1516ad46 263 // TODO: add CRC check here
dnleek 0:80dd1516ad46 264 }
dnleek 0:80dd1516ad46 265
dnleek 0:80dd1516ad46 266 ReceivePacketBuffer::ReceivePacketBuffer(HalInterface& hal) :
dnleek 0:80dd1516ad46 267 hal(hal) {
dnleek 0:80dd1516ad46 268 new_packet();
dnleek 0:80dd1516ad46 269 }
dnleek 0:80dd1516ad46 270
dnleek 0:80dd1516ad46 271 void ReceivePacketBuffer::new_packet() {
dnleek 0:80dd1516ad46 272 packet_length = 0;
dnleek 0:80dd1516ad46 273 read_loc = 0;
dnleek 0:80dd1516ad46 274 }
dnleek 0:80dd1516ad46 275
dnleek 0:80dd1516ad46 276 void ReceivePacketBuffer::add_byte(uint8_t byte) {
dnleek 0:80dd1516ad46 277 if (packet_length >= MAX_RECEIVE_PACKET_LENGTH) {
dnleek 0:80dd1516ad46 278 hal.do_error("RX packet over length");
dnleek 0:80dd1516ad46 279 return;
dnleek 0:80dd1516ad46 280 }
dnleek 0:80dd1516ad46 281
dnleek 0:80dd1516ad46 282 data[packet_length] = byte;
dnleek 0:80dd1516ad46 283 packet_length++;
dnleek 0:80dd1516ad46 284 }
dnleek 0:80dd1516ad46 285
dnleek 0:80dd1516ad46 286 uint8_t ReceivePacketBuffer::read_uint8() {
dnleek 0:80dd1516ad46 287 if (read_loc + 1 > packet_length) {
dnleek 0:80dd1516ad46 288 hal.do_error("Read uint8 over length");
dnleek 0:80dd1516ad46 289 return 0;
dnleek 0:80dd1516ad46 290 }
dnleek 0:80dd1516ad46 291 read_loc += 1;
dnleek 0:80dd1516ad46 292 return data[read_loc - 1];
dnleek 0:80dd1516ad46 293 }
dnleek 0:80dd1516ad46 294
dnleek 0:80dd1516ad46 295 uint16_t ReceivePacketBuffer::read_uint16() {
dnleek 0:80dd1516ad46 296 if (read_loc + 2 > packet_length) {
dnleek 0:80dd1516ad46 297 hal.do_error("Read uint16 over length");
dnleek 0:80dd1516ad46 298 return 0;
dnleek 0:80dd1516ad46 299 }
dnleek 0:80dd1516ad46 300 read_loc += 2;
dnleek 0:80dd1516ad46 301 return ((uint16_t)data[read_loc - 2] << 8)
dnleek 0:80dd1516ad46 302 | ((uint16_t)data[read_loc - 1] << 0);
dnleek 0:80dd1516ad46 303 }
dnleek 0:80dd1516ad46 304
dnleek 0:80dd1516ad46 305 uint32_t ReceivePacketBuffer::read_uint32() {
dnleek 0:80dd1516ad46 306 if (read_loc + 4 > packet_length) {
dnleek 0:80dd1516ad46 307 hal.do_error("Read uint32 over length");
dnleek 0:80dd1516ad46 308 return 0;
dnleek 0:80dd1516ad46 309 }
dnleek 0:80dd1516ad46 310 read_loc += 4;
dnleek 0:80dd1516ad46 311 return ((uint32_t)data[read_loc - 4] << 24)
dnleek 0:80dd1516ad46 312 | ((uint32_t)data[read_loc - 3] << 16)
dnleek 0:80dd1516ad46 313 | ((uint32_t)data[read_loc - 2] << 8)
dnleek 0:80dd1516ad46 314 | ((uint32_t)data[read_loc - 1] << 0);
dnleek 0:80dd1516ad46 315 }
dnleek 0:80dd1516ad46 316
dnleek 0:80dd1516ad46 317 float ReceivePacketBuffer::read_float() {
dnleek 0:80dd1516ad46 318 if (read_loc + 4 > packet_length) {
dnleek 0:80dd1516ad46 319 hal.do_error("Read float over length");
dnleek 0:80dd1516ad46 320 return 0;
dnleek 0:80dd1516ad46 321 }
dnleek 0:80dd1516ad46 322 read_loc += 4;
dnleek 0:80dd1516ad46 323 float out = 0;
dnleek 0:80dd1516ad46 324 uint8_t* out_array = (uint8_t*)&out;
dnleek 0:80dd1516ad46 325 out_array[0] = data[read_loc - 1];
dnleek 0:80dd1516ad46 326 out_array[1] = data[read_loc - 2];
dnleek 0:80dd1516ad46 327 out_array[2] = data[read_loc - 3];
dnleek 0:80dd1516ad46 328 out_array[3] = data[read_loc - 4];
dnleek 0:80dd1516ad46 329 return out;
dnleek 0:80dd1516ad46 330 }
dnleek 0:80dd1516ad46 331
dnleek 0:80dd1516ad46 332 }