Added preflight_check app which checks core system sensors, so far only mag

1 files changed, 14 insertions, 9 deletions

diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index a1587b783..8f35c484c 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/apps/drivers/hmc5883/hmc5883.cpp
@@ -647,6 +647,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);
@@ -1032,24 +1035,24 @@ int HMC5883::check_calibration()
 	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 {
+		/* 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 = {
@@ -1059,7 +1062,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)