From 8c8f57b5b4e45e8b84ee4341607dd8f5c07bde35 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Apr 2015 15:03:21 -0700 Subject: [PATCH] Fixed std::isfinite vs isfinite differences 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 --- src/drivers/device/module.mk | 4 +-- src/drivers/device/{vcdev.cpp => vdev.cpp} | 0 src/drivers/device/{vdevice.h => vdev.h} | 0 .../{vcdev_posix.cpp => vdev_posix.cpp} | 0 .../attitude_estimator_ekf_main.cpp | 17 ++++-------- .../ekf_att_pos_estimator_main.cpp | 15 ++++++----- .../estimator_22states.cpp | 27 ++++++++++--------- .../mc_att_control/mc_att_control_main.cpp | 11 ++++---- src/platforms/px4_defines.h | 4 +++ 9 files changed, 39 insertions(+), 39 deletions(-) rename src/drivers/device/{vcdev.cpp => vdev.cpp} (100%) rename src/drivers/device/{vdevice.h => vdev.h} (100%) rename src/drivers/device/{vcdev_posix.cpp => vdev_posix.cpp} (100%) diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index a3660d70a5..c927426ae7 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -44,8 +44,8 @@ SRCS = \ spi.cpp endif ifeq ($(PX4_TARGET_OS),linux) -SRCS = vcdev.cpp \ +SRCS = vdev.cpp \ device.cpp \ - vcdev_posix.cpp \ + vdev_posix.cpp \ i2c_linux.cpp endif diff --git a/src/drivers/device/vcdev.cpp b/src/drivers/device/vdev.cpp similarity index 100% rename from src/drivers/device/vcdev.cpp rename to src/drivers/device/vdev.cpp diff --git a/src/drivers/device/vdevice.h b/src/drivers/device/vdev.h similarity index 100% rename from src/drivers/device/vdevice.h rename to src/drivers/device/vdev.h diff --git a/src/drivers/device/vcdev_posix.cpp b/src/drivers/device/vdev_posix.cpp similarity index 100% rename from src/drivers/device/vcdev_posix.cpp rename to src/drivers/device/vdev_posix.cpp diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 0a55756163..affe6a68a4 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -42,6 +42,7 @@ */ #include +#include #include #include #include @@ -536,7 +537,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) debugOutput); /* swap values for next iteration, check for fatal inputs */ - if (std::isfinite(euler[0]) && std::isfinite(euler[1]) && std::isfinite(euler[2])) { + if (PX4_ISFINITE(euler[0]) && PX4_ISFINITE(euler[1]) && PX4_ISFINITE(euler[2])) { memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); @@ -546,11 +547,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) } if (last_data > 0 && raw.timestamp - last_data > 30000) { -<<<<<<< HEAD - warnx("sensor data missed! (%llu)\n", raw.timestamp - last_data); -======= - warnx("data missed! (%llu)\n", static_cast(raw.timestamp - last_data)); ->>>>>>> att EKF: Enable execution on Linux + warnx("sensor data missed! (%llu)\n", static_cast(raw.timestamp - last_data)); } last_data = raw.timestamp; @@ -587,12 +584,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) memcpy(&att.q[0],&q.data[0],sizeof(att.q)); att.R_valid = true; -<<<<<<< HEAD - if (isfinite(att.q[0]) && isfinite(att.q[1]) - && isfinite(att.q[2]) && isfinite(att.q[3])) { -======= - if (std::isfinite(att.roll) && std::isfinite(att.pitch) && std::isfinite(att.yaw)) { ->>>>>>> att EKF: Enable execution on Linux + if (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1]) + && PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) { // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index ecd67ecf7a..41c62875bb 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -44,6 +44,7 @@ #include "estimator_22states.h" #include +#include #include #include #include @@ -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]; diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 6235aa5b8b..d3200047ed 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -37,6 +37,7 @@ * @author Lorenz Meier */ +#include #include "estimator_22states.h" #include #include @@ -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]); diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 93d2116b46..37f7eae671 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -54,6 +54,7 @@ */ #include +#include #include #include #include @@ -722,7 +723,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; - if (std::isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { _rates_int(i) = rate_i; } @@ -859,10 +860,10 @@ MulticopterAttitudeControl::task_main() control_attitude_rates(dt); /* publish actuator controls */ - _actuators.control[0] = (std::isfinite(_att_control(0))) ? _att_control(0) : 0.0f; - _actuators.control[1] = (std::isfinite(_att_control(1))) ? _att_control(1) : 0.0f; - _actuators.control[2] = (std::isfinite(_att_control(2))) ? _att_control(2) : 0.0f; - _actuators.control[3] = (std::isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _v_att.timestamp; diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 2777367568..1cca8e55de 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -104,6 +104,8 @@ typedef param_t px4_param_t; #define px4_statfs_buf_f_bavail_t int +#define PX4_ISFINITE(x) isfinite(x) + /* Linux Specific defines */ #elif defined(__PX4_LINUX) @@ -165,6 +167,8 @@ __END_DECLS #define M_DEG_TO_RAD 0.01745329251994 #define M_RAD_TO_DEG 57.2957795130823 +#define PX4_ISFINITE(x) std::isfinite(x) + #endif /* Defines for all platforms */