telemetry.cpp

00001 /*
00002  * telemetry.cpp
00003  *
00004  *  Created on: Mar 2, 2015
00005  *      Author: Ducky
00006  *
00007  * Implementation for the base Telemetry class.
00008  */
00009 
00010 #include <telemetry.h>
00011 
00012 namespace telemetry {
00013 
00014 size_t Telemetry::add_data(Data& new_data) {
00015   if (data_count >= MAX_DATA_PER_TELEMETRY) {
00016     do_error("MAX_DATA_PER_TELEMETRY limit reached.");
00017     return 0;
00018   }
00019   if (header_transmitted) {
00020     do_error("Cannot add new data after header transmitted.");
00021     return 0;
00022   }
00023   data[data_count] = &new_data;
00024   data_updated[data_count] = true;
00025   data_count++;
00026   return data_count - 1;
00027 }
00028 
00029 void Telemetry::mark_data_updated(size_t data_id) {
00030   data_updated[data_id] = true;
00031 }
00032 
00033 void Telemetry::transmit_header() {
00034   if (header_transmitted) {
00035     do_error("Cannot retransmit header.");
00036     return;
00037   }
00038 
00039   size_t packet_legnth = 2; // opcode + sequence
00040   for (int data_idx = 0; data_idx < data_count; data_idx++) {
00041     packet_legnth += 2; // data ID, data type
00042     packet_legnth += data[data_idx]->get_header_kvrs_length();
00043     packet_legnth += 1; // terminator record id
00044   }
00045   packet_legnth++;  // terminator "record"
00046 
00047   FixedLengthTransmitPacket packet(hal, packet_legnth);
00048 
00049   packet.write_uint8(OPCODE_HEADER);
00050   packet.write_uint8(packet_tx_sequence);
00051   for (int data_idx = 0; data_idx < data_count; data_idx++) {
00052     packet.write_uint8(data_idx+1);
00053     packet.write_uint8(data[data_idx]->get_data_type());
00054     data[data_idx]->write_header_kvrs(packet);
00055     packet.write_uint8(RECORDID_TERMINATOR);
00056   }
00057   packet.write_uint8(DATAID_TERMINATOR);
00058 
00059   packet.finish();
00060 
00061   packet_tx_sequence++;
00062   header_transmitted = true;
00063 }
00064 
00065 void Telemetry::do_io() {
00066   transmit_data();
00067   process_received_data();
00068 }
00069 
00070 void Telemetry::transmit_data() {
00071   if (!header_transmitted) {
00072     do_error("Must transmit header before transmitting data.");
00073     return;
00074   }
00075 
00076   // Keep a local copy to make it more thread-safe
00077   bool data_updated_local[MAX_DATA_PER_TELEMETRY];
00078 
00079   size_t packet_legnth = 2; // opcode + sequence
00080   for (int data_idx = 0; data_idx < data_count; data_idx++) {
00081     data_updated_local[data_idx] = data_updated[data_idx];
00082     data_updated[data_idx] = 0;
00083     if (data_updated_local[data_idx]) {
00084       packet_legnth += 1; // data ID
00085       packet_legnth += data[data_idx]->get_payload_length();
00086     }
00087   }
00088   packet_legnth++;  // terminator "record"
00089 
00090   FixedLengthTransmitPacket packet(hal, packet_legnth);
00091 
00092   packet.write_uint8(OPCODE_DATA);
00093   packet.write_uint8(packet_tx_sequence);
00094   for (int data_idx = 0; data_idx < data_count; data_idx++) {
00095     if (data_updated_local[data_idx]) {
00096       packet.write_uint8(data_idx+1);
00097       data[data_idx]->write_payload(packet);
00098     }
00099   }
00100   packet.write_uint8(DATAID_TERMINATOR);
00101 
00102   packet.finish();
00103 
00104   packet_tx_sequence++;
00105 }
00106 
00107 void Telemetry::process_received_data() {
00108   uint32_t current_time = hal.get_time_ms();
00109 
00110   if (decoder_last_receive_ms <= current_time) {
00111     if (!decoder_last_received && decoder_state != SOF && decoder_pos != 0
00112         && (decoder_last_receive_ms - current_time > DECODER_TIMEOUT_MS)) {
00113       decoder_pos = 0;
00114       packet_length = 0;
00115       decoder_state = SOF;
00116       hal.do_error("RX timeout");
00117     }
00118   } else {
00119     // timer overflowed, do nothing
00120   }
00121   decoder_last_receive_ms = current_time;
00122 
00123   decoder_last_received = false;
00124   while (hal.rx_available()) {
00125     decoder_last_received = true;
00126 
00127     uint8_t rx_byte = hal.receive_byte();
00128 
00129     if (decoder_state == SOF) {
00130       if (rx_byte == SOF_SEQ[decoder_pos]) {
00131         decoder_pos++;
00132         if (decoder_pos >= (sizeof(SOF_SEQ) / sizeof(SOF_SEQ[0]))) {
00133           decoder_pos = 0;
00134           packet_length = 0;
00135           decoder_state = LENGTH;
00136         }
00137       } else {
00138         decoder_pos = 0;
00139         // TODO: pass rest of data through
00140       }
00141     } else if (decoder_state == LENGTH) {
00142       packet_length = (packet_length << 8) | rx_byte;
00143       decoder_pos++;
00144       if (decoder_pos >= LENGTH_SIZE) {
00145         decoder_pos = 0;
00146         decoder_state = DATA;
00147       }
00148     } else if (decoder_state == DATA) {
00149       received_packet.add_byte(rx_byte);
00150       decoder_pos++;
00151       if (decoder_pos >= packet_length) {
00152         process_received_packet();
00153 
00154         decoder_pos = 0;
00155         if (rx_byte == SOF_SEQ[0]) {
00156           decoder_state = DATA_DESTUFF_END;
00157         } else {
00158           decoder_state = SOF;
00159         }
00160       } else {
00161         if (rx_byte == SOF_SEQ[0]) {
00162           decoder_state = DATA_DESTUFF;
00163         }
00164       }
00165     } else if (decoder_state == DATA_DESTUFF) {
00166       decoder_state = DATA;
00167     } else if (decoder_state == DATA_DESTUFF_END) {
00168       decoder_state = SOF;
00169     }
00170   }
00171 }
00172 
00173 void Telemetry::process_received_packet() {
00174   uint8_t opcode = received_packet.read_uint8();
00175   if (opcode == OPCODE_DATA) {
00176     uint8_t data_id = received_packet.read_uint8();
00177     while (data_id != DATAID_TERMINATOR) {
00178       if (data_id < data_count + 1) {
00179         data[data_id - 1]->set_from_packet(received_packet);
00180       } else {
00181         hal.do_error("Unknown data ID");
00182       }
00183       data_id = received_packet.read_uint8();
00184     }
00185   } else {
00186     hal.do_error("Unknown opcode");
00187   }
00188 }
00189 
00190 size_t Telemetry::receive_available() {
00191   // TODO: implement me
00192   return 0;
00193 }
00194 
00195 uint8_t read_receive() {
00196   // TODO: implement me
00197   return 0;
00198 }
00199 
00200 FixedLengthTransmitPacket::FixedLengthTransmitPacket(HalInterface& hal,
00201     size_t length) :
00202         hal(hal),
00203         length(length),
00204         count(0) {
00205   hal.transmit_byte(SOF1);
00206   hal.transmit_byte(SOF2);
00207 
00208   hal.transmit_byte((length >> 8) & 0xff);
00209   hal.transmit_byte((length >> 0) & 0xff);
00210 
00211   valid = true;
00212 }
00213 
00214 void FixedLengthTransmitPacket::write_byte(uint8_t data) {
00215   if (!valid) {
00216     hal.do_error("Writing to invalid packet");
00217     return;
00218   } else if (count + 1 > length) {
00219     hal.do_error("Writing over packet length");
00220     return;
00221   }
00222   hal.transmit_byte(data);
00223   if (data == SOF1) {
00224     hal.transmit_byte(0x00);  // TODO: proper abstraction and magic numbers
00225   }
00226   count++;
00227 }
00228 
00229 void FixedLengthTransmitPacket::write_uint8(uint8_t data) {
00230   write_byte(data);
00231 }
00232 
00233 void FixedLengthTransmitPacket::write_uint16(uint16_t data) {
00234   write_byte((data >> 8) & 0xff);
00235   write_byte((data >> 0) & 0xff);
00236 }
00237 
00238 void FixedLengthTransmitPacket::write_uint32(uint32_t data) {
00239   write_byte((data >> 24) & 0xff);
00240   write_byte((data >> 16) & 0xff);
00241   write_byte((data >> 8) & 0xff);
00242   write_byte((data >> 0) & 0xff);
00243 }
00244 
00245 void FixedLengthTransmitPacket::write_float(float data) {
00246   // TODO: THIS IS ENDIANNESS DEPENDENT, ABSTRACT INTO HAL?
00247   uint8_t *float_array = (uint8_t*) &data;
00248   write_byte(float_array[3]);
00249   write_byte(float_array[2]);
00250   write_byte(float_array[1]);
00251   write_byte(float_array[0]);
00252 }
00253 
00254 void FixedLengthTransmitPacket::finish() {
00255   if (!valid) {
00256     hal.do_error("Finish invalid packet");
00257     return;
00258   } else if (count != length) {
00259     hal.do_error("TX packet under length");
00260     return;
00261   }
00262 
00263   // TODO: add CRC check here
00264 }
00265 
00266 ReceivePacketBuffer::ReceivePacketBuffer(HalInterface& hal) :
00267     hal(hal) {
00268   new_packet();
00269 }
00270 
00271 void ReceivePacketBuffer::new_packet() {
00272   packet_length = 0;
00273   read_loc = 0;
00274 }
00275 
00276 void ReceivePacketBuffer::add_byte(uint8_t byte) {
00277   if (packet_length >= MAX_RECEIVE_PACKET_LENGTH) {
00278     hal.do_error("RX packet over length");
00279     return;
00280   }
00281 
00282   data[packet_length] = byte;
00283   packet_length++;
00284 }
00285 
00286 uint8_t ReceivePacketBuffer::read_uint8() {
00287   if (read_loc + 1 > packet_length) {
00288     hal.do_error("Read uint8 over length");
00289     return 0;
00290   }
00291   read_loc += 1;
00292   return data[read_loc - 1];
00293 }
00294 
00295 uint16_t ReceivePacketBuffer::read_uint16() {
00296   if (read_loc + 2 > packet_length) {
00297     hal.do_error("Read uint16 over length");
00298     return 0;
00299   }
00300   read_loc += 2;
00301   return ((uint16_t)data[read_loc - 2] << 8)
00302        | ((uint16_t)data[read_loc - 1] << 0);
00303 }
00304 
00305 uint32_t ReceivePacketBuffer::read_uint32() {
00306   if (read_loc + 4 > packet_length) {
00307     hal.do_error("Read uint32 over length");
00308     return 0;
00309   }
00310   read_loc += 4;
00311   return ((uint32_t)data[read_loc - 4] << 24)
00312        | ((uint32_t)data[read_loc - 3] << 16)
00313        | ((uint32_t)data[read_loc - 2] << 8)
00314        | ((uint32_t)data[read_loc - 1] << 0);
00315 }
00316 
00317 float ReceivePacketBuffer::read_float() {
00318   if (read_loc + 4 > packet_length) {
00319     hal.do_error("Read float over length");
00320     return 0;
00321   }
00322   read_loc += 4;
00323   float out = 0;
00324   uint8_t* out_array = (uint8_t*)&out;
00325   out_array[0] = data[read_loc - 1];
00326   out_array[1] = data[read_loc - 2];
00327   out_array[2] = data[read_loc - 3];
00328   out_array[3] = data[read_loc - 4];
00329   return out;
00330 }
00331 
00332 }