diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-15 13:21:00 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-15 13:21:00 +0100 |
| commit | f803540415f8b4dc7baef98b4a0270d68a33cdcd (patch) | |
| tree | 7580c2c86fdfe05f9994be9e503a56687423c571 /apps | |
| parent | c4bf3ea3ed81acdce9f972534c56e2dc68135d19 (diff) | |
| download | px4-firmware-f803540415f8b4dc7baef98b4a0270d68a33cdcd.tar.gz px4-firmware-f803540415f8b4dc7baef98b4a0270d68a33cdcd.tar.bz2 px4-firmware-f803540415f8b4dc7baef98b4a0270d68a33cdcd.zip | |
Added preflight_check app which checks core system sensors, so far only mag
Diffstat (limited to 'apps')
| -rw-r--r-- | apps/drivers/drv_mag.h | 3 | ||||
| -rw-r--r-- | apps/drivers/hmc5883/hmc5883.cpp | 23 |
2 files changed, 17 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) |