diff --git a/src/mavsdk/plugins/rtk/CMakeLists.txt b/src/mavsdk/plugins/rtk/CMakeLists.txt index f2521a13a5..e395de254b 100644 --- a/src/mavsdk/plugins/rtk/CMakeLists.txt +++ b/src/mavsdk/plugins/rtk/CMakeLists.txt @@ -12,4 +12,9 @@ target_include_directories(mavsdk PUBLIC install(FILES include/plugins/rtk/rtk.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/mavsdk/plugins/rtk -) \ No newline at end of file +) + +list(APPEND UNIT_TEST_SOURCES + ${CMAKE_CURRENT_SOURCE_DIR}/rtk_parse_test.cpp +) +set(UNIT_TEST_SOURCES ${UNIT_TEST_SOURCES} PARENT_SCOPE) diff --git a/src/mavsdk/plugins/rtk/rtk_impl.cpp b/src/mavsdk/plugins/rtk/rtk_impl.cpp index f7e0bb6045..d837cb2c8d 100644 --- a/src/mavsdk/plugins/rtk/rtk_impl.cpp +++ b/src/mavsdk/plugins/rtk/rtk_impl.cpp @@ -39,13 +39,38 @@ Rtk::Result RtkImpl::send_rtcm_data(Rtk::RtcmData rtcm_data) return Rtk::Result::TooLong; } - // Copy length before we change it. - size_t bytes_to_send = rtcm_data.data.size(); + // From Python, we get the bytes in a Python str such as: + // b'\xd3\x00\x13>\xd0\x00\x034\x14|\t\xe0\x81\x17\xca\xde[7=s\xf0?\x1b\t\x8a' + + // Instead of changing the interface at this point for MAVSDK v1.4, we just + // try to detect Python strings instead and convert it back. + + const std::string original_bytes = [&]() { + // Some use single inverted commas. + if (rtcm_data.data.size() >= 2 && char(rtcm_data.data[0]) == 'b' && + char(rtcm_data.data[1]) == '\'' && + char(rtcm_data.data[rtcm_data.data.size() - 1]) == '\'') { + return convert_from_python_string(rtcm_data.data); + } + + // Others use double inverted commas. + else if ( + rtcm_data.data.size() >= 2 && char(rtcm_data.data[0]) == 'b' && + char(rtcm_data.data[1]) == '"' && + char(rtcm_data.data[rtcm_data.data.size() - 1]) == '"') { + return convert_from_python_string(rtcm_data.data); + + } else { + return rtcm_data.data; + } + }(); // The mavlink helpers memcpy, so we need to make sure we're not // copying from where we shouldn't. rtcm_data.data.resize(num_packets_required * field_len); + size_t bytes_to_send = original_bytes.size(); + for (size_t i = 0; i < num_packets_required; ++i) { const uint8_t flags = (num_packets_required > 1 ? 0x1 : 0x0) | ((i & 0x3) << 1) | ((_sequence & 0x1F) << 3); @@ -57,7 +82,7 @@ Rtk::Result RtkImpl::send_rtcm_data(Rtk::RtcmData rtcm_data) &message, flags, static_cast(std::min(field_len, bytes_to_send)), - reinterpret_cast(rtcm_data.data.c_str() + (i * field_len))); + reinterpret_cast(original_bytes.c_str() + (i * field_len))); if (!_parent->send_message(message)) { ++_sequence; @@ -71,4 +96,67 @@ Rtk::Result RtkImpl::send_rtcm_data(Rtk::RtcmData rtcm_data) return Rtk::Result::Success; } +std::string RtkImpl::convert_from_python_string(const std::string& pstr) +{ + std::string bytes; + + // Skip first two bytes and last. + const size_t end = pstr.size() - 1; + size_t i = 2; + while (i < end) { + // According to: + // https://www.w3schools.com/python/gloss_python_escape_characters.asp + + // \' -> '. + if (i + 1 < end && pstr[i] == '\\' && pstr[i + 1] == '\'') { + bytes += '\''; + i += 2; + + // \\ -> \. + } else if (i + 1 < end && pstr[i] == '\\' && pstr[i + 1] == '\\') { + bytes += '\\'; + i += 2; + + // \n -> 0x0A + } else if (i + 1 < end && pstr[i] == '\\' && pstr[i + 1] == 'n') { + bytes += 0x0A; + i += 2; + + // \r -> 0x0D + } else if (i + 1 < end && pstr[i] == '\\' && pstr[i + 1] == 'r') { + bytes += 0x0D; + i += 2; + + // \t -> 0x09 + } else if (i + 1 < end && pstr[i] == '\\' && pstr[i + 1] == 't') { + bytes += 0x09; + i += 2; + + // \b -> 0x08 + } else if (i + 1 < end && pstr[i] == '\\' && pstr[i + 1] == 'b') { + bytes += 0x08; + i += 2; + + // \f -> 0x0C + } else if (i + 1 < end && pstr[i] == '\\' && pstr[i + 1] == 'f') { + bytes += 0x0C; + i += 2; + + // \x00 -> hex number + } else if (i + 3 < end && pstr[i] == '\\' && pstr[i + 1] == 'x') { + std::string hex = {pstr[i + 2], pstr[i + 3]}; + const uint8_t byte = std::stoul(hex, nullptr, 16); + bytes += byte; + i += 4; + + // some valid char that we can use as is + } else { + bytes += pstr[i]; + i += 1; + } + } + + return bytes; +} + } // namespace mavsdk diff --git a/src/mavsdk/plugins/rtk/rtk_impl.h b/src/mavsdk/plugins/rtk/rtk_impl.h index 754029e279..2fd695fbdb 100644 --- a/src/mavsdk/plugins/rtk/rtk_impl.h +++ b/src/mavsdk/plugins/rtk/rtk_impl.h @@ -19,6 +19,8 @@ class RtkImpl : public PluginImplBase { Rtk::Result send_rtcm_data(Rtk::RtcmData rtcm_data); + static std::string convert_from_python_string(const std::string& python_string); + private: unsigned _sequence{0}; }; diff --git a/src/mavsdk/plugins/rtk/rtk_parse_test.cpp b/src/mavsdk/plugins/rtk/rtk_parse_test.cpp new file mode 100644 index 0000000000..58366c4310 --- /dev/null +++ b/src/mavsdk/plugins/rtk/rtk_parse_test.cpp @@ -0,0 +1,37 @@ +#include +#include + +#include "plugins/rtk/rtk.h" +#include "rtk_impl.h" + +using namespace mavsdk; + +// String generated using: +// +// python -c "print(bytes(range(256)))" with " and \ escaped + +static std::string sample_python_string = + "b'\\x00\\x01\\x02\\x03\\x04\\x05\\x06\\x07\\x08\\t\\n\\x0b\\x0c\\r\\x0e" + "\\x0f\\x10\\x11\\x12\\x13\\x14\\x15\\x16\\x17\\x18\\x19\\x1a\\x1b\\x1c" + "\\x1d\\x1e\\x1f !\"#$%&\\'()*+,-./0123456789:;<=>?@" + "ABCDEFGHIJKLMNOPQRSTUVWXYZ[\\\\]^_`abcdefghijklmnopqrstuvwxyz{|}~\\x7f" + "\\x80\\x81\\x82\\x83\\x84\\x85\\x86\\x87\\x88\\x89\\x8a\\x8b\\x8c\\x8d" + "\\x8e\\x8f\\x90\\x91\\x92\\x93\\x94\\x95\\x96\\x97\\x98\\x99\\x9a\\x9b" + "\\x9c\\x9d\\x9e\\x9f\\xa0\\xa1\\xa2\\xa3\\xa4\\xa5\\xa6\\xa7\\xa8\\xa9" + "\\xaa\\xab\\xac\\xad\\xae\\xaf\\xb0\\xb1\\xb2\\xb3\\xb4\\xb5\\xb6\\xb7" + "\\xb8\\xb9\\xba\\xbb\\xbc\\xbd\\xbe\\xbf\\xc0\\xc1\\xc2\\xc3\\xc4\\xc5" + "\\xc6\\xc7\\xc8\\xc9\\xca\\xcb\\xcc\\xcd\\xce\\xcf\\xd0\\xd1\\xd2\\xd3" + "\\xd4\\xd5\\xd6\\xd7\\xd8\\xd9\\xda\\xdb\\xdc\\xdd\\xde\\xdf\\xe0\\xe1" + "\\xe2\\xe3\\xe4\\xe5\\xe6\\xe7\\xe8\\xe9\\xea\\xeb\\xec\\xed\\xee\\xef" + "\\xf0\\xf1\\xf2\\xf3\\xf4\\xf5\\xf6\\xf7\\xf8\\xf9\\xfa\\xfb\\xfc\\xfd" + "\\xfe\\xff'"; + +TEST(Rtk, ParsePythonString) +{ + auto result = RtkImpl::convert_from_python_string(sample_python_string); + + uint8_t counter = 0; + for (uint8_t ch : result) { + EXPECT_EQ(ch, counter++); + } +}