Add type 2 encode and decode methods

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:

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