Merge remote-tracking branch 'upstream/master' into io

Fixed Conflicts:
	apps/multirotor_att_control/multirotor_att_control_main.c

rc loss failsafe throttle tested

6 files changed, 76 insertions, 20 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/drv_led.h b/apps/drivers/drv_led.h
index 7eb9e4b7c..21044f620 100644
--- a/apps/drivers/drv_led.h
+++ b/apps/drivers/drv_led.h
@@ -47,9 +47,9 @@
 #define _LED_BASE		0x2800
 
 /* PX4 LED colour codes */
-#define LED_AMBER		0
-#define LED_RED			0	/* some boards have red rather than amber */
-#define LED_BLUE		1
+#define LED_AMBER		1
+#define LED_RED			1	/* some boards have red rather than amber */
+#define LED_BLUE		0
 #define LED_SAFETY		2
 
 #define LED_ON			_IOC(_LED_BASE, 0)
diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h
index 114bcb646..9aab995a1 100644
--- a/apps/drivers/drv_mag.h
+++ b/apps/drivers/drv_mag.h
@@ -108,4 +108,7 @@ ORB_DECLARE(sensor_mag);
 /** excite strap */
 #define MAGIOCEXSTRAP		_MAGIOC(6)
 
+/** perform self test and report status */
+#define MAGIOCSELFTEST		_MAGIOC(7)
+
 #endif /* _DRV_MAG_H */
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index 81bc8954b..3849a2e05 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/apps/drivers/hmc5883/hmc5883.cpp
@@ -634,6 +634,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case MAGIOCSSCALE:
 		/* set new scale factors */
 		memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
+		/* check calibration, but not actually return an error */
 		(void)check_calibration();
 		return 0;
 
@@ -648,6 +649,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case MAGIOCEXSTRAP:
 		return set_excitement(arg);
 
+	case MAGIOCSELFTEST:
+		return check_calibration();
+
 	default:
 		/* give it to the superclass */
 		return I2C::ioctl(filp, cmd, arg);
@@ -1012,7 +1016,12 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 out:
 
 	if (ret == OK) {
-		warnx("mag scale calibration successfully finished.");
+		if (!check_calibration()) {
+			warnx("mag scale calibration successfully finished.");
+		} else {
+			warnx("mag scale calibration finished with invalid results.");
+			ret == ERROR;
+		}
 
 	} else {
 		warnx("mag scale calibration failed.");
@@ -1025,27 +1034,27 @@ 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)) {
-		/* scale is different from one */
-		scale_valid = true;
-	} else {
+	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 {
+		scale_valid = true;
 	}
 
 	if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) &&
 		(-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) &&
 		(-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) {
-		/* offset is different from zero */
-		offset_valid = true;
-	} else {
+		/* offset is zero */
 		offset_valid = false;
+	} else {
+		offset_valid = true;
 	}
 
 	if (_calibrated != (offset_valid && scale_valid)) {
-		warnx("warning: mag cal changed: %s%s", (scale_valid) ? "" : "scale invalid. ",
-					  (offset_valid) ? "" : "offset invalid.");
+		warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
+					  (offset_valid) ? "" : "offset invalid");
 		_calibrated = (offset_valid && scale_valid);
 		/* notify about state change */
 		struct subsystem_info_s info = {
@@ -1055,7 +1064,9 @@ int HMC5883::check_calibration()
 			SUBSYSTEM_TYPE_MAG};
 		orb_advert_t pub = orb_advertise(ORB_ID(subsystem_info), &info);
 	}
-	return 0;
+
+	/* return 0 if calibrated, 1 else */
+	return (!_calibrated);
 }
 
 int HMC5883::set_excitement(unsigned enable)
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
index 90786886a..ed79440cc 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)
 {
@@ -592,9 +610,17 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return -EINVAL;
 
 	case ACCELIOCSSCALE:
-		/* copy scale in */
-		memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale));
-		return OK;
+		{
+			/* copy scale, but only if off by a few percent */
+			struct accel_scale *s = (struct accel_scale *) arg;
+			float sum = s->x_scale + s->y_scale + s->z_scale;
+			if (sum > 2.0f && sum < 4.0f) {
+				memcpy(&_accel_scale, s, sizeof(_accel_scale));
+				return OK;
+			} else {
+				return -EINVAL;
+			}
+		}
 
 	case ACCELIOCGSCALE:
 		/* copy scale out */
@@ -609,6 +635,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 +685,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 +845,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