EE192 Team 4 / telemetry

telemetry

Dependents:   Everything Sequential_Timing FixedPWM FixedPWMWill

packet.cpp@0:aca5a32d2759, 2016-03-18 (annotated)

Committer:
vsutardja
Date:
Fri Mar 18 22:33:32 2016 +0000
Revision:
0:aca5a32d2759
init

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsutardja 0:aca5a32d2759 1 /**
vsutardja 0:aca5a32d2759 2 * Transmit and receive packet interfaces
vsutardja 0:aca5a32d2759 3 */
vsutardja 0:aca5a32d2759 4
vsutardja 0:aca5a32d2759 5 #include "telemetry.h"
vsutardja 0:aca5a32d2759 6
vsutardja 0:aca5a32d2759 7 namespace telemetry {
vsutardja 0:aca5a32d2759 8
vsutardja 0:aca5a32d2759 9 namespace internal {
vsutardja 0:aca5a32d2759 10 template<> void pkt_write<uint8_t>(TransmitPacket& interface, uint8_t data) {
vsutardja 0:aca5a32d2759 11 interface.write_uint8(data);
vsutardja 0:aca5a32d2759 12 }
vsutardja 0:aca5a32d2759 13 template<> void pkt_write<uint16_t>(TransmitPacket& interface, uint16_t data) {
vsutardja 0:aca5a32d2759 14 interface.write_uint16(data);
vsutardja 0:aca5a32d2759 15 }
vsutardja 0:aca5a32d2759 16 template<> void pkt_write<uint32_t>(TransmitPacket& interface, uint32_t data) {
vsutardja 0:aca5a32d2759 17 interface.write_uint32(data);
vsutardja 0:aca5a32d2759 18 }
vsutardja 0:aca5a32d2759 19 template<> void pkt_write<float>(TransmitPacket& interface, float data) {
vsutardja 0:aca5a32d2759 20 interface.write_float(data);
vsutardja 0:aca5a32d2759 21 }
vsutardja 0:aca5a32d2759 22
vsutardja 0:aca5a32d2759 23 template<> uint8_t buf_read<uint8_t>(ReceivePacketBuffer& buffer) {
vsutardja 0:aca5a32d2759 24 return buffer.read_float();
vsutardja 0:aca5a32d2759 25 }
vsutardja 0:aca5a32d2759 26 template<> uint16_t buf_read<uint16_t>(ReceivePacketBuffer& buffer) {
vsutardja 0:aca5a32d2759 27 return buffer.read_uint16();
vsutardja 0:aca5a32d2759 28 }
vsutardja 0:aca5a32d2759 29 template<> uint32_t buf_read<uint32_t>(ReceivePacketBuffer& buffer) {
vsutardja 0:aca5a32d2759 30 return buffer.read_uint32();
vsutardja 0:aca5a32d2759 31 }
vsutardja 0:aca5a32d2759 32 template<> float buf_read<float>(ReceivePacketBuffer& buffer) {
vsutardja 0:aca5a32d2759 33 return buffer.read_float();
vsutardja 0:aca5a32d2759 34 }
vsutardja 0:aca5a32d2759 35 }
vsutardja 0:aca5a32d2759 36
vsutardja 0:aca5a32d2759 37 FixedLengthTransmitPacket::FixedLengthTransmitPacket(HalInterface& hal,
vsutardja 0:aca5a32d2759 38 size_t length) :
vsutardja 0:aca5a32d2759 39 hal(hal),
vsutardja 0:aca5a32d2759 40 length(length),
vsutardja 0:aca5a32d2759 41 count(0) {
vsutardja 0:aca5a32d2759 42 for (int i=0; i<protocol::SOF_LENGTH; i++) {
vsutardja 0:aca5a32d2759 43 hal.transmit_byte(protocol::SOF_SEQ[i]);
vsutardja 0:aca5a32d2759 44 }
vsutardja 0:aca5a32d2759 45
vsutardja 0:aca5a32d2759 46 hal.transmit_byte((length >> 8) & 0xff);
vsutardja 0:aca5a32d2759 47 hal.transmit_byte((length >> 0) & 0xff);
vsutardja 0:aca5a32d2759 48
vsutardja 0:aca5a32d2759 49 valid = true;
vsutardja 0:aca5a32d2759 50 }
vsutardja 0:aca5a32d2759 51
vsutardja 0:aca5a32d2759 52 void FixedLengthTransmitPacket::write_byte(uint8_t data) {
vsutardja 0:aca5a32d2759 53 if (!valid) {
vsutardja 0:aca5a32d2759 54 hal.do_error("Writing to invalid packet");
vsutardja 0:aca5a32d2759 55 return;
vsutardja 0:aca5a32d2759 56 } else if (count + 1 > length) {
vsutardja 0:aca5a32d2759 57 hal.do_error("Writing over packet length");
vsutardja 0:aca5a32d2759 58 return;
vsutardja 0:aca5a32d2759 59 }
vsutardja 0:aca5a32d2759 60 hal.transmit_byte(data);
vsutardja 0:aca5a32d2759 61 #if SOF_LENGTH > 2
vsutardja 0:aca5a32d2759 62 #error "Byte stuffing algorithm does not work for SOF_LENGTH > 2"
vsutardja 0:aca5a32d2759 63 #endif
vsutardja 0:aca5a32d2759 64 if (data == protocol::SOF_SEQ[0]) {
vsutardja 0:aca5a32d2759 65 hal.transmit_byte(protocol::SOF_SEQ0_STUFF);
vsutardja 0:aca5a32d2759 66 }
vsutardja 0:aca5a32d2759 67 count++;
vsutardja 0:aca5a32d2759 68 }
vsutardja 0:aca5a32d2759 69
vsutardja 0:aca5a32d2759 70 void FixedLengthTransmitPacket::write_uint8(uint8_t data) {
vsutardja 0:aca5a32d2759 71 write_byte(data);
vsutardja 0:aca5a32d2759 72 }
vsutardja 0:aca5a32d2759 73
vsutardja 0:aca5a32d2759 74 void FixedLengthTransmitPacket::write_uint16(uint16_t data) {
vsutardja 0:aca5a32d2759 75 write_byte((data >> 8) & 0xff);
vsutardja 0:aca5a32d2759 76 write_byte((data >> 0) & 0xff);
vsutardja 0:aca5a32d2759 77 }
vsutardja 0:aca5a32d2759 78
vsutardja 0:aca5a32d2759 79 void FixedLengthTransmitPacket::write_uint32(uint32_t data) {
vsutardja 0:aca5a32d2759 80 write_byte((data >> 24) & 0xff);
vsutardja 0:aca5a32d2759 81 write_byte((data >> 16) & 0xff);
vsutardja 0:aca5a32d2759 82 write_byte((data >> 8) & 0xff);
vsutardja 0:aca5a32d2759 83 write_byte((data >> 0) & 0xff);
vsutardja 0:aca5a32d2759 84 }
vsutardja 0:aca5a32d2759 85
vsutardja 0:aca5a32d2759 86 void FixedLengthTransmitPacket::write_float(float data) {
vsutardja 0:aca5a32d2759 87 // TODO: THIS IS ENDIANNESS DEPENDENT, ABSTRACT INTO HAL?
vsutardja 0:aca5a32d2759 88 uint8_t *float_array = (uint8_t*) &data;
vsutardja 0:aca5a32d2759 89 write_byte(float_array[3]);
vsutardja 0:aca5a32d2759 90 write_byte(float_array[2]);
vsutardja 0:aca5a32d2759 91 write_byte(float_array[1]);
vsutardja 0:aca5a32d2759 92 write_byte(float_array[0]);
vsutardja 0:aca5a32d2759 93 }
vsutardja 0:aca5a32d2759 94
vsutardja 0:aca5a32d2759 95 void FixedLengthTransmitPacket::finish() {
vsutardja 0:aca5a32d2759 96 if (!valid) {
vsutardja 0:aca5a32d2759 97 hal.do_error("Finish invalid packet");
vsutardja 0:aca5a32d2759 98 return;
vsutardja 0:aca5a32d2759 99 } else if (count != length) {
vsutardja 0:aca5a32d2759 100 hal.do_error("TX packet under length");
vsutardja 0:aca5a32d2759 101 return;
vsutardja 0:aca5a32d2759 102 }
vsutardja 0:aca5a32d2759 103
vsutardja 0:aca5a32d2759 104 // TODO: add CRC check here
vsutardja 0:aca5a32d2759 105 }
vsutardja 0:aca5a32d2759 106
vsutardja 0:aca5a32d2759 107 ReceivePacketBuffer::ReceivePacketBuffer(HalInterface& hal) :
vsutardja 0:aca5a32d2759 108 hal(hal) {
vsutardja 0:aca5a32d2759 109 new_packet();
vsutardja 0:aca5a32d2759 110 }
vsutardja 0:aca5a32d2759 111
vsutardja 0:aca5a32d2759 112 void ReceivePacketBuffer::new_packet() {
vsutardja 0:aca5a32d2759 113 packet_length = 0;
vsutardja 0:aca5a32d2759 114 read_loc = 0;
vsutardja 0:aca5a32d2759 115 }
vsutardja 0:aca5a32d2759 116
vsutardja 0:aca5a32d2759 117 void ReceivePacketBuffer::add_byte(uint8_t byte) {
vsutardja 0:aca5a32d2759 118 if (packet_length >= MAX_RECEIVE_PACKET_LENGTH) {
vsutardja 0:aca5a32d2759 119 hal.do_error("RX packet over length");
vsutardja 0:aca5a32d2759 120 return;
vsutardja 0:aca5a32d2759 121 }
vsutardja 0:aca5a32d2759 122
vsutardja 0:aca5a32d2759 123 data[packet_length] = byte;
vsutardja 0:aca5a32d2759 124 packet_length++;
vsutardja 0:aca5a32d2759 125 }
vsutardja 0:aca5a32d2759 126
vsutardja 0:aca5a32d2759 127 uint8_t ReceivePacketBuffer::read_uint8() {
vsutardja 0:aca5a32d2759 128 if (read_loc + 1 > packet_length) {
vsutardja 0:aca5a32d2759 129 hal.do_error("Read uint8 over length");
vsutardja 0:aca5a32d2759 130 return 0;
vsutardja 0:aca5a32d2759 131 }
vsutardja 0:aca5a32d2759 132 read_loc += 1;
vsutardja 0:aca5a32d2759 133 return data[read_loc - 1];
vsutardja 0:aca5a32d2759 134 }
vsutardja 0:aca5a32d2759 135
vsutardja 0:aca5a32d2759 136 uint16_t ReceivePacketBuffer::read_uint16() {
vsutardja 0:aca5a32d2759 137 if (read_loc + 2 > packet_length) {
vsutardja 0:aca5a32d2759 138 hal.do_error("Read uint16 over length");
vsutardja 0:aca5a32d2759 139 return 0;
vsutardja 0:aca5a32d2759 140 }
vsutardja 0:aca5a32d2759 141 read_loc += 2;
vsutardja 0:aca5a32d2759 142 return ((uint16_t)data[read_loc - 2] << 8)
vsutardja 0:aca5a32d2759 143 | ((uint16_t)data[read_loc - 1] << 0);
vsutardja 0:aca5a32d2759 144 }
vsutardja 0:aca5a32d2759 145
vsutardja 0:aca5a32d2759 146 uint32_t ReceivePacketBuffer::read_uint32() {
vsutardja 0:aca5a32d2759 147 if (read_loc + 4 > packet_length) {
vsutardja 0:aca5a32d2759 148 hal.do_error("Read uint32 over length");
vsutardja 0:aca5a32d2759 149 return 0;
vsutardja 0:aca5a32d2759 150 }
vsutardja 0:aca5a32d2759 151 read_loc += 4;
vsutardja 0:aca5a32d2759 152 return ((uint32_t)data[read_loc - 4] << 24)
vsutardja 0:aca5a32d2759 153 | ((uint32_t)data[read_loc - 3] << 16)
vsutardja 0:aca5a32d2759 154 | ((uint32_t)data[read_loc - 2] << 8)
vsutardja 0:aca5a32d2759 155 | ((uint32_t)data[read_loc - 1] << 0);
vsutardja 0:aca5a32d2759 156 }
vsutardja 0:aca5a32d2759 157
vsutardja 0:aca5a32d2759 158 float ReceivePacketBuffer::read_float() {
vsutardja 0:aca5a32d2759 159 if (read_loc + 4 > packet_length) {
vsutardja 0:aca5a32d2759 160 hal.do_error("Read float over length");
vsutardja 0:aca5a32d2759 161 return 0;
vsutardja 0:aca5a32d2759 162 }
vsutardja 0:aca5a32d2759 163 read_loc += 4;
vsutardja 0:aca5a32d2759 164 float out = 0;
vsutardja 0:aca5a32d2759 165 uint8_t* out_array = (uint8_t*)&out;
vsutardja 0:aca5a32d2759 166 out_array[0] = data[read_loc - 1];
vsutardja 0:aca5a32d2759 167 out_array[1] = data[read_loc - 2];
vsutardja 0:aca5a32d2759 168 out_array[2] = data[read_loc - 3];
vsutardja 0:aca5a32d2759 169 out_array[3] = data[read_loc - 4];
vsutardja 0:aca5a32d2759 170 return out;
vsutardja 0:aca5a32d2759 171 }
vsutardja 0:aca5a32d2759 172
vsutardja 0:aca5a32d2759 173 }