dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

hmc5883: properly reset mag to normal state on calibration fail 

and add current output in "hmc5883 info"
This commit is contained in:
Andrew Tridgell 2014-03-11 12:42:46 +11:00 committed by Lorenz Meier
parent ec2197fd1b
commit 30d1ce3a51
1 changed files with 19 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
src/drivers/hmc5883/hmc5883.cpp
View File

@ -169,6 +169,8 @@ private:
int _bus; /**< the bus the device is connected to */ int _bus; /**< the bus the device is connected to */
struct mag_report _last_report; /**< used for info() */
/** /**
* Test whether the device supported by the driver is present at a * Test whether the device supported by the driver is present at a
* specific address. * specific address.
@ -870,6 +872,8 @@ HMC5883::collect()
} }
} }
_last_report = new_report;
/* post a report to the ring */ /* post a report to the ring */
if (_reports->force(&new_report)) { if (_reports->force(&new_report)) {
perf_count(_buffer_overflows); perf_count(_buffer_overflows);
@ -1042,32 +1046,29 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
/* set back to normal mode */
/* Set to 1.1 Gauss */
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
warnx("failed to set 1.1 Ga range");
goto out;
}
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
warnx("failed to disable sensor calibration mode");
goto out;
}
/* set scaling in device */ /* set scaling in device */
mscale_previous.x_scale = scaling[0]; mscale_previous.x_scale = scaling[0];
mscale_previous.y_scale = scaling[1]; mscale_previous.y_scale = scaling[1];
mscale_previous.z_scale = scaling[2]; mscale_previous.z_scale = scaling[2];
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to set new scale / offsets for mag");
goto out;
}
ret = OK; ret = OK;
out: out:
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to set new scale / offsets for mag");
}
/* set back to normal mode */
/* Set to 1.1 Gauss */
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
warnx("failed to set 1.1 Ga range");
}
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
warnx("failed to disable sensor calibration mode");
}
if (ret == OK) { if (ret == OK) {
if (!check_scale()) { if (!check_scale()) {
warnx("mag scale calibration successfully finished."); warnx("mag scale calibration successfully finished.");
@ -1221,6 +1222,7 @@ HMC5883::print_info()
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows); perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks); printf("poll interval: %u ticks\n", _measure_ticks);
printf("output (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z);
printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset);
printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n",
(double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,