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>

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

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

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

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

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

@@ -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]);

src/modules/mc_att_control/mc_att_control_main.cpp

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

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