dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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 <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois 2015-04-01 15:03:21 -07:00
parent 07b16ffa01
commit 8c8f57b5b4
9 changed files with 39 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/drivers/device/module.mk
View File

@ -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

0
src/drivers/device/vcdev.cpp → src/drivers/device/vdev.cpp
View File

0
src/drivers/device/vdevice.h → src/drivers/device/vdev.h
View File

0
src/drivers/device/vcdev_posix.cpp → src/drivers/device/vdev_posix.cpp
View File

17
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -42,6 +42,7 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
@ -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<unsigned long long>(raw.timestamp - last_data));
>>>>>>> att EKF: Enable execution on Linux
warnx("sensor data missed! (%llu)\n", static_cast<unsigned long long>(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);

15
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
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];

27
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
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]);

11
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -54,6 +54,7 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <stdio.h>
#include <stdlib.h>
@ -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;

4
src/platforms/px4_defines.h
View File

@ -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 */