Merge branch 'master' into seatbelt_multirotor_new

4 files changed, 188 insertions, 7 deletions

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 1ffca2f43..744abfa00 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -260,6 +260,13 @@ private:
 	 * @return		OK if the value can be supported.
 	 */
 	int			set_samplerate(unsigned frequency);
+
+	/**
+	 * Self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	 int 			self_test();
 };
 
 /* helper macro for handling report buffer indices */
@@ -519,6 +526,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case GYROIOCGRANGE:
 		return _current_range;
 
+	case GYROIOCSELFTEST:
+		return self_test();
+
 	default:
 		/* give it to the superclass */
 		return SPI::ioctl(filp, cmd, arg);
@@ -713,7 +723,8 @@ L3GD20::measure()
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
+	if (_gyro_topic > 0)
+		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
 
 	/* stop the perf counter */
 	perf_end(_sample_perf);
@@ -727,6 +738,28 @@ L3GD20::print_info()
 	       _num_reports, _oldest_report, _next_report, _reports);
 }
 
+int
+L3GD20::self_test()
+{
+	/* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */
+	if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f)
+		return 1;
+
+	if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f)
+		return 1;
+
+	if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f)
+		return 1;
+
+	return 0;
+}
+
 /**
  * Local functions in support of the shell command.
  */
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index c4e331a30..4f7075600 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -285,12 +285,26 @@ private:
 	uint16_t		swap16(uint16_t val) { return (val >> 8) | (val << 8);	}
 
 	/**
-	 * Self test
+	 * Measurement self test
 	 *
 	 * @return 0 on success, 1 on failure
 	 */
 	 int 			self_test();
 
+	/**
+	 * Accel self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int 			accel_self_test();
+
+	/**
+	 * Gyro self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	 int 			gyro_self_test();
+
 	/*
 	  set low pass filter frequency
 	 */
@@ -321,6 +335,7 @@ protected:
 	void			parent_poll_notify();
 private:
 	MPU6000			*_parent;
+
 };
 
 /** driver 'main' command */
@@ -653,6 +668,56 @@ MPU6000::self_test()
 	return (_reads > 0) ? 0 : 1;
 }
 
+int
+MPU6000::accel_self_test()
+{
+	if (self_test())
+		return 1;
+
+	/* inspect accel offsets */
+	if (fabsf(_accel_scale.x_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.y_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.z_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	return 0;
+}
+
+int
+MPU6000::gyro_self_test()
+{
+	if (self_test())
+		return 1;
+
+	/* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */
+	if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f)
+		return 1;
+
+	if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f)
+		return 1;
+
+	if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f)
+		return 1;
+
+	return 0;
+}
+
 ssize_t
 MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
 {
@@ -835,7 +900,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _accel_range_m_s2;
 
 	case ACCELIOCSELFTEST:
-		return self_test();
+		return accel_self_test();
 
 	default:
 		/* give it to the superclass */
@@ -918,7 +983,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _gyro_range_rad_s;
 
 	case GYROIOCSELFTEST:
-		return self_test();
+		return gyro_self_test();
 
 	default:
 		/* give it to the superclass */
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index b3a5d012b..b6217a414 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -230,6 +230,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 		if (orient < 0)
 			return ERROR;
 
+		if (data_collected[orient]) {
+			sprintf(str, "%s direction already measured, please rotate", orientation_strs[orient]);
+			mavlink_log_info(mavlink_fd, str);
+			continue;
+		}
+
 		sprintf(str, "meas started: %s", orientation_strs[orient]);
 		mavlink_log_info(mavlink_fd, str);
 		read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
@@ -391,6 +397,8 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 	int count = 0;
 	float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
 
+	int errcount = 0;
+
 	while (count < samples_num) {
 		int poll_ret = poll(fds, 1, 1000);
 		if (poll_ret == 1) {
@@ -400,8 +408,12 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 				accel_sum[i] += sensor.accelerometer_m_s2[i];
 			count++;
 		} else {
-			return ERROR;
+			errcount++;
+			continue;
 		}
+
+		if (errcount > samples_num / 10)
+			return ERROR;
 	}
 
 	for (int i = 0; i < 3; i++) {
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
index 2dad2261b..5a02fd620 100644
--- a/src/systemcmds/config/config.c
+++ b/src/systemcmds/config/config.c
@@ -129,7 +129,23 @@ do_gyro(int argc, char *argv[])
 				ioctl(fd, GYROIOCSRANGE, i);
 			}
 
-		} else if (!(argc > 0 && !strcmp(argv[0], "info"))) {
+		} else if (argc > 0) {
+
+			if(!strcmp(argv[0], "check")) {
+				int ret = ioctl(fd, GYROIOCSELFTEST, 0);
+
+				if (ret) {
+					warnx("gyro self test FAILED! Check calibration:");
+					struct gyro_scale scale;
+					ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
+					warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+					warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+				} else {
+					warnx("gyro calibration and self test OK");
+				}
+			}
+
+		} else {
 			warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t");
 		}
 
@@ -148,6 +164,45 @@ do_gyro(int argc, char *argv[])
 static void
 do_mag(int argc, char *argv[])
 {
+	int fd;
+
+	fd = open(MAG_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		warn("%s", MAG_DEVICE_PATH);
+		errx(1, "FATAL: no magnetometer found");
+
+	} else {
+
+		if (argc > 0) {
+
+			if (!strcmp(argv[0], "check")) {
+				int ret = ioctl(fd, MAGIOCSELFTEST, 0);
+
+				if (ret) {
+					warnx("mag self test FAILED! Check calibration.");
+					struct mag_scale scale;
+					ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
+					warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+					warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+				} else {
+					warnx("mag calibration and self test OK");
+				}
+			}
+
+		} else {
+			warnx("no arguments given. Try: \n\n\t'check' or 'info'\n\t");
+		}
+
+		int srate = -1;//ioctl(fd, MAGIOCGSAMPLERATE, 0);
+		int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
+		int range = -1;//ioctl(fd, MAGIOCGRANGE, 0);
+
+		warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range);
+
+		close(fd);
+	}
+
 	exit(0);
 }
 
@@ -183,7 +238,23 @@ do_accel(int argc, char *argv[])
 				/* set the range to i dps */
 				ioctl(fd, ACCELIOCSRANGE, i);
 			}
-		} else if (!(argc > 0 && !strcmp(argv[0], "info"))) {
+		} else if (argc > 0) {
+
+			if (!strcmp(argv[0], "check")) {
+				int ret = ioctl(fd, ACCELIOCSELFTEST, 0);
+
+				if (ret) {
+					warnx("accel self test FAILED! Check calibration.");
+					struct accel_scale scale;
+					ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
+					warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+					warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+				} else {
+					warnx("accel calibration and self test OK");
+				}
+			}
+
+		} else {
 			warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 2 G\n\t");
 		}