mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-27 20:44:07 +08:00
FloatSpec<> implementation and tests
This commit is contained in:
Pavel Kirienko
parent
1e1fdc613b
commit
51e42038c6
155
libuavcan/include/uavcan/internal/marshalling/float_spec.hpp
Normal file
155
libuavcan/include/uavcan/internal/marshalling/float_spec.hpp
Normal file
@ -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();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
64
libuavcan/src/marshalling/float_spec.cpp
Normal file
64
libuavcan/src/marshalling/float_spec.cpp
Normal file
@ -0,0 +1,64 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include <uavcan/internal/marshalling/float_spec.hpp>
|
||||
#include <cmath>
|
||||
|
||||
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 <typename T>
|
||||
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<int>(value);
|
||||
hbits |= static_cast<uint16_t>(std::abs(ival) & 0x3FF);
|
||||
float diff = std::abs(value - static_cast<float>(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<float>::has_quiet_NaN ? std::numeric_limits<float>::quiet_NaN() : 0.0f;
|
||||
else if (abs == 0x7C00)
|
||||
out = std::numeric_limits<float>::has_infinity ?
|
||||
std::numeric_limits<float>::infinity() : std::numeric_limits<float>::max();
|
||||
else if (abs > 0x3FF)
|
||||
out = std::ldexp(static_cast<float>((value & 0x3FF) | 0x400), (abs >> 10) - 25);
|
||||
else
|
||||
out = std::ldexp(static_cast<float>(abs), -24);
|
||||
return (value & 0x8000) ? -out : out;
|
||||
}
|
||||
|
||||
}
|
||||
180
libuavcan/test/marshalling/float_spec.cpp
Normal file
180
libuavcan/test/marshalling/float_spec.cpp
Normal file
@ -0,0 +1,180 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <uavcan/internal/marshalling/float_spec.hpp>
|
||||
#include <uavcan/internal/marshalling/types.hpp>
|
||||
|
||||
|
||||
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<float>::max(), F32T::max());
|
||||
ASSERT_FLOAT_EQ(std::numeric_limits<float>::epsilon(), F32T::epsilon());
|
||||
|
||||
ASSERT_TRUE(F64S::IsExactRepresentation);
|
||||
ASSERT_EQ(0, F64S::init());
|
||||
ASSERT_FLOAT_EQ(std::numeric_limits<double>::max(), F64S::max());
|
||||
ASSERT_FLOAT_EQ(std::numeric_limits<double>::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<float>::max(),
|
||||
-std::numeric_limits<float>::max(),
|
||||
std::numeric_limits<double>::infinity(),
|
||||
-std::numeric_limits<double>::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<F16S::StorageType>::infinity(),
|
||||
-std::numeric_limits<F16S::StorageType>::infinity(),
|
||||
nanl("")
|
||||
};
|
||||
static const long double ValuesF16T[] =
|
||||
{
|
||||
0.0,
|
||||
1.0,
|
||||
3.140625,
|
||||
123,
|
||||
-123,
|
||||
std::numeric_limits<F16S::StorageType>::infinity(),
|
||||
-std::numeric_limits<F16S::StorageType>::infinity(),
|
||||
std::numeric_limits<F16S::StorageType>::infinity(),
|
||||
-std::numeric_limits<F16S::StorageType>::infinity(),
|
||||
std::numeric_limits<F16S::StorageType>::infinity(),
|
||||
-std::numeric_limits<F16S::StorageType>::infinity(),
|
||||
nanl("")
|
||||
};
|
||||
|
||||
/*
|
||||
* Writing
|
||||
*/
|
||||
uavcan::StaticTransferBuffer<NumValues * (2 + 4 + 8) * 2> 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<FloatType>::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());
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user