Added required scalings, added gyro to MPU6000 test, changed sensors app to read from new drivers

5 files changed, 236 insertions, 104 deletions

diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h
index 370cc5d87..65ef8420f 100644
--- a/apps/drivers/drv_accel.h
+++ b/apps/drivers/drv_accel.h
@@ -50,10 +50,15 @@
  * structure.
  */
 struct accel_report {
+	uint64_t timestamp;
 	float x;
 	float y;
 	float z;
-	uint64_t timestamp;
+	float range_m_s2;
+	float scaling;
+	uint16_t x_raw;
+	uint16_t y_raw;
+	uint16_t z_raw;
 };
 
 /** accel scaling factors; Vout = (Vin * Vscale) + Voffset */
diff --git a/apps/drivers/drv_gyro.h b/apps/drivers/drv_gyro.h
index 82e23f62a..2f1dab6ba 100644
--- a/apps/drivers/drv_gyro.h
+++ b/apps/drivers/drv_gyro.h
@@ -50,10 +50,16 @@
  * structure.
  */
 struct gyro_report {
+	uint64_t timestamp;
 	float x;
 	float y;
 	float z;
-	uint64_t timestamp;
+	float range_rad_s;
+	float scaling;
+	
+	int16_t x_raw;
+	int16_t y_raw;
+	int16_t z_raw;
 };
 
 /** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */
diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h
index a59291778..f87cb7704 100644
--- a/apps/drivers/drv_mag.h
+++ b/apps/drivers/drv_mag.h
@@ -52,10 +52,16 @@
  * Output values are in gauss.
  */
 struct mag_report {
+	uint64_t timestamp;
 	float x;
 	float y;
 	float z;
-	uint64_t timestamp;
+	float range_ga;
+	float scaling;
+	
+	uint16_t x_raw;
+	uint16_t y_raw;
+	uint16_t z_raw;
 };
 
 /** mag scaling factors; Vout = (Vin * Vscale) + Voffset */
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
index 999790e8a..b86e2da2f 100644
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ b/apps/drivers/mpu6000/mpu6000.cpp
@@ -364,12 +364,26 @@ MPU6000::init()
 	write_reg(MPUREG_SMPLRT_DIV, 0x04);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
 	usleep(1000);
 
-	// FS & DLPF   FS=2000¼/s, DLPF = 98Hz (low pass filter)
+	// FS & DLPF   FS=2000 deg/s, DLPF = 98Hz (low pass filter)
 	write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
 	usleep(1000);
-	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); // Gyro scale 2000¼/s
+	// Gyro scale 2000 deg/s ()
+	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
 	usleep(1000);
 
+	// correct gyro scale factors
+	// scale to rad/s in SI units
+	// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
+	// scaling factor:
+	// 1/(2^15)*(2000/180)*PI
+	_gyro_scale.x_offset = 0;
+	_gyro_scale.x_scale  = 1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
+	_gyro_scale.y_offset = 0;
+	_gyro_scale.y_scale  = 1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
+	_gyro_scale.z_offset = 0;
+	_gyro_scale.z_scale  = 1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
+	_gyro_range_scale = 1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
+
 	// product-specific scaling
 	switch (_product) {
 	case MPU6000ES_REV_C4:
@@ -394,6 +408,16 @@ MPU6000::init()
 		break;
 	}
 
+	// Correct accel scale factors of 4096 LSB/g
+	// scale to m/s^2 ( 1g = 9.81 m/s^2)
+	_accel_scale.x_offset = 0;
+	_accel_scale.x_scale  = 1.0f / (4096.0f / 9.81f);
+	_accel_scale.y_offset = 0;
+	_accel_scale.y_scale  = 1.0f / (4096.0f / 9.81f);
+	_accel_scale.z_offset = 0;
+	_accel_scale.z_scale  = 1.0f / (4096.0f / 9.81f);
+	_accel_range_scale = 1.0f / (4096.0f / 9.81f);
+
 	usleep(1000);
 
 	// INT CFG => Interrupt on Data Ready
@@ -472,7 +496,7 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
 	if (_call_interval == 0)
 		measure();
 
-	/* copy out the latest report */
+	/* copy out the latest reports */
 	memcpy(buffer, &_accel_report, sizeof(_accel_report));
 	ret = sizeof(_accel_report);
 
@@ -746,10 +770,18 @@ MPU6000::measure()
 	 */
 	_gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time();
 
+	_accel_report.x_raw = report.accel_x;
+	_accel_report.y_raw = report.accel_x;
+	_accel_report.z_raw = report.accel_x;
+
 	_accel_report.x = report.accel_x * _accel_range_scale;
 	_accel_report.y = report.accel_y * _accel_range_scale;
 	_accel_report.z = report.accel_z * _accel_range_scale;
 
+	_gyro_report.x_raw = report.gyro_x;
+	_gyro_report.y_raw = report.gyro_y;
+	_gyro_report.z_raw = report.gyro_z;
+
 	_gyro_report.x = report.gyro_x * _gyro_range_scale;
 	_gyro_report.y = report.gyro_y * _gyro_range_scale;
 	_gyro_report.z = report.gyro_z * _gyro_range_scale;
@@ -816,17 +848,26 @@ int
 test()
 {
 	int fd = -1;
+	int fd_gyro = -1;
 	struct accel_report report;
+	struct gyro_report g_report;
 	ssize_t sz;
 	const char *reason = "test OK";
 
 	do {
-
 		/* get the driver */
 		fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
 
 		if (fd < 0) {
-			reason = "can't open driver, use <mpu6000 start> first";
+			reason = "can't open accel driver, use <mpu6000 start> first";
+			break;
+		}
+
+		/* get the driver */
+		fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
+
+		if (fd_gyro < 0) {
+			reason = "can't open gyro driver, use <mpu6000 start> first";
 			break;
 		}
 
@@ -834,17 +875,32 @@ test()
 		sz = read(fd, &report, sizeof(report));
 
 		if (sz != sizeof(report)) {
-			reason = "immediate read failed";
+			reason = "immediate acc read failed";
 			break;
 		}
 
 		printf("single read\n");
 		fflush(stdout);
-		printf("time:        %lld\n", report.timestamp);
-		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);
+		printf("time:     %lld\n", report.timestamp);
+		printf("acc  x:   %5.2f m/s^2\n", (double)report.x);
+		printf("acc  y:   %5.2f m/s^2\n", (double)report.y);
+		printf("acc  z:   %5.2f m/s^2\n", (double)report.z);
+		printf("acc range: %4.0f m/s^2 (%3.0f g)\n", (double)report.range_m_s2,
+			(double)(report.range_m_s2 / 9.81f));
+
+		/* do a simple demand read */
+		sz = read(fd, &g_report, sizeof(g_report));
+
+		if (sz != sizeof(g_report)) {
+			reason = "immediate gyro read failed";
+			break;
+		}
+
+		printf("gyro x:   %5.2f rad/s\n", (double)g_report.x);
+		printf("gyro y:   %5.2f rad/s\n", (double)g_report.y);
+		printf("gyro z:   %5.2f rad/s\n", (double)g_report.z);
+		printf("gyro range: %3.0f rad/s (%5.0f deg/s)\n", (double)g_report.range_rad_s,
+			(double)(g_report.range_rad_s / M_PI_F * 180.0f));
 
 	} while (0);
 
diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c
index 37a9e8913..9249e971e 100644
--- a/apps/sensors/sensors.c
+++ b/apps/sensors/sensors.c
@@ -54,11 +54,14 @@
 #include <float.h>
 
 #include <arch/board/up_hrt.h>
-#include <arch/board/drv_lis331.h>
 #include <arch/board/drv_bma180.h>
 #include <arch/board/drv_l3gd20.h>
-#include <arch/board/drv_hmc5883l.h>
+
 #include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_baro.h>
+
 #include <arch/board/up_adc.h>
 
 #include <systemlib/systemlib.h>
@@ -407,28 +410,28 @@ static int sensors_init(void)
 	printf("[sensors] Sensor configuration..\n");
 
 	/* open magnetometer */
-	fd_magnetometer = open("/dev/hmc5883l", O_RDONLY);
+	fd_magnetometer = open("/dev/mag", O_RDONLY);
 
 	if (fd_magnetometer < 0) {
-		fprintf(stderr, "[sensors]   HMC5883L open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
+		fprintf(stderr, "[sensors]   MAG 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 {
-		printf("[sensors]   HMC5883L open ok\n");
+		printf("[sensors]   MAG open ok\n");
 	}
 
 	/* open barometer */
-	fd_barometer = open("/dev/ms5611", O_RDONLY);
+	fd_barometer = open("/dev/baro", O_RDONLY);
 
 	if (fd_barometer < 0) {
-		fprintf(stderr, "[sensors]   MS5611 open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
+		fprintf(stderr, "[sensors]   BARO open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
 		fflush(stderr);
 
 	} else {
-		printf("[sensors]   MS5611 open ok\n");
+		printf("[sensors]   BARO open ok\n");
 	}
 
 	/* open gyro */
@@ -444,7 +447,7 @@ static int sensors_init(void)
 	int errno_accelerometer = (int)*get_errno_ptr();
 
 	if (!(fd_accelerometer < 0)) {
-		printf("[sensors]   Accelerometer open ok\n");
+		printf("[sensors]   ACCEL open ok\n");
 	}
 
 	/* only attempt to use BMA180 if MPU-6000 is not available */
@@ -454,7 +457,7 @@ static int sensors_init(void)
 		errno_bma180 = (int)*get_errno_ptr();
 
 		if (!(fd_bma180 < 0)) {
-			printf("[sensors]   Accelerometer (BMA180) open ok\n");
+			printf("[sensors]   ACCEL (BMA180) open ok\n");
 		}
 	} else {
 		fd_bma180 = -1;
@@ -464,7 +467,7 @@ static int sensors_init(void)
 	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));
+			fprintf(stderr, "[sensors]   ACCEL: MPU-6000: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer));
 			fflush(stderr);
 			/* this sensor is redundant with BMA180 */
 		}
@@ -483,7 +486,7 @@ static int sensors_init(void)
 	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));
+			fprintf(stderr, "[sensors]   GYRO: MPU-6000: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer));
 			fflush(stderr);
 			/* this sensor is redundant with BMA180 */
 		}
@@ -620,11 +623,13 @@ int sensors_thread_main(int argc, char *argv[])
 	ssize_t	ret_adc;
 	int 	nsamples_adc;
 
-	int16_t buf_gyro[3];
+	/* for PX4FMU 1.5 compatibility */
 	int16_t buf_accelerometer[3];
+
+	struct gyro_report buf_gyro;
 	struct accel_report buf_accel_report;
-	int16_t buf_magnetometer[7];
-	float	buf_barometer[3];
+	struct mag_report buf_magnetometer;
+	struct baro_report buf_barometer;
 
 	bool mag_calibration_enabled = false;
 
@@ -662,29 +667,28 @@ int sensors_thread_main(int argc, char *argv[])
 
 	/* Empty sensor buffers, avoid junk values */
 	/* Read first two values of each sensor into void */
-	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));
+	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));
 
 	struct sensor_combined_s raw = {
 		.timestamp = hrt_absolute_time(),
-		.gyro_raw = {buf_gyro[0], buf_gyro[1], buf_gyro[2]},
+		.gyro_raw = {buf_gyro.x_raw, buf_gyro.y_raw, buf_gyro.z_raw},
 		.gyro_raw_counter = 0,
-		.gyro_rad_s = {0, 0, 0},
-		.accelerometer_raw = {buf_accelerometer[0], buf_accelerometer[1], buf_accelerometer[2]},
+		.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},
 		.accelerometer_raw_counter = 0,
-		.accelerometer_m_s2 = {0, 0, 0},
-		.magnetometer_raw = {buf_magnetometer[0], buf_magnetometer[1], buf_magnetometer[2]},
+		.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},
 		.magnetometer_raw_counter = 0,
-		.baro_pres_mbar = 0,
-		.baro_alt_meter = 0,
-		.baro_temp_celcius = 0,
-		.battery_voltage_v = BAT_VOL_INITIAL,
-		.adc_voltage_v = {0, 0 , 0},
+		.baro_pres_mbar = buf_barometer.pressure,
+		.baro_alt_meter = buf_barometer.altitude,
+		.baro_temp_celcius = buf_barometer.temperature,
 		.baro_raw_counter = 0,
+		.battery_voltage_v = BAT_VOL_INITIAL,
+		.adc_voltage_v = {0, 0, 0},
 		.battery_voltage_counter = 0,
 		.battery_voltage_valid = false,
 	};
@@ -822,7 +826,7 @@ int sensors_thread_main(int argc, char *argv[])
 			if (fd_gyro > 0) {
 				/* try reading gyro */
 				uint64_t start_gyro = hrt_absolute_time();
-				ret_gyro = read(fd_gyro, buf_gyro, sizeof(buf_gyro));
+				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);
@@ -832,7 +836,7 @@ int sensors_thread_main(int argc, char *argv[])
 					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()));
+						fprintf(stderr, "[sensors] GYRO ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
 					}
 
 					if (gyro_healthy && gyro_fail_count >= GYRO_HEALTH_COUNTER_LIMIT_ERROR) {
@@ -859,6 +863,46 @@ int sensors_thread_main(int argc, char *argv[])
 				if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime);
 			}
 
+			// if (fd_gyro_l3gd20 > 0) {
+			// 	/* try reading gyro */
+			// 	uint64_t start_gyro = hrt_absolute_time();
+			// 	ret_gyro = read(fd_gyro, buf_gyro_l3gd20, sizeof(buf_gyro_l3gd20));
+			// 	int gyrotime = hrt_absolute_time() - start_gyro;
+
+			// 	if (gyrotime > 500) printf("L3GD20 GYRO (pure read): %d us\n", gyrotime);
+
+			// 	/* 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_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++;
+
+			// 		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;
+			// 	}
+
+			// 	gyrotime = hrt_absolute_time() - start_gyro;
+
+			// 	if (gyrotime > 500) printf("L3GD20 GYRO (complete): %d us\n", gyrotime);
+			// }
+
 			/* read MPU-6000 */
 			if (fd_accelerometer > 0) {
 				/* try reading acc */
@@ -869,7 +913,7 @@ int sensors_thread_main(int argc, char *argv[])
 				if (ret_accelerometer != sizeof(struct accel_report)) {
 					acc_fail_count++;
 
-					if ((acc_fail_count % 20) == 0 || (acc_fail_count > 20 && acc_fail_count < 100)) {
+					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()));
 					}
 
@@ -908,7 +952,7 @@ int sensors_thread_main(int argc, char *argv[])
 				if (ret_accelerometer != 6) {
 					acc_fail_count++;
 
-					if ((acc_fail_count % 20) == 0 || (acc_fail_count > 20 && acc_fail_count < 100)) {
+					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()));
 					}
 
@@ -939,31 +983,31 @@ int sensors_thread_main(int argc, char *argv[])
 			/* MAGNETOMETER */
 			if (magcounter == 4) { /* 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;
-				}
-
-				ret_magnetometer = read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer));
+				// /* 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;
+				// }
+
+				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("MAG (pure read): %d us\n", magtime);
+					printf("[sensors] WARN: MAG (pure read): %d us\n", magtime);
 				}
 
 				if (ret_magnetometer != sizeof(buf_magnetometer)) {
 					mag_fail_count++;
 
 
-					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()));
+					if ((mag_fail_count % 200) == 0 || (mag_fail_count > 20 && mag_fail_count < 40)) {
+						fprintf(stderr, "[sensors] MAG ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
 					}
 
 					if (magn_healthy && mag_fail_count >= MAGN_HEALTH_COUNTER_LIMIT_ERROR) {
@@ -987,8 +1031,8 @@ int sensors_thread_main(int argc, char *argv[])
 				magtime = hrt_absolute_time() - start_mag;
 
 				if (magtime > 2000) {
-					printf("MAG (overall time): %d us\n", magtime);
-					fprintf(stderr, "[sensors] TIMEOUT HMC5883L ERROR #%d: %s\n", errcode_mag, strerror(errcode_mag));
+					printf("[sensors] WARN: MAG (overall time): %d us\n", magtime);
+					fprintf(stderr, "[sensors] TIMEOUT MAG ERROR #%d: %s\n", errcode_mag, strerror(errcode_mag));
 				}
 
 				magcounter = 0;
@@ -1000,12 +1044,12 @@ int sensors_thread_main(int argc, char *argv[])
 			if (barocounter == 5 && (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));
+				ret_barometer = read(fd_barometer, &buf_barometer, sizeof(buf_barometer));
 
 				if (ret_barometer != sizeof(buf_barometer)) {
 					baro_fail_count++;
 
-					if (((baro_fail_count % 20) == 0 || (baro_fail_count > 20 && baro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
+					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()));
 					}
 
@@ -1133,20 +1177,39 @@ int sensors_thread_main(int argc, char *argv[])
 			/* Copy values of gyro, acc, magnetometer & barometer */
 
 			/* GYROSCOPE */
-			if (gyro_updated) {
+			// 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[1] == -32768) ? -32768 : buf_gyro[1]); // x of the board is y of the sensor
+				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[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
+				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] = (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_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++;
 			}
@@ -1159,14 +1222,13 @@ int sensors_thread_main(int argc, char *argv[])
 					/* MPU-6000 values */
 
 					/* scale from 14 bit to m/s2 */
-					raw.accelerometer_m_s2[0] = buf_accel_report.x - rcp.acc_offset[0] / 1000.0f;
-					raw.accelerometer_m_s2[1] = buf_accel_report.y - rcp.acc_offset[1] / 1000.0f;
-					raw.accelerometer_m_s2[2] = buf_accel_report.z - rcp.acc_offset[2] / 1000.0f;
+					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;
 
-					/* 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[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) {
@@ -1187,19 +1249,16 @@ int sensors_thread_main(int argc, char *argv[])
 					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
+				/* 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;
 
-					/* 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;
+					/* 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;
 
 					raw.gyro_raw_counter++;
 					/* mark as updated */
@@ -1212,17 +1271,17 @@ int sensors_thread_main(int argc, char *argv[])
 				/* 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[1] == -32768) ? 32767 : buf_magnetometer[1];  // x of the board is y of the sensor
-				raw.magnetometer_raw[1] = (buf_magnetometer[0] == -32768) ? 32767 : -buf_magnetometer[0]; // y on the board is -x of the sensor
-				raw.magnetometer_raw[2] = (buf_magnetometer[2] == -32768) ? -32768 : buf_magnetometer[2]; // z of the board is z of the sensor
+				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] = ((raw.magnetometer_raw[0] - rcp.mag_offset[0]) / 4096.0f) * 0.88f;
-				raw.magnetometer_ga[1] = ((raw.magnetometer_raw[1] - rcp.mag_offset[1]) / 4096.0f) * 0.88f;
-				raw.magnetometer_ga[2] = ((raw.magnetometer_raw[2] - rcp.mag_offset[2]) / 4096.0f) * 0.88f;
+				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 = buf_magnetometer[3];
+				raw.magnetometer_mode = 0;
 
 				raw.magnetometer_raw_counter++;
 			}
@@ -1231,9 +1290,9 @@ int sensors_thread_main(int argc, char *argv[])
 			if (baro_updated) {
 				/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
 
-				raw.baro_pres_mbar = buf_barometer[0]; // Pressure in mbar
-				raw.baro_alt_meter = buf_barometer[1]; // Altitude in meters
-				raw.baro_temp_celcius = buf_barometer[2]; // Temperature in degrees celcius
+				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++;
 			}