fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix unit test, create matrix isfinite.

Browse Source
This commit is contained in:
James Goppert 2017-02-03 16:25:55 -05:00
parent 7e3eff7b2d
commit 230e84702a
2 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
matrix/SquareMatrix.hpp
View File

@ -239,9 +239,9 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
}
//check sanity of results
for (uint8_t i = 0; i < M; i++) {
for (uint8_t j = 0; j < M; j++) {
if (!PX4_ISFINITE(P(i,j))) {
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < M; j++) {
if (!isfinite(P(i,j))) {
return false;
}
}

13
matrix/helper_functions.hpp
View File

@ -13,14 +13,19 @@
namespace matrix
{
template<typename Type>
bool isfinite(Type x) {
#if defined (__PX4_NUTTX) || defined (__PX4_QURT)
return PX4_ISFINITE(x);
#else
return std::isfinite(x);
#endif
}
template<typename Type>
Type wrap_pi(Type x)
{
#if defined (__PX4_NUTTX) || defined (__PX4_QURT)
if (!isfinite(x)) {
#else
if (!std::isfinite(x)) {
#endif
return x;
}