Sensor readout, testing and driver adjustments

6 files changed, 254 insertions, 113 deletions

diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS
index 7b8d51338..9e564e2cc 100755
--- a/ROMFS/scripts/rcS
+++ b/ROMFS/scripts/rcS
@@ -31,6 +31,8 @@ else
 	echo "[init] no microSD card found"
 fi
 
+usleep 500
+
 #
 # Look for an init script on the microSD card.
 #
@@ -82,6 +84,7 @@ else
 		if [ -f /etc/init.d/rc.PX4IOAR ]
 		then
 			echo "[init] reading /etc/init.d/rc.PX4IOAR"
+			usleep 500
 			sh /etc/init.d/rc.PX4IOAR
 		fi
 	else
@@ -97,6 +100,7 @@ else
 		if [ -f /etc/init.d/rc.PX4IO ]
 		then
 			echo "[init] reading /etc/init.d/rc.PX4IO"
+			usleep 500
 			sh /etc/init.d/rc.PX4IO
 		fi
 	else
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
index 951e1a062..999790e8a 100644
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ b/apps/drivers/mpu6000/mpu6000.cpp
@@ -288,9 +288,9 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
 	_gyro(new MPU6000_gyro(this)),
 	_product(0),
 	_call_interval(0),
-	_accel_range_scale(1.0f),
+	_accel_range_scale(0.02f),
 	_accel_topic(-1),
-	_gyro_range_scale(1.0f),
+	_gyro_range_scale(0.02f),
 	_gyro_topic(-1),
 	_reads(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read"))
@@ -826,7 +826,7 @@ test()
 		fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
 
 		if (fd < 0) {
-			reason = "can't open driver";
+			reason = "can't open driver, use <mpu6000 start> first";
 			break;
 		}
 
@@ -841,9 +841,10 @@ test()
 		printf("single read\n");
 		fflush(stdout);
 		printf("time:        %lld\n", report.timestamp);
-		printf("x:           %f\n", report.x);
-		printf("y:           %f\n", report.y);
-		printf("z:           %f\n", report.z);
+		printf("x:         %5.2f\n", (double)report.x);
+		printf("y:         %5.2f\n", (double)report.y);
+		printf("z:         %5.2f\n", (double)report.z);
+		printf("test:         %8.4f\n", 1.5);
 
 	} while (0);
 
diff --git a/apps/px4/tests/Makefile b/apps/px4/tests/Makefile
index ad8849454..cb1c3c618 100644
--- a/apps/px4/tests/Makefile
+++ b/apps/px4/tests/Makefile
@@ -37,6 +37,6 @@
 
 APPNAME		 = tests
 PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 8096
+STACKSIZE	 = 12000
 
 include $(APPDIR)/mk/app.mk
diff --git a/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c
index 91397d11c..87ea7f058 100644
--- a/apps/px4/tests/test_sensors.c
+++ b/apps/px4/tests/test_sensors.c
@@ -90,11 +90,11 @@ struct {
 	const char	*path;
 	int	(* test)(int argc, char *argv[]);
 } sensors[] = {
-	{"l3gd20",	"/dev/l3gd20",	l3gd20},
 	{"bma180",	"/dev/bma180",	bma180},
+	{"mpu6000",	"/dev/accel",	mpu6000},
+	{"l3gd20",	"/dev/l3gd20",	l3gd20},
 	{"hmc5883l",	"/dev/hmc5883l",	hmc5883l},
 	{"ms5611",	"/dev/ms5611",	ms5611},
-	{"mpu6000",	"/dev/accel",	mpu6000},
 //    {"lis331",	"/dev/lis331",	lis331},
 	{NULL, NULL, NULL}
 };
@@ -253,6 +253,9 @@ l3gd20(int argc, char *argv[])
 static int
 bma180(int argc, char *argv[])
 {
+	// XXX THIS SENSOR IS OBSOLETE
+	// TEST REMAINS, BUT ALWAYS RETURNS OK
+
 	printf("\tBMA180: test start\n");
 	fflush(stdout);
 
@@ -264,7 +267,7 @@ bma180(int argc, char *argv[])
 
 	if (fd < 0) {
 		printf("\tBMA180: open fail\n");
-		return ERROR;
+		return OK;
 	}
 
 //	if (ioctl(fd, LIS331_SETRATE, LIS331_RATE_50Hz) ||
@@ -283,7 +286,7 @@ bma180(int argc, char *argv[])
 	if (ret != sizeof(buf)) {
 		printf("\tBMA180: read1 fail (%d)\n", ret);
 		close(fd);
-		return ERROR;
+		return OK;
 
 	} else {
 		printf("\tBMA180 values: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
@@ -297,7 +300,7 @@ bma180(int argc, char *argv[])
 	if (ret != sizeof(buf)) {
 		printf("\tBMA180: read2 fail (%d)\n", ret);
 		close(fd);
-		return ERROR;
+		return OK;
 
 	} else {
 		printf("\tBMA180: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
diff --git a/apps/sensors/Makefile b/apps/sensors/Makefile
index f8dc64ed1..125839bb3 100644
--- a/apps/sensors/Makefile
+++ b/apps/sensors/Makefile
@@ -37,7 +37,6 @@
 
 APPNAME		= sensors
 PRIORITY	= SCHED_PRIORITY_MAX-5
-# Reduce to 4096 on next occasion
-STACKSIZE	= 8000
+STACKSIZE	= 4096
 
 include $(APPDIR)/mk/app.mk
diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c
index 2739b3971..65ae336d9 100644
--- a/apps/sensors/sensors.c
+++ b/apps/sensors/sensors.c
@@ -57,6 +57,7 @@
 #include <arch/board/drv_bma180.h>
 #include <arch/board/drv_l3gd20.h>
 #include <arch/board/drv_hmc5883l.h>
+#include <drivers/drv_accel.h>
 #include <arch/board/up_adc.h>
 
 #include <systemlib/systemlib.h>
@@ -108,15 +109,17 @@ static int sensors_timer_loop_counter = 0;
 
 /* File descriptors for all sensors */
 static int fd_gyro = -1;
-static int fd_accelerometer = -1;
-static int fd_magnetometer = -1;
-static int fd_barometer = -1;
-static int fd_adc = -1;
 
 static bool thread_should_exit = false;
 static bool thread_running = false;
 static int sensors_task;
 
+static int fd_bma180 = -1;
+static int fd_magnetometer = -1;
+static int fd_barometer = -1;
+static int fd_adc = -1;
+static int fd_accelerometer = -1;
+
 /* Private functions declared static */
 static void sensors_timer_loop(void *arg);
 
@@ -240,30 +243,70 @@ static int sensors_init(void)
 
 	/* open gyro */
 	fd_gyro = open("/dev/l3gd20", O_RDONLY);
+	int errno_gyro = (int)*get_errno_ptr();
 
-	if (fd_gyro < 0) {
-		fprintf(stderr, "[sensors]   L3GD20 open 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 {
+	if (!(fd_gyro < 0)) {
 		printf("[sensors]   L3GD20 open ok\n");
 	}
 
-	/* open accelerometer */
-	fd_accelerometer = open("/dev/bma180", O_RDONLY);
+	/* open accelerometer, prefer the MPU-6000 */
+	fd_accelerometer = open("/dev/accel", O_RDONLY);
+	int errno_accelerometer = (int)*get_errno_ptr();
+
+	if (!(fd_accelerometer < 0)) {
+		printf("[sensors]   Accelerometer open ok\n");
+	}
 
+
+	/* only attempt to use BMA180 if MPU-6000 is not available */
+	int errno_bma180 = 0;
 	if (fd_accelerometer < 0) {
-		fprintf(stderr, "[sensors]   BMA180: open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
-		fflush(stderr);
-		/* this sensor is critical, exit on failed init */
+		fd_bma180 = open("/dev/bma180", O_RDONLY);
+		errno_bma180 = (int)*get_errno_ptr();
+
+		if (!(fd_bma180 < 0)) {
+			printf("[sensors]   Accelerometer (BMA180) open ok\n");
+		}
+	} else {
+		fd_bma180 = -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]   MPU-6000: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer));
+			fflush(stderr);
+			/* this sensor is redundant with BMA180 */
+		}
+		
+		if (fd_bma180 < 0) {
+			fprintf(stderr, "[sensors]   BMA180: open fail (err #%d): %s\n", errno_bma180, strerror(errno_bma180));
+			fflush(stderr);
+			/* this sensor is redundant with MPU-6000 */
+		}
+
 		errno = ENOSYS;
 		return ERROR;
+	}
 
-	} else {
-		printf("[sensors]   BMA180 open ok\n");
+	/* fail if no gyro 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]   MPU-6000: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer));
+			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));
+			fflush(stderr);
+			/* this sensor is critical, exit on failed init */
+		}
+
+		errno = ENOSYS;
+		return ERROR;
 	}
 
 	/* open adc */
@@ -280,16 +323,18 @@ static int sensors_init(void)
 		printf("[sensors]   ADC open ok\n");
 	}
 
-	/* configure gyro */
-	if (ioctl(fd_gyro, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_30HZ) || ioctl(fd_gyro, 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;
+	/* configure gyro - if its not available and we got here the MPU-6000 is for sure available */
+	if (fd_gyro > 0) {
+		if (ioctl(fd_gyro, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_30HZ) || ioctl(fd_gyro, 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 {
+			printf("[sensors]   L3GD20 configuration ok\n");
+		}
 	}
 
 	/* XXX Add IOCTL configuration of remaining sensors */
@@ -326,7 +371,7 @@ int sensors_thread_main(int argc, char *argv[])
 		fprintf(stderr, "[sensors] ERROR: Failed to initialize all sensors\n");
 		/* Clean up */
 		close(fd_gyro);
-		close(fd_accelerometer);
+		close(fd_bma180);
 		close(fd_magnetometer);
 		close(fd_barometer);
 		close(fd_adc);
@@ -382,6 +427,7 @@ int sensors_thread_main(int argc, char *argv[])
 
 	int16_t buf_gyro[3];
 	int16_t buf_accelerometer[3];
+	struct accel_report buf_accel_report;
 	int16_t buf_magnetometer[7];
 	float	buf_barometer[3];
 
@@ -425,8 +471,9 @@ int sensors_thread_main(int argc, char *argv[])
 
 	/* Empty sensor buffers, avoid junk values */
 	/* Read first two values of each sensor into void */
-	(void)read(fd_gyro, buf_gyro, sizeof(buf_gyro));
-	(void)read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer));
+	if (fd_gyro > 0)(void)read(fd_gyro, buf_gyro, sizeof(buf_gyro));
+	if (fd_bma180 > 0)(void)read(fd_bma180, buf_accelerometer, 6);
+	if (fd_accelerometer > 0)(void)read(fd_accelerometer, buf_accelerometer, 12);
 	(void)read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer));
 
 	if (fd_barometer > 0)(void)read(fd_barometer, buf_barometer, sizeof(buf_barometer));
@@ -528,7 +575,13 @@ int sensors_thread_main(int argc, char *argv[])
 				if (vstatus.flag_hil_enabled && !hil_enabled) {
 					hil_enabled = true;
 					publishing = false;
-					printf("[sensors] Closing sensor pub: %i \n", ret);
+
+					int sens_ret = close(sensor_pub);
+					if (sens_ret == OK) {
+						printf("[sensors] Closing sensor pub OK\n");
+					} else {
+						printf("[sensors] FAILED Closing sensor pub, result: %i \n", sens_ret);
+					}
 
 					/* switching from HIL to non-HIL mode */
 
@@ -581,79 +634,122 @@ int sensors_thread_main(int argc, char *argv[])
 
 			paramcounter++;
 
-			/* 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;
+			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;
 
-			if (gyrotime > 500) printf("GYRO (pure read): %d us\n", gyrotime);
+				if (gyrotime > 500) printf("GYRO (pure read): %d us\n", gyrotime);
 
-			/* GYROSCOPE */
-			if (ret_gyro != sizeof(buf_gyro)) {
-				gyro_fail_count++;
+				/* GYROSCOPE */
+				if (ret_gyro != sizeof(buf_gyro)) {
+					gyro_fail_count++;
 
-				if ((((gyro_fail_count % 20) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
-					fprintf(stderr, "[sensors] L3GD20 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
-				}
+					if ((((gyro_fail_count % 20) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
+						fprintf(stderr, "[sensors] L3GD20 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
+					}
 
-				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;
-				}
+					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;
+					}
 
-			} else {
-				gyro_success_count++;
+				} else {
+					gyro_success_count++;
+
+					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;
 
-				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;
+					}
 
+					gyro_updated = true;
 				}
 
-				gyro_updated = true;
+				gyrotime = hrt_absolute_time() - start_gyro;
+
+				if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime);
 			}
 
-			gyrotime = hrt_absolute_time() - start_gyro;
+			/* 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));
 
-			if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime);
+				/* ACCELEROMETER */
+				if (ret_accelerometer != sizeof(struct accel_report)) {
+					acc_fail_count++;
 
-			/* try reading acc */
-			uint64_t start_acc = hrt_absolute_time();
-			ret_accelerometer = read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer));
+					if ((acc_fail_count % 20) == 0 || (acc_fail_count > 20 && acc_fail_count < 100)) {
+						fprintf(stderr, "[sensors] MPU-6000 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
+					}
 
-			/* ACCELEROMETER */
-			if (ret_accelerometer != sizeof(buf_accelerometer)) {
-				acc_fail_count++;
 
-				if (acc_fail_count & 0b111 || (acc_fail_count > 20 && acc_fail_count < 100)) {
-					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;
+					}
 
-				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++;
 
-			} else {
-				acc_success_count++;
+					if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) {
 
-				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;
 
-					// global_data_send_subsystem_info(&acc_present_enabled_healthy);
-					acc_healthy = true;
-					acc_fail_count = 0;
+					}
 
+					acc_updated = true;
 				}
 
-				acc_updated = true;
+				int acctime = hrt_absolute_time() - start_acc;
+				if (acctime > 500) printf("ACC: %d us\n", acctime);
 			}
 
-			int acctime = hrt_absolute_time() - start_acc;
+			/* 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);
 
-			if (acctime > 500) printf("ACC: %d us\n", acctime);
+				/* ACCELEROMETER */
+				if (ret_accelerometer != 6) {
+					acc_fail_count++;
+
+					if ((acc_fail_count % 20) == 0 || (acc_fail_count > 20 && acc_fail_count < 100)) {
+						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);
+			}
 
 			/* MAGNETOMETER */
 			if (magcounter == 4) { /* 120 Hz */
@@ -680,7 +776,8 @@ int sensors_thread_main(int argc, char *argv[])
 				if (ret_magnetometer != sizeof(buf_magnetometer)) {
 					mag_fail_count++;
 
-					if (mag_fail_count & 0b111 || (mag_fail_count > 20 && mag_fail_count < 100)) {
+
+					if ((mag_fail_count % 20) == 0 || (mag_fail_count > 20 && mag_fail_count < 100)) {
 						fprintf(stderr, "[sensors] HMC5883L ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
 					}
 
@@ -723,7 +820,7 @@ int sensors_thread_main(int argc, char *argv[])
 				if (ret_barometer != sizeof(buf_barometer)) {
 					baro_fail_count++;
 
-					if ((baro_fail_count & 0b1000 || (baro_fail_count > 20 && baro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
+					if (((baro_fail_count % 20) == 0 || (baro_fail_count > 20 && baro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
 						fprintf(stderr, "[sensors] MS5611 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
 					}
 
@@ -760,10 +857,10 @@ int sensors_thread_main(int argc, char *argv[])
 				ret_adc = read(fd_adc, &buf_adc, adc_readsize);
 				nsamples_adc = ret_adc / sizeof(struct adc_msg_s);
 
-				if (ret_adc  < 0 || nsamples_adc * sizeof(struct adc_msg_s) != ret_adc) {
+				if (ret_adc  < 0 || ((int)(nsamples_adc * sizeof(struct adc_msg_s))) != ret_adc) {
 					adc_fail_count++;
 
-					if ((adc_fail_count & 0b1000 || adc_fail_count < 10) && (int)*get_errno_ptr() != EAGAIN) {
+					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()));
 					}
 
@@ -803,7 +900,7 @@ int sensors_thread_main(int argc, char *argv[])
 				 */
 				if (ppm_decoded_channels > 1 && (hrt_absolute_time() - ppm_last_valid_decode) < 45000) {
 					/* Read out values from HRT */
-					for (int i = 0; i < ppm_decoded_channels; i++) {
+					for (unsigned int i = 0; i < ppm_decoded_channels; i++) {
 						rc.chan[i].raw = ppm_buffer[i];
 						/* Set the range to +-, then scale up */
 						rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor * 10000;
@@ -873,19 +970,56 @@ int sensors_thread_main(int argc, char *argv[])
 			if (acc_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.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) ? -32768 : buf_accelerometer[0]; // y on the board is x of the sensor
-				raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32768 : 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] - acc_offset[0]) / 8192.0f) * range_g) * 9.81f;
-				raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) / 8192.0f) * range_g) * 9.81f;
-				raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) / 8192.0f) * range_g) * 9.81f;
-
-				raw.accelerometer_raw_counter++;
+				if (fd_accelerometer > 0) {
+					/* MPU-6000 values */
+
+					/* scale from 14 bit to m/s2 */
+					raw.accelerometer_m_s2[0] = buf_accel_report.x;
+					raw.accelerometer_m_s2[1] = buf_accel_report.y;
+					raw.accelerometer_m_s2[2] = buf_accel_report.z;
+
+					/* assign negated value, except for -SHORT_MAX, as it would wrap there */
+					raw.accelerometer_raw[0] = buf_accel_report.x*1000; // x of the board is -y of the sensor
+					raw.accelerometer_raw[1] = buf_accel_report.y*1000; // y on the board is x of the sensor
+					raw.accelerometer_raw[2] = buf_accel_report.z*1000; // z of the board is z of the sensor
+
+					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] - acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
+					raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) * range_g) / 8192.0f) / 9.81f;
+					raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) * range_g) / 8192.0f) / 9.81f;
+
+					raw.accelerometer_raw_counter++;
+				}
+
+				/* L3GD20 is not available, use MPU-6000 */
+				if (fd_accelerometer > 0 && fd_gyro < 0) {
+					raw.gyro_raw[0] = ((buf_accelerometer[3] == -32768) ? -32767 : buf_accelerometer[3]); // 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_accelerometer[4] == -32768) ? 32767 : -buf_accelerometer[4]); // y on the board is -x of the sensor
+					raw.gyro_raw[2] = ((buf_accelerometer[5] == -32768) ? -32767 : buf_accelerometer[5]); // 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] - gyro_offset[0]) * 0.000266316109f;
+					raw.gyro_rad_s[1] = (raw.gyro_raw[1] - gyro_offset[1]) * 0.000266316109f;
+					raw.gyro_rad_s[2] = (raw.gyro_raw[2] - gyro_offset[2]) * 0.000266316109f;
+
+					raw.gyro_raw_counter++;
+					/* mark as updated */
+					gyro_updated = true;
+				}
 			}
 
 			/* MAGNETOMETER */
@@ -987,7 +1121,7 @@ int sensors_thread_main(int argc, char *argv[])
 	printf("[sensors] sensor readout stopped\n");
 
 	close(fd_gyro);
-	close(fd_accelerometer);
+	close(fd_bma180);
 	close(fd_magnetometer);
 	close(fd_barometer);
 	close(fd_adc);