diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-01 08:11:55 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-01 08:11:55 +0100 |
| commit | 18dbdf0a54cdcfdd5b43f94263be75c01b4e7f65 (patch) | |
| tree | bf306a0ce97cec6457e5e0778d9975829073b967 /apps/drivers/hmc5883/hmc5883.cpp | |
| parent | 4d6ac93affe0e8f219d071baa5f8217016a1068d (diff) | |
| download | px4-firmware-18dbdf0a54cdcfdd5b43f94263be75c01b4e7f65.tar.gz px4-firmware-18dbdf0a54cdcfdd5b43f94263be75c01b4e7f65.tar.bz2 px4-firmware-18dbdf0a54cdcfdd5b43f94263be75c01b4e7f65.zip | |
Report mag status to system, sanity-check calibration
Diffstat (limited to 'apps/drivers/hmc5883/hmc5883.cpp')
| -rw-r--r-- | apps/drivers/hmc5883/hmc5883.cpp | 18 |
1 files changed, 14 insertions, 4 deletions
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 2b4fab151..81bc8954b 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -66,6 +66,9 @@ #include <drivers/drv_mag.h> #include <drivers/drv_hrt.h> +#include <uORB/uORB.h> +#include <uORB/topics/subsystem_info.h> + #include <float.h> /* @@ -631,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)); + (void)check_calibration(); return 0; case MAGIOCGSCALE: @@ -1039,11 +1043,17 @@ int HMC5883::check_calibration() offset_valid = false; } - if (_calibrated && !(offset_valid && scale_valid)) { - warnx("warning: mag %s%s", (scale_valid) ? "" : "scale invalid. ", + if (_calibrated != (offset_valid && scale_valid)) { + warnx("warning: mag cal changed: %s%s", (scale_valid) ? "" : "scale invalid. ", (offset_valid) ? "" : "offset invalid."); - _calibrated = false; - // XXX Notify system via uORB + _calibrated = (offset_valid && scale_valid); + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + _calibrated, + SUBSYSTEM_TYPE_MAG}; + orb_advert_t pub = orb_advertise(ORB_ID(subsystem_info), &info); } return 0; } |