diff --git a/arduino/Locapack/Locapack.cpp b/arduino/Locapack/Locapack.cpp index 787edcd7f2646579177aa1a94f1317c845f964c5..8631142cfe1fb2b8e58c5e0eb59272e8dcf1ec09 100644 --- a/arduino/Locapack/Locapack.cpp +++ b/arduino/Locapack/Locapack.cpp @@ -134,16 +134,19 @@ int Locapack::decodePacket(protocol_version_t* protocol_version, uint8_t* moveme } -int Locapack::makeUniversalGnssPayload(float latitude, float longitude, bool altitude_present, float altitude, +int Locapack::makeUniversalGnssPayload(bool valid_gnss_position, 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); + buffer[0] = (altitude_present == true ? 0x80 : 0) | (dop_present == true ? 0x40 : 0) | (valid_gnss_position == true ? 0x20 : 0); len += 1; - - encodeFloat(latitude, &buffer[len]); len += 4; - encodeFloat(longitude, &buffer[len]); len += 4; + + if (valid_gnss_position) + { + encodeFloat(latitude, &buffer[len]); len += 4; + encodeFloat(longitude, &buffer[len]); len += 4; + } if (altitude_present) { encodeFloat(altitude, &buffer[len]); len += 4; @@ -156,16 +159,20 @@ int Locapack::makeUniversalGnssPayload(float latitude, float longitude, bool alt } -int Locapack::decodeUniversalGnssPayload(float* latitude, float* longitude, bool* altitude_present, float* altitude, +int Locapack::decodeUniversalGnssPayload(bool* valid_gnss_position, 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; + if (buffer[0] & 0x20) *valid_gnss_position = true; else *valid_gnss_position = false; len += 1; - *latitude = decodeFloat(&buffer[len]); len += 4; - *longitude = decodeFloat(&buffer[len]); len += 4; + if (*valid_gnss_position) + { + *latitude = decodeFloat(&buffer[len]); len += 4; + *longitude = decodeFloat(&buffer[len]); len += 4; + } if (*altitude_present) { *altitude = decodeFloat(&buffer[len]); len += 4; @@ -185,6 +192,7 @@ int Locapack::createUniversalGnssPacket(universalGnssPacket_t* universalGnssPack int payload_buffer_len = 0; payload_buffer_len = makeUniversalGnssPayload( + universalGnssPacket->valid_gnss_position, universalGnssPacket->latitude, universalGnssPacket->longitude, universalGnssPacket->altitude_present, @@ -327,7 +335,8 @@ int Locapack::parseLocaPacket(locapacket_t* locapacket, uint8_t* buffer) switch (packet_type) { case PACKET_TYPE_UNIVERSAL_GNSS: - decoded_payload_len = decodeUniversalGnssPayload(&locapacket->universalGnssPacket.latitude, + decoded_payload_len = decodeUniversalGnssPayload(&locapacket->universalGnssPacket.valid_gnss_position, + &locapacket->universalGnssPacket.latitude, &locapacket->universalGnssPacket.longitude, &locapacket->universalGnssPacket.altitude_present, &locapacket->universalGnssPacket.altitude, diff --git a/arduino/Locapack/Locapack.h b/arduino/Locapack/Locapack.h index 6b9670821e37f1cfd066a9c3156f984f8b646d89..7347edd4f64d5438c526e4b9e62b31418b96b745 100644 --- a/arduino/Locapack/Locapack.h +++ b/arduino/Locapack/Locapack.h @@ -45,6 +45,7 @@ class Locapack { /// @brief /// typedef struct { + bool valid_gnss_position; float latitude; float longitude; bool altitude_present; @@ -149,14 +150,14 @@ class Locapack { /// @brief /// @param /// - int makeUniversalGnssPayload(float latitude, float longitude, bool altitude_present, float altitude, + int makeUniversalGnssPayload(bool valid_gnss_position, 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, + int decodeUniversalGnssPayload(bool* valid_gnss_position, float* latitude, float* longitude, bool* altitude_present, float* altitude, bool* dop_present, float* dop, uint8_t* buffer); ///