diff --git a/libuavcan/include/uavcan/marshal/array.hpp b/libuavcan/include/uavcan/marshal/array.hpp index ba50e111ee..c3384c678a 100644 --- a/libuavcan/include/uavcan/marshal/array.hpp +++ b/libuavcan/include/uavcan/marshal/array.hpp @@ -6,6 +6,7 @@ #include #include +#include #include #include #include @@ -308,6 +309,7 @@ public: typedef typename Base::SizeType SizeType; using Base::size; + using Base::capacity; enum { IsDynamic = ArrayMode == ArrayModeDynamic }; enum { MaxSize = MaxSize_ }; @@ -416,6 +418,44 @@ public: return *this; } + /* + * Formatting appender. + * This method doesn't raise an overflow error; instead it silently truncates the data to fit the array capacity. + */ + template + void appendFormatted(const char* const format, const A value) + { + StaticAssert::check(); + StaticAssert::check(); + + StaticAssert::check(); // This check allows to weed out most non-trivial types + StaticAssert::check();// Another stupid check to catch non-trivial types + + if (!format) + { + assert(0); + return; + } + // Add some hardcore runtime checks for the format string correctness? + + ValueType* const ptr = Base::end(); + assert(capacity() >= size()); + const SizeType max_size = capacity() - size(); + + // We have one extra byte for the null terminator, hence +1 + const int ret = std::snprintf(reinterpret_cast(ptr), max_size + 1, format, value); + + for (int i = 0; i < std::min(ret, int(max_size)); i++) + { + Base::grow(); + } + if (ret < 0) + { + assert(0); // Likely an invalid format string + (*this) += format; // So we print it as is in release builds + } + } + typedef ValueType value_type; typedef SizeType size_type; }; diff --git a/libuavcan/test/marshal/array.cpp b/libuavcan/test/marshal/array.cpp index ae50829a4e..62829aafe8 100644 --- a/libuavcan/test/marshal/array.cpp +++ b/libuavcan/test/marshal/array.cpp @@ -726,6 +726,24 @@ TEST(Array, Strings) } +TEST(Array, AppendFormatted) +{ + typedef Array, ArrayModeDynamic, 45> A8; + + A8 a; + + ASSERT_TRUE("" == a); + + a.appendFormatted("%4.1f", 12.3); // 4 + a += " "; // 1 + a.appendFormatted("%li", -123456789L); // 10 + a.appendFormatted("%s", " TOTAL PERSPECTIVE VORTEX "); // 26 + a.appendFormatted("0x%X", 0xDEADBEEF); // 10 --> 4 + + ASSERT_STREQ("12.3 -123456789 TOTAL PERSPECTIVE VORTEX 0xDE", a.c_str()); +} + + TEST(Array, FlatStreaming) { typedef Array, ArrayModeDynamic, 32> A8D;