fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 08:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Report mag status to system, sanity-check calibration

Browse Source
This commit is contained in:
Lorenz Meier
2012-11-01 08:11:55 +01:00
parent 4d6ac93aff
commit 18dbdf0a54
1 changed files with 14 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/hmc5883/hmc5883.cpp
+14 -4
View File
@@ -66,6 +66,9 @@
#include <drivers/drv_mag.h> #include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <float.h> #include <float.h>
/* /*
@@ -631,6 +634,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
case MAGIOCSSCALE: case MAGIOCSSCALE:
/* set new scale factors */ /* set new scale factors */
memcpy(&_scale, (mag_scale *)arg, sizeof(_scale)); memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
(void)check_calibration();
return 0; return 0;
case MAGIOCGSCALE: case MAGIOCGSCALE:
@@ -1039,11 +1043,17 @@ int HMC5883::check_calibration()
offset_valid = false; offset_valid = false;
} }
if (_calibrated && !(offset_valid && scale_valid)) { if (_calibrated != (offset_valid && scale_valid)) {
warnx("warning: mag %s%s", (scale_valid) ? "" : "scale invalid. ", warnx("warning: mag cal changed: %s%s", (scale_valid) ? "" : "scale invalid. ",
(offset_valid) ? "" : "offset invalid."); (offset_valid) ? "" : "offset invalid.");
_calibrated = false; _calibrated = (offset_valid && scale_valid);
// XXX Notify system via uORB /* 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; return 0;
} }