fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-12 19:33:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Using proper float comparison everywhere

Browse Source
This commit is contained in:
Pavel Kirienko
2014-08-27 19:13:45 +04:00
parent 6446d05446
commit 61c4f89ea3
6 changed files with 111 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/data_type_template.tmpl
+6 -3
View File
@@ -107,8 +107,12 @@ struct UAVCAN_EXPORT ${t.cpp_type_name}
#endif
}
bool operator!=(ParameterType rhs) const { return !operator==(rhs); }
/**
* Comparison operators are based on @ref uavcan::areClose(),
* which allows to safely compare floating point values.
*/
bool operator==(ParameterType rhs) const;
bool operator!=(ParameterType rhs) const { return !operator==(rhs); }
static int encode(ParameterType self, ::uavcan::ScalarCodec& codec,
::uavcan::TailArrayOptimizationMode tao_mode = ::uavcan::TailArrayOptEnabled);
@@ -179,11 +183,10 @@ UAVCAN_PACKED_END
template <int _tmpl>
bool ${scope_prefix}<_tmpl>::operator==(ParameterType rhs) const
{
// TODO: Proper float comparison.
% if fields:
return
% for idx,a in enumerate(fields):
${a.name} == rhs.${a.name} ${'&&' if (idx + 1) < len(fields) else ';'}
::uavcan::areClose(${a.name}, rhs.${a.name})${' &&' if (idx + 1) < len(fields) else ';'}
% endfor
% else:
(void)rhs;
libuavcan/include/uavcan/marshal/array.hpp
+13 -11
View File
@@ -148,7 +148,7 @@ private:
BufferType data_;
template <typename U>
typename EnableIf<sizeof(U(0) == U())>::Type initialize(int)
typename EnableIf<sizeof(U(0) >= U())>::Type initialize(int)
{
if (ArrayMode != ArrayModeDynamic)
{
@@ -405,23 +405,18 @@ class UAVCAN_EXPORT Array : public ArrayImpl<T, ArrayMode, MaxSize_>
for (InputIter it = src_row_major; index < MaxSize; ++it, ++index)
{
const bool on_diagonal = (index / Width) == (index % Width);
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
const bool nan = std::isnan(*it);
#else
// coverity[same_on_both_sides : FALSE]
const bool nan = (*it) != (*it);
#endif
const bool nan = isNaN(*it);
if (!nan)
{
all_nans = false;
}
if (!on_diagonal && (*it) != 0) // TODO: Proper float comparison
if (!on_diagonal && !isCloseToZero(*it))
{
scalar_matrix = false; // This matrix cannot be compressed.
diagonal_matrix = false;
break;
}
if (on_diagonal && (*it) != (*src_row_major)) // TODO: Proper float comparison
if (on_diagonal && !areClose(*it, *src_row_major))
{
scalar_matrix = false;
}
@@ -575,6 +570,7 @@ public:
/**
* This operator accepts any container with size() and [].
* Members are compared via @ref areClose().
*/
template <typename R>
typename EnableIf<sizeof(((const R*)(0U))->size()) && sizeof((*((const R*)(0U)))[0]), bool>::Type
@@ -586,7 +582,7 @@ public:
}
for (SizeType i = 0; i < size(); i++) // Bitset does not have iterators
{
if (!(Base::at(i) == rhs[i]))
if (!areClose(Base::at(i), rhs[i]))
{
return false;
}
@@ -607,6 +603,9 @@ public:
return std::strncmp(Base::c_str(), ch, MaxSize) == 0;
}
/**
* @ref operator==()
*/
template <typename R> bool operator!=(const R& rhs) const { return !operator==(rhs); }
/**
@@ -678,7 +677,7 @@ public:
StaticAssert<Base::IsStringLike>::check();
StaticAssert<IsDynamic>::check();
StaticAssert<sizeof(A() == A(0))>::check(); // This check allows to weed out most compound types
StaticAssert<sizeof(A() >= A(0))>::check(); // This check allows to weed out most compound types
StaticAssert<sizeof(A) <= sizeof(long double)>::check(); // Another stupid check to catch non-primitive types
if (!format)
@@ -710,6 +709,7 @@ public:
/**
* Fills this array as a packed square matrix from a static array.
* Please refer to the specification to learn more about matrix packing.
* Note that matrix packing code uses @ref areClose() for comparison.
*/
template <typename ScalarType>
void packSquareMatrix(const ScalarType (&src_row_major)[MaxSize])
@@ -720,6 +720,7 @@ public:
/**
* Fills this array as a packed square matrix in place.
* Please refer to the specification to learn more about matrix packing.
* Note that matrix packing code uses @ref areClose() for comparison.
*/
void packSquareMatrix()
{
@@ -751,6 +752,7 @@ public:
/**
* Fills this array as a packed square matrix from any container that implements the methods begin() and size().
* Please refer to the specification to learn more about matrix packing.
* Note that matrix packing code uses @ref areClose() for comparison.
*/
template <typename R>
typename EnableIf<sizeof(((const R*)(0U))->begin()) && sizeof(((const R*)(0U))->size())>::Type
libuavcan/include/uavcan/util/comparison.hpp
+33
View File
@@ -128,4 +128,37 @@ inline bool areClose<long double, double>(const long double& left, const double&
return areClose(static_cast<double>(left), right);
}
/**
* Comparison against zero.
* Helps to compare a floating point number against zero if the exact type is unknown.
* For non-floating point types performs exact comparison against integer zero.
*/
template <typename T>
UAVCAN_EXPORT
inline bool isCloseToZero(const T& x)
{
return x == 0;
}
template <>
UAVCAN_EXPORT
inline bool isCloseToZero<float>(const float& x)
{
return areClose(x, static_cast<float>(0.0F));
}
template <>
UAVCAN_EXPORT
inline bool isCloseToZero<double>(const double& x)
{
return areClose(x, static_cast<double>(0.0));
}
template <>
UAVCAN_EXPORT
inline bool isCloseToZero<long double>(const long double& x)
{
return areClose(x, static_cast<long double>(0.0L));
}
}
libuavcan/include/uavcan/util/templates.hpp
+1
View File
@@ -437,6 +437,7 @@ inline bool isNaN(T arg)
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
return std::isnan(arg);
#else
// coverity[same_on_both_sides : FALSE]
// cppcheck-suppress duplicateExpression
return arg != arg;
#endif
libuavcan/test/dsdl_test/dsdl_test.cpp
+16
View File
@@ -4,9 +4,11 @@
#include <gtest/gtest.h>
#include <uavcan/transport/transfer_buffer.hpp>
#include <limits>
#include <root_ns_a/EmptyService.hpp>
#include <root_ns_a/EmptyMessage.hpp>
#include <root_ns_a/NestedMessage.hpp>
#include <root_ns_a/A.hpp>
#include <root_ns_a/ReportBackSoldier.hpp>
#include <root_ns_b/ServiceWithEmptyRequest.hpp>
#include <root_ns_b/ServiceWithEmptyResponse.hpp>
@@ -75,6 +77,20 @@ TEST(Dsdl, Operators)
}
}
TEST(Dsdl, CloseComparison)
{
root_ns_a::A first, second;
ASSERT_TRUE(first == second);
first.vector[1].vector[1] = std::numeric_limits<double>::epsilon();
ASSERT_TRUE(first == second); // Still equals
first.vector[1].vector[1] = std::numeric_limits<float>::epsilon();
ASSERT_FALSE(first == second); // Nope
}
/*
* This test assumes that it will be executed before other GDTR tests; otherwise it fails.
* TODO: Probably it needs to be called directly from main()
libuavcan/test/marshal/array.cpp
+42
View File
@@ -1014,6 +1014,48 @@ TEST(Array, SquareMatrixPacking)
}
TEST(Array, FuzzySquareMatrixPacking)
{
Array<FloatSpec<64, CastModeSaturate>, ArrayModeDynamic, 36> m6x6d;
// Diagonal matrix will be reduced to an array of length Width
{
float diagonal6x6[] =
{
1, 0, 0, 0, 0, 0,
0, -2, 0, 0, 0, 0,
0, 0, 3, 0, 0, 0,
0, 0, 0, -4, 0, 0,
0, 0, 0, 0, 5, 0,
0, 0, 0, 0, 0, -6
};
// Some almost-zeroes
diagonal6x6[1] = std::numeric_limits<float>::epsilon();
diagonal6x6[4] = -std::numeric_limits<float>::epsilon();
diagonal6x6[34] = -std::numeric_limits<float>::epsilon();
m6x6d.packSquareMatrix(diagonal6x6);
ASSERT_EQ(6, m6x6d.size());
ASSERT_FLOAT_EQ(1, m6x6d[0]);
ASSERT_FLOAT_EQ(-2, m6x6d[1]);
ASSERT_FLOAT_EQ(3, m6x6d[2]);
ASSERT_FLOAT_EQ(-4, m6x6d[3]);
ASSERT_FLOAT_EQ(5, m6x6d[4]);
ASSERT_FLOAT_EQ(-6, m6x6d[5]);
std::vector<long double> output(36);
m6x6d.unpackSquareMatrix(output);
// This comparison will fail because epsilons
ASSERT_FALSE(std::equal(output.begin(), output.end(), diagonal6x6));
// This comparison will be ok
ASSERT_TRUE(std::equal(output.begin(), output.end(), diagonal6x6, &uavcan::areClose<float, float>));
}
}
TEST(Array, SquareMatrixPackingIntegers)
{
Array<IntegerSpec<30, SignednessSigned, CastModeSaturate>, ArrayModeDynamic, 9> m3x3int;