Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: matrix_alg: disable FE_OVERFLOW in inverse4x4() for SITL

Browse Source 
There are occasional overflows on the determinant calculation in inverse4x4()
when using calibration SITL model.
This commit is contained in:
Gustavo Jose de Sousa 2016-05-09 14:53:30 -03:00 committed by Andrew Tridgell
parent a7543d369f
commit b594b5a08b
1 changed files with 20 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_Math/matrix_alg.cpp
View File

@ -18,9 +18,15 @@
*/ */
#pragma GCC optimize("O3") #pragma GCC optimize("O3")
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <stdio.h> #include <stdio.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <fenv.h>
#endif
#include <AP_Math/AP_Math.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
//TODO: use higher precision datatypes to achieve more accuracy for matrix algebra operations //TODO: use higher precision datatypes to achieve more accuracy for matrix algebra operations
@ -279,6 +285,13 @@ bool inverse4x4(float m[],float invOut[])
float inv[16], det; float inv[16], det;
uint8_t i; uint8_t i;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
int old = fedisableexcept(FE_OVERFLOW);
if (old < 0) {
hal.console->printf("inverse4x4(): warning: error on disabling FE_OVERFLOW floating point exception\n");
}
#endif
inv[0] = m[5] * m[10] * m[15] - inv[0] = m[5] * m[10] * m[15] -
m[5] * m[11] * m[14] - m[5] * m[11] * m[14] -
m[9] * m[6] * m[15] + m[9] * m[6] * m[15] +
@ -393,6 +406,12 @@ bool inverse4x4(float m[],float invOut[])
det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12]; det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (old >= 0 && feenableexcept(old) < 0) {
hal.console->printf("inverse4x4(): warning: error on restoring floating exception mask\n");
}
#endif
if (is_zero(det) || isinf(det)){ if (is_zero(det) || isinf(det)){
return false; return false;
} }