fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 20:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed std::isfinite vs isfinite differences

Browse Source 
Added PX4_ISFINITE(x) to px4_defines.h to handle the differences on
NuttX and Linux.

This change also picked up some file renaming for virtual character devices

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
2015-04-20 11:34:12 -07:00
parent 07b16ffa01
commit 8c8f57b5b4
9 changed files with 39 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+8 -7
View File
@@ -44,6 +44,7 @@
#include "estimator_22states.h"
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <stdio.h>
#include <stdlib.h>
@@ -1155,7 +1156,7 @@ void AttitudePositionEstimatorEKF::pollData()
float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
/* guard against too large deltaT's */
if (!std::isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f;
}
@@ -1167,9 +1168,9 @@ void AttitudePositionEstimatorEKF::pollData()
int last_gyro_main = _gyro_main;
if (std::isfinite(_sensor_combined.gyro_rad_s[0]) &&
std::isfinite(_sensor_combined.gyro_rad_s[1]) &&
std::isfinite(_sensor_combined.gyro_rad_s[2]) &&
if (PX4_ISFINITE(_sensor_combined.gyro_rad_s[0]) &&
PX4_ISFINITE(_sensor_combined.gyro_rad_s[1]) &&
PX4_ISFINITE(_sensor_combined.gyro_rad_s[2]) &&
(_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
@@ -1178,9 +1179,9 @@ void AttitudePositionEstimatorEKF::pollData()
_gyro_main = 0;
_gyro_valid = true;
} else if (std::isfinite(_sensor_combined.gyro1_rad_s[0]) &&
std::isfinite(_sensor_combined.gyro1_rad_s[1]) &&
std::isfinite(_sensor_combined.gyro1_rad_s[2])) {
} else if (PX4_ISFINITE(_sensor_combined.gyro1_rad_s[0]) &&
PX4_ISFINITE(_sensor_combined.gyro1_rad_s[1]) &&
PX4_ISFINITE(_sensor_combined.gyro1_rad_s[2])) {
_ekf->angRate.x = _sensor_combined.gyro1_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro1_rad_s[1];
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+14 -13
View File
@@ -37,6 +37,7 @@
* @author Lorenz Meier <lorenz@px4.io>
*/
#include <px4_defines.h>
#include "estimator_22states.h"
#include <string.h>
#include <stdio.h>
@@ -2392,9 +2393,9 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
{
for (size_t i=0; i < EKF_STATE_ESTIMATES; i++) {
if (std::isfinite(storedStates[i][bestStoreIndex])) {
if (PX4_ISFINITE(storedStates[i][bestStoreIndex])) {
statesForFusion[i] = storedStates[i][bestStoreIndex];
} else if (std::isfinite(states[i])) {
} else if (PX4_ISFINITE(states[i])) {
statesForFusion[i] = states[i];
} else {
// There is not much we can do here, except reporting the error we just
@@ -2406,7 +2407,7 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
else // otherwise output current state
{
for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
if (std::isfinite(states[i])) {
if (PX4_ISFINITE(states[i])) {
statesForFusion[i] = states[i];
} else {
ret++;
@@ -2630,7 +2631,7 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val)
ret = val;
}
if (!std::isfinite(val)) {
if (!PX4_ISFINITE(val)) {
ekf_debug("constrain: non-finite!");
}
@@ -2710,7 +2711,7 @@ void AttPosEKF::ConstrainStates()
// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
// Constrain dtIMUfilt
if (!isfinite(dtIMUfilt) || (fabsf(dtIMU - dtIMUfilt) > 0.01f)) {
if (!PX4_ISFINITE(dtIMUfilt) || (fabsf(dtIMU - dtIMUfilt) > 0.01f)) {
dtIMUfilt = dtIMU;
}
@@ -2922,21 +2923,21 @@ bool AttPosEKF::StatesNaN() {
bool err = false;
// check all integrators
if (!std::isfinite(summedDelAng.x) || !std::isfinite(summedDelAng.y) || !std::isfinite(summedDelAng.z)) {
if (!PX4_ISFINITE(summedDelAng.x) || !PX4_ISFINITE(summedDelAng.y) || !PX4_ISFINITE(summedDelAng.z)) {
current_ekf_state.angNaN = true;
ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z);
err = true;
goto out;
} // delta angles
if (!std::isfinite(correctedDelAng.x) || !std::isfinite(correctedDelAng.y) || !std::isfinite(correctedDelAng.z)) {
if (!PX4_ISFINITE(correctedDelAng.x) || !PX4_ISFINITE(correctedDelAng.y) || !PX4_ISFINITE(correctedDelAng.z)) {
current_ekf_state.angNaN = true;
ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z);
err = true;
goto out;
} // delta angles
if (!std::isfinite(summedDelVel.x) || !std::isfinite(summedDelVel.y) || !std::isfinite(summedDelVel.z)) {
if (!PX4_ISFINITE(summedDelVel.x) || !PX4_ISFINITE(summedDelVel.y) || !PX4_ISFINITE(summedDelVel.z)) {
current_ekf_state.summedDelVelNaN = true;
ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z);
err = true;
@@ -2946,7 +2947,7 @@ bool AttPosEKF::StatesNaN() {
// check all states and covariance matrices
for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) {
if (!std::isfinite(KH[i][j])) {
if (!PX4_ISFINITE(KH[i][j])) {
current_ekf_state.KHNaN = true;
err = true;
@@ -2954,7 +2955,7 @@ bool AttPosEKF::StatesNaN() {
goto out;
} // intermediate result used for covariance updates
if (!std::isfinite(KHP[i][j])) {
if (!PX4_ISFINITE(KHP[i][j])) {
current_ekf_state.KHPNaN = true;
err = true;
@@ -2962,7 +2963,7 @@ bool AttPosEKF::StatesNaN() {
goto out;
} // intermediate result used for covariance updates
if (!std::isfinite(P[i][j])) {
if (!PX4_ISFINITE(P[i][j])) {
current_ekf_state.covarianceNaN = true;
err = true;
@@ -2970,7 +2971,7 @@ bool AttPosEKF::StatesNaN() {
} // covariance matrix
}
if (!std::isfinite(Kfusion[i])) {
if (!PX4_ISFINITE(Kfusion[i])) {
current_ekf_state.kalmanGainsNaN = true;
ekf_debug("Kfusion NaN");
@@ -2978,7 +2979,7 @@ bool AttPosEKF::StatesNaN() {
goto out;
} // Kalman gains
if (!std::isfinite(states[i])) {
if (!PX4_ISFINITE(states[i])) {
current_ekf_state.statesNaN = true;
ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]);