From 642f3426a7aadd9fd345590a4c0881d7e64014a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Oct 2012 08:52:17 +0200 Subject: Added mag calibration routine, fixed minor typos without runtime effects --- apps/drivers/hmc5883/hmc5883.cpp | 394 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 388 insertions(+), 6 deletions(-) (limited to 'apps/drivers/hmc5883/hmc5883.cpp') diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index c0a5f4049..893119407 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -175,6 +175,24 @@ private: */ void stop(); + /** + * Perform the on-sensor scale calibration routine. + * + * @note The sensor will continue to provide measurements, these + * will however reflect the uncalibrated sensor state until + * the calibration routine has been completed. + * + * @param enable set to 1 to enable self-test strap, 0 to disable + */ + int calibrate(unsigned enable); + + /** + * Set the sensor range. + * + * Sets the internal range to handle at least the argument in Gauss. + */ + int set_range(unsigned range); + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. @@ -255,8 +273,8 @@ HMC5883::HMC5883(int bus) : _oldest_report(0), _reports(nullptr), _mag_topic(-1), - _range_scale(1.0f / 1090.0f), /* default range scale from counts to gauss */ - _range_ga(0.88f), + _range_scale(0), /* default range scale from counts to gauss */ + _range_ga(1.3f), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")) @@ -308,11 +326,71 @@ HMC5883::init() if (_mag_topic < 0) debug("failed to create sensor_mag object"); + /* set range */ + set_range(_range_ga); + ret = OK; out: return ret; } +int HMC5883::set_range(unsigned range) +{ + uint8_t range_bits; + + if (range < 1) { + range_bits = 0x00; + _range_scale = 1.0f / 1370.0f; + _range_ga = 0.88f; + } else if (range <= 1) { + range_bits = 0x01; + _range_scale = 1.0f / 1090.0f; + _range_ga = 1.3f; + } else if (range <= 2) { + range_bits = 0x02; + _range_scale = 1.0f / 820.0f; + _range_ga = 1.9f; + } else if (range <= 3) { + range_bits = 0x03; + _range_scale = 1.0f / 660.0f; + _range_ga = 2.5f; + } else if (range <= 4) { + range_bits = 0x04; + _range_scale = 1.0f / 440.0f; + _range_ga = 4.0f; + } else if (range <= 4.7f) { + range_bits = 0x05; + _range_scale = 1.0f / 390.0f; + _range_ga = 4.7f; + } else if (range <= 5.6f) { + range_bits = 0x06; + _range_scale = 1.0f / 330.0f; + _range_ga = 5.6f; + } else { + range_bits = 0x07; + _range_scale = 1.0f / 230.0f; + _range_ga = 8.1f; + } + + int ret; + + /* + * Send the command to set the range + */ + ret = write_reg(ADDR_CONF_B, (range_bits << 5)); + + if (OK != ret) + perf_count(_comms_errors); + + uint8_t range_bits_in; + ret = read_reg(ADDR_CONF_B, range_bits_in); + + if (OK != ret) + perf_count(_comms_errors); + + return !(range_bits_in == (range_bits << 5)); +} + int HMC5883::probe() { @@ -495,6 +573,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) /* not supported, always 1 sample per poll */ return -EINVAL; + case MAGIOCSRANGE: + return set_range(arg); + case MAGIOCSLOWPASS: /* not supported, no internal filtering */ return -EINVAL; @@ -510,8 +591,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return 0; case MAGIOCCALIBRATE: - /* XXX perform auto-calibration */ - return -EINVAL; + return calibrate(arg); default: /* give it to the superclass */ @@ -718,6 +798,29 @@ out: return ret; } +int HMC5883::calibrate(unsigned enable) +{ + int ret; + /* arm the excitement strap */ + uint8_t conf_reg; + ret = read_reg(ADDR_CONF_A, conf_reg); + if (OK != ret) + perf_count(_comms_errors); + if (enable) { + conf_reg |= 0x01; + } else { + conf_reg &= ~0x03; + } + ret = write_reg(ADDR_CONF_A, conf_reg); + if (OK != ret) + perf_count(_comms_errors); + + uint8_t conf_reg_ret; + read_reg(ADDR_CONF_A, conf_reg_ret); + + return !(conf_reg == conf_reg_ret); +} + int HMC5883::write_reg(uint8_t reg, uint8_t val) { @@ -775,6 +878,7 @@ void start(); void test(); void reset(); void info(); +int calibrate(); /** * Start the driver. @@ -872,6 +976,273 @@ test() errx(0, "PASS"); } + +/** + * Automatic scale calibration. + * + * Basic idea: + * + * output = (ext field +- 1.1 Ga self-test) * scale factor + * + * and consequently: + * + * 1.1 Ga = (excited - normal) * scale factor + * scale factor = (excited - normal) / 1.1 Ga + * + * sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3 + * sz = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3 + * + * By subtracting the non-excited measurement the pure 1.1 Ga reading + * can be extracted and the sensitivity of all axes can be matched. + * + * SELF TEST OPERATION + * To check the HMC5883L for proper operation, a self test feature in incorporated + * in which the sensor offset straps are excited to create a nominal field strength + * (bias field) to be measured. To implement self test, the least significant bits + * (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias) + * or 10 (negetive bias), e.g. 0x11 or 0x12. + * Then, by placing the mode register into single-measurement mode (0x01), + * two data acquisition cycles will be made on each magnetic vector. + * The first acquisition will be a set pulse followed shortly by measurement + * data of the external field. The second acquisition will have the offset strap + * excited (about 10 mA) in the positive bias mode for X, Y, and Z axes to create + * about a ±1.1 gauss self test field plus the external field. The first acquisition + * values will be subtracted from the second acquisition, and the net measurement + * will be placed into the data output registers. + * Since self test adds ~1.1 Gauss additional field to the existing field strength, + * using a reduced gain setting prevents sensor from being saturated and data registers + * overflowed. For example, if the configuration register B is set to 0x60 (Gain=3), + * values around +766 LSB (1.16 Ga * 660 LSB/Ga) will be placed in the X and Y data + * output registers and around +713 (1.08 Ga * 660 LSB/Ga) will be placed in Z data + * output register. To leave the self test mode, change MS1 and MS0 bit of the + * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10. + * Using the self test method described above, the user can scale sensor + */ +int calibrate() +{ + + struct mag_report report; + ssize_t sz; + int ret; + + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + if (fd < 0) + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + if (sz != sizeof(report)) + err(1, "immediate read failed"); + + warnx("single read"); + warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + warnx("time: %lld", report.timestamp); + + /* set the queue depth to 10 */ + if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) + errx(1, "failed to set queue depth"); + + /* start the sensor polling at 10 Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) + errx(1, "failed to set 2Hz poll rate"); + + /* Set to 2.5 Gauss */ + if (OK != ioctl(fd, MAGIOCSRANGE, 2)) { + warnx("failed to set 2.5 Ga range"); + } + + if (OK != ioctl(fd, MAGIOCCALIBRATE, 1)) { + warnx("failed to enable sensor calibration mode"); + } + + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + warn("WARNING: failed to set null scale / offsets for mag"); + } + + float avg_excited[3]; + unsigned i; + + /* read the sensor 10x and report each value */ + for (i = 0; i < 10; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } else { + avg_excited[0] += report.x; + avg_excited[1] += report.y; + avg_excited[2] += report.z; + } + + warnx("periodic read %u", i); + warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + warnx("time: %lld", report.timestamp); + } + + // warnx("starting calibration"); + + // struct mag_report report; + // ssize_t sz; + // int ret; + + // int fd = open(MAG_DEVICE_PATH, O_RDONLY); + // if (fd < 0) + // err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + + // /* do a simple demand read */ + // sz = read(fd, &report, sizeof(report)); + // if (sz != sizeof(report)) + // err(1, "immediate read failed"); + + // warnx("single read"); + // warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + // warnx("time: %lld", report.timestamp); + + // /* get scaling, set to zero */ + // struct mag_scale mscale_previous; + + // if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) { + // warn("WARNING: failed to get scale / offsets for mag"); + // } + + // struct mag_scale mscale_null = { + // 0.0f, + // 1.0f, + // 0.0f, + // 1.0f, + // 0.0f, + // 1.0f, + // }; + + // if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + // warn("WARNING: failed to set null scale / offsets for mag"); + // } + + // warnx("sensor ready"); + + // float avg_excited[3] = {0.0f, 0.0f, 0.0f}; + + // if (OK != ioctl(fd, MAGIOCCALIBRATE, 1)) { + // warnx("failed to enable sensor calibration mode"); + // } + + // /* Set to 2.5 Gauss */ + // if (OK != ioctl(fd, MAGIOCSRANGE, 2)) { + // warnx("failed to set 2.5 Ga range"); + // } + + // /* set the queue depth to 10 */ + // if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { + // warnx("failed to set queue depth"); + // return 1; + // } else { + // warnx("set queue depth"); + // } + + // /* start the sensor polling at 100Hz */ + // if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 100)) { + // warnx("failed to set 100 Hz poll rate"); + // return 1; + // } else { + // warnx("set 100 Hz poll rate"); + // } + + // int i; + // for (i = 0; i < 10; i++) { + // struct pollfd fds; + + // (void) ioctl(fd, MAGIOCCALIBRATE, 1); + + // /* wait for data to be ready */ + // fds.fd = fd; + // fds.events = POLLIN; + // ret = poll(&fds, 1, 2000); + + // if (ret != 1) { + // warnx("timed out waiting for sensor data"); + // return 1; + // } + + // /* now go get it */ + // sz = read(fd, &report, sizeof(report)); + + // if (sz != sizeof(report)) { + // warn("periodic read failed"); + // return 1; + // } else { + // avg_excited[0] += report.x; + // avg_excited[1] += report.y; + // avg_excited[2] += report.z; + // } + // warnx("excited read %u", i); + // warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + // warnx("time: %lld", report.timestamp); + + // } + + avg_excited[0] /= i; + avg_excited[1] /= i; + avg_excited[2] /= i; + + warnx("periodic excited reads %u", i); + warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]); + + /* Set to 1.1 Gauss and end calibration */ + ret = ioctl(fd, MAGIOCCALIBRATE, 0); + ret = ioctl(fd, MAGIOCSRANGE, 1); + + float scaling[3]; + + /* calculate axis scaling */ + scaling[0] = 1.16f / avg_excited[0]; + /* second axis inverted */ + scaling[1] = 1.16f / -avg_excited[1]; + scaling[2] = 1.08f / avg_excited[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"); + } + + if (OK != ioctl(fd, MAGIOCCALIBRATE, 0)) { + warnx("failed to disable sensor calibration mode"); + } + + /* set scaling in device */ + // mscale_previous.x_scale = scaling[0]; + // mscale_previous.y_scale = scaling[1]; + // mscale_previous.z_scale = scaling[2]; + + // if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { + // warn("WARNING: failed to set new scale / offsets for mag"); + // } + + errx(0, "PASS"); +} + /** * Reset the driver. */ @@ -930,8 +1301,19 @@ hmc5883_main(int argc, char *argv[]) /* * Print driver information. */ - if (!strcmp(argv[1], "info")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) hmc5883::info(); - errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); + /* + * Autocalibrate the scaling + */ + if (!strcmp(argv[1], "calibrate")) { + if (hmc5883::calibrate() == 0) { + errx(0, "calibration successful"); + } else { + errx(1, "calibration failed"); + } + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate' or 'info'"); } -- cgit v1.2.3