From 51e42038c6fa47fe2bc8f3f19427a366bee26b6d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 22 Feb 2014 15:06:08 +0400 Subject: [PATCH] FloatSpec<> implementation and tests --- .../internal/marshalling/float_spec.hpp | 155 +++++++++++++++ libuavcan/src/marshalling/float_spec.cpp | 64 +++++++ libuavcan/test/marshalling/float_spec.cpp | 180 ++++++++++++++++++ 3 files changed, 399 insertions(+) create mode 100644 libuavcan/include/uavcan/internal/marshalling/float_spec.hpp create mode 100644 libuavcan/src/marshalling/float_spec.cpp create mode 100644 libuavcan/test/marshalling/float_spec.cpp diff --git a/libuavcan/include/uavcan/internal/marshalling/float_spec.hpp b/libuavcan/include/uavcan/internal/marshalling/float_spec.hpp new file mode 100644 index 0000000000..b265f59923 --- /dev/null +++ b/libuavcan/include/uavcan/internal/marshalling/float_spec.hpp @@ -0,0 +1,155 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include // Needed for isfinite +#include +#include +#include + +namespace uavcan +{ + +template +struct NativeFloatSelector +{ + struct ErrorNoSuchFloat; + typedef typename StaticIf<(sizeof(float) * 8 >= BitLen), float, + typename StaticIf<(sizeof(double) * 8 >= BitLen), double, + typename StaticIf<(sizeof(long double) * 8 >= BitLen), long double, + ErrorNoSuchFloat>::Result>::Result>::Result Type; +}; + + +class IEEE754Converter +{ + // TODO: Non-IEEE float support for float32 and float64 + static uint16_t nativeNonIeeeToHalf(float value); + static float halfToNativeNonIeee(uint16_t value); + +public: + /// UAVCAN requires rounding to nearest for all float conversions + static std::float_round_style roundstyle() { return std::round_to_nearest; } + + template + static typename IntegerSpec::StorageType + toIeee(typename NativeFloatSelector::Type value) + { + typedef typename IntegerSpec::StorageType IntType; + typedef typename NativeFloatSelector::Type FloatType; + StaticAssert::is_iec559>::check(); + union { IntType i; FloatType f; } u; + u.f = value; + return u.i; + } + + template + static typename NativeFloatSelector::Type + toNative(typename IntegerSpec::StorageType value) + { + typedef typename IntegerSpec::StorageType IntType; + typedef typename NativeFloatSelector::Type FloatType; + StaticAssert::is_iec559>::check(); + union { IntType i; FloatType f; } u; + u.i = value; + return u.f; + } +}; +template <> +typename IntegerSpec<16, SignednessUnsigned, CastModeTruncate>::StorageType +IEEE754Converter::toIeee<16>(typename NativeFloatSelector<16>::Type value) +{ + return nativeNonIeeeToHalf(value); +} +template <> +typename NativeFloatSelector<16>::Type +IEEE754Converter::toNative<16>(typename IntegerSpec<16, SignednessUnsigned, CastModeTruncate>::StorageType value) +{ + return halfToNativeNonIeee(value); +} + + +template struct IEEE754Limits; +template <> struct IEEE754Limits<16> +{ + static typename NativeFloatSelector<16>::Type max() { return 65504.0; } + static typename NativeFloatSelector<16>::Type epsilon() { return 9.77e-04; } +}; +template <> struct IEEE754Limits<32> +{ + static typename NativeFloatSelector<32>::Type max() { return 3.40282346638528859812e+38; } + static typename NativeFloatSelector<32>::Type epsilon() { return 1.19209289550781250000e-7; } +}; +template <> struct IEEE754Limits<64> +{ + static typename NativeFloatSelector<64>::Type max() { return 1.79769313486231570815e+308L; } + static typename NativeFloatSelector<64>::Type epsilon() { return 2.22044604925031308085e-16L; } +}; + + +template +class FloatSpec : public IEEE754Limits +{ +public: + enum { BitLen = BitLen_ }; + + typedef typename NativeFloatSelector::Type StorageType; + + enum { IsExactRepresentation = (sizeof(StorageType) * 8 == BitLen) && std::numeric_limits::is_iec559 }; + + using IEEE754Limits::max; + using IEEE754Limits::epsilon; + static StorageType init() { return 0.0; } + static std::float_round_style roundstyle() { return IEEE754Converter::roundstyle(); } + + static int encode(StorageType value, ScalarCodec& codec, bool enable_tail_array_optimization = false) + { + (void)enable_tail_array_optimization; + // cppcheck-suppress duplicateExpression + if (CastMode == CastModeSaturate) + saturate(value); + else + truncate(value); + return codec.encode(IEEE754Converter::toIeee(value)); + } + + static int decode(StorageType& out_value, ScalarCodec& codec, bool enable_tail_array_optimization = false) + { + (void)enable_tail_array_optimization; + typename IntegerSpec::StorageType ieee = 0; + const int res = codec.decode(ieee); + if (res <= 0) + return res; + out_value = IEEE754Converter::toNative(ieee); + return res; + } + +private: + static inline void saturate(StorageType& value) + { + if (!IsExactRepresentation && isfinite(value)) + { + if (value > max()) + value = max(); + else if (value < -max()) + value = -max(); + } + } + + static inline void truncate(StorageType& value) + { + if (!IsExactRepresentation && isfinite(value)) + { + if (value > max()) + value = std::numeric_limits::infinity(); + else if (value < -max()) + value = -std::numeric_limits::infinity(); + } + } +}; + +} diff --git a/libuavcan/src/marshalling/float_spec.cpp b/libuavcan/src/marshalling/float_spec.cpp new file mode 100644 index 0000000000..631007638b --- /dev/null +++ b/libuavcan/src/marshalling/float_spec.cpp @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +namespace uavcan +{ +/* + * IEEE754Converter + * Float16 conversion algorithm: http://half.sourceforge.net/ (MIT License) + * TODO: Use conversion tables (conditional compilation - it would require something like 10Kb+ ROM). + */ +template +static inline bool signbit(T arg) +{ + return arg < T(0) || (arg == T(0) && T(1) / arg < T(0)); +} + +uint16_t IEEE754Converter::nativeNonIeeeToHalf(float value) +{ + uint16_t hbits = signbit(value) << 15; + if (value == 0.0f) + return hbits; + if (isnanf(value)) + return hbits | 0x7FFF; + if (isinff(value)) + return hbits | 0x7C00; + int exp; + std::frexp(value, &exp); + if (exp > 16) + return hbits | 0x7C00; + if (exp < -13) + value = std::ldexp(value, 24); + else + { + value = std::ldexp(value, 11 - exp); + hbits |= ((exp + 14) << 10); + } + const int ival = static_cast(value); + hbits |= static_cast(std::abs(ival) & 0x3FF); + float diff = std::abs(value - static_cast(ival)); + hbits += diff >= 0.5f; + return hbits; +} + +float IEEE754Converter::halfToNativeNonIeee(uint16_t value) +{ + float out; + int abs = value & 0x7FFF; + if (abs > 0x7C00) + out = std::numeric_limits::has_quiet_NaN ? std::numeric_limits::quiet_NaN() : 0.0f; + else if (abs == 0x7C00) + out = std::numeric_limits::has_infinity ? + std::numeric_limits::infinity() : std::numeric_limits::max(); + else if (abs > 0x3FF) + out = std::ldexp(static_cast((value & 0x3FF) | 0x400), (abs >> 10) - 25); + else + out = std::ldexp(static_cast(abs), -24); + return (value & 0x8000) ? -out : out; +} + +} diff --git a/libuavcan/test/marshalling/float_spec.cpp b/libuavcan/test/marshalling/float_spec.cpp new file mode 100644 index 0000000000..71595a37a3 --- /dev/null +++ b/libuavcan/test/marshalling/float_spec.cpp @@ -0,0 +1,180 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + + +TEST(FloatSpec, Limits) +{ + using uavcan::FloatSpec; + using uavcan::CastModeSaturate; + using uavcan::CastModeTruncate; + + typedef FloatSpec<16, CastModeSaturate> F16S; + typedef FloatSpec<16, CastModeTruncate> F16T; + typedef FloatSpec<32, CastModeSaturate> F32S; + typedef FloatSpec<32, CastModeTruncate> F32T; + typedef FloatSpec<64, CastModeSaturate> F64S; + typedef FloatSpec<64, CastModeTruncate> F64T; + + ASSERT_FALSE(F16S::IsExactRepresentation); + ASSERT_EQ(0, F16S::init()); + ASSERT_FLOAT_EQ(65504.0, F16S::max()); + ASSERT_FLOAT_EQ(9.77e-04, F16S::epsilon()); + + ASSERT_TRUE(F32T::IsExactRepresentation); + ASSERT_EQ(0, F32T::init()); + ASSERT_FLOAT_EQ(std::numeric_limits::max(), F32T::max()); + ASSERT_FLOAT_EQ(std::numeric_limits::epsilon(), F32T::epsilon()); + + ASSERT_TRUE(F64S::IsExactRepresentation); + ASSERT_EQ(0, F64S::init()); + ASSERT_FLOAT_EQ(std::numeric_limits::max(), F64S::max()); + ASSERT_FLOAT_EQ(std::numeric_limits::epsilon(), F64S::epsilon()); +} + +TEST(FloatSpec, Basic) +{ + using uavcan::FloatSpec; + using uavcan::CastModeSaturate; + using uavcan::CastModeTruncate; + using uavcan::StorageType; + + typedef FloatSpec<16, CastModeSaturate> F16S; + typedef FloatSpec<16, CastModeTruncate> F16T; + typedef FloatSpec<32, CastModeSaturate> F32S; + typedef FloatSpec<32, CastModeTruncate> F32T; + typedef FloatSpec<64, CastModeSaturate> F64S; + typedef FloatSpec<64, CastModeTruncate> F64T; + + static const long double Values[] = + { + 0.0, + 1.0, + M_PI, + 123, + -123, + 99999, + -999999, + std::numeric_limits::max(), + -std::numeric_limits::max(), + std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), + nanl("") + }; + static const int NumValues = sizeof(Values) / sizeof(Values[0]); + + static const long double ValuesF16S[] = + { + 0.0, + 1.0, + 3.140625, + 123, + -123, + F16S::max(), + -F16S::max(), + F16S::max(), + -F16S::max(), + std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), + nanl("") + }; + static const long double ValuesF16T[] = + { + 0.0, + 1.0, + 3.140625, + 123, + -123, + std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), + std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), + std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), + nanl("") + }; + + /* + * Writing + */ + uavcan::StaticTransferBuffer buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + for (int i = 0; i < NumValues; i++) + { + ASSERT_EQ(1, F16S::encode(Values[i], sc_wr)); + ASSERT_EQ(1, F16T::encode(Values[i], sc_wr)); + ASSERT_EQ(1, F32S::encode(Values[i], sc_wr)); + ASSERT_EQ(1, F32T::encode(Values[i], sc_wr)); + ASSERT_EQ(1, F64S::encode(Values[i], sc_wr)); + ASSERT_EQ(1, F64T::encode(Values[i], sc_wr)); + } + + ASSERT_EQ(0, F16S::encode(0, sc_wr)); // Out of buffer space now + + /* + * Reading + */ + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + +#define CHECK(FloatType, expected_value) \ + do { \ + StorageType::Type var(FloatType::init()); \ + ASSERT_EQ(1, FloatType::decode(var, sc_rd)); \ + if (!isnan(expected_value)) \ + ASSERT_FLOAT_EQ(expected_value, var); \ + else \ + ASSERT_EQ(!!isnan(expected_value), !!isnan(var)); \ + } while (0) + + for (int i = 0; i < NumValues; i++) + { + CHECK(F16S, ValuesF16S[i]); + CHECK(F16T, ValuesF16T[i]); + CHECK(F32S, Values[i]); + CHECK(F32T, Values[i]); + CHECK(F64S, Values[i]); + CHECK(F64T, Values[i]); + } + +#undef CHECK +} + +TEST(FloatSpec, Float16Representation) +{ + using uavcan::FloatSpec; + using uavcan::CastModeSaturate; + using uavcan::CastModeTruncate; + + typedef FloatSpec<16, CastModeSaturate> F16S; + typedef FloatSpec<16, CastModeTruncate> F16T; + + uavcan::StaticTransferBuffer<2 * 6> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + ASSERT_EQ(1, F16S::encode(0.0, sc_wr)); + ASSERT_EQ(1, F16S::encode(1.0, sc_wr)); + ASSERT_EQ(1, F16S::encode(-2.0, sc_wr)); + ASSERT_EQ(1, F16T::encode(999999, sc_wr)); // +inf + ASSERT_EQ(1, F16S::encode(-999999, sc_wr)); // -max + ASSERT_EQ(1, F16S::encode(nan(""), sc_wr)); // nan + + ASSERT_EQ(0, F16S::encode(0, sc_wr)); // Out of buffer space now + + static const std::string Reference = // Keep in mind that this is LITTLE ENDIAN representation + "00000000 00000000 " // 0.0 + "00000000 00111100 " // 1.0 + "00000000 11000000 " // -2.0 + "00000000 01111100 " // +inf + "11111111 11111011 " // -max + "11111111 01111111"; // nan + + ASSERT_EQ(Reference, bs_wr.toString()); +}