diff --git a/arduino/Locapack/Locapack.cpp b/arduino/Locapack/Locapack.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1db62bbf935ea7fa70c5808ece5848ded2de1ab6
--- /dev/null
+++ b/arduino/Locapack/Locapack.cpp
@@ -0,0 +1,457 @@
+// Locapack.cpp
+//
+// Locapack is an Arduino library that implements the localisation packet protocol from the IRIT/RMESS team.
+
+#include <SPI.h>
+#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;
+ setdevice_id16(0);
+}
+
+
+void Locapack::setdevice_id16(uint16_t device_id)
+{
+ encodeUint16(device_id, _device_id.device_id_buf);
+ _device_id.device_id_len = 2;
+}
+
+
+void Locapack::setdevice_id32(uint32_t device_id)
+{
+ encodeUint32(device_id, _device_id.device_id_buf);
+ _device_id.device_id_len = 4;
+}
+
+
+void Locapack::setdevice_id64(uint64_t device_id)
+{
+ encodeUint64(device_id, _device_id.device_id_buf);
+ _device_id.device_id_len = 8;
+}
+
+
+int Locapack::makeHeader(protocol_version_t protocol_version, uint8_t movement_id_presence_flag,
+ uint8_t timestamp_presence_flag, uint8_t device_id_size, packet_type_t packet_type, uint8_t* buffer)
+{
+ buffer[1] = (uint8_t)(((protocol_version & 0x0f) << 4) | (device_id_size & 0x0f));
+ buffer[0] = (uint8_t)(((packet_type & 0x0f) << 4)
+ | ((movement_id_presence_flag & 0x01) << 3)
+ | ((timestamp_presence_flag & 0x01) << 2));
+ return 2;
+}
+
+
+int Locapack::decodeHeader(protocol_version_t* protocol_version, uint8_t* movement_id_presence_flag,
+ uint8_t* timestamp_presence_flag, uint8_t* device_id_size, packet_type_t* packet_type, uint8_t* buffer)
+{
+ *protocol_version = (protocol_version_t)((buffer[1] >> 4) & 0x0f);
+ *device_id_size = buffer[1] & 0x0f;
+ *packet_type = (packet_type_t)((buffer[0] >> 4) & 0x0f);
+ *movement_id_presence_flag = (buffer[0] >> 3) & 0x01;
+ *timestamp_presence_flag = (buffer[0] >> 2) & 0x01;
+ return 2;
+}
+
+
+int Locapack::makePacket(protocol_version_t protocol_version, uint8_t movement_id_presence_flag,
+ uint8_t timestamp_presence_flag, packet_type_t packet_type, uint16_t sequence_number,
+ uint64_t timestamp, uint8_t movement_id, device_id_t* device_id, uint8_t* payload,
+ uint8_t payload_len, uint8_t* buffer)
+{
+ int len = 0;
+ int device_id_len = 0;
+ if ( device_id != NULL ) device_id_len = device_id->device_id_len;
+
+ len += makeHeader(protocol_version, movement_id_presence_flag, timestamp_presence_flag,
+ device_id_len, packet_type, buffer);
+ encodeUint16(sequence_number, &buffer[len]); len += 2;
+ if ( timestamp_presence_flag )
+ {
+ encodeUint40(timestamp, &buffer[len]);
+ len += 5;
+ }
+ if ( movement_id_presence_flag )
+ {
+ buffer[len] = movement_id;
+ len += 1;
+ }
+ if ( device_id_len != 0 )
+ {
+ memcpy(&buffer[len], device_id->device_id_buf, device_id->device_id_len);
+ len += device_id->device_id_len;
+ }
+ buffer[len] = payload_len;
+ len += 1;
+ if ( payload_len > 0 )
+ {
+ memcpy(&buffer[len], payload, payload_len);
+ len += payload_len;
+ }
+
+ return len;
+}
+
+
+int Locapack::decodePacket(protocol_version_t* protocol_version, uint8_t* movement_id_presence_flag,
+ uint8_t* timestamp_presence_flag, packet_type_t* packet_type, uint16_t* sequence_number,
+ uint64_t* timestamp, uint8_t* movement_id, device_id_t* device_id, uint8_t* payload,
+ uint8_t* payload_len, uint8_t* buffer)
+{
+ int len = 0;
+
+ len += decodeHeader(protocol_version, movement_id_presence_flag, timestamp_presence_flag,
+ &device_id->device_id_len, packet_type, buffer);
+ *sequence_number = decodeUint16(&buffer[len]); len += 2;
+ if ( *timestamp_presence_flag )
+ {
+ *timestamp = decodeUint40(&buffer[len]);
+ len += 5;
+ }
+ if ( *movement_id_presence_flag )
+ {
+ *movement_id = buffer[len];
+ len += 1;
+ }
+ if ( device_id->device_id_len > 0 )
+ {
+ memcpy(device_id->device_id_buf, &buffer[len], device_id->device_id_len);
+ len += device_id->device_id_len;
+ }
+ *payload_len = buffer[len];
+ len += 1;
+ if ( *payload_len > 0 )
+ {
+ memcpy(payload, &buffer[len], *payload_len);
+ len += *payload_len;
+ }
+ return len;
+}
+
+
+int Locapack::makeUniversalGnssPayload(float latitude, float longitude, bool altitude_present, float altitude,
+ bool dop_present, float dop, uint8_t* buffer)
+{
+ int len = 0;
+
+ buffer[0] = (altitude_present == true ? 0x80 : 0) | (dop_present == true ? 0x40 : 0);
+ len += 1;
+
+ encodeFloat(latitude, &buffer[len]); len += 4;
+ encodeFloat(longitude, &buffer[len]); len += 4;
+ if (altitude_present)
+ {
+ encodeFloat(altitude, &buffer[len]); len += 4;
+ }
+ if (dop_present)
+ {
+ encodeFloat(dop, &buffer[len]); len += 4;
+ }
+ return len;
+}
+
+
+int Locapack::decodeUniversalGnssPayload(float* latitude, float* longitude, bool* altitude_present, float* altitude,
+ bool* dop_present, float* dop, uint8_t* buffer)
+{
+ int len = 0;
+
+ if (buffer[0] & 0x80) *altitude_present = true; else *altitude_present = false;
+ if (buffer[0] & 0x40) *dop_present = true; else *dop_present = false;
+ len += 1;
+ *latitude = decodeFloat(&buffer[len]); len += 4;
+ *longitude = decodeFloat(&buffer[len]); len += 4;
+ if (*altitude_present)
+ {
+ *altitude = decodeFloat(&buffer[len]); len += 4;
+ }
+ if (*dop_present)
+ {
+ *dop = decodeFloat(&buffer[len]); len += 4;
+ }
+ return len;
+}
+
+
+int Locapack::createUniversalGnssPacket(universalGnssPacket_t* universalGnssPacket, uint8_t* buffer)
+{
+ int len = 0;
+ uint8_t payload_buffer[MAX_BUFFER_LEN];
+ int payload_buffer_len = 0;
+
+ payload_buffer_len = makeUniversalGnssPayload(
+ universalGnssPacket->latitude,
+ universalGnssPacket->longitude,
+ universalGnssPacket->altitude_present,
+ universalGnssPacket->altitude,
+ universalGnssPacket->dop_present,
+ universalGnssPacket->dop,
+ 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_UNIVERSAL_GNSS
+ // 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 UniversalGnssPacket generated
+ // 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,
+ (uint64_t)millis(), 0, &_device_id, payload_buffer, payload_buffer_len, buffer);
+
+ return len;
+}
+
+
+int Locapack::parseLocaPacket(locapacket_t* locapacket, uint8_t* buffer)
+{
+ int len = 0;
+ int decoded_payload_len = 0;
+
+ protocol_version_t protocol_version;
+ uint8_t movement_id_presence_flag;
+ uint8_t timestamp_presence_flag;
+ packet_type_t packet_type;
+ uint16_t sequence_number;
+ uint64_t timestamp;
+ uint8_t movement_id;
+ device_id_t device_id;
+ uint8_t payload[MAX_BUFFER_LEN];
+ uint8_t payload_len;
+
+ len = decodePacket(&protocol_version, &movement_id_presence_flag, ×tamp_presence_flag, &packet_type,
+ &sequence_number, ×tamp, &movement_id, &device_id, payload, &payload_len, buffer);
+
+ if (protocol_version != PROTOCOL_V1) return 0;
+ if (payload_len == 0) return 0;
+
+ switch (packet_type)
+ {
+ case PACKET_TYPE_UNIVERSAL_GNSS:
+ decoded_payload_len = decodeUniversalGnssPayload(&locapacket->universalGnssPacket.latitude,
+ &locapacket->universalGnssPacket.longitude,
+ &locapacket->universalGnssPacket.altitude_present,
+ &locapacket->universalGnssPacket.altitude,
+ &locapacket->universalGnssPacket.dop_present,
+ &locapacket->universalGnssPacket.dop,
+ payload);
+ if (decoded_payload_len != payload_len) return 0;
+ break;
+
+ case PACKET_TYPE_LOCALLY_REFERENCED:
+ return 0; // ToDO
+ break;
+
+ default:
+ return 0;
+ }
+
+ return len;
+}
+
+
+uint16_t Locapack::decodeUint16(uint8_t *data)
+{
+ return 0 | (data[1] << 8) | data[0];
+}
+
+
+void Locapack::encodeUint16(uint16_t from, uint8_t *to)
+{
+ to[1] = (from & 0xFF00) >> 8;
+ to[0] = from & 0xFF;
+}
+
+
+uint32_t Locapack::decodeUint32(uint8_t *data)
+{
+ return 0 | (data[3] << 24) | (data[2] << 16) | (data[1] << 8) | data[0];
+}
+
+
+void Locapack::encodeUint32 ( uint32_t from, uint8_t *to )
+{
+ to[3] = (from & 0xFF000000) >> 24;
+ to[2] = (from & 0xFF0000) >> 16;
+ to[1] = (from & 0xFF00) >> 8;
+ to[0] = from & 0xFF;
+}
+
+
+uint64_t Locapack::decodeUint40 ( uint8_t *data )
+{
+ uint64_t tmp = 0;
+ tmp = data[4];
+ tmp = tmp << 32;
+ tmp = tmp | decodeUint32(data);
+
+ return tmp;
+}
+
+
+void Locapack::encodeUint40 ( uint64_t from, uint8_t *to )
+{
+ to[4] = (from & 0xFF00000000) >> 32;
+ to[3] = (from & 0xFF000000) >> 24;
+ to[2] = (from & 0xFF0000) >> 16;
+ to[1] = (from & 0xFF00) >> 8;
+ to[0] = from & 0xFF;
+}
+
+
+uint64_t Locapack::decodeUint64 ( uint8_t *data )
+{
+ uint64_t tmp = 0;
+ tmp = decodeUint32(&data[4]); // | decodeUint32(data);
+ tmp = tmp << 32;
+ tmp = tmp | decodeUint32(data);
+
+ return tmp;
+}
+
+
+void Locapack::encodeUint64 ( uint64_t from, uint8_t *to )
+{
+ to[7] = (from & 0xFF00000000000000) >> 56;
+ to[6] = (from & 0xFF000000000000) >> 48;
+ to[5] = (from & 0xFF0000000000) >> 40;
+ to[4] = (from & 0xFF00000000) >> 32;
+ to[3] = (from & 0xFF000000) >> 24;
+ to[2] = (from & 0xFF0000) >> 16;
+ to[1] = (from & 0xFF00) >> 8;
+ to[0] = from & 0xFF;
+}
+
+
+float Locapack::decodeFloat ( uint8_t *data ) {
+
+ typedef union _data {
+ float f;
+ char s[4];
+ } myData;
+
+ myData q;
+
+#ifndef BYTE_ORDER
+ #error "BYTE_ORDER must be defined"
+#endif
+#if BYTE_ORDER==LITTLE_ENDIAN
+ q.s[0] = data[0];
+ q.s[1] = data[1];
+ q.s[2] = data[2];
+ q.s[3] = data[3];
+#else
+ q.s[0] = data[3];
+ q.s[1] = data[2];
+ q.s[2] = data[1];
+ q.s[3] = data[0];
+#endif
+
+ return q.f;
+}
+
+
+void Locapack::encodeFloat ( float from, uint8_t *to ) {
+
+ typedef union _data {
+ float f;
+ char s[4];
+ } myData;
+
+ myData q;
+ q.f = from;
+
+#ifndef BYTE_ORDER
+ #error "BYTE_ORDER must be defined"
+#endif
+#if BYTE_ORDER==LITTLE_ENDIAN
+ to[0] = q.s[0];
+ to[1] = q.s[1];
+ to[2] = q.s[2];
+ to[3] = q.s[3];
+#else
+ to[0] = q.s[3];
+ to[1] = q.s[2];
+ to[2] = q.s[1];
+ to[3] = q.s[0];
+#endif
+}
+
+double Locapack::decodeDouble ( uint8_t *data ) {
+
+ typedef union _data {
+ double f;
+ char s[8];
+ } myData;
+
+ myData q;
+#ifndef BYTE_ORDER
+ #error "BYTE_ORDER must be defined"
+#endif
+#if BYTE_ORDER==LITTLE_ENDIAN
+ q.s[0] = data[0];
+ q.s[1] = data[1];
+ q.s[2] = data[2];
+ q.s[3] = data[3];
+ q.s[4] = data[4];
+ q.s[5] = data[5];
+ q.s[6] = data[6];
+ q.s[7] = data[7];
+#else
+ q.s[0] = data[7];
+ q.s[1] = data[6];
+ q.s[2] = data[5];
+ q.s[3] = data[4];
+ q.s[4] = data[3];
+ q.s[5] = data[2];
+ q.s[6] = data[1];
+ q.s[7] = data[0];
+#endif
+ return q.f;
+}
+
+
+void Locapack::encodeDouble ( double from, uint8_t *to ) {
+
+ typedef union _data {
+ double f;
+ char s[8];
+ } myData;
+
+ myData q;
+ q.f = from;
+#ifndef BYTE_ORDER
+ #error "BYTE_ORDER must be defined"
+#endif
+#if BYTE_ORDER==LITTLE_ENDIAN
+ to[0] = q.s[0];
+ to[1] = q.s[1];
+ to[2] = q.s[2];
+ to[3] = q.s[3];
+ to[4] = q.s[4];
+ to[5] = q.s[5];
+ to[6] = q.s[6];
+ to[7] = q.s[7];
+#else
+ to[0] = q.s[7];
+ to[1] = q.s[6];
+ to[2] = q.s[5];
+ to[3] = q.s[4];
+ to[4] = q.s[3];
+ to[5] = q.s[2];
+ to[6] = q.s[1];
+ to[7] = q.s[0];
+#endif
+}
diff --git a/arduino/Locapack/Locapack.h b/arduino/Locapack/Locapack.h
new file mode 100644
index 0000000000000000000000000000000000000000..74229795d565201a14ec9d6b23b7f2f1b5d4f3d8
--- /dev/null
+++ b/arduino/Locapack/Locapack.h
@@ -0,0 +1,261 @@
+// Locapack.h
+//
+// Locapack is an Arduino library that implements the localisation packet protocol from the IRIT/RMESS team.
+//
+/// \mainpage Locapack library for Arduino
+///
+/// @author Adrien van den Bossche <vandenbo@univ-tlse2.fr>
+
+#ifndef Locapack_h
+#define Locapack_h
+
+#include "Arduino.h"
+
+class Locapack {
+
+ public:
+
+ #define MAX_DEVICE_ID_LEN 8
+ #define MAX_BUFFER_LEN 128
+
+ ///
+ /// @brief
+ ///
+ typedef enum {
+ PROTOCOL_V1 = 1
+ } protocol_version_t;
+
+ ///
+ /// @brief
+ ///
+ typedef enum {
+ PACKET_TYPE_UNIVERSAL_GNSS = 1,
+ PACKET_TYPE_LOCALLY_REFERENCED = 2
+ } packet_type_t;
+
+ ///
+ /// @brief
+ ///
+ typedef struct {
+ uint8_t device_id_buf[MAX_DEVICE_ID_LEN];
+ uint8_t device_id_len;
+ } device_id_t;
+
+ ///
+ /// @brief
+ ///
+ typedef struct {
+ float latitude;
+ float longitude;
+ bool altitude_present;
+ float altitude;
+ bool dop_present;
+ float dop;
+ } universalGnssPacket_t;
+
+ ///
+ /// @brief
+ ///
+ typedef struct {
+ float x;
+ float y;
+ bool z_present;
+ float z;
+ bool dop_present;
+ float dop;
+ bool frameofref_id_presence_flag;
+ uint64_t frameofref_id;
+ } locallyReferencedPacket_t;
+
+ ///
+ /// @brief
+ ///
+ typedef union locapacket {
+ universalGnssPacket_t universalGnssPacket;
+ locallyReferencedPacket_t locallyReferencedPacket;
+ } locapacket_t;
+
+ ///
+ /// @brief
+ ///
+ device_id_t _device_id;
+
+ ///
+ /// @brief
+ ///
+ uint16_t _sequence_number;
+
+ ///
+ /// @brief Locapack Constructor
+ ///
+ Locapack();
+
+ ///
+ /// @brief
+ /// @param
+ ///
+ void setdevice_id16(uint16_t sourceAddress);
+
+ ///
+ /// @brief
+ /// @param
+ ///
+ void setdevice_id32(uint32_t sourceAddress);
+
+ ///
+ /// @brief
+ /// @param
+ ///
+ void setdevice_id64(uint64_t sourceAddress);
+
+ ///
+ /// @brief
+ /// @param
+ ///
+ int makeHeader(protocol_version_t protocol_version, uint8_t movement_id_presence_flag,
+ uint8_t timestamp_presence_flag, uint8_t device_id_size, packet_type_t packet_type, uint8_t* buffer);
+
+ ///
+ /// @brief
+ /// @param
+ ///
+ int decodeHeader(protocol_version_t* protocol_version, uint8_t* movement_id_presence_flag,
+ uint8_t* timestamp_presence_flag, uint8_t* device_id_size, packet_type_t* packet_type, uint8_t* buffer);
+
+ ///
+ /// @brief
+ /// @param
+ ///
+ int makePacket(protocol_version_t protocol_version, uint8_t movement_id_presence_flag,
+ uint8_t timestamp_presence_flag, packet_type_t packet_type, uint16_t sequence_number,
+ uint64_t timestamp, uint8_t movement_id, device_id_t* device_id, uint8_t* payload,
+ uint8_t payload_len, uint8_t* buffer);
+
+ ///
+ /// @brief
+ /// @param
+ ///
+ int decodePacket(protocol_version_t* protocol_version, uint8_t* movement_id_presence_flag,
+ uint8_t* timestamp_presence_flag, packet_type_t* packet_type, uint16_t* sequence_number,
+ uint64_t* timestamp, uint8_t* movement_id, device_id_t* device_id, uint8_t* payload,
+ uint8_t* payload_len, uint8_t* buffer);
+
+ ///
+ /// @brief
+ /// @param
+ ///
+ int makeUniversalGnssPayload(float latitude, float longitude, bool altitude_present, float altitude,
+ bool dop_present, float dop, uint8_t* buffer);
+
+ ///
+ /// @brief
+ /// @param
+ ///
+ int decodeUniversalGnssPayload(float* latitude, float* longitude, bool* altitude_present, float* altitude,
+ bool* dop_present, float* dop, uint8_t* buffer);
+
+ ///
+ /// @brief
+ /// @param
+ ///
+ int createUniversalGnssPacket(universalGnssPacket_t* gnss, uint8_t* buffer);
+
+ ///
+ /// @brief
+ /// @param
+ ///
+ int parseLocaPacket(locapacket_t* locapacket, uint8_t* buffer);
+
+ private:
+
+ ///
+ /// @brief Builds an uint16 value from two uint8 values
+ /// @param data The address of the uint8_t buffer
+ /// @return The decoded uint16_t
+ ///
+ uint16_t decodeUint16 ( uint8_t *data );
+
+ ///
+ /// @brief Formats an uint16 value as a list of uint8 values
+ /// @param from The uint16_t value
+ /// @param to The address of the uint8_t buffer
+ ///
+ void encodeUint16 ( uint16_t from, uint8_t *to );
+
+ ///
+ /// @brief Builds an uint32 value from four uint8 values
+ /// @param data The address of the uint8_t buffer
+ /// @return The decoded uint32_t
+ ///
+ uint32_t decodeUint32 ( uint8_t *data );
+
+ ///
+ /// @brief Formats an uint32 value as a list of uint8 values
+ /// @param from The uint32_t value
+ /// @param to The address of the uint8_t buffer
+ ///
+ void encodeUint32 ( uint32_t from, uint8_t *to );
+
+ ///
+ /// @brief Builds an uint64 value from five uint8 values
+ /// @param data The address of the uint8_t buffer
+ /// @return The decoded uint64_t
+ ///
+ uint64_t decodeUint40 ( uint8_t *data );
+
+ ///
+ /// @brief Formats an uint64 value with only 5 LSbytes as a list of uint8 values
+ /// @param from The uint64_t value
+ /// @param to The address of the uint8_t buffer
+ ///
+ void encodeUint40 ( uint64_t from, uint8_t *to );
+
+ ///
+ /// @brief Builds an uint64 value from eight uint8 values
+ /// @param data The address of the uint8_t buffer
+ /// @return The decoded uint64_t
+ ///
+ uint64_t decodeUint64 ( uint8_t *data );
+
+ ///
+ /// @brief Formats an uint64 value as a list of uint8 values
+ /// @param from The uint64_t value
+ /// @param to The address of the uint8_t buffer
+ ///
+ void encodeUint64 ( uint64_t from, uint8_t *to );
+
+ ///
+ /// @brief Builds a float value from four uint8 values
+ /// @param data The address of the uint8_t buffer
+ /// @return The decoded float
+ ///
+ float decodeFloat ( uint8_t *data );
+
+ ///
+ /// @brief Formats an float value as a list of uint8 values
+ /// @param from The float value
+ /// @param to The address of the uint8_t buffer
+ /// @return No return
+ ///
+ void encodeFloat ( float from, uint8_t *to );
+
+ ///
+ /// @brief Builds a double value from four uint8 values
+ /// @param data The address of the uint8_t buffer
+ /// @return The decoded double
+ ///
+ double decodeDouble ( uint8_t *data );
+
+ ///
+ /// @brief Formats an float value as a list of uint8 values
+ /// @param from The float value
+ /// @param to The address of the uint8_t buffer
+ /// @return No return
+ ///
+ void encodeDouble ( double from, uint8_t *to );
+
+ protected:
+
+};
+
+#endif
diff --git a/arduino/Locapack/examples/tinygpsplus/tinygpsplus.ino b/arduino/Locapack/examples/tinygpsplus/tinygpsplus.ino
new file mode 100644
index 0000000000000000000000000000000000000000..18a4f9c3d3e9d701325c3c0ba7c60332645b1396
--- /dev/null
+++ b/arduino/Locapack/examples/tinygpsplus/tinygpsplus.ino
@@ -0,0 +1,81 @@
+#include <Locapack.h>
+#include <TinyGPS++.h>
+
+#define LOCAPACK_PACKET_PERIOD 5000
+
+Locapack locapack;
+TinyGPSPlus gps;
+uint8_t buffer[128];
+Locapack::device_id_t device_id;
+Locapack::universalGnssPacket_t gnss;
+Locapack::locapacket_t locapacket;
+bool gnss_valid = false;
+uint32_t gnss_age = 0;
+uint32_t timetosend = LOCAPACK_PACKET_PERIOD;
+
+void setup()
+{
+ // Set console and GPS serial baudrate
+ Serial.begin(115200);
+ Serial1.begin(9600);
+
+ locapack.setdevice_id16(0x1234);
+ gnss.altitude_present = true;
+ gnss.dop_present = true;
+}
+
+void loop()
+{
+ if (millis() > timetosend)
+ {
+ timetosend += LOCAPACK_PACKET_PERIOD;
+
+ if (gnss_valid)
+ {
+ Serial.print("I create a new packet with position recorded ");
+ Serial.print(millis()-gnss_age);
+ Serial.println("ms ago.");
+
+ int len = locapack.createUniversalGnssPacket(&gnss, buffer);
+ for (int i=0; i<len; i++)
+ {
+ Serial.print(buffer[i], HEX);
+ Serial.print(" ");
+ }
+ Serial.println();
+
+ Serial.println("Now decode this packet...");
+ len = locapack.parseLocaPacket(&locapacket, buffer);
+ Serial.print("packet len="); Serial.println(len);
+ Serial.print("latitude="); Serial.println(locapacket.universalGnssPacket.latitude);
+ Serial.print("longitude="); Serial.println(locapacket.universalGnssPacket.longitude);
+ Serial.print("altitude="); Serial.println(locapacket.universalGnssPacket.altitude);
+ Serial.print("hdop="); Serial.println(locapacket.universalGnssPacket.dop);
+ }
+ else
+ {
+ Serial.println("\n\n*** Waiting for a valid position ***\n\n");
+ }
+ }
+
+ // Get chars from GPS receiver
+ while (Serial1.available())
+ {
+ char c = Serial1.read();
+ //Serial.write(c); // uncomment this line if you want to see the GPS data flowing
+
+ if (gps.encode(c))
+ {
+ // Did a new valid sentence come in?
+ if ( gps.location.isValid() )
+ {
+ gnss.latitude = gps.location.lat();
+ gnss.longitude = gps.location.lng();
+ gnss.altitude = gps.altitude.meters();
+ gnss.dop = gps.hdop.hdop();
+ gnss_age = millis();
+ gnss_valid = true;
+ }
+ }
+ }
+}