Improved param load / store text feedback, ported sensors app to new driver model, ready for merge and test

4 files changed, 368 insertions, 561 deletions

diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c
index dbf5b75d8..8ccab3fee 100644
--- a/apps/mavlink/mavlink_parameters.c
+++ b/apps/mavlink/mavlink_parameters.c
@@ -47,6 +47,8 @@
 #include <stdbool.h>
 #include <string.h>
 #include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <sys/stat.h>
 
 extern mavlink_system_t mavlink_system;
 
@@ -65,6 +67,20 @@ static unsigned int mavlink_param_queue_index = 0;
  */
 void mavlink_pm_callback(void *arg, param_t param);
 
+/**
+ * Save parameters to EEPROM.
+ *
+ * Stores the parameters to /eeprom/parameters
+ */
+static int mavlink_pm_save_eeprom(void);
+
+/**
+ * Load parameters from EEPROM.
+ *
+ * Loads the parameters from /eeprom/parameters
+ */
+static int mavlink_pm_load_eeprom(void);
+
 void mavlink_pm_callback(void *arg, param_t param)
 {
 	mavlink_pm_send_param(param);
@@ -126,6 +142,8 @@ int mavlink_pm_send_param(param_t param)
 		mavlink_type = MAVLINK_TYPE_INT32_T;
 	} else if (type == PARAM_TYPE_FLOAT) {
 		mavlink_type = MAVLINK_TYPE_FLOAT;
+	} else {
+		mavlink_type = MAVLINK_TYPE_FLOAT;
 	}
 
 	/*
@@ -150,27 +168,28 @@ int mavlink_pm_send_param(param_t param)
 }
 
 static const char *mavlink_parameter_file = "/eeprom/parameters";
+
 /**
  * @return 0 on success, -1 if device open failed, -2 if writing parameters failed
  */
-static int
-mavlink_pm_save_eeprom()
+static int mavlink_pm_save_eeprom()
 {
+	/* delete the file in case it exists */
 	unlink(mavlink_parameter_file);
 
+	/* create the file */
 	int fd = open(mavlink_parameter_file, O_WRONLY | O_CREAT | O_EXCL);
 
-	if (fd < 0) {
-		warn("opening '%s' failed", mavlink_parameter_file);
+	if (fd < 0)
+		warn("opening '%s' for writing failed", mavlink_parameter_file);
 		return -1;
-	}
 
 	int result = param_export(fd, false);
 	close(fd);
 
 	if (result < 0) {
 		unlink(mavlink_parameter_file);
-		warn("error exporting to '%s'", mavlink_parameter_file);
+		warn("error exporting parameters to '%s'", mavlink_parameter_file);
 		return -2;
 	}
 
@@ -178,16 +197,24 @@ mavlink_pm_save_eeprom()
 }
 
 /**
- * @return 0 on success, -1 if device open failed, -2 if writing parameters failed
+ * @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed
  */
 static int
 mavlink_pm_load_eeprom()
 {
+	/* check if eeprom is mounted - if yes and an eeprom open fail is no error */
+	struct stat buffer;
+	int eeprom_stat = stat("/eeprom", &buffer);
+
 	int fd = open(mavlink_parameter_file, O_RDONLY);
 
 	if (fd < 0) {
-		warn("open '%s' failed", mavlink_parameter_file);
-		return -1;
+		if (eeprom_stat == OK) {
+			return 1;
+		} else {
+			warn("open '%s' for reading failed", mavlink_parameter_file);
+			return -1;
+		}
 	}
 
 	int result = param_import(fd);
@@ -266,7 +293,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
 			mavlink_command_long_t cmd_mavlink;
 			mavlink_msg_command_long_decode(msg, &cmd_mavlink);
 
-			uint8_t result;
+			uint8_t result = MAV_RESULT_UNSUPPORTED;
 
 			if (cmd_mavlink.target_system == mavlink_system.sysid &&
 				((cmd_mavlink.target_component == mavlink_system.compid) ||(cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
@@ -283,7 +310,9 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
 							//printf("[mavlink pm] Loaded EEPROM params in RAM\n");
 							mavlink_missionlib_send_gcs_string("[mavlink pm] OK loaded EEPROM params");
 							result = MAV_RESULT_ACCEPTED;
-
+						} else if (read_ret == 1) {
+							mavlink_missionlib_send_gcs_string("[mavlink pm] No stored parameters to load");
+							result = MAV_RESULT_ACCEPTED;
 						} else {
 							if (read_ret < -1) {
 								mavlink_missionlib_send_gcs_string("[mavlink pm] ERR loading params from EEPROM");
@@ -317,6 +346,9 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
 					}
 				}
 			}
+
+			/* send back command result */
+			// mavlink_message_t tx;
 		} break;
 	}
 }
diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c
index a994e618c..da90fa98c 100644
--- a/apps/sensors/sensors.c
+++ b/apps/sensors/sensors.c
@@ -41,9 +41,9 @@
 
 #include <nuttx/config.h>
 
-#include <pthread.h>
 #include <fcntl.h>
 #include <sys/prctl.h>
+#include <poll.h>
 #include <nuttx/analog/adc.h>
 #include <unistd.h>
 #include <stdlib.h>
@@ -66,6 +66,7 @@
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
+#include <systemlib/err.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
@@ -106,9 +107,6 @@
 
 #define PPM_MID (PPM_MIN+PPM_MAX)/2
 
-static pthread_cond_t sensors_read_ready;
-static pthread_mutex_t sensors_read_ready_mutex;
-
 static int sensors_timer_loop_counter = 0;
 
 /* File descriptors for all sensors */
@@ -422,6 +420,13 @@ static int sensors_init(void)
 
 	} else {
 		printf("[sensors]   MAG open ok\n");
+		/* set the queue depth to 1 */
+		if (OK != ioctl(fd_magnetometer, MAGIOCSQUEUEDEPTH, 1))
+			warn("failed to set queue depth for mag");
+
+		/* start the sensor polling at 150Hz */
+		if (OK != ioctl(fd_magnetometer, MAGIOCSPOLLRATE, 150))
+			warn("failed to set 150Hz poll rate for mag");
 	}
 
 	/* open barometer */
@@ -433,33 +438,48 @@ static int sensors_init(void)
 
 	} else {
 		printf("[sensors]   BARO open ok\n");
-	}
-
-	/* open gyro */
-	fd_gyro_l3gd20  = open("/dev/l3gd20", O_RDONLY);
-	int errno_gyro = (int)*get_errno_ptr();
+		/* set the queue depth to 1 */
+		if (OK != ioctl(fd_barometer, BAROIOCSQUEUEDEPTH, 1))
+			warn("failed to set queue depth for baro");
 
-	if (!(fd_gyro_l3gd20 < 0)) {
-		printf("[sensors]   L3GD20 open ok\n");
+		/* start the sensor polling at 100Hz */
+		if (OK != ioctl(fd_barometer, BAROIOCSPOLLRATE, 100))
+			warn("failed to set 100Hz poll rate for baro");
 	}
 
 	/* open gyro */
 	fd_gyro = open("/dev/gyro", O_RDONLY);
-	errno_gyro = (int)*get_errno_ptr();
+	int errno_gyro = (int)*get_errno_ptr();
 
 	if (!(fd_gyro < 0)) {
 		printf("[sensors]   GYRO open ok\n");
+		// /* set the queue depth to 1 */
+		// if (OK != ioctl(fd_gyro, GYROIOCSQUEUEDEPTH, 1))
+		// 	warn("failed to set queue depth for gyro");
+
+		// /* start the sensor polling at 500Hz */
+		// if (OK != ioctl(fd_gyro, GYROIOCSPOLLRATE, 500))
+		// 	warn("failed to set 500Hz poll rate for gyro");
 	}
 
-	/* open accelerometer, prefer the MPU-6000 */
+	printf("just before accel\n");
+
+	/* open accelerometer */
 	fd_accelerometer = open("/dev/accel", O_RDONLY);
 	int errno_accelerometer = (int)*get_errno_ptr();
 
 	if (!(fd_accelerometer < 0)) {
 		printf("[sensors]   ACCEL open ok\n");
+		// /* set the queue depth to 1 */
+		// if (OK != ioctl(fd_accelerometer, ACCELIOCSQUEUEDEPTH, 1))
+		// 	warn("failed to set queue depth for accel");
+
+		// /* start the sensor polling at 500Hz */
+		// if (OK != ioctl(fd_accelerometer, ACCELIOCSPOLLRATE, 500))
+		// 	warn("failed to set 500Hz poll rate for accel");
 	}
 
-	/* only attempt to use BMA180 if MPU-6000 is not available */
+	/* only attempt to use BMA180 if main accel is not available */
 	int errno_bma180 = 0;
 	if (fd_accelerometer < 0) {
 		fd_bma180 = open("/dev/bma180", O_RDONLY);
@@ -472,11 +492,36 @@ static int sensors_init(void)
 		fd_bma180 = -1;
 	}
 
+	/* only attempt to use L3GD20 is main gyro is not available */
+	int errno_gyro_l3gd20 = 0;
+	if (fd_gyro < 0) {
+		fd_gyro_l3gd20  = open("/dev/l3gd20", O_RDONLY);
+		int errno_gyro_l3gd20 = (int)*get_errno_ptr();
+
+		if (!(fd_gyro_l3gd20 < 0)) {
+			printf("[sensors]   GYRO (L3GD20) open ok\n");
+		}
+
+		if (ioctl(fd_gyro_l3gd20 , L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_30HZ) ||
+			ioctl(fd_gyro_l3gd20 , L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) {
+			fprintf(stderr, "[sensors]   L3GD20 configuration (ioctl) fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
+			fflush(stderr);
+			/* this sensor is critical, exit on failed init */
+			errno = ENOSYS;
+			return ERROR;
+
+		} else {
+			printf("[sensors]   L3GD20 configuration ok\n");
+		}
+	} else {
+		fd_gyro_l3gd20 = -1;
+	}
+
 	/* fail if no accelerometer is available */
 	if (fd_accelerometer < 0 && fd_bma180 < 0) {
 		/* print error message only if both failed, discard message else at all to not confuse users */
 		if (fd_accelerometer < 0) {
-			fprintf(stderr, "[sensors]   ACCEL: MPU-6000: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer));
+			fprintf(stderr, "[sensors]   ACCEL: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer));
 			fflush(stderr);
 			/* this sensor is redundant with BMA180 */
 		}
@@ -492,16 +537,16 @@ static int sensors_init(void)
 	}
 
 	/* fail if no gyro is available */
-	if (fd_accelerometer < 0 && fd_bma180 < 0) {
+	if (fd_gyro < 0 && fd_gyro_l3gd20 < 0) {
 		/* print error message only if both failed, discard message else at all to not confuse users */
-		if (fd_accelerometer < 0) {
-			fprintf(stderr, "[sensors]   GYRO: MPU-6000: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer));
+		if (fd_gyro < 0) {
+			fprintf(stderr, "[sensors]   GYRO: open fail (err #%d): %s\n", errno_gyro, strerror(errno_gyro));
 			fflush(stderr);
 			/* this sensor is redundant with BMA180 */
 		}
 		
-		if (fd_gyro < 0) {
-			fprintf(stderr, "[sensors]   L3GD20 open fail (err #%d): %s\n", errno_gyro, strerror(errno_gyro));
+		if (fd_gyro_l3gd20 < 0) {
+			fprintf(stderr, "[sensors]   L3GD20 open fail (err #%d): %s\n", errno_gyro_l3gd20, strerror(errno_gyro_l3gd20));
 			fflush(stderr);
 			/* this sensor is critical, exit on failed init */
 		}
@@ -524,68 +569,30 @@ static int sensors_init(void)
 		printf("[sensors]   ADC open ok\n");
 	}
 
-	/* configure gyro - if its not available and we got here the MPU-6000 is for sure available */
-	if (fd_gyro_l3gd20  > 0) {
-		if (ioctl(fd_gyro_l3gd20 , L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_30HZ) || ioctl(fd_gyro_l3gd20 , L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) {
-			fprintf(stderr, "[sensors]   L3GD20 configuration (ioctl) fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
-			fflush(stderr);
-			/* this sensor is critical, exit on failed init */
-			errno = ENOSYS;
-			return ERROR;
-
-		} else {
-			printf("[sensors]   L3GD20 configuration ok\n");
-		}
-	}
-
-	/* XXX Add IOCTL configuration of remaining sensors */
-
 	printf("[sensors] All sensors configured\n");
 	return OK;
 }
 
-/**
- * Callback function called by high resolution timer.
- *
- * This function signals a pthread condition and wakes up the
- * sensor main loop.
- */
-static void sensors_timer_loop(void *arg)
-{
-	/* Inform the read thread that it is now time to read */
-	sensors_timer_loop_counter++;
-	/* Do not use global data broadcast because of
-	 * use of printf() in call - would be fatal here
-	 */
-	pthread_cond_broadcast(&sensors_read_ready);
-}
-
 int sensors_thread_main(int argc, char *argv[])
 {
 	/* inform about start */
 	printf("[sensors] Initializing..\n");
 	fflush(stdout);
+
 	int ret = OK;
 
 	/* start sensor reading */
 	if (sensors_init() != OK) {
-		fprintf(stderr, "[sensors] ERROR: Failed to initialize all sensors\n");
+		fprintf(stderr, "[sensors] ERROR: Failed to initialize all sensors, exiting.\n");
 		/* Clean up */
 		close(fd_gyro);
 		close(fd_bma180);
+		close(fd_gyro_l3gd20);
 		close(fd_magnetometer);
 		close(fd_barometer);
 		close(fd_adc);
 
-		fprintf(stderr, "[sensors] REBOOTING SYSTEM\n\n");
-		fflush(stderr);
-		fflush(stdout);
-		usleep(100000);
-
-		/* Sensors are critical, immediately reboot system on failure */
-		reboot();
-		/* Not ever reaching here */
-
+		exit(1);
 	} else {
 		/* flush stdout from init routine */
 		fflush(stdout);
@@ -597,48 +604,33 @@ int sensors_thread_main(int argc, char *argv[])
 	parameters_init(&rch);
 	parameters_update(&rch, &rcp);
 
-	bool gyro_healthy = false;
-	bool acc_healthy = false;
-	bool magn_healthy = false;
-	bool baro_healthy = false;
-	bool adc_healthy = false;
+	// bool gyro_healthy = false;
+	// bool acc_healthy = false;
+	// bool magn_healthy = false;
+	// bool baro_healthy = false;
+	// bool adc_healthy = false;
 
 	bool hil_enabled = false;		/**< HIL is disabled by default	*/
 	bool publishing = false;		/**< the app is not publishing by default, only if HIL is disabled on first run */
 
-	int magcounter = 0;
-	int barocounter = 0;
-	int adccounter = 0;
-
-	unsigned int mag_fail_count = 0;
-	unsigned int mag_success_count = 0;
-
-	unsigned int baro_fail_count = 0;
-	unsigned int baro_success_count = 0;
+	// unsigned int mag_fail_count = 0;
+	// unsigned int mag_success_count = 0;
 
-	unsigned int gyro_fail_count = 0;
-	unsigned int gyro_success_count = 0;
+	// unsigned int baro_fail_count = 0;
+	// unsigned int baro_success_count = 0;
 
-	unsigned int acc_fail_count = 0;
-	unsigned int acc_success_count = 0;
+	// unsigned int gyro_fail_count = 0;
+	// unsigned int gyro_success_count = 0;
 
-	unsigned int adc_fail_count = 0;
-	unsigned int adc_success_count = 0;
+	// unsigned int acc_fail_count = 0;
+	// unsigned int acc_success_count = 0;
 
-	ssize_t	ret_gyro;
-	ssize_t	ret_accelerometer;
-	ssize_t	ret_magnetometer;
-	ssize_t	ret_barometer;
-	ssize_t	ret_adc;
-	int 	nsamples_adc;
+	// unsigned int adc_fail_count = 0;
+	// unsigned int adc_success_count = 0;
 
 	/* for PX4FMU 1.5 compatibility */
 	int16_t buf_accelerometer[3];
-
-	struct gyro_report buf_gyro;
-	struct accel_report buf_accel_report;
-	struct mag_report buf_magnetometer;
-	struct baro_report buf_barometer;
+	int16_t buf_gyro[3];
 
 	// bool mag_calibration_enabled = false;
 
@@ -666,49 +658,54 @@ int sensors_thread_main(int argc, char *argv[])
 		battery_voltage_conversion = 3.3f * 52.0f / 5.0f / 4095.0f;
 	}
 
-#ifdef CONFIG_HRT_PPM
-	int ppmcounter = 0;
-#endif
 	/* initialize to 100 to execute immediately */
 	int paramcounter = 100;
-	int excessive_readout_time_counter = 0;
 	int read_loop_counter = 0;
 
 	/* Empty sensor buffers, avoid junk values */
 	/* Read first two values of each sensor into void */
 	if (fd_bma180 > 0)(void)read(fd_bma180, buf_accelerometer, sizeof(buf_accelerometer));
-	if (fd_gyro > 0)(void)read(fd_gyro, &buf_gyro, sizeof(buf_gyro));
-	if (fd_accelerometer > 0)(void)read(fd_accelerometer, &buf_accel_report, sizeof(buf_accel_report));
-	(void)read(fd_magnetometer, &buf_magnetometer, sizeof(buf_magnetometer));
-	if (fd_barometer > 0)(void)read(fd_barometer, &buf_barometer, sizeof(buf_barometer));
+	if (fd_gyro_l3gd20 > 0)(void)read(fd_gyro_l3gd20, &buf_gyro, sizeof(buf_gyro));
+
+	/* ORB sensor subscriptions */
+	int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
+	int accel_sub = orb_subscribe(ORB_ID(sensor_accel));
+	int mag_sub = orb_subscribe(ORB_ID(sensor_mag));
+	int baro_sub = orb_subscribe(ORB_ID(sensor_baro));
+
+	struct gyro_report gyro_report;
+	struct accel_report accel_report;
+	struct mag_report mag_report;
+	struct baro_report baro_report;
 
 	struct sensor_combined_s raw = {
 		.timestamp = hrt_absolute_time(),
-		.gyro_raw = {buf_gyro.x_raw, buf_gyro.y_raw, buf_gyro.z_raw},
+		.gyro_raw = {gyro_report.x_raw, gyro_report.y_raw, gyro_report.z_raw},
 		.gyro_raw_counter = 0,
-		.gyro_rad_s = {buf_gyro.x, buf_gyro.y, buf_gyro.z},
-		.accelerometer_raw = {buf_accel_report.x_raw, buf_accel_report.y_raw, buf_accel_report.z_raw},
+		.gyro_rad_s = {gyro_report.x, gyro_report.y, gyro_report.z},
+		.accelerometer_raw = {accel_report.x_raw, accel_report.y_raw, accel_report.z_raw},
 		.accelerometer_raw_counter = 0,
-		.accelerometer_m_s2 = {buf_accel_report.x, buf_accel_report.y, buf_accel_report.z},
-		.magnetometer_raw = {buf_magnetometer.x_raw, buf_magnetometer.y_raw, buf_magnetometer.z_raw},
+		.accelerometer_m_s2 = {accel_report.x, accel_report.y, accel_report.z},
+		.magnetometer_raw = {mag_report.x_raw, mag_report.y_raw, mag_report.z_raw},
+		.magnetometer_ga = {mag_report.x, mag_report.y, mag_report.z},
 		.magnetometer_raw_counter = 0,
-		.baro_pres_mbar = buf_barometer.pressure,
-		.baro_alt_meter = buf_barometer.altitude,
-		.baro_temp_celcius = buf_barometer.temperature,
+		.baro_pres_mbar = baro_report.pressure,
+		.baro_alt_meter = baro_report.altitude,
+		.baro_temp_celcius = baro_report.temperature,
 		.baro_raw_counter = 0,
 		.battery_voltage_v = BAT_VOL_INITIAL,
-		.adc_voltage_v = {0, 0, 0},
+		.adc_voltage_v = {0.9f , 0.0f , 0.0f},
 		.battery_voltage_counter = 0,
 		.battery_voltage_valid = false,
 	};
 
-	/* condition to wait for */
-	pthread_mutex_init(&sensors_read_ready_mutex, NULL);
-	pthread_cond_init(&sensors_read_ready, NULL);
-
 	/* advertise the sensor_combined topic and make the initial publication */
 	sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
-	publishing = true;
+	if (sensor_pub < 0) {
+		fprintf(stderr, "[sensors] ERROR: orb_advertise for topic sensor_combined failed.\n");
+	} else {
+		publishing = true;
+	}
 
 	/* advertise the manual_control topic */
 	struct manual_control_setpoint_s manual_control = { .mode = ROLLPOS_PITCHPOS_YAWRATE_THROTTLE,
@@ -737,30 +734,32 @@ int sensors_thread_main(int argc, char *argv[])
 	memset(&vstatus, 0, sizeof(vstatus));
 	int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 
-	printf("[sensors] rate: %u Hz\n", (unsigned int)(1000000 / SENSOR_INTERVAL_MICROSEC));
-
-	struct hrt_call sensors_hrt_call;
-	/* Enable high resolution timer callback to unblock main thread, run after 2 ms */
-	hrt_call_every(&sensors_hrt_call, 2000, SENSOR_INTERVAL_MICROSEC, &sensors_timer_loop, NULL);
-
 	thread_running = true;
 
 	while (!thread_should_exit) {
-		pthread_mutex_lock(&sensors_read_ready_mutex);
+		
+		bool gyro_updated = false;
+
+		struct pollfd fds[4];
+
+		/* wait for data to be ready */
+		fds[0].fd = gyro_sub;
+		fds[0].events = POLLIN;
+
+		fds[1].fd = accel_sub;
+		fds[1].events = POLLIN;
 
-		struct timespec time_to_wait = {0, 0};
-		/* Wait 2 seconds until timeout */
-		time_to_wait.tv_nsec = 0;
-		time_to_wait.tv_sec = time(NULL) + 2;
+		fds[2].fd = mag_sub;
+		fds[2].events = POLLIN;
 
-		if (pthread_cond_timedwait(&sensors_read_ready, &sensors_read_ready_mutex, &time_to_wait) == OK) {
-			pthread_mutex_unlock(&sensors_read_ready_mutex);
+		fds[3].fd = baro_sub;
+		fds[3].events = POLLIN;
 
-			bool gyro_updated = false;
-			bool acc_updated = false;
-			bool magn_updated = false;
-			bool baro_updated = false;
-			bool adc_updated = false;
+		int pret = poll(fds, 4, 500);
+
+		if (pret <= 0) {
+			/* do silently nothing */
+		} else {
 
 			/* store the time closest to all measurements */
 			uint64_t current_time = hrt_absolute_time();
@@ -768,7 +767,7 @@ int sensors_thread_main(int argc, char *argv[])
 
 			/* Update at 5 Hz */
 			if (paramcounter == ((unsigned int)(1000000 / SENSOR_INTERVAL_MICROSEC)/5)) {
-				
+
 				/* Check HIL state */
 				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
 
@@ -832,46 +831,130 @@ int sensors_thread_main(int argc, char *argv[])
 			}
 			paramcounter++;
 
-			if (fd_gyro > 0) {
-				/* try reading gyro */
-				uint64_t start_gyro = hrt_absolute_time();
-				ret_gyro = read(fd_gyro, &buf_gyro, sizeof(buf_gyro));
-				int gyrotime = hrt_absolute_time() - start_gyro;
+			/* --- GYRO --- */
+			if (fds[0].revents & POLLIN) {
 
-				if (gyrotime > 500) printf("GYRO (pure read): %d us\n", gyrotime);
+				orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report);
 
-				/* GYROSCOPE */
-				if (ret_gyro != sizeof(buf_gyro)) {
-					gyro_fail_count++;
+				raw.gyro_rad_s[0] = gyro_report.x;
+				raw.gyro_rad_s[1] = gyro_report.y;
+				raw.gyro_rad_s[2] = gyro_report.z;
 
-					if ((((gyro_fail_count % 500) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
-						fprintf(stderr, "[sensors] GYRO ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
-					}
+				raw.gyro_raw[0] = gyro_report.x_raw;
+				raw.gyro_raw[1] = gyro_report.y_raw;
+				raw.gyro_raw[2] = gyro_report.z_raw;
 
-					if (gyro_healthy && gyro_fail_count >= GYRO_HEALTH_COUNTER_LIMIT_ERROR) {
-						// global_data_send_subsystem_info(&gyro_present_enabled);
-						gyro_healthy = false;
-						gyro_success_count = 0;
-					}
+				raw.gyro_raw_counter++;
+				/* gyro is clocking synchronous data output */
+				gyro_updated = true;
+			}
 
-				} else {
-					gyro_success_count++;
+			/* --- ACCEL --- */
+			if (fds[1].revents & POLLIN) {
 
-					if (!gyro_healthy && gyro_success_count >= GYRO_HEALTH_COUNTER_LIMIT_OK) {
-						// global_data_send_subsystem_info(&gyro_present_enabled_healthy);
-						gyro_healthy = true;
-						gyro_fail_count = 0;
+				orb_copy(ORB_ID(sensor_mag), mag_sub, &mag_report);
 
-					}
+				raw.accelerometer_m_s2[0] = gyro_report.x;
+				raw.gyro_rad_s[1] = gyro_report.y;
+				raw.gyro_rad_s[2] = gyro_report.z;
 
-					gyro_updated = true;
-				}
+				raw.gyro_raw[0] = gyro_report.x_raw;
+				raw.gyro_raw[1] = gyro_report.y_raw;
+				raw.gyro_raw[2] = gyro_report.z_raw;
+
+				raw.accelerometer_raw_counter++;
+			}
+
+			/* --- MAG --- */
+			if (fds[2].revents & POLLIN) {
+
+				orb_copy(ORB_ID(sensor_mag), mag_sub, &mag_report);
+
+				raw.magnetometer_ga[0] = mag_report.x;
+				raw.magnetometer_ga[1] = mag_report.y;
+				raw.magnetometer_ga[2] = mag_report.z;
+
+				raw.magnetometer_raw[0] = mag_report.x_raw;
+				raw.magnetometer_raw[1] = mag_report.y_raw;
+				raw.magnetometer_raw[2] = mag_report.z_raw;
+				
+				raw.magnetometer_raw_counter++;
+			}
 
-				gyrotime = hrt_absolute_time() - start_gyro;
+			/* --- BARO --- */
+			if (fds[3].revents & POLLIN) {
 
-				if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime);
+				orb_copy(ORB_ID(sensor_baro), baro_sub, &baro_report);
+
+				raw.baro_pres_mbar = baro_report.pressure; // Pressure in mbar
+				raw.baro_alt_meter = baro_report.altitude; // Altitude in meters
+				raw.baro_temp_celcius = baro_report.temperature; // Temperature in degrees celcius
+
+				raw.baro_raw_counter++;
 			}
 
+			// /* read BMA180. If the MPU-6000 is present, the BMA180 file descriptor won't be open */
+			// if (fd_bma180 > 0) {
+			// 	/* try reading acc */
+			// 	uint64_t start_acc = hrt_absolute_time();
+			// 	ret_accelerometer = read(fd_bma180, buf_accelerometer, 6);
+
+			// 	/* ACCELEROMETER */
+			// 	if (ret_accelerometer != 6) {
+			// 		acc_fail_count++;
+
+			// 		if ((acc_fail_count % 500) == 0 || (acc_fail_count > 20 && acc_fail_count < 40)) {
+			// 			fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
+			// 		}
+
+			// 		if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) {
+			// 			// global_data_send_subsystem_info(&acc_present_enabled);
+			// 			gyro_healthy = false;
+			// 			acc_success_count = 0;
+			// 		}
+
+			// 	} else {
+			// 		acc_success_count++;
+
+			// 		if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) {
+
+			// 			// global_data_send_subsystem_info(&acc_present_enabled_healthy);
+			// 			acc_healthy = true;
+			// 			acc_fail_count = 0;
+
+			// 		}
+
+			// 		acc_updated = true;
+			// 	}
+
+			// 	int acctime = hrt_absolute_time() - start_acc;
+			// 	if (acctime > 500) printf("ACC: %d us\n", acctime);
+			// }
+
+
+			// /* ACCELEROMETER */
+			// if (acc_updated) {
+			// 	/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
+
+			// 	if (fd_bma180 > 0) {
+
+			// 		/* assign negated value, except for -SHORT_MAX, as it would wrap there */
+			// 		raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor
+			// 		raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor
+			// 		raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor
+
+
+			// 		// XXX read range from sensor
+			// 		float range_g = 4.0f;
+			// 		/* scale from 14 bit to m/s2 */
+			// 		raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - rcp.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
+			// 		raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - rcp.acc_offset[1]) * range_g) / 8192.0f) / 9.81f;
+			// 		raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - rcp.acc_offset[2]) * range_g) / 8192.0f) / 9.81f;
+
+			// 		raw.accelerometer_raw_counter++;
+			// 	}
+			// }
+
 			// if (fd_gyro_l3gd20 > 0) {
 			// 	/* try reading gyro */
 			// 	uint64_t start_gyro = hrt_absolute_time();
@@ -912,224 +995,83 @@ int sensors_thread_main(int argc, char *argv[])
 			// 	if (gyrotime > 500) printf("L3GD20 GYRO (complete): %d us\n", gyrotime);
 			// }
 
-			/* read MPU-6000 */
-			if (fd_accelerometer > 0) {
-				/* try reading acc */
-				uint64_t start_acc = hrt_absolute_time();
-				ret_accelerometer = read(fd_accelerometer, &buf_accel_report, sizeof(struct accel_report));
-
-				/* ACCELEROMETER */
-				if (ret_accelerometer != sizeof(struct accel_report)) {
-					acc_fail_count++;
-
-					if ((acc_fail_count % 500) == 0 || (acc_fail_count > 20 && acc_fail_count < 40)) {
-						fprintf(stderr, "[sensors] MPU-6000 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
-					}
-
-
-					if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) {
-						// global_data_send_subsystem_info(&acc_present_enabled);
-						gyro_healthy = false;
-						acc_success_count = 0;
-					}
-
-				} else {
-					acc_success_count++;
-
-					if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) {
-
-						// global_data_send_subsystem_info(&acc_present_enabled_healthy);
-						acc_healthy = true;
-						acc_fail_count = 0;
-
-					}
-
-					acc_updated = true;
-				}
-
-				int acctime = hrt_absolute_time() - start_acc;
-				if (acctime > 500) printf("ACC: %d us\n", acctime);
-			}
-
-			/* read BMA180. If the MPU-6000 is present, the BMA180 file descriptor won't be open */
-			if (fd_bma180 > 0) {
-				/* try reading acc */
-				uint64_t start_acc = hrt_absolute_time();
-				ret_accelerometer = read(fd_bma180, buf_accelerometer, 6);
+			/* GYROSCOPE */
+			// if (gyro_updated) {
+			// 	/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
 
-				/* ACCELEROMETER */
-				if (ret_accelerometer != 6) {
-					acc_fail_count++;
+			// 	raw.gyro_raw[0] = ((buf_gyro[1] == -32768) ? -32768 : buf_gyro[1]); // x of the board is y of the sensor
+			// 	/* assign negated value, except for -SHORT_MAX, as it would wrap there */
+			// 	raw.gyro_raw[1] = ((buf_gyro[0] == -32768) ? 32767 : -buf_gyro[0]); // y on the board is -x of the sensor
+			// 	raw.gyro_raw[2] = ((buf_gyro[2] == -32768) ? -32768 : buf_gyro[2]); // z of the board is z of the sensor
 
-					if ((acc_fail_count % 500) == 0 || (acc_fail_count > 20 && acc_fail_count < 40)) {
-						fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
-					}
+			// 	/* scale measurements */
+			// 	// XXX request scaling from driver instead of hardcoding it
+			// 	/* scaling calculated as: raw * (1/(32768*(500/180*PI))) */
+			// 	raw.gyro_rad_s[0] = (raw.gyro_raw[0] - rcp.gyro_offset[0]) * 0.000266316109f;
+			// 	raw.gyro_rad_s[1] = (raw.gyro_raw[1] - rcp.gyro_offset[1]) * 0.000266316109f;
+			// 	raw.gyro_rad_s[2] = (raw.gyro_raw[2] - rcp.gyro_offset[2]) * 0.000266316109f;
 
-					if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) {
-						// global_data_send_subsystem_info(&acc_present_enabled);
-						gyro_healthy = false;
-						acc_success_count = 0;
-					}
+			// 	raw.gyro_raw_counter++;
+			// }
 
-				} else {
-					acc_success_count++;
+			static uint64_t last_adc = 0;
+			/* ADC */
+			if (hrt_absolute_time() - last_adc >= 10000) {
+				int ret_adc = read(fd_adc, &buf_adc, adc_readsize);
+				int nsamples_adc = ret_adc / sizeof(struct adc_msg_s);
 
-					if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) {
+				// if (ret_adc  < 0 || ((int)(nsamples_adc * sizeof(struct adc_msg_s))) != ret_adc) {
+				// 	adc_fail_count++;
 
-						// global_data_send_subsystem_info(&acc_present_enabled_healthy);
-						acc_healthy = true;
-						acc_fail_count = 0;
+				// 	if (((adc_fail_count % 20) == 0 || adc_fail_count < 10) && (int)*get_errno_ptr() != EAGAIN) {
+				// 		fprintf(stderr, "[sensors] ADC ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
+				// 	}
 
-					}
+				// 	if (adc_healthy && adc_fail_count >= ADC_HEALTH_COUNTER_LIMIT_ERROR) {
+				// 		adc_healthy = false;
+				// 		adc_success_count = 0;
+				// 	}
 
-					acc_updated = true;
-				}
+				// } else {
+				// 	adc_success_count++;
 
-				int acctime = hrt_absolute_time() - start_acc;
-				if (acctime > 500) printf("ACC: %d us\n", acctime);
-			}
+				// 	if (!adc_healthy && adc_success_count >= ADC_HEALTH_COUNTER_LIMIT_OK) {
+				// 		adc_healthy = true;
+				// 		adc_fail_count = 0;
+				// 	}
 
-			/* MAGNETOMETER */
-			if (magcounter == 210) { /* 120 Hz */
-				uint64_t start_mag = hrt_absolute_time();
-				// /* start calibration mode if requested */
-				// if (!mag_calibration_enabled && vstatus.preflight_mag_calibration) {
-				// 	ioctl(fd_magnetometer, HMC5883L_CALIBRATION_ON, 0);
-				// 	printf("[sensors] enabling mag calibration mode\n");
-				// 	mag_calibration_enabled = true;
-				// } else if (mag_calibration_enabled && !vstatus.preflight_mag_calibration) {
-				// 	ioctl(fd_magnetometer, HMC5883L_CALIBRATION_OFF, 0);
-				// 	printf("[sensors] disabling mag calibration mode\n");
-				// 	mag_calibration_enabled = false;
+				// 	adc_updated = true;
 				// }
-				*get_errno_ptr() = 0;
-				ret_magnetometer = read(fd_magnetometer, &buf_magnetometer, sizeof(buf_magnetometer));
-				int errcode_mag = (int) * get_errno_ptr();
-				int magtime = hrt_absolute_time() - start_mag;
 
-				if (magtime > 2000) {
-					printf("[sensors] WARN: MAG (pure read): %d us\n", magtime);
-				}
-
-				if (ret_magnetometer != sizeof(buf_magnetometer)) {
-					mag_fail_count++;
-
-
-					if ((mag_fail_count % 200) == 0 || (mag_fail_count > 20 && mag_fail_count < 40)) {
-						fprintf(stderr, "[sensors] MAG ERROR #%d: %s\n", errcode_mag, strerror(errcode_mag));
-					}
-
-					if (magn_healthy && mag_fail_count >= MAGN_HEALTH_COUNTER_LIMIT_ERROR) {
-						// global_data_send_subsystem_info(&magn_present_enabled);
-						magn_healthy = false;
-						mag_success_count = 0;
-					}
-
-				} else {
-					mag_success_count++;
-
-					if (!magn_healthy && mag_success_count >= MAGN_HEALTH_COUNTER_LIMIT_OK) {
-						// global_data_send_subsystem_info(&magn_present_enabled_healthy);
-						magn_healthy = true;
-						mag_fail_count = 0;
-					}
-
-					magn_updated = true;
-				}
-
-				magtime = hrt_absolute_time() - start_mag;
-
-				if (magtime > 2000 && (read_loop_counter % 5) == 0) {
-					printf("[sensors] WARN: MAG (overall time): %d us, code:\n", magtime);
-				}
-
-				magcounter = 0;
-			}
-
-			magcounter++;
-
-			/* BAROMETER */
-			if (barocounter == 200 && (fd_barometer > 0)) { /* 100 Hz */
-				uint64_t start_baro = hrt_absolute_time();
-				*get_errno_ptr() = 0;
-				ret_barometer = read(fd_barometer, &buf_barometer, sizeof(buf_barometer));
-
-				if (ret_barometer != sizeof(buf_barometer)) {
-					baro_fail_count++;
-
-					if (((baro_fail_count % 200) == 0 || (baro_fail_count > 20 && baro_fail_count < 40)) && (int)*get_errno_ptr() != EAGAIN) {
-						fprintf(stderr, "[sensors] MS5611 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
-					}
-
-					if (baro_healthy && baro_fail_count >= BARO_HEALTH_COUNTER_LIMIT_ERROR) {
-						/* switched from healthy to unhealthy */
-						baro_healthy = false;
-						baro_success_count = 0;
-						// global_data_send_subsystem_info(&baro_present_enabled);
-					}
+				if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) {
+					/* Voltage in volts */
+					raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (buf_adc.am_data1 * battery_voltage_conversion)));
 
-				} else {
-					baro_success_count++;
+					if ((buf_adc.am_data1 * battery_voltage_conversion) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
+						raw.battery_voltage_valid = false;
+						raw.battery_voltage_v = 0.f;
 
-					if (!baro_healthy && baro_success_count >= MAGN_HEALTH_COUNTER_LIMIT_OK) {
-						/* switched from unhealthy to healthy */
-						baro_healthy = true;
-						baro_fail_count = 0;
-						// global_data_send_subsystem_info(&baro_present_enabled_healthy);
+					} else {
+						raw.battery_voltage_valid = true;
 					}
 
-					baro_updated = true;
+					raw.battery_voltage_counter++;
 				}
 
-				barocounter = 0;
-				int barotime = hrt_absolute_time() - start_baro;
-
-				if (barotime > 2000 && (read_loop_counter % 5) == 0) printf("[sensors] WARN: BARO %d us\n", barotime);
+				last_adc = hrt_absolute_time();
 			}
 
-			barocounter++;
-
-			/* ADC */
-			if (adccounter == 5) {
-				ret_adc = read(fd_adc, &buf_adc, adc_readsize);
-				nsamples_adc = ret_adc / sizeof(struct adc_msg_s);
-
-				if (ret_adc  < 0 || ((int)(nsamples_adc * sizeof(struct adc_msg_s))) != ret_adc) {
-					adc_fail_count++;
-
-					if (((adc_fail_count % 20) == 0 || adc_fail_count < 10) && (int)*get_errno_ptr() != EAGAIN) {
-						fprintf(stderr, "[sensors] ADC ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
-					}
-
-					if (adc_healthy && adc_fail_count >= ADC_HEALTH_COUNTER_LIMIT_ERROR) {
-						adc_healthy = false;
-						adc_success_count = 0;
-					}
-
-				} else {
-					adc_success_count++;
-
-					if (!adc_healthy && adc_success_count >= ADC_HEALTH_COUNTER_LIMIT_OK) {
-						adc_healthy = true;
-						adc_fail_count = 0;
-					}
-
-					adc_updated = true;
-				}
-
-				adccounter = 0;
-
+			/* Inform other processes that new data is available to copy */
+			if (gyro_updated && publishing) {
+				/* Values changed, publish */
+				orb_publish(ORB_ID(sensor_combined), sensor_pub, &raw);
 			}
 
-			adccounter++;
-
-
-
 #ifdef CONFIG_HRT_PPM
-			bool ppm_updated = false;
+			static uint64_t last_ppm = 0;
 
 			/* PPM */
-			if (ppmcounter == 5) {
+			if (hrt_absolute_time() - last_ppm >= 10000) {
 
 				/* require at least two channels
 				 * to consider the signal valid
@@ -1147,9 +1089,6 @@ int sensors_thread_main(int argc, char *argv[])
 					rc.chan_count = ppm_decoded_channels;
 					rc.timestamp = ppm_last_valid_decode;
 
-					/* publish a few lines of code later if set to true */
-					ppm_updated = true;
-
 					/* roll input */
 					manual_control.roll = rc.chan[rc.function[ROLL]].scaled;
 					if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
@@ -1175,209 +1114,34 @@ int sensors_thread_main(int argc, char *argv[])
 					if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f;
 					if (manual_control.override_mode_switch >  1.0f) manual_control.override_mode_switch =  1.0f;
 
-				}
-				ppmcounter = 0;
-			}
-
-			ppmcounter++;
-#endif
-
-			/* Copy values of gyro, acc, magnetometer & barometer */
-
-			/* GYROSCOPE */
-			// if (gyro_updated) {
-			// 	/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
-
-			// 	raw.gyro_raw[0] = ((buf_gyro[1] == -32768) ? -32768 : buf_gyro[1]); // x of the board is y of the sensor
-			// 	/* assign negated value, except for -SHORT_MAX, as it would wrap there */
-			// 	raw.gyro_raw[1] = ((buf_gyro[0] == -32768) ? 32767 : -buf_gyro[0]); // y on the board is -x of the sensor
-			// 	raw.gyro_raw[2] = ((buf_gyro[2] == -32768) ? -32768 : buf_gyro[2]); // z of the board is z of the sensor
-
-			// 	/* scale measurements */
-			// 	// XXX request scaling from driver instead of hardcoding it
-			// 	/* scaling calculated as: raw * (1/(32768*(500/180*PI))) */
-			// 	raw.gyro_rad_s[0] = (raw.gyro_raw[0] - rcp.gyro_offset[0]) * 0.000266316109f;
-			// 	raw.gyro_rad_s[1] = (raw.gyro_raw[1] - rcp.gyro_offset[1]) * 0.000266316109f;
-			// 	raw.gyro_rad_s[2] = (raw.gyro_raw[2] - rcp.gyro_offset[2]) * 0.000266316109f;
-
-			// 	raw.gyro_raw_counter++;
-			// }
-
-			/* MPU-6000 update */
-			if (gyro_updated && fd_accelerometer > 0) {
-				/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
-
-				raw.gyro_raw[0] = ((buf_gyro.y_raw == -32768) ? -32768 : buf_gyro.y_raw); // x of the board is y of the sensor
-				/* assign negated value, except for -SHORT_MAX, as it would wrap there */
-				raw.gyro_raw[1] = ((buf_gyro.x_raw == -32768) ? 32767 : -buf_gyro.x_raw); // y on the board is -x of the sensor
-				raw.gyro_raw[2] = ((buf_gyro.z_raw == -32768) ? -32768 : buf_gyro.z_raw); // z of the board is z of the sensor
-
-				/* scale measurements */
-				// XXX request scaling from driver instead of hardcoding it
-				/* scaling calculated as: raw * (1/(32768*(500/180*PI))) */
-				raw.gyro_rad_s[0] = buf_gyro.y;
-				raw.gyro_rad_s[1] = buf_gyro.x;
-				raw.gyro_rad_s[2] = buf_gyro.z;
-
-				raw.gyro_raw_counter++;
-			}
-
-			/* ACCELEROMETER */
-			if (acc_updated) {
-				/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
-
-				if (fd_accelerometer > 0) {
-					/* MPU-6000 values */
-
-					/* scale from 14 bit to m/s2 */
-					raw.accelerometer_m_s2[0] = buf_accel_report.x - rcp.acc_offset[0] * buf_accel_report.scaling;
-					raw.accelerometer_m_s2[1] = buf_accel_report.y - rcp.acc_offset[1] * buf_accel_report.scaling;
-					raw.accelerometer_m_s2[2] = buf_accel_report.z - rcp.acc_offset[2] * buf_accel_report.scaling;
-
-					raw.accelerometer_raw[0] = buf_accel_report.x_raw;
-					raw.accelerometer_raw[1] = buf_accel_report.y_raw;
-					raw.accelerometer_raw[2] = buf_accel_report.z_raw;
-
-					raw.accelerometer_raw_counter++;
-				} else if (fd_bma180 > 0) {
-
-					/* assign negated value, except for -SHORT_MAX, as it would wrap there */
-					raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor
-					raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor
-					raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor
-
-
-					// XXX read range from sensor
-					float range_g = 4.0f;
-					/* scale from 14 bit to m/s2 */
-					raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - rcp.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
-					raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - rcp.acc_offset[1]) * range_g) / 8192.0f) / 9.81f;
-					raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - rcp.acc_offset[2]) * range_g) / 8192.0f) / 9.81f;
-
-					raw.accelerometer_raw_counter++;
-				}
-
-				/* Use MPU-6000 */
-				if (fd_accelerometer > 0) {
-					raw.gyro_raw[0] = buf_gyro.x_raw;
-					raw.gyro_raw[1] = buf_gyro.y_raw;
-					raw.gyro_raw[2] = buf_gyro.z_raw;
-
-					/* scaled measurements */
-					raw.gyro_rad_s[0] = (buf_gyro.x - rcp.gyro_offset[0]) * buf_gyro.scaling;
-					raw.gyro_rad_s[1] = (buf_gyro.y - rcp.gyro_offset[1]) * buf_gyro.scaling;
-					raw.gyro_rad_s[2] = (buf_gyro.z - rcp.gyro_offset[2]) * buf_gyro.scaling;
+					orb_publish(ORB_ID(rc_channels), rc_pub, &rc);
+					orb_publish(ORB_ID(manual_control_setpoint), manual_control_pub, &manual_control);
 
-					raw.gyro_raw_counter++;
-					/* mark as updated */
-					gyro_updated = true;
 				}
+				last_ppm = hrt_absolute_time();
 			}
-
-			/* MAGNETOMETER */
-			if (magn_updated) {
-				/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
-
-				/* assign negated value, except for -SHORT_MAX, as it would wrap there */
-				raw.magnetometer_raw[0] = buf_magnetometer.x_raw;
-				raw.magnetometer_raw[1] = buf_magnetometer.y_raw;
-				raw.magnetometer_raw[2] = buf_magnetometer.z_raw;
-
-				// XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here
-				raw.magnetometer_ga[0] = buf_magnetometer.x - rcp.mag_offset[0] * buf_magnetometer.scaling;
-				raw.magnetometer_ga[1] = buf_magnetometer.y - rcp.mag_offset[1] * buf_magnetometer.scaling;
-				raw.magnetometer_ga[2] = buf_magnetometer.z - rcp.mag_offset[2] * buf_magnetometer.scaling;
-
-				/* store mode */
-				raw.magnetometer_mode = 0;
-
-				raw.magnetometer_raw_counter++;
-			}
-
-			/* BAROMETER */
-			if (baro_updated) {
-				/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
-
-				raw.baro_pres_mbar = buf_barometer.pressure; // Pressure in mbar
-				raw.baro_alt_meter = buf_barometer.altitude; // Altitude in meters
-				raw.baro_temp_celcius = buf_barometer.temperature; // Temperature in degrees celcius
-
-				raw.baro_raw_counter++;
-			}
-
-			/* ADC */
-			if (adc_updated) {
-				/* copy sensor readings to global data*/
-
-				if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) {
-					/* Voltage in volts */
-					raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (buf_adc.am_data1 * battery_voltage_conversion)));
-
-					if ((buf_adc.am_data1 * battery_voltage_conversion) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
-						raw.battery_voltage_valid = false;
-						raw.battery_voltage_v = 0.f;
-
-					} else {
-						raw.battery_voltage_valid = true;
-					}
-
-					raw.battery_voltage_counter++;
-				}
-			}
-
-			uint64_t total_time = hrt_absolute_time() - current_time;
-
-			/* Inform other processes that new data is available to copy */
-			if ((gyro_updated || acc_updated || magn_updated || baro_updated) && publishing) {
-				/* Values changed, publish */
-				orb_publish(ORB_ID(sensor_combined), sensor_pub, &raw);
-			}
-
-#ifdef CONFIG_HRT_PPM
-
-			if (ppm_updated) {
-				orb_publish(ORB_ID(rc_channels), rc_pub, &rc);
-				orb_publish(ORB_ID(manual_control_setpoint), manual_control_pub, &manual_control);
-			}
-
 #endif
 
-			if (total_time > 2600) {
-				excessive_readout_time_counter++;
-			}
-
-			if (total_time > 2600 && excessive_readout_time_counter > 100 && excessive_readout_time_counter % 100 == 0) {
-				fprintf(stderr, "[sensors] slow update (>2600 us): %d us (#%d)\n", (int)total_time, excessive_readout_time_counter);
-
-			} else if (total_time > 6000) {
-				if (excessive_readout_time_counter < 100 || excessive_readout_time_counter % 100 == 0) fprintf(stderr, "[sensors] WARNING: Slow update (>6000 us): %d us (#%d)\n", (int)total_time, excessive_readout_time_counter);
-			}
-
-
 			read_loop_counter++;
-#ifdef CONFIG_SENSORS_DEBUG_ENABLED
-
-			if (read_loop_counter % 1000 == 0) printf("[sensors] read loop counter: %d\n", read_loop_counter);
-
-			fflush(stdout);
-
-			if (sensors_timer_loop_counter % 1000 == 0) printf("[sensors] timer/trigger loop counter: %d\n", sensors_timer_loop_counter);
-
-#endif
 		}
-
-		if (thread_should_exit) break;
 	}
 
-	/* Never really getting here */
 	printf("[sensors] sensor readout stopped\n");
 
 	close(fd_gyro);
-	close(fd_bma180);
 	close(fd_magnetometer);
 	close(fd_barometer);
 	close(fd_adc);
 
+	/* maintained for backwards-compatibility with v1.5 */
+	close(fd_gyro_l3gd20);
+	close(fd_bma180);
+
+	close(gyro_sub);
+	close(accel_sub);
+	close(mag_sub);
+	close(baro_sub);
+
 	printf("[sensors] exiting.\n");
 
 	thread_running = false;
diff --git a/apps/systemcmds/eeprom/eeprom.c b/apps/systemcmds/eeprom/eeprom.c
index 20727e764..ed727cd33 100644
--- a/apps/systemcmds/eeprom/eeprom.c
+++ b/apps/systemcmds/eeprom/eeprom.c
@@ -48,6 +48,7 @@
 #include <fcntl.h>
 #include <sys/mount.h>
 #include <sys/ioctl.h>
+#include <sys/stat.h>
 
 #include <nuttx/i2c.h>
 #include <nuttx/mtd.h>
@@ -99,7 +100,7 @@ int eeprom_main(int argc, char *argv[])
 		}
 	}
 
-	errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/params'\n\t'load_param /eeprom/params'\n\t'erase'\n");
+	errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n");
 }
 
 
@@ -174,9 +175,16 @@ eeprom_ioctl(unsigned operation)
 static void
 eeprom_save(const char *name)
 {
+	if (!started)
+		errx(1, "must be started first");
+
 	if (!name) 
-		err(1, "missing argument for device name, try '/eeprom'");
+		err(1, "missing argument for device name, try '/eeprom/parameters'");
+
+	/* delete the file in case it exists */
+	unlink(name);
 
+	/* create the file */
 	int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
 
 	if (fd < 0)
@@ -196,8 +204,11 @@ eeprom_save(const char *name)
 static void
 eeprom_load(const char *name)
 {
+	if (!started)
+		errx(1, "must be started first");
+
 	if (!name) 
-		err(1, "missing argument for device name, try '/eeprom'");
+		err(1, "missing argument for device name, try '/eeprom/parameters'");
 	
 	int fd = open(name, O_RDONLY);
 
diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig
index 337a825d8..5eb3f1f29 100755
--- a/nuttx/configs/px4fmu/nsh/defconfig
+++ b/nuttx/configs/px4fmu/nsh/defconfig
@@ -651,8 +651,8 @@ CONFIG_ARCH_BZERO=n
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=8
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=20
-CONFIG_NFILE_STREAMS=20
+CONFIG_NFILE_DESCRIPTORS=25
+CONFIG_NFILE_STREAMS=25
 CONFIG_NAME_MAX=32
 CONFIG_STDIO_BUFFER_SIZE=256
 CONFIG_STDIO_LINEBUFFER=y