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?
User | Revision | Line number | New 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 | } |