Ian Krase / telemetry

ducky's telemetry library

telemetry.cpp@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?

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