Working towards full sensor flexibility

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("x:         %5.2f\n", (double)report.x);
-		printf("y:           %f\n", report.y);
+		printf("y:         %5.2f\n", (double)report.y);
-		printf("z:           %f\n", report.z);
+		printf("z:         %5.2f\n", (double)report.z);
+		printf("test:         %8.4f\n", 1.5);
 
 	} while (0);
 

apps/sensors/Makefile

@@ -37,6 +37,6 @@
 
 APPNAME		= sensors
 PRIORITY	= SCHED_PRIORITY_MAX-5
-STACKSIZE	= 2048
+STACKSIZE	= 3096
 
 include $(APPDIR)/mk/app.mk

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>
@@ -106,10 +107,11 @@ 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_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);
@@ -169,30 +171,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) {
+	if (!(fd_gyro < 0)) {
-		fprintf(stderr, "[sensors]   L3GD20 open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
+		printf("[sensors]   L3GD20 open ok\n");
-		fflush(stderr);
+	}
-		/* this sensor is critical, exit on failed init */
+
-		errno = ENOSYS;
+	/* open accelerometer, prefer the MPU-6000 */
-		return ERROR;
+	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) {
+		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 {
-		printf("[sensors]   L3GD20 open ok\n");
+		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 */
 		}
 
-	/* open accelerometer */
+		errno = ENOSYS;
-	fd_accelerometer = open("/dev/bma180", O_RDONLY);
+		return ERROR;
+	}
 
+	/* 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]   BMA180: open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
+			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;
-
-	} else {
-		printf("[sensors]   BMA180 open ok\n");
 	}
 
 	/* open adc */
@@ -209,7 +251,8 @@ static int sensors_init(void)
 		printf("[sensors]   ADC open ok\n");
 	}
 
-	/* configure gyro */
+	/* 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);
@@ -220,6 +263,7 @@ static int sensors_init(void)
 		} else {
 			printf("[sensors]   L3GD20 configuration ok\n");
 		}
+	}
 
 	/* XXX Add IOCTL configuration of remaining sensors */
 
@@ -255,7 +299,7 @@ int sensors_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);
@@ -311,6 +355,7 @@ int sensors_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];
 
@@ -354,8 +399,9 @@ int sensors_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));
+	if (fd_gyro > 0)(void)read(fd_gyro, buf_gyro, sizeof(buf_gyro));
-	(void)read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer));
+	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));
@@ -455,8 +501,12 @@ int sensors_main(int argc, char *argv[])
 				if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) {
 					hil_enabled = true;
 					publishing = false;
-					int ret = close(sensor_pub);
+					int sens_ret = close(sensor_pub);
-					printf("[sensors] Closing sensor pub: %i \n", ret);
+					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 */
 
@@ -508,6 +558,7 @@ int sensors_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));
@@ -545,17 +596,20 @@ int sensors_main(int argc, char *argv[])
 				gyrotime = hrt_absolute_time() - start_gyro;
 
 				if (gyrotime > 500) printf("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_accelerometer, sizeof(buf_accelerometer));
+				ret_accelerometer = read(fd_accelerometer, &buf_accel_report, sizeof(struct accel_report));
 
 				/* ACCELEROMETER */
-			if (ret_accelerometer != sizeof(buf_accelerometer)) {
+				if (ret_accelerometer != sizeof(struct accel_report)) {
 					acc_fail_count++;
 
-				if (acc_fail_count & 0b1000 || (acc_fail_count > 20 && acc_fail_count < 100)) {
+					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()));
+						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) {
@@ -579,8 +633,46 @@ int sensors_main(int argc, char *argv[])
 				}
 
 				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);
+
+				/* 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 */
@@ -607,7 +699,7 @@ int sensors_main(int argc, char *argv[])
 				if (ret_magnetometer != sizeof(buf_magnetometer)) {
 					mag_fail_count++;
 
-					if (mag_fail_count & 0b1000 || (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()));
 					}
 
@@ -650,7 +742,7 @@ int sensors_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()));
 					}
 
@@ -687,10 +779,10 @@ int sensors_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()));
 					}
 
@@ -730,7 +822,7 @@ int sensors_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;
@@ -759,9 +851,9 @@ int sensors_main(int argc, char *argv[])
 					if (manual_control.yaw >  1.0f) manual_control.yaw =  1.0f;
 					
 					/* throttle input */
-					manual_control.throttle = rc.chan[rc.function[THROTTLE]].scaled;
+					manual_control.throttle = rc.chan[rc.function[THROTTLE]].scaled/2.0f;
 					if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
-					if (manual_control.throttle > 2.0f) manual_control.throttle = 2.0f;
+					if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
 
 					/* mode switch input */
 					manual_control.override_mode_switch = rc.chan[rc.function[OVERRIDE]].scaled;
@@ -800,11 +892,28 @@ int sensors_main(int argc, char *argv[])
 			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;
+					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 */
@@ -815,6 +924,26 @@ int sensors_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
+
+					/* 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 */
 			if (magn_updated) {
 				/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
@@ -912,7 +1041,7 @@ int sensors_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);

nuttx/configs/px4fmu/src/up_nsh.c

@@ -236,19 +236,6 @@ int nsh_archinitialize(void)
 
   if (acc_fail) message("[boot] FAILED to attach BMA180 accelerometer\r\n");
 
-  int mpu_attempts = 0;
-  int mpu_fail = 0;
-
-  while (mpu_attempts < 1)
-  {
-	  mpu_fail = mpu6000_attach(spi1, PX4_SPIDEV_MPU);
-	  mpu_attempts++;
-	  if (mpu_fail == 0) break;
-	  up_udelay(200);
-  }
-
-  if (mpu_fail) message("[boot] FAILED to attach MPU 6000 gyro/acc\r\n");
-
   /* initialize I2C2 bus */
 
   i2c2 = up_i2cinitialize(2);