Addes sensor self test commands

4 files changed, 38 insertions, 4 deletions

diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h
index 3834795e0..794de584b 100644
--- a/apps/drivers/drv_accel.h
+++ b/apps/drivers/drv_accel.h
@@ -115,4 +115,7 @@ ORB_DECLARE(sensor_accel);
 /** get the current accel measurement range in g */
 #define ACCELIOCGRANGE		_ACCELIOC(8)
 
+/** get the result of a sensor self-test */
+#define ACCELIOCSELFTEST	_ACCELIOC(9)
+
 #endif /* _DRV_ACCEL_H */
diff --git a/apps/drivers/drv_gyro.h b/apps/drivers/drv_gyro.h
index 0dbf05c5b..1d0fef6fc 100644
--- a/apps/drivers/drv_gyro.h
+++ b/apps/drivers/drv_gyro.h
@@ -111,4 +111,7 @@ ORB_DECLARE(sensor_gyro);
 /** get the current gyro measurement range in degrees per second */
 #define GYROIOCGRANGE		_GYROIOC(7)
 
+/** check the status of the sensor */
+#define GYROIOCSELFTEST		_GYROIOC(8)
+
 #endif /* _DRV_GYRO_H */
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index 8f35c484c..e44341639 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/apps/drivers/hmc5883/hmc5883.cpp
@@ -1032,9 +1032,9 @@ int HMC5883::check_calibration()
 {
 	bool scale_valid, offset_valid;
 
-	if ((-2.0f * FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < 2.0f * FLT_EPSILON + 1.0f) &&
-		(-2.0f * FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < 2.0f * FLT_EPSILON + 1.0f) &&
-		(-2.0f * FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < 2.0f * FLT_EPSILON + 1.0f)) {
+	if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) &&
+		(-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) &&
+		(-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) {
 		/* scale is one */
 		scale_valid = false;
 	} else {
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
index 90786886a..2ac71d89c 100644
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ b/apps/drivers/mpu6000/mpu6000.cpp
@@ -260,6 +260,13 @@ private:
 	 */
 	uint16_t		swap16(uint16_t val) { return (val >> 8) | (val << 8);	}
 
+	/**
+	 * Self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	 int 			self_test();
+
 };
 
 /**
@@ -494,6 +501,17 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
 	return ret;
 }
 
+int
+MPU6000::self_test()
+{
+	if (_reads == 0) {
+		measure();
+	}
+
+	/* return 0 on success, 1 else */
+	return (_reads > 0) ? 0 : 1;
+}
+
 ssize_t
 MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
 {
@@ -609,6 +627,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		// _accel_range_rad_s = 8.0f * 9.81f;
 		return -EINVAL;
 
+	case ACCELIOCSELFTEST:
+		return self_test();
+
 	default:
 		/* give it to the superclass */
 		return SPI::ioctl(filp, cmd, arg);
@@ -656,6 +677,9 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		// _gyro_range_m_s2 = xx
 		return -EINVAL;
 
+	case GYROIOCSELFTEST:
+		return self_test();
+
 	default:
 		/* give it to the superclass */
 		return SPI::ioctl(filp, cmd, arg);
@@ -813,7 +837,11 @@ MPU6000::measure()
 	 * Fetch the full set of measurements from the MPU6000 in one pass.
 	 */
 	mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
-	transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report));
+	if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)))
+		return;
+
+	/* count measurement */
+	_reads++;
 
 	/*
 	 * Convert from big to little endian