From df5e4d19042985bd567845dfa464170c169829b4 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 15 Nov 2012 17:19:21 +0100
Subject: [PATCH] Improved self-test logic

---
 apps/drivers/hmc5883/hmc5883.cpp |  4 +++-
 apps/drivers/mpu6000/mpu6000.cpp | 14 +++++++++++---
 2 files changed, 14 insertions(+), 4 deletions(-)

diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index e443416392..3849a2e052 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/apps/drivers/hmc5883/hmc5883.cpp
@@ -634,7 +634,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case MAGIOCSSCALE:
 		/* set new scale factors */
 		memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
-		return check_calibration();
+		/* check calibration, but not actually return an error */
+		(void)check_calibration();
+		return 0;
 
 	case MAGIOCGSCALE:
 		/* copy out scale factors */
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
index 2ac71d89ce..ed79440ccd 100644
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ b/apps/drivers/mpu6000/mpu6000.cpp
@@ -610,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 */