telemetry
Dependents: Everything Sequential_Timing FixedPWM FixedPWMWill
Diff: packet.cpp
- Revision:
- 0:aca5a32d2759
diff -r 000000000000 -r aca5a32d2759 packet.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/packet.cpp Fri Mar 18 22:33:32 2016 +0000 @@ -0,0 +1,173 @@ +/** + * Transmit and receive packet interfaces + */ + +#include "telemetry.h" + +namespace telemetry { + +namespace internal { + template<> void pkt_write<uint8_t>(TransmitPacket& interface, uint8_t data) { + interface.write_uint8(data); + } + template<> void pkt_write<uint16_t>(TransmitPacket& interface, uint16_t data) { + interface.write_uint16(data); + } + template<> void pkt_write<uint32_t>(TransmitPacket& interface, uint32_t data) { + interface.write_uint32(data); + } + template<> void pkt_write<float>(TransmitPacket& interface, float data) { + interface.write_float(data); + } + + template<> uint8_t buf_read<uint8_t>(ReceivePacketBuffer& buffer) { + return buffer.read_float(); + } + template<> uint16_t buf_read<uint16_t>(ReceivePacketBuffer& buffer) { + return buffer.read_uint16(); + } + template<> uint32_t buf_read<uint32_t>(ReceivePacketBuffer& buffer) { + return buffer.read_uint32(); + } + template<> float buf_read<float>(ReceivePacketBuffer& buffer) { + return buffer.read_float(); + } +} + +FixedLengthTransmitPacket::FixedLengthTransmitPacket(HalInterface& hal, + size_t length) : + hal(hal), + length(length), + count(0) { + for (int i=0; i<protocol::SOF_LENGTH; i++) { + hal.transmit_byte(protocol::SOF_SEQ[i]); + } + + hal.transmit_byte((length >> 8) & 0xff); + hal.transmit_byte((length >> 0) & 0xff); + + valid = true; +} + +void FixedLengthTransmitPacket::write_byte(uint8_t data) { + if (!valid) { + hal.do_error("Writing to invalid packet"); + return; + } else if (count + 1 > length) { + hal.do_error("Writing over packet length"); + return; + } + hal.transmit_byte(data); +#if SOF_LENGTH > 2 +#error "Byte stuffing algorithm does not work for SOF_LENGTH > 2" +#endif + if (data == protocol::SOF_SEQ[0]) { + hal.transmit_byte(protocol::SOF_SEQ0_STUFF); + } + count++; +} + +void FixedLengthTransmitPacket::write_uint8(uint8_t data) { + write_byte(data); +} + +void FixedLengthTransmitPacket::write_uint16(uint16_t data) { + write_byte((data >> 8) & 0xff); + write_byte((data >> 0) & 0xff); +} + +void FixedLengthTransmitPacket::write_uint32(uint32_t data) { + write_byte((data >> 24) & 0xff); + write_byte((data >> 16) & 0xff); + write_byte((data >> 8) & 0xff); + write_byte((data >> 0) & 0xff); +} + +void FixedLengthTransmitPacket::write_float(float data) { + // TODO: THIS IS ENDIANNESS DEPENDENT, ABSTRACT INTO HAL? + uint8_t *float_array = (uint8_t*) &data; + write_byte(float_array[3]); + write_byte(float_array[2]); + write_byte(float_array[1]); + write_byte(float_array[0]); +} + +void FixedLengthTransmitPacket::finish() { + if (!valid) { + hal.do_error("Finish invalid packet"); + return; + } else if (count != length) { + hal.do_error("TX packet under length"); + return; + } + + // TODO: add CRC check here +} + +ReceivePacketBuffer::ReceivePacketBuffer(HalInterface& hal) : + hal(hal) { + new_packet(); +} + +void ReceivePacketBuffer::new_packet() { + packet_length = 0; + read_loc = 0; +} + +void ReceivePacketBuffer::add_byte(uint8_t byte) { + if (packet_length >= MAX_RECEIVE_PACKET_LENGTH) { + hal.do_error("RX packet over length"); + return; + } + + data[packet_length] = byte; + packet_length++; +} + +uint8_t ReceivePacketBuffer::read_uint8() { + if (read_loc + 1 > packet_length) { + hal.do_error("Read uint8 over length"); + return 0; + } + read_loc += 1; + return data[read_loc - 1]; +} + +uint16_t ReceivePacketBuffer::read_uint16() { + if (read_loc + 2 > packet_length) { + hal.do_error("Read uint16 over length"); + return 0; + } + read_loc += 2; + return ((uint16_t)data[read_loc - 2] << 8) + | ((uint16_t)data[read_loc - 1] << 0); +} + +uint32_t ReceivePacketBuffer::read_uint32() { + if (read_loc + 4 > packet_length) { + hal.do_error("Read uint32 over length"); + return 0; + } + read_loc += 4; + return ((uint32_t)data[read_loc - 4] << 24) + | ((uint32_t)data[read_loc - 3] << 16) + | ((uint32_t)data[read_loc - 2] << 8) + | ((uint32_t)data[read_loc - 1] << 0); +} + +float ReceivePacketBuffer::read_float() { + if (read_loc + 4 > packet_length) { + hal.do_error("Read float over length"); + return 0; + } + read_loc += 4; + float out = 0; + uint8_t* out_array = (uint8_t*)&out; + out_array[0] = data[read_loc - 1]; + out_array[1] = data[read_loc - 2]; + out_array[2] = data[read_loc - 3]; + out_array[3] = data[read_loc - 4]; + return out; +} + +}