fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-31 05:31:36 +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/drivers/device/module.mk
+2 -2
View File
@@ -44,8 +44,8 @@ SRCS = \
spi.cpp spi.cpp
endif endif
ifeq ($(PX4_TARGET_OS),linux) ifeq ($(PX4_TARGET_OS),linux)
SRCS = vcdev.cpp \ SRCS = vdev.cpp \
device.cpp \ device.cpp \
vcdev_posix.cpp \ vdev_posix.cpp \
i2c_linux.cpp i2c_linux.cpp
endif endif
src/drivers/device/vcdev.cpp → src/drivers/device/vdev.cpp
View File
src/drivers/device/vdevice.h → src/drivers/device/vdev.h
View File
src/drivers/device/vcdev_posix.cpp → src/drivers/device/vdev_posix.cpp
View File
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+5 -12
View File
@@ -42,6 +42,7 @@
*/ */
#include <px4_config.h> #include <px4_config.h>
#include <px4_defines.h>
#include <unistd.h> #include <unistd.h>
#include <stdlib.h> #include <stdlib.h>
#include <stdio.h> #include <stdio.h>
@@ -536,7 +537,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
debugOutput); debugOutput);
/* swap values for next iteration, check for fatal inputs */ /* 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(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_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) { if (last_data > 0 && raw.timestamp - last_data > 30000) {
<<<<<<< HEAD warnx("sensor data missed! (%llu)\n", static_cast<unsigned long long>(raw.timestamp - last_data));
warnx("sensor data missed! (%llu)\n", raw.timestamp - last_data);
=======
warnx("data missed! (%llu)\n", static_cast<unsigned long long>(raw.timestamp - last_data));
>>>>>>> att EKF: Enable execution on Linux
} }
last_data = raw.timestamp; 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)); memcpy(&att.q[0],&q.data[0],sizeof(att.q));
att.R_valid = true; att.R_valid = true;
<<<<<<< HEAD if (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1])
if (isfinite(att.q[0]) && isfinite(att.q[1]) && PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) {
&& 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
// Broadcast // Broadcast
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
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 "estimator_22states.h"
#include <px4_config.h> #include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h> #include <px4_tasks.h>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
@@ -1155,7 +1156,7 @@ void AttitudePositionEstimatorEKF::pollData()
float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
/* guard against too large deltaT's */ /* 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; deltaT = 0.01f;
} }
@@ -1167,9 +1168,9 @@ void AttitudePositionEstimatorEKF::pollData()
int last_gyro_main = _gyro_main; int last_gyro_main = _gyro_main;
if (std::isfinite(_sensor_combined.gyro_rad_s[0]) && if (PX4_ISFINITE(_sensor_combined.gyro_rad_s[0]) &&
std::isfinite(_sensor_combined.gyro_rad_s[1]) && PX4_ISFINITE(_sensor_combined.gyro_rad_s[1]) &&
std::isfinite(_sensor_combined.gyro_rad_s[2]) && PX4_ISFINITE(_sensor_combined.gyro_rad_s[2]) &&
(_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) { (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
@@ -1178,9 +1179,9 @@ void AttitudePositionEstimatorEKF::pollData()
_gyro_main = 0; _gyro_main = 0;
_gyro_valid = true; _gyro_valid = true;
} else if (std::isfinite(_sensor_combined.gyro1_rad_s[0]) && } else if (PX4_ISFINITE(_sensor_combined.gyro1_rad_s[0]) &&
std::isfinite(_sensor_combined.gyro1_rad_s[1]) && PX4_ISFINITE(_sensor_combined.gyro1_rad_s[1]) &&
std::isfinite(_sensor_combined.gyro1_rad_s[2])) { PX4_ISFINITE(_sensor_combined.gyro1_rad_s[2])) {
_ekf->angRate.x = _sensor_combined.gyro1_rad_s[0]; _ekf->angRate.x = _sensor_combined.gyro1_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro1_rad_s[1]; _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> * @author Lorenz Meier <lorenz@px4.io>
*/ */
#include <px4_defines.h>
#include "estimator_22states.h" #include "estimator_22states.h"
#include <string.h> #include <string.h>
#include <stdio.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 if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
{ {
for (size_t i=0; i < EKF_STATE_ESTIMATES; i++) { 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]; statesForFusion[i] = storedStates[i][bestStoreIndex];
} else if (std::isfinite(states[i])) { } else if (PX4_ISFINITE(states[i])) {
statesForFusion[i] = states[i]; statesForFusion[i] = states[i];
} else { } else {
// There is not much we can do here, except reporting the error we just // 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 else // otherwise output current state
{ {
for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
if (std::isfinite(states[i])) { if (PX4_ISFINITE(states[i])) {
statesForFusion[i] = states[i]; statesForFusion[i] = states[i];
} else { } else {
ret++; ret++;
@@ -2630,7 +2631,7 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val)
ret = val; ret = val;
} }
if (!std::isfinite(val)) { if (!PX4_ISFINITE(val)) {
ekf_debug("constrain: non-finite!"); ekf_debug("constrain: non-finite!");
} }
@@ -2710,7 +2711,7 @@ void AttPosEKF::ConstrainStates()
// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
// Constrain dtIMUfilt // Constrain dtIMUfilt
if (!isfinite(dtIMUfilt) || (fabsf(dtIMU - dtIMUfilt) > 0.01f)) { if (!PX4_ISFINITE(dtIMUfilt) || (fabsf(dtIMU - dtIMUfilt) > 0.01f)) {
dtIMUfilt = dtIMU; dtIMUfilt = dtIMU;
} }
@@ -2922,21 +2923,21 @@ bool AttPosEKF::StatesNaN() {
bool err = false; bool err = false;
// check all integrators // 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; current_ekf_state.angNaN = true;
ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z); ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z);
err = true; err = true;
goto out; goto out;
} // delta angles } // 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; current_ekf_state.angNaN = true;
ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z);
err = true; err = true;
goto out; goto out;
} // delta angles } // 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; current_ekf_state.summedDelVelNaN = true;
ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z); ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z);
err = true; err = true;
@@ -2946,7 +2947,7 @@ bool AttPosEKF::StatesNaN() {
// check all states and covariance matrices // check all states and covariance matrices
for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) { 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; current_ekf_state.KHNaN = true;
err = true; err = true;
@@ -2954,7 +2955,7 @@ bool AttPosEKF::StatesNaN() {
goto out; goto out;
} // intermediate result used for covariance updates } // intermediate result used for covariance updates
if (!std::isfinite(KHP[i][j])) { if (!PX4_ISFINITE(KHP[i][j])) {
current_ekf_state.KHPNaN = true; current_ekf_state.KHPNaN = true;
err = true; err = true;
@@ -2962,7 +2963,7 @@ bool AttPosEKF::StatesNaN() {
goto out; goto out;
} // intermediate result used for covariance updates } // intermediate result used for covariance updates
if (!std::isfinite(P[i][j])) { if (!PX4_ISFINITE(P[i][j])) {
current_ekf_state.covarianceNaN = true; current_ekf_state.covarianceNaN = true;
err = true; err = true;
@@ -2970,7 +2971,7 @@ bool AttPosEKF::StatesNaN() {
} // covariance matrix } // covariance matrix
} }
if (!std::isfinite(Kfusion[i])) { if (!PX4_ISFINITE(Kfusion[i])) {
current_ekf_state.kalmanGainsNaN = true; current_ekf_state.kalmanGainsNaN = true;
ekf_debug("Kfusion NaN"); ekf_debug("Kfusion NaN");
@@ -2978,7 +2979,7 @@ bool AttPosEKF::StatesNaN() {
goto out; goto out;
} // Kalman gains } // Kalman gains
if (!std::isfinite(states[i])) { if (!PX4_ISFINITE(states[i])) {
current_ekf_state.statesNaN = true; current_ekf_state.statesNaN = true;
ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]); ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]);
src/modules/mc_att_control/mc_att_control_main.cpp
+6 -5
View File
@@ -54,6 +54,7 @@
*/ */
#include <px4_config.h> #include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h> #include <px4_tasks.h>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
@@ -722,7 +723,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
if (fabsf(_att_control(i)) < _thrust_sp) { if (fabsf(_att_control(i)) < _thrust_sp) {
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; 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) { _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
_rates_int(i) = rate_i; _rates_int(i) = rate_i;
} }
@@ -859,10 +860,10 @@ MulticopterAttitudeControl::task_main()
control_attitude_rates(dt); control_attitude_rates(dt);
/* publish actuator controls */ /* publish actuator controls */
_actuators.control[0] = (std::isfinite(_att_control(0))) ? _att_control(0) : 0.0f; _actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
_actuators.control[1] = (std::isfinite(_att_control(1))) ? _att_control(1) : 0.0f; _actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
_actuators.control[2] = (std::isfinite(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (std::isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp = hrt_absolute_time(); _actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _v_att.timestamp; _actuators.timestamp_sample = _v_att.timestamp;
src/platforms/px4_defines.h
+4
View File
@@ -104,6 +104,8 @@ typedef param_t px4_param_t;
#define px4_statfs_buf_f_bavail_t int #define px4_statfs_buf_f_bavail_t int
#define PX4_ISFINITE(x) isfinite(x)
/* Linux Specific defines */ /* Linux Specific defines */
#elif defined(__PX4_LINUX) #elif defined(__PX4_LINUX)
@@ -165,6 +167,8 @@ __END_DECLS
#define M_DEG_TO_RAD 0.01745329251994 #define M_DEG_TO_RAD 0.01745329251994
#define M_RAD_TO_DEG 57.2957795130823 #define M_RAD_TO_DEG 57.2957795130823
#define PX4_ISFINITE(x) std::isfinite(x)
#endif #endif
/* Defines for all platforms */ /* Defines for all platforms */