diff --git a/arduino/Locapack/Locapack.cpp b/arduino/Locapack/Locapack.cpp index 1db62bbf935ea7fa70c5808ece5848ded2de1ab6..787edcd7f2646579177aa1a94f1317c845f964c5 100644 --- a/arduino/Locapack/Locapack.cpp +++ b/arduino/Locapack/Locapack.cpp @@ -6,14 +6,11 @@ #include "Locapack.h" #include <machine/endian.h> -/// -/// @brief The protocols name enumeration as strings -/// -const char* protocolsListAsString[] = { "TWR", "SDS-TWR", "BB-TWR" }; Locapack::Locapack() { - _sequence_number = 0; + _sequence_number_universalGnssPacket = 0; + _sequence_number_locallyReferencedPacket = 0; setdevice_id16(0); } @@ -208,7 +205,97 @@ int Locapack::createUniversalGnssPacket(universalGnssPacket_t* universalGnssPack // uint8_t payload_len the generated packet length // uint8_t* buffer the given buffer - len = makePacket(PROTOCOL_V1, false, true, PACKET_TYPE_UNIVERSAL_GNSS, ++_sequence_number, + len = makePacket(PROTOCOL_V1, false, true, PACKET_TYPE_UNIVERSAL_GNSS, ++_sequence_number_universalGnssPacket, + (uint64_t)millis(), 0, &_device_id, payload_buffer, payload_buffer_len, buffer); + + return len; +} + + +int Locapack::makeLocallyReferencedPayload(float x, float y, bool z_present, float z, bool dop_present, float dop, + bool frameofref_id_present, uint64_t frameofref_id, uint8_t* buffer) +{ + int len = 0; + + buffer[0] = (z_present == true ? 0x80 : 0) | (dop_present == true ? 0x40 : 0) | (frameofref_id_present == true ? 0x20 : 0); + len += 1; + + encodeFloat(x, &buffer[len]); len += 4; + encodeFloat(y, &buffer[len]); len += 4; + if (z_present) + { + encodeFloat(z, &buffer[len]); len += 4; + } + if (dop_present) + { + encodeFloat(dop, &buffer[len]); len += 4; + } + if (frameofref_id_present) + { + encodeUint64(frameofref_id, &buffer[len]); len += 8; + } + return len; +} + + +int Locapack::decodeLocallyReferencedPayload(float* x, float* y, bool* z_present, float* z, bool* dop_present, float* dop, + bool* frameofref_id_present, uint64_t* frameofref_id, uint8_t* buffer) +{ + int len = 0; + + if (buffer[0] & 0x80) *z_present = true; else *z_present = false; + if (buffer[0] & 0x40) *dop_present = true; else *dop_present = false; + if (buffer[0] & 0x20) *frameofref_id_present = true; else *frameofref_id_present = false; + + len += 1; + *x = decodeFloat(&buffer[len]); len += 4; + *y = decodeFloat(&buffer[len]); len += 4; + if (*z_present) + { + *z = decodeFloat(&buffer[len]); len += 4; + } + if (*dop_present) + { + *dop = decodeFloat(&buffer[len]); len += 4; + } + if (*frameofref_id_present) + { + *frameofref_id = decodeUint64(&buffer[len]); len += 8; + } + return len; +} + + +int Locapack::createLocallyReferencedPacket(locallyReferencedPacket_t* locallyReferencedPacket, uint8_t* buffer) +{ + int len = 0; + uint8_t payload_buffer[MAX_BUFFER_LEN]; + int payload_buffer_len = 0; + + payload_buffer_len = makeLocallyReferencedPayload( + locallyReferencedPacket->x, + locallyReferencedPacket->y, + locallyReferencedPacket->z_present, + locallyReferencedPacket->z, + locallyReferencedPacket->dop_present, + locallyReferencedPacket->dop, + locallyReferencedPacket->frameofref_id_present, + locallyReferencedPacket->frameofref_id, + payload_buffer); + + // protocol_version_t PROTOCOL_V1 + // uint8_t movement_id_presence_flag false + // uint8_t timestamp_presence_flag true + // packet_type_t packet_type PACKET_TYPE_LOCALLY_REFERENCED + // uint16_t sequence_number (internal sequence_number) + // uint64_t timestamp (millis) + // uint8_t movement_id disabled + // device_id_t* device_id (internal device_id) + // uint8_t* payload the LocallyReferencedPacket generated + // uint8_t payload_len the generated packet length + // uint8_t* buffer the given buffer + + len = makePacket(PROTOCOL_V1, false, true, PACKET_TYPE_LOCALLY_REFERENCED, ++_sequence_number_locallyReferencedPacket, (uint64_t)millis(), 0, &_device_id, payload_buffer, payload_buffer_len, buffer); return len; @@ -251,7 +338,16 @@ int Locapack::parseLocaPacket(locapacket_t* locapacket, uint8_t* buffer) break; case PACKET_TYPE_LOCALLY_REFERENCED: - return 0; // ToDO + decoded_payload_len = decodeLocallyReferencedPayload(&locapacket->locallyReferencedPacket.x, + &locapacket->locallyReferencedPacket.y, + &locapacket->locallyReferencedPacket.z_present, + &locapacket->locallyReferencedPacket.z, + &locapacket->locallyReferencedPacket.dop_present, + &locapacket->locallyReferencedPacket.dop, + &locapacket->locallyReferencedPacket.frameofref_id_present, + &locapacket->locallyReferencedPacket.frameofref_id, + payload); + if (decoded_payload_len != payload_len) return 0; break; default: diff --git a/arduino/Locapack/Locapack.h b/arduino/Locapack/Locapack.h index 74229795d565201a14ec9d6b23b7f2f1b5d4f3d8..6b9670821e37f1cfd066a9c3156f984f8b646d89 100644 --- a/arduino/Locapack/Locapack.h +++ b/arduino/Locapack/Locapack.h @@ -63,7 +63,7 @@ class Locapack { float z; bool dop_present; float dop; - bool frameofref_id_presence_flag; + bool frameofref_id_present; uint64_t frameofref_id; } locallyReferencedPacket_t; @@ -83,7 +83,12 @@ class Locapack { /// /// @brief /// - uint16_t _sequence_number; + uint16_t _sequence_number_universalGnssPacket; + + /// + /// @brief + /// + uint16_t _sequence_number_locallyReferencedPacket; /// /// @brief Locapack Constructor @@ -160,6 +165,24 @@ class Locapack { /// int createUniversalGnssPacket(universalGnssPacket_t* gnss, uint8_t* buffer); + /// + /// @brief + /// @param + /// + int makeLocallyReferencedPayload(float x, float y, bool z_present, float z, bool dop_present, float dop, + bool frameofref_id_present, uint64_t frameofref_id, uint8_t* buffer); + /// + /// @brief + /// @param + /// + int decodeLocallyReferencedPayload(float* x, float* y, bool* z_present, float* z, bool* dop_present, float* dop, + bool* frameofref_id_present, uint64_t* frameofref_id, uint8_t* buffer); + /// + /// @brief + /// @param + /// + int createLocallyReferencedPacket(locallyReferencedPacket_t* LocallyReferencedPacket, uint8_t* buffer); + /// /// @brief /// @param