FloatSpec<> implementation and tests

2026-10-18 01:33:34 +08:00 · 2014-02-22 15:06:08 +04:00
parent 1e1fdc613b
commit 51e42038c6
3 changed files with 399 additions and 0 deletions
@@ -0,0 +1,155 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <limits>
+#include <math.h>            // Needed for isfinite
+#include <uavcan/internal/util.hpp>
+#include <uavcan/internal/marshalling/cast_mode.hpp>
+#include <uavcan/internal/marshalling/integer_spec.hpp>
+
+namespace uavcan
+{
+
+template <unsigned int BitLen>
+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 <unsigned int BitLen>
+    static typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType
+    toIeee(typename NativeFloatSelector<BitLen>::Type value)
+    {
+        typedef typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType IntType;
+        typedef typename NativeFloatSelector<BitLen>::Type FloatType;
+        StaticAssert<sizeof(FloatType) * 8 == BitLen && std::numeric_limits<FloatType>::is_iec559>::check();
+        union { IntType i; FloatType f; } u;
+        u.f = value;
+        return u.i;
+    }
+
+    template <unsigned int BitLen>
+    static typename NativeFloatSelector<BitLen>::Type
+    toNative(typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType value)
+    {
+        typedef typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType IntType;
+        typedef typename NativeFloatSelector<BitLen>::Type FloatType;
+        StaticAssert<sizeof(FloatType) * 8 == BitLen && std::numeric_limits<FloatType>::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 <unsigned int BitLen> 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 <unsigned int BitLen_, CastMode CastMode>
+class FloatSpec : public IEEE754Limits<BitLen_>
+{
+public:
+    enum { BitLen = BitLen_ };
+
+    typedef typename NativeFloatSelector<BitLen>::Type StorageType;
+
+    enum { IsExactRepresentation = (sizeof(StorageType) * 8 == BitLen) && std::numeric_limits<StorageType>::is_iec559 };
+
+    using IEEE754Limits<BitLen>::max;
+    using IEEE754Limits<BitLen>::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<BitLen>(IEEE754Converter::toIeee<BitLen>(value));
+    }
+
+    static int decode(StorageType& out_value, ScalarCodec& codec, bool enable_tail_array_optimization = false)
+    {
+        (void)enable_tail_array_optimization;
+        typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType ieee = 0;
+        const int res = codec.decode<BitLen>(ieee);
+        if (res <= 0)
+            return res;
+        out_value = IEEE754Converter::toNative<BitLen>(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<StorageType>::infinity();
+            else if (value < -max())
+                value = -std::numeric_limits<StorageType>::infinity();
+        }
+    }
+};
+
+}