diff --git a/dsdl b/dsdl index fd12483ddd..1abd370556 160000 --- a/dsdl +++ b/dsdl @@ -1 +1 @@ -Subproject commit fd12483ddd4e58242d61d74a163e7aeaa1e0f466 +Subproject commit 1abd370556aea6aa93dfb0683965a8a43a26a29f diff --git a/libuavcan/include/uavcan/build_config.hpp b/libuavcan/include/uavcan/build_config.hpp index 6b8768c3e5..89688a3056 100644 --- a/libuavcan/include/uavcan/build_config.hpp +++ b/libuavcan/include/uavcan/build_config.hpp @@ -100,6 +100,13 @@ # define UAVCAN_TINY 0 #endif +/** + * Disable the global data type registry, which can save some space on embedded systems. + */ +#ifndef UAVCAN_NO_GLOBAL_DATA_TYPE_REGISTRY +# define UAVCAN_NO_GLOBAL_DATA_TYPE_REGISTRY 0 +#endif + /** * toString() methods will be disabled by default, unless the library is built for a general-purpose target like Linux. * It is not recommended to enable toString() on embedded targets as code size will explode. @@ -136,6 +143,14 @@ # define UAVCAN_USE_EXTERNAL_SNPRINTF 0 #endif +/** + * Allows the user's application to provide a custom implementation of IEEE754Converter::nativeIeeeToHalf and + * IEEE754Converter::halfToNativeIeee. + */ +#ifndef UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION +# define UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION 0 +#endif + /** * Run time checks. * Resolves to the standard assert() by default. diff --git a/libuavcan/include/uavcan/marshal/float_spec.hpp b/libuavcan/include/uavcan/marshal/float_spec.hpp index b77328f44a..ef3e93b884 100644 --- a/libuavcan/include/uavcan/marshal/float_spec.hpp +++ b/libuavcan/include/uavcan/marshal/float_spec.hpp @@ -36,12 +36,25 @@ struct NativeFloatSelector class UAVCAN_EXPORT IEEE754Converter { - // TODO: Non-IEEE float support for float32 and float64 - static uint16_t nativeNonIeeeToHalf(float value); - static float halfToNativeNonIeee(uint16_t value); + // TODO: Non-IEEE float support + + static uint16_t nativeIeeeToHalf(float value); + static float halfToNativeIeee(uint16_t value); IEEE754Converter(); + template + static void enforceIeee() + { + /* + * Some compilers may have is_iec559 to be defined false despite the fact that IEEE754 is supported. + * An acceptable workaround would be to put an #ifdef here. + */ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + StaticAssert::Type>::is_iec559>::check(); +#endif + } + public: #if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 /// UAVCAN requires rounding to nearest for all float conversions @@ -52,6 +65,7 @@ public: static typename IntegerSpec::StorageType toIeee(typename NativeFloatSelector::Type value) { + enforceIeee(); union { typename IntegerSpec::StorageType i; @@ -66,6 +80,7 @@ public: static typename NativeFloatSelector::Type toNative(typename IntegerSpec::StorageType value) { + enforceIeee(); union { typename IntegerSpec::StorageType i; @@ -80,13 +95,13 @@ template <> inline typename IntegerSpec<16, SignednessUnsigned, CastModeTruncate>::StorageType IEEE754Converter::toIeee<16>(typename NativeFloatSelector<16>::Type value) { - return nativeNonIeeeToHalf(value); + return nativeIeeeToHalf(value); } template <> inline typename NativeFloatSelector<16>::Type IEEE754Converter::toNative<16>(typename IntegerSpec<16, SignednessUnsigned, CastModeTruncate>::StorageType value) { - return halfToNativeNonIeee(value); + return halfToNativeIeee(value); } diff --git a/libuavcan/include/uavcan/node/global_data_type_registry.hpp b/libuavcan/include/uavcan/node/global_data_type_registry.hpp index cedf0d9724..fb8d43c39b 100644 --- a/libuavcan/include/uavcan/node/global_data_type_registry.hpp +++ b/libuavcan/include/uavcan/node/global_data_type_registry.hpp @@ -187,6 +187,7 @@ struct UAVCAN_EXPORT DefaultDataTypeRegistrator { DefaultDataTypeRegistrator() { +#if !UAVCAN_NO_GLOBAL_DATA_TYPE_REGISTRY const GlobalDataTypeRegistry::RegistrationResult res = GlobalDataTypeRegistry::instance().registerDataType(Type::DefaultDataTypeID); @@ -194,6 +195,7 @@ struct UAVCAN_EXPORT DefaultDataTypeRegistrator { handleFatalError("Type reg failed"); } +#endif } }; diff --git a/libuavcan/src/marshal/uc_float_spec.cpp b/libuavcan/src/marshal/uc_float_spec.cpp index 18a6e6b05e..13a1a7ac62 100644 --- a/libuavcan/src/marshal/uc_float_spec.cpp +++ b/libuavcan/src/marshal/uc_float_spec.cpp @@ -6,87 +6,84 @@ #include #include -#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) -# error UAVCAN_CPP_VERSION -#endif - -#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 -# include -#endif - namespace uavcan { + +#if !UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION + +union Fp32 +{ + uint32_t u; + float f; +}; + /* * IEEE754Converter - * Float16 conversion algorithm: http://half.sourceforge.net/ (MIT License) */ -uint16_t IEEE754Converter::nativeNonIeeeToHalf(float value) +uint16_t IEEE754Converter::nativeIeeeToHalf(float value) { - uint16_t hbits = uint16_t(getSignBit(value) ? 0x8000U : 0); - if (areFloatsExactlyEqual(value, 0.0F)) + /* + * https://gist.github.com/rygorous/2156668 + * Public domain, by Fabian "ryg" Giesen + */ + const Fp32 f32infty = { 255U << 23 }; + const Fp32 f16infty = { 31U << 23 }; + const Fp32 magic = { 15U << 23 }; + const uint32_t sign_mask = 0x80000000U; + const uint32_t round_mask = ~0xFFFU; + + Fp32 in; + uint16_t out; + + in.f = value; + + uint32_t sign = in.u & sign_mask; + in.u ^= sign; + + if (in.u >= f32infty.u) /* Inf or NaN (all exponent bits set) */ { - return hbits; + /* NaN->sNaN and Inf->Inf */ + out = (in.u > f32infty.u) ? 0x7FFFU : 0x7C00U; } - if (isNaN(value)) + else /* (De)normalized number or zero */ { - return hbits | 0x7FFFU; + in.u &= round_mask; + in.f *= magic.f; + in.u -= round_mask; + if (in.u > f16infty.u) + { + in.u = f16infty.u; /* Clamp to signed infinity if overflowed */ + } + + out = uint16_t(in.u >> 13); /* Take the bits! */ } - if (isInfinity(value)) - { - return hbits | 0x7C00U; - } - int exp; - (void)std::frexp(value, &exp); - if (exp > 16) - { - return hbits | 0x7C00U; - } - if (exp < -13) - { - value = std::ldexp(value, 24); - } - else - { - value = std::ldexp(value, 11 - exp); - hbits |= uint16_t((exp + 14) << 10); - } - const int32_t ival = static_cast(value); - hbits = uint16_t(hbits | (uint32_t((ival < 0) ? (-ival) : ival) & 0x3FFU)); - float diff = std::fabs(value - static_cast(ival)); - hbits = uint16_t(hbits + (diff >= 0.5F)); - return hbits; + + out |= uint16_t(sign >> 16); + + return out; } -float IEEE754Converter::halfToNativeNonIeee(uint16_t value) +float IEEE754Converter::halfToNativeIeee(uint16_t value) { - float out; - unsigned abs = value & 0x7FFFU; - if (abs > 0x7C00U) + /* + * https://gist.github.com/rygorous/2144712 + * Public domain, by Fabian "ryg" Giesen + */ + const Fp32 magic = { (254U - 15U) << 23 }; + const Fp32 was_infnan = { (127U + 16U) << 23 }; + Fp32 out; + + out.u = (value & 0x7FFFU) << 13; /* exponent/mantissa bits */ + out.f *= magic.f; /* exponent adjust */ + if (out.f >= was_infnan.f) /* make sure Inf/NaN survive */ { -#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 - out = std::numeric_limits::has_quiet_NaN ? std::numeric_limits::quiet_NaN() : 0.0F; -#else - out = nanf(""); -#endif + out.u |= 255U << 23; } - else if (abs == 0x7C00U) - { -#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 - out = std::numeric_limits::has_infinity ? - std::numeric_limits::infinity() : std::numeric_limits::max(); -#else - out = NumericTraits::infinity(); -#endif - } - else if (abs > 0x3FFU) - { - out = std::ldexp(static_cast((value & 0x3FFU) | 0x400U), int(abs >> 10) - 25); - } - else - { - out = std::ldexp(static_cast(abs), -24); - } - return (value & 0x8000U) ? -out : out; + out.u |= (value & 0x8000U) << 16; /* sign bit */ + + return out.f; } +#endif // !UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION + } diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index 11e291a955..54fc5de6a3 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -75,7 +75,7 @@ protected: friend FDCache; FDCacheItem* next_; - time_t last_access_; + std::time_t last_access_; const int fd_; const int oflags_; const char* const path_; @@ -101,6 +101,7 @@ protected: ~FDCacheItem() { + using namespace std; if (valid()) { ::free(const_cast(path_)); @@ -117,13 +118,14 @@ protected: return fd_; } - time_t getAccess() const + std::time_t getAccess() const { return last_access_; } - time_t acessed() + std::time_t acessed() { + using namespace std; last_access_ = time(NULL); return getAccess(); } @@ -135,11 +137,13 @@ protected: bool expired() const { + using namespace std; return 0 == last_access_ || (time(NULL) - last_access_) > MaxAgeSeconds; } bool equals(const char* path, int oflags) const { + using namespace std; return oflags_ == oflags && 0 == ::strcmp(path, path_); } @@ -275,7 +279,6 @@ protected: if (pi && !pi->valid()) { /* Allocation worked but clone or path failed */ - delete pi; pi = NULL; } @@ -286,7 +289,6 @@ protected: * If allocation fails no harm just can not cache it * return open fd */ - return fd; } /* add new */ @@ -334,7 +336,7 @@ protected: * Implementation of this method is required. * On success the method must return zero. */ - virtual int16_t getInfo(const Path& path, uint64_t& out_size, EntryType& out_type) + virtual uavcan::int16_t getInfo(const Path& path, uavcan::uint64_t& out_size, EntryType& out_type) { int rv = uavcan::protocol::file::Error::INVALID_VALUE; @@ -376,12 +378,15 @@ protected: * if the end of file is reached. * On success the method must return zero. */ - virtual int16_t read(const Path& path, const uint64_t offset, uint8_t* out_buffer, uint16_t& inout_size) + virtual uavcan::int16_t read(const Path& path, const uavcan::uint64_t offset, uavcan::uint8_t* out_buffer, + uavcan::uint16_t& inout_size) { int rv = uavcan::protocol::file::Error::INVALID_VALUE; - if (path.size() > 0) + if (path.size() > 0 && inout_size != 0) { + using namespace std; + FDCacheBase& cache = getFDCache(); int fd = cache.open(path.c_str(), O_RDONLY); @@ -391,9 +396,9 @@ protected: } else { - rv = ::lseek(fd, offset, SEEK_SET); + ssize_t total_read = 0; - ssize_t len = 0; + rv = ::lseek(fd, offset, SEEK_SET); if (rv < 0) { @@ -401,21 +406,27 @@ protected: } else { - // TODO use a read at offset to fill on EAGAIN - len = ::read(fd, out_buffer, inout_size); - - if (len < 0) + rv = 0; + ssize_t remaining = inout_size; + ssize_t nread = 0; + do { - rv = errno; - } - else - { - rv = 0; + nread = ::read(fd, &out_buffer[total_read], remaining); + if (nread < 0) + { + rv = errno; + } + else + { + remaining -= nread, + total_read += nread; + } } + while (nread > 0 && remaining > 0); } - (void)cache.close(fd, rv != 0 || len != inout_size); - inout_size = len; + (void)cache.close(fd, rv != 0 || total_read != inout_size); + inout_size = total_read; } } return rv; diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp index d07f6d5624..50b8202184 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp @@ -51,10 +51,22 @@ protected: { const int FormatBufferLength = 63; char buffer[FormatBufferLength + 1]; - int n = snprintf(buffer, FormatBufferLength, "%ld.%06ld\t%d\t%lld\n", - static_cast(ts.tv_sec), static_cast(ts.tv_nsec / 1000L), - static_cast(code), static_cast(argument)); - (void)write(fd, buffer, n); // TODO FIXME Write loop + ssize_t remaining = snprintf(buffer, FormatBufferLength, "%ld.%06ld\t%d\t%lld\n", + static_cast(ts.tv_sec), static_cast(ts.tv_nsec / 1000L), + static_cast(code), static_cast(argument)); + + ssize_t total_written = 0; + ssize_t written = 0; + do + { + written = write(fd, &buffer[total_written], remaining); + if (written > 0) + { + total_written += written; + remaining -= written; + } + } + while (written > 0 && remaining > 0); (void)close(fd); } } @@ -82,7 +94,6 @@ public: return rv; } }; - } } diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp index 7374c3cc0d..03ee6175bb 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -55,13 +55,25 @@ protected: { char buffer[MaxStringLength + 1]; (void)memset(buffer, 0, sizeof(buffer)); - int len = read(fd, buffer, MaxStringLength); - (void)close(fd); - if (len > 0) + ssize_t remaining = MaxStringLength; + ssize_t total_read = 0; + ssize_t nread = 0; + do { - for (int i = 0; i < len; i++) + nread = ::read(fd, &buffer[total_read], remaining); + if (nread > 0) { - if (buffer[i] == ' ' || buffer[i] == '\n' || buffer[i] == '\r' ) + remaining -= nread, + total_read += nread; + } + } + while (nread > 0 && remaining > 0); + (void)close(fd); + if (total_read > 0) + { + for (int i = 0; i < total_read; i++) + { + if (buffer[i] == ' ' || buffer[i] == '\n' || buffer[i] == '\r') { buffer[i] = '\0'; break; @@ -81,7 +93,20 @@ protected: int fd = open(path.c_str(), O_WRONLY | O_CREAT | O_TRUNC, FilePermissions); if (fd >= 0) { - (void)write(fd, value.c_str(), value.size()); // TODO FIXME Write loop + ssize_t remaining = value.size(); + ssize_t total_written = 0; + ssize_t written = 0; + do + { + written = write(fd, &value.c_str()[total_written], remaining); + if (written > 0) + { + total_written += written; + remaining -= written; + } + } + while (written > 0 && remaining > 0); + (void)fsync(fd); (void)close(fd); } @@ -117,7 +142,7 @@ public: // coverity[toctou] rv = mkdir(base_path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); } - if (rv >= 0 ) + if (rv >= 0) { base_path.push_back('/'); if ((base_path.size() + MaxStringLength) > MaxPathLength) @@ -129,7 +154,6 @@ public: return rv; } }; - } } diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp index 58143e1056..a258a6a368 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -86,6 +86,8 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker int copyIfNot(const char* srcpath, const char* destpath) { + using namespace std; + // Does the file exist int rv = 0; int dfd = open(destpath, O_RDONLY, 0); @@ -113,7 +115,7 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker } else { - ssize_t size; + ssize_t size = 0; do { size = ::read(sfd, buffer, sizeof(buffer)); @@ -125,14 +127,28 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker } else { - if (size != write(dfd, buffer, size)) + rv = 0; + ssize_t remaining = size; + ssize_t total_written = 0; + ssize_t written = 0; + do { - rv = -errno; + written = write(dfd, &buffer[total_written], remaining); + if (written < 0) + { + rv = -errno; + } + else + { + total_written += written; + remaining -= written; + } } + while (written > 0 && remaining > 0); } } } - while (rv == 0 && size); + while (rv == 0 && size != 0); (void)close(sfd); } @@ -144,13 +160,13 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker struct AppDescriptor { - uint8_t signature[sizeof(uavcan::uint64_t)]; - uint64_t image_crc; - uint32_t image_size; - uint32_t vcs_commit; - uint8_t major_version; - uint8_t minor_version; - uint8_t reserved[6]; + uavcan::uint8_t signature[sizeof(uavcan::uint64_t)]; + uavcan::uint64_t image_crc; + uavcan::uint32_t image_size; + uavcan::uint32_t vcs_commit; + uavcan::uint8_t major_version; + uavcan::uint8_t minor_version; + uavcan::uint8_t reserved[6]; }; static int getFileInfo(const char* path, AppDescriptor& descriptor) @@ -251,7 +267,7 @@ protected: node_info.hardware_version.major, node_info.hardware_version.minor); - if (n > 0 && n < (int) sizeof(fname_root) - 2) + if (n > 0 && n < (int)sizeof(fname_root) - 2) { DIR* const fwdir = opendir(fname_root); diff --git a/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp b/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp index 784cee33e3..a5481c94a1 100644 --- a/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp +++ b/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp @@ -32,22 +32,26 @@ void ledSet(bool state) palWritePad(GPIO_PORT_LED, GPIO_PIN_LED, state); } -int init() +void init() { - int res = 0; - halInit(); chibios_rt::System::init(); sdStart(&STDOUT_SD, NULL); - res = can.init(1000000); - if (res < 0) + int res = 0; + do { - goto leave; - } + ::sleep(1); + ::lowsyslog("CAN auto bit rate detection...\n"); -leave: - return res; + std::uint32_t bitrate = 0; + res = can.init([]() { ::usleep(can.getRecommendedListeningDelay().toUSec()); }, bitrate); + if (res >= 0) + { + ::lowsyslog("CAN inited at %u bps\n", unsigned(bitrate)); + } + } + while (res < 0); } #if __GNUC__ @@ -55,7 +59,7 @@ __attribute__((noreturn)) #endif void die(int status) { - lowsyslog("Now I am dead x_x %i\n", status); + lowsyslog("Initialization failure %i\n", status); while (1) { ledSet(false); @@ -166,11 +170,7 @@ public: int main() { - const int init_res = app::init(); - if (init_res != 0) - { - app::die(init_res); - } + app::init(); lowsyslog("Starting the UAVCAN thread\n"); app::uavcan_node_thread.start(LOWPRIO);