diff --git a/src/mesh/PositionPrecision.cpp b/src/mesh/PositionPrecision.cpp new file mode 100644 index 000000000..04db01c79 --- /dev/null +++ b/src/mesh/PositionPrecision.cpp @@ -0,0 +1,75 @@ +#include "PositionPrecision.h" +#include "Channels.h" +#include "mesh-pb-constants.h" + +#include + +uint32_t getPositionPrecisionForChannel(uint8_t channelIndex) +{ + const meshtastic_Channel &channel = channels.getByIndex(channelIndex); + + if (channel.settings.has_module_settings) { + return channel.settings.module_settings.position_precision; + } else if (channel.role == meshtastic_Channel_Role_PRIMARY) { + return 32; + } else { + return 0; + } +} + +static int32_t truncateCoordinate(int32_t coordinate, uint32_t precision) +{ + uint32_t coordinateBits = static_cast(coordinate); + uint32_t truncated = coordinateBits & (UINT32_MAX << (32 - precision)); + + // Use the middle of the possible location, not the low edge of the bucket. + truncated += (1UL << (31 - precision)); + + return static_cast(truncated); +} + +void applyPositionPrecision(meshtastic_Position &position, uint32_t precision) +{ + if (precision == 0) { + uint32_t time = position.time; + position = meshtastic_Position_init_default; + position.time = time; + return; + } + + uint32_t effectivePrecision = precision > 32 ? 32 : precision; + position.precision_bits = effectivePrecision; + + if (effectivePrecision < 32) { + position.latitude_i = truncateCoordinate(position.latitude_i, effectivePrecision); + position.longitude_i = truncateCoordinate(position.longitude_i, effectivePrecision); + } +} + +bool applyPositionPrecision(meshtastic_MeshPacket &packet, uint32_t precision) +{ + if (packet.which_payload_variant != meshtastic_MeshPacket_decoded_tag || + packet.decoded.portnum != meshtastic_PortNum_POSITION_APP) { + return true; + } + + meshtastic_Position position = meshtastic_Position_init_default; + if (!pb_decode_from_bytes(packet.decoded.payload.bytes, packet.decoded.payload.size, &meshtastic_Position_msg, &position)) { + return false; + } + + applyPositionPrecision(position, precision); + packet.decoded.payload.size = pb_encode_to_bytes(packet.decoded.payload.bytes, sizeof(packet.decoded.payload.bytes), + &meshtastic_Position_msg, &position); + return true; +} + +bool applyPositionPrecisionForChannel(meshtastic_MeshPacket &packet, uint8_t channelIndex) +{ + if (packet.which_payload_variant != meshtastic_MeshPacket_decoded_tag || + packet.decoded.portnum != meshtastic_PortNum_POSITION_APP) { + return true; + } + + return applyPositionPrecision(packet, getPositionPrecisionForChannel(channelIndex)); +} diff --git a/src/mesh/PositionPrecision.h b/src/mesh/PositionPrecision.h new file mode 100644 index 000000000..6fdbd2f64 --- /dev/null +++ b/src/mesh/PositionPrecision.h @@ -0,0 +1,9 @@ +#pragma once + +#include "meshtastic/mesh.pb.h" +#include + +uint32_t getPositionPrecisionForChannel(uint8_t channelIndex); +void applyPositionPrecision(meshtastic_Position &position, uint32_t precision); +bool applyPositionPrecision(meshtastic_MeshPacket &packet, uint32_t precision); +bool applyPositionPrecisionForChannel(meshtastic_MeshPacket &packet, uint8_t channelIndex); diff --git a/src/mesh/Router.cpp b/src/mesh/Router.cpp index 980ceb6df..52731fe43 100644 --- a/src/mesh/Router.cpp +++ b/src/mesh/Router.cpp @@ -4,6 +4,7 @@ #include "MeshRadio.h" #include "MeshService.h" #include "NodeDB.h" +#include "PositionPrecision.h" #include "RTC.h" #include "configuration.h" @@ -367,6 +368,11 @@ ErrorCode Router::send(meshtastic_MeshPacket *p) } fixPriority(p); // Before encryption, fix the priority if it's unset + if (!applyPositionPrecisionForChannel(*p, p->channel)) { + LOG_ERROR("Dropping malformed position packet before send"); + packetPool.release(p); + return meshtastic_Routing_Error_BAD_REQUEST; + } // If the packet is not yet encrypted, do so now if (p->which_payload_variant == meshtastic_MeshPacket_decoded_tag) { diff --git a/src/modules/PositionModule.cpp b/src/modules/PositionModule.cpp index c13904c31..d2603627b 100644 --- a/src/modules/PositionModule.cpp +++ b/src/modules/PositionModule.cpp @@ -4,6 +4,7 @@ #include "GPS.h" #include "MeshService.h" #include "NodeDB.h" +#include "PositionPrecision.h" #include "RTC.h" #include "Router.h" #include "TransmitHistory.h" @@ -107,13 +108,7 @@ bool PositionModule::handleReceivedProtobuf(const meshtastic_MeshPacket &mp, mes } nodeDB->updatePosition(getFrom(&mp), p); - if (channels.getByIndex(mp.channel).settings.has_module_settings) { - precision = channels.getByIndex(mp.channel).settings.module_settings.position_precision; - } else if (channels.getByIndex(mp.channel).role == meshtastic_Channel_Role_PRIMARY) { - precision = 32; - } else { - precision = 0; - } + precision = getPositionPrecisionForChannel(mp.channel); return false; // Let others look at this message also if they want } @@ -121,15 +116,12 @@ bool PositionModule::handleReceivedProtobuf(const meshtastic_MeshPacket &mp, mes void PositionModule::alterReceivedProtobuf(meshtastic_MeshPacket &mp, meshtastic_Position *p) { // Phone position packets need to be truncated to the channel precision - if (isFromUs(&mp) && (precision < 32 && precision > 0)) { - LOG_DEBUG("Truncate phone position to channel precision %i", precision); - p->latitude_i = p->latitude_i & (UINT32_MAX << (32 - precision)); - p->longitude_i = p->longitude_i & (UINT32_MAX << (32 - precision)); - - // We want the imprecise position to be the middle of the possible location, not - p->latitude_i += (1 << (31 - precision)); - p->longitude_i += (1 << (31 - precision)); - + if (isFromUs(&mp)) { + if (precision == 0) + LOG_DEBUG("Strip phone position due to channel precision 0"); + else if (precision < 32) + LOG_DEBUG("Truncate phone position to channel precision %i", precision); + applyPositionPrecision(*p, precision); mp.decoded.payload.size = pb_encode_to_bytes(mp.decoded.payload.bytes, sizeof(mp.decoded.payload.bytes), &meshtastic_Position_msg, p); } @@ -206,20 +198,11 @@ meshtastic_MeshPacket *PositionModule::allocPositionPacket() // lat/lon are unconditionally included - IF AVAILABLE! LOG_DEBUG("Send location with precision %i", precision); - if (precision < 32 && precision > 0) { - p.latitude_i = localPosition.latitude_i & (UINT32_MAX << (32 - precision)); - p.longitude_i = localPosition.longitude_i & (UINT32_MAX << (32 - precision)); - - // We want the imprecise position to be the middle of the possible location, not - p.latitude_i += (1 << (31 - precision)); - p.longitude_i += (1 << (31 - precision)); - } else { - p.latitude_i = localPosition.latitude_i; - p.longitude_i = localPosition.longitude_i; - } - p.precision_bits = precision; + p.latitude_i = localPosition.latitude_i; + p.longitude_i = localPosition.longitude_i; p.has_latitude_i = true; p.has_longitude_i = true; + applyPositionPrecision(p, precision); // Always use NTP / GPS time if available if (getValidTime(RTCQualityNTP) > 0) { p.time = getValidTime(RTCQualityNTP); @@ -350,8 +333,7 @@ void PositionModule::sendOurPosition() // If we changed channels, ask everyone else for their latest info LOG_INFO("Send pos@%x:6 to mesh (wantReplies=%d)", localPosition.timestamp, requestReplies); for (uint8_t channelNum = 0; channelNum < 8; channelNum++) { - if (channels.getByIndex(channelNum).settings.has_module_settings && - channels.getByIndex(channelNum).settings.module_settings.position_precision != 0) { + if (getPositionPrecisionForChannel(channelNum) != 0) { sendOurPosition(NODENUM_BROADCAST, requestReplies, channelNum); return; } @@ -369,10 +351,8 @@ void PositionModule::sendOurPosition(NodeNum dest, bool wantReplies, uint8_t cha if (prevPacketId) // if we wrap around to zero, we'll simply fail to cancel in that rare case (no big deal) service->cancelSending(prevPacketId); - // Set's the class precision value for this particular packet - if (channels.getByIndex(channel).settings.has_module_settings) { - precision = channels.getByIndex(channel).settings.module_settings.position_precision; - } + // Set the class precision value for this particular packet. + precision = getPositionPrecisionForChannel(channel); meshtastic_MeshPacket *p = allocPositionPacket(); if (p == nullptr) { diff --git a/test/test_position_precision/test_main.cpp b/test/test_position_precision/test_main.cpp new file mode 100644 index 000000000..4f5aecfda --- /dev/null +++ b/test/test_position_precision/test_main.cpp @@ -0,0 +1,100 @@ +#include "PositionPrecision.h" +#include "TestUtil.h" +#include "mesh-pb-constants.h" +#include + +static meshtastic_Position makePosition() +{ + meshtastic_Position position = meshtastic_Position_init_default; + position.has_latitude_i = true; + position.latitude_i = static_cast(0x12345678); + position.has_longitude_i = true; + position.longitude_i = static_cast(0x22345678); + position.has_altitude = true; + position.altitude = 123; + position.time = 42; + position.location_source = meshtastic_Position_LocSource_LOC_EXTERNAL; + position.timestamp = 43; + position.sats_in_view = 10; + return position; +} + +static void test_applyPositionPrecision_clampsLatLonAndSetsPrecisionBits() +{ + meshtastic_Position position = makePosition(); + + applyPositionPrecision(position, 16); + + TEST_ASSERT_EQUAL_INT32(static_cast(0x12348000), position.latitude_i); + TEST_ASSERT_EQUAL_INT32(static_cast(0x22348000), position.longitude_i); + TEST_ASSERT_EQUAL_UINT32(16, position.precision_bits); + TEST_ASSERT_TRUE(position.has_latitude_i); + TEST_ASSERT_TRUE(position.has_longitude_i); +} + +static void test_applyPositionPrecision_fullPrecisionKeepsLatLon() +{ + meshtastic_Position position = makePosition(); + + applyPositionPrecision(position, 32); + + TEST_ASSERT_EQUAL_INT32(static_cast(0x12345678), position.latitude_i); + TEST_ASSERT_EQUAL_INT32(static_cast(0x22345678), position.longitude_i); + TEST_ASSERT_EQUAL_UINT32(32, position.precision_bits); +} + +static void test_applyPositionPrecision_zeroScrubsLocationButKeepsTime() +{ + meshtastic_Position position = makePosition(); + + applyPositionPrecision(position, 0); + + TEST_ASSERT_FALSE(position.has_latitude_i); + TEST_ASSERT_EQUAL_INT32(0, position.latitude_i); + TEST_ASSERT_FALSE(position.has_longitude_i); + TEST_ASSERT_EQUAL_INT32(0, position.longitude_i); + TEST_ASSERT_FALSE(position.has_altitude); + TEST_ASSERT_EQUAL_INT32(0, position.altitude); + TEST_ASSERT_EQUAL_UINT32(42, position.time); + TEST_ASSERT_EQUAL_UINT32(0, position.timestamp); + TEST_ASSERT_EQUAL_UINT32(0, position.sats_in_view); + TEST_ASSERT_EQUAL_UINT32(0, position.precision_bits); +} + +static void test_applyPositionPrecision_reencodesPositionPacket() +{ + meshtastic_Position position = makePosition(); + meshtastic_MeshPacket packet = meshtastic_MeshPacket_init_default; + packet.which_payload_variant = meshtastic_MeshPacket_decoded_tag; + packet.decoded.portnum = meshtastic_PortNum_POSITION_APP; + packet.decoded.payload.size = pb_encode_to_bytes(packet.decoded.payload.bytes, sizeof(packet.decoded.payload.bytes), + &meshtastic_Position_msg, &position); + + TEST_ASSERT_TRUE(applyPositionPrecision(packet, 16)); + + meshtastic_Position decoded = meshtastic_Position_init_default; + TEST_ASSERT_TRUE( + pb_decode_from_bytes(packet.decoded.payload.bytes, packet.decoded.payload.size, &meshtastic_Position_msg, &decoded)); + TEST_ASSERT_EQUAL_INT32(static_cast(0x12348000), decoded.latitude_i); + TEST_ASSERT_EQUAL_INT32(static_cast(0x22348000), decoded.longitude_i); + TEST_ASSERT_EQUAL_UINT32(16, decoded.precision_bits); +} + +void setUp(void) {} + +void tearDown(void) {} + +extern "C" { +void setup() +{ + initializeTestEnvironment(); + UNITY_BEGIN(); + RUN_TEST(test_applyPositionPrecision_clampsLatLonAndSetsPrecisionBits); + RUN_TEST(test_applyPositionPrecision_fullPrecisionKeepsLatLon); + RUN_TEST(test_applyPositionPrecision_zeroScrubsLocationButKeepsTime); + RUN_TEST(test_applyPositionPrecision_reencodesPositionPacket); + exit(UNITY_END()); +} + +void loop() {} +}