QuRT and POSIX changes

Partial commit of the changes for QuRT and related changes for POSIX

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/modules/sensors/sensors.cpp

@@ -49,6 +49,7 @@
 #include <px4_config.h>
 #include <px4_tasks.h>
 #include <px4_posix.h>
+#include <px4_time.h>
 
 #include <fcntl.h>
 #include <poll.h>
@@ -62,6 +63,7 @@
 #include <mathlib/mathlib.h>
 
 #include <px4_adc.h>
+//#include <nuttx/analog/adc.h>
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_accel.h>
@@ -113,7 +115,7 @@
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 #define ADC_BATTERY_VOLTAGE_CHANNEL	10
-#define ADC_BATTERY_CURRENT_CHANNEL	-1
+#define ADC_BATTERY_CURRENT_CHANNEL	((uint8_t)(-1))
 #define ADC_AIRSPEED_VOLTAGE_CHANNEL	11
 #endif
 
@@ -126,8 +128,8 @@
 
 #ifdef CONFIG_ARCH_BOARD_AEROCORE
 #define ADC_BATTERY_VOLTAGE_CHANNEL	10
-#define ADC_BATTERY_CURRENT_CHANNEL	-1
-#define ADC_AIRSPEED_VOLTAGE_CHANNEL	-1
+#define ADC_BATTERY_CURRENT_CHANNEL	((uint8_t)(-1))
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL	((uint8_t)(-1))
 #endif
 
 #define BATT_V_LOWPASS			0.001f
@@ -147,7 +149,7 @@
 #endif
 static const int ERROR = -1;
 
-#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING SENSOR CAL"
+#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"
 
 /**
  * Sensor app start / stop handling function
@@ -225,7 +227,7 @@ private:
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_baro_sub;			/**< raw baro data subscription */
 	int		_baro1_sub;			/**< raw baro data subscription */
-	int		_airspeed_sub;			/**< airspeed subscription */
+	//int		_airspeed_sub;			/**< airspeed subscription */
 	int		_diff_pres_sub;			/**< raw differential pressure subscription */
 	int		_vcontrol_mode_sub;			/**< vehicle control mode subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
@@ -671,11 +673,7 @@ Sensors::parameters_update()
 		tmpRevFactor = tmpScaleFactor * _parameters.rev[i];
 
 		/* handle blowup in the scaling factor calculation */
-#ifdef __PX4_NUTTX
-		if (!isfinite(tmpScaleFactor) ||
-#else
-		if (!std::isfinite(tmpScaleFactor) ||
-#endif
+		if (!PX4_ISFINITE(tmpScaleFactor) ||
 		    (tmpRevFactor < 0.000001f) ||
 		    (tmpRevFactor > 0.2f)) {
 			warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i]));
@@ -855,7 +853,7 @@ Sensors::parameters_update()
 	barofd = px4_open(BARO0_DEVICE_PATH, 0);
 
 	if (barofd < 0) {
-		warnx("ERROR: no barometer found on %s", BARO0_DEVICE_PATH);
+		warnx("ERROR: no barometer foundon %s", BARO0_DEVICE_PATH);
 		return ERROR;
 
 	} else {
@@ -1021,6 +1019,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 		raw.accelerometer_timestamp = accel_report.timestamp;
 		raw.accelerometer_errcount = accel_report.error_count;
+		raw.accelerometer_temp = accel_report.temperature;
 	}
 
 	orb_check(_accel1_sub, &accel_updated);
@@ -1043,6 +1042,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 		raw.accelerometer1_timestamp = accel_report.timestamp;
 		raw.accelerometer1_errcount = accel_report.error_count;
+		raw.accelerometer1_temp = accel_report.temperature;
 	}
 
 	orb_check(_accel2_sub, &accel_updated);
@@ -1065,6 +1065,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 		raw.accelerometer2_timestamp = accel_report.timestamp;
 		raw.accelerometer2_errcount = accel_report.error_count;
+		raw.accelerometer2_temp = accel_report.temperature;
 	}
 }
 
@@ -1092,6 +1093,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 		raw.timestamp = gyro_report.timestamp;
 		raw.gyro_errcount = gyro_report.error_count;
+		raw.gyro_temp = gyro_report.temperature;
 	}
 
 	orb_check(_gyro1_sub, &gyro_updated);
@@ -1114,6 +1116,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 		raw.gyro1_timestamp = gyro_report.timestamp;
 		raw.gyro1_errcount = gyro_report.error_count;
+		raw.gyro1_temp = gyro_report.temperature;
 	}
 
 	orb_check(_gyro2_sub, &gyro_updated);
@@ -1136,6 +1139,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 		raw.gyro2_timestamp = gyro_report.timestamp;
 		raw.gyro2_errcount = gyro_report.error_count;
+		raw.gyro2_temp = gyro_report.temperature;
 	}
 }
 
@@ -1164,6 +1168,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		raw.magnetometer_timestamp = mag_report.timestamp;
 		raw.magnetometer_errcount = mag_report.error_count;
+		raw.magnetometer_temp = mag_report.temperature;
 	}
 
 	orb_check(_mag1_sub, &mag_updated);
@@ -1187,6 +1192,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		raw.magnetometer1_timestamp = mag_report.timestamp;
 		raw.magnetometer1_errcount = mag_report.error_count;
+		raw.magnetometer1_temp = mag_report.temperature;
 	}
 
 	orb_check(_mag2_sub, &mag_updated);
@@ -1210,6 +1216,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		raw.magnetometer2_timestamp = mag_report.timestamp;
 		raw.magnetometer2_errcount = mag_report.error_count;
+		raw.magnetometer2_temp = mag_report.temperature;
 	}
 }
 
@@ -1379,14 +1386,13 @@ Sensors::parameter_update_poll(bool forced)
 					failed = failed || (OK != param_get(param_find(str), &gscale.z_scale));
 
 					if (failed) {
-						warnx("%s: gyro #%u", CAL_FAILED_APPLY_CAL_MSG, gyro_count);
+						warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i);
 					} else {
 						/* apply new scaling and offsets */
 						res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale);
 						if (res) {
-							warnx(CAL_FAILED_APPLY_CAL_MSG);
+							warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i);
 						} else {
-							gyro_count++;
 							config_ok = true;
 						}
 					}
@@ -1394,11 +1400,11 @@ Sensors::parameter_update_poll(bool forced)
 				}
 			}
 
-			px4_close(fd);
-
-			if (!config_ok) {
-				warnx("NO CONFIG FOR GYRO #%u", s);
+			if (config_ok) {
+				gyro_count++;
 			}
+
+			px4_close(fd);
 		}
 
 		/* run through all accel sensors */
@@ -1446,14 +1452,13 @@ Sensors::parameter_update_poll(bool forced)
 					failed = failed || (OK != param_get(param_find(str), &gscale.z_scale));
 
 					if (failed) {
-						warnx("%s: acc #%u", CAL_FAILED_APPLY_CAL_MSG, accel_count);
+						warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i);
 					} else {
 						/* apply new scaling and offsets */
 						res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale);
 						if (res) {
-							warnx(CAL_FAILED_APPLY_CAL_MSG);
+							warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i);
 						} else {
-							accel_count++;
 							config_ok = true;
 						}
 					}
@@ -1461,11 +1466,11 @@ Sensors::parameter_update_poll(bool forced)
 				}
 			}
 
-			px4_close(fd);
-
-			if (!config_ok) {
-				warnx("NO CONFIG FOR ACCEL #%u", s);
+			if (config_ok) {
+				accel_count++;
 			}
+
+			px4_close(fd);
 		}
 
 		/* run through all mag sensors */
@@ -1477,9 +1482,16 @@ Sensors::parameter_update_poll(bool forced)
 			int fd = px4_open(str, 0);
 
 			if (fd < 0) {
+				/* the driver is not running, abort */
 				continue;
 			}
 
+			/* set a valid default rotation (same as board).
+			 * if the mag is configured, this might be replaced
+			 * in the section below.
+			 */
+			_mag_rotation[s] = _board_rotation;
+
 			bool config_ok = false;
 
 			/* run through all stored calibrations */ 
@@ -1563,14 +1575,13 @@ Sensors::parameter_update_poll(bool forced)
 					}
 
 					if (failed) {
-						warnx("%s: mag #%u", CAL_FAILED_APPLY_CAL_MSG, mag_count);
+						warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i);
 					} else {
 						/* apply new scaling and offsets */
 						res = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale);
 						if (res) {
-							warnx(CAL_FAILED_APPLY_CAL_MSG);
+							warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i);
 						} else {
-							mag_count++;
 							config_ok = true;
 						}
 					}
@@ -1578,11 +1589,11 @@ Sensors::parameter_update_poll(bool forced)
 				}
 			}
 
-			px4_close(fd);
-
-			if (!config_ok) {
-				warnx("NO CONFIG FOR MAG #%u", s);
+			if (config_ok) {
+				mag_count++;
 			}
+
+			px4_close(fd);
 		}
 
 		int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
@@ -1602,7 +1613,7 @@ Sensors::parameter_update_poll(bool forced)
 			px4_close(fd);
 		}
 
-		warnx("config: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count);
+		warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count);
 	}
 }
 
@@ -1947,11 +1958,7 @@ Sensors::rc_poll()
 			_rc.channels[i] *= _parameters.rev[i];
 
 			/* handle any parameter-induced blowups */
-#ifdef __PX4_NUTTX
-			if (!isfinite(_rc.channels[i])) {
-#else
-			if (!std::isfinite(_rc.channels[i])) {
-#endif
+			if (!PX4_ISFINITE(_rc.channels[i])) {
 				_rc.channels[i] = 0.0f;
 			}
 		}
@@ -2152,12 +2159,8 @@ Sensors::task_main()
 	/* advertise the sensor_combined topic and make the initial publication */
 	_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
 
-	if (_sensor_pub < 0) {
-		warnx("Sensors::task_main ERROR - uORB not started");
-	}
-
 	/* wakeup source(s) */
-	struct pollfd fds[1];
+	px4_pollfd_struct_t fds[1];
 
 	/* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */
 	fds[0].fd = _gyro_sub;
@@ -2168,7 +2171,7 @@ Sensors::task_main()
 	while (!_task_should_exit) {
 
 		/* wait for up to 50ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
 
 		/* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */
 

src/modules/simulator/simulator.cpp

@@ -37,13 +37,15 @@
  */
 
 #include <px4_tasks.h>
-#include <err.h>
+#include <systemlib/err.h>
 #include <errno.h>
 #include <stdio.h>
 #include <string.h>
 #include <sys/types.h> 
+#ifndef __PX4_QURT
 #include <sys/socket.h>
 #include <netinet/in.h>
+#endif
 #include "simulator.h"
 
 using namespace simulator;
@@ -76,8 +78,11 @@ int Simulator::start(int argc, char *argv[])
 {
 	int ret = 0;
 	_instance = new Simulator();
-	if (_instance) 
+	if (_instance) {
+#ifndef __PX4_QURT
 		_instance->updateSamples();
+#endif
+	}
 	else {
 		printf("Simulator creation failed\n");
 		ret = 1;
@@ -86,6 +91,7 @@ int Simulator::start(int argc, char *argv[])
 }
 
 
+#ifndef __PX4_QURT
 void Simulator::updateSamples()
 {
 	// get samples from external provider
@@ -133,6 +139,7 @@ void Simulator::updateSamples()
                 }
         }
 }
+#endif
 
 static void usage()
 {

src/modules/simulator/simulator.h

@@ -148,7 +148,9 @@ private:
 	Simulator() : _accel(1), _mpu(1), _baro(1) {}
 	~Simulator() { _instance=NULL; }
 
+#ifndef __PX4_QURT
 	void updateSamples();
+#endif
 
 	static Simulator *_instance;
 

src/modules/uORB/MuORB.cpp

@@ -37,6 +37,7 @@
  */
 
 #include <px4_config.h>
+#include <px4_time.h>
 
 #include <drivers/device/device.h>