From f803540415f8b4dc7baef98b4a0270d68a33cdcd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Nov 2012 13:21:00 +0100 Subject: Added preflight_check app which checks core system sensors, so far only mag --- apps/drivers/drv_mag.h | 3 +++ apps/drivers/hmc5883/hmc5883.cpp | 23 ++++++++++++++--------- nuttx/configs/px4fmu/nsh/appconfig | 1 + 3 files changed, 18 insertions(+), 9 deletions(-) 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 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) diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 798f57e93..b03ec1eb3 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -54,6 +54,7 @@ CONFIGURED_APPS += systemcmds/mixer CONFIGURED_APPS += systemcmds/eeprom CONFIGURED_APPS += systemcmds/param CONFIGURED_APPS += systemcmds/bl_update +CONFIGURED_APPS += systemcmds/preflight_check #CONFIGURED_APPS += systemcmds/calibration # Tutorial code from -- cgit v1.2.3