diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/hmc5883/hmc5883.cpp | 141 |
1 files changed, 114 insertions, 27 deletions
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index cd2b5c48e..4aef43102 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -166,6 +166,8 @@ private: perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + perf_counter_t _range_errors; + perf_counter_t _conf_errors; /* status reporting */ bool _sensor_ok; /**< sensor was found and reports ok */ @@ -176,6 +178,9 @@ private: struct mag_report _last_report; /**< used for info() */ + uint8_t _range_bits; + uint8_t _conf_reg; + /** * Test whether the device supported by the driver is present at a * specific address. @@ -234,6 +239,23 @@ private: int set_range(unsigned range); /** + * check the sensor range. + * + * checks that the range of the sensor is correctly set, to + * cope with communication errors causing the range to change + */ + void check_range(void); + + /** + * check the sensor configuration. + * + * checks that the config of the sensor is correctly set, to + * cope with communication errors causing the configuration to + * change + */ + void check_conf(void); + + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. * @@ -336,10 +358,15 @@ HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : _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")), + _range_errors(perf_alloc(PC_COUNT, "hmc5883_range_errors")), + _conf_errors(perf_alloc(PC_COUNT, "hmc5883_conf_errors")), _sensor_ok(false), _calibrated(false), _bus(bus), - _rotation(rotation) + _rotation(rotation), + _last_report{0}, + _range_bits(0), + _conf_reg(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; @@ -373,6 +400,8 @@ HMC5883::~HMC5883() perf_free(_sample_perf); perf_free(_comms_errors); perf_free(_buffer_overflows); + perf_free(_range_errors); + perf_free(_conf_errors); } int @@ -403,45 +432,43 @@ out: int HMC5883::set_range(unsigned range) { - uint8_t range_bits; - if (range < 1) { - range_bits = 0x00; + _range_bits = 0x00; _range_scale = 1.0f / 1370.0f; _range_ga = 0.88f; } else if (range <= 1) { - range_bits = 0x01; + _range_bits = 0x01; _range_scale = 1.0f / 1090.0f; _range_ga = 1.3f; } else if (range <= 2) { - range_bits = 0x02; + _range_bits = 0x02; _range_scale = 1.0f / 820.0f; _range_ga = 1.9f; } else if (range <= 3) { - range_bits = 0x03; + _range_bits = 0x03; _range_scale = 1.0f / 660.0f; _range_ga = 2.5f; } else if (range <= 4) { - range_bits = 0x04; + _range_bits = 0x04; _range_scale = 1.0f / 440.0f; _range_ga = 4.0f; } else if (range <= 4.7f) { - range_bits = 0x05; + _range_bits = 0x05; _range_scale = 1.0f / 390.0f; _range_ga = 4.7f; } else if (range <= 5.6f) { - range_bits = 0x06; + _range_bits = 0x06; _range_scale = 1.0f / 330.0f; _range_ga = 5.6f; } else { - range_bits = 0x07; + _range_bits = 0x07; _range_scale = 1.0f / 230.0f; _range_ga = 8.1f; } @@ -451,7 +478,7 @@ int HMC5883::set_range(unsigned range) /* * Send the command to set the range */ - ret = write_reg(ADDR_CONF_B, (range_bits << 5)); + ret = write_reg(ADDR_CONF_B, (_range_bits << 5)); if (OK != ret) perf_count(_comms_errors); @@ -462,7 +489,53 @@ int HMC5883::set_range(unsigned range) if (OK != ret) perf_count(_comms_errors); - return !(range_bits_in == (range_bits << 5)); + return !(range_bits_in == (_range_bits << 5)); +} + +/** + check that the range register has the right value. This is done + periodically to cope with I2C bus noise causing the range of the + compass changing. + */ +void HMC5883::check_range(void) +{ + int ret; + + uint8_t range_bits_in; + ret = read_reg(ADDR_CONF_B, range_bits_in); + if (OK != ret) { + perf_count(_comms_errors); + return; + } + if (range_bits_in != (_range_bits<<5)) { + perf_count(_range_errors); + ret = write_reg(ADDR_CONF_B, (_range_bits << 5)); + if (OK != ret) + perf_count(_comms_errors); + } +} + +/** + check that the configuration register has the right value. This is + done periodically to cope with I2C bus noise causing the + configuration of the compass to change. + */ +void HMC5883::check_conf(void) +{ + int ret; + + uint8_t conf_reg_in; + ret = read_reg(ADDR_CONF_A, conf_reg_in); + if (OK != ret) { + perf_count(_comms_errors); + return; + } + if (conf_reg_in != _conf_reg) { + perf_count(_conf_errors); + ret = write_reg(ADDR_CONF_A, _conf_reg); + if (OK != ret) + perf_count(_comms_errors); + } } int @@ -796,7 +869,7 @@ HMC5883::collect() } report; int ret = -EIO; uint8_t cmd; - + uint8_t check_counter; perf_begin(_sample_perf); struct mag_report new_report; @@ -895,6 +968,21 @@ HMC5883::collect() /* notify anyone waiting for data */ poll_notify(POLLIN); + /* + periodically check the range register and configuration + registers. With a bad I2C cable it is possible for the + registers to become corrupt, leading to bad readings. It + doesn't happen often, but given the poor cables some + vehicles have it is worth checking for. + */ + check_counter = perf_event_count(_sample_perf) % 256; + if (check_counter == 0) { + check_range(); + } + if (check_counter == 128) { + check_conf(); + } + ret = OK; out: @@ -1168,25 +1256,24 @@ int HMC5883::set_excitement(unsigned enable) { int ret; /* arm the excitement strap */ - uint8_t conf_reg; - ret = read_reg(ADDR_CONF_A, conf_reg); + ret = read_reg(ADDR_CONF_A, _conf_reg); if (OK != ret) perf_count(_comms_errors); if (((int)enable) < 0) { - conf_reg |= 0x01; + _conf_reg |= 0x01; } else if (enable > 0) { - conf_reg |= 0x02; + _conf_reg |= 0x02; } else { - conf_reg &= ~0x03; + _conf_reg &= ~0x03; } - // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg); + // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg); - ret = write_reg(ADDR_CONF_A, conf_reg); + ret = write_reg(ADDR_CONF_A, _conf_reg); if (OK != ret) perf_count(_comms_errors); @@ -1196,7 +1283,7 @@ int HMC5883::set_excitement(unsigned enable) //print_info(); - return !(conf_reg == conf_reg_ret); + return !(_conf_reg == conf_reg_ret); } int @@ -1257,12 +1344,12 @@ const int ERROR = -1; HMC5883 *g_dev_int; HMC5883 *g_dev_ext; -void hmc5883_usage(); void start(int bus, enum Rotation rotation); void test(int bus); void reset(int bus); void info(int bus); int calibrate(int bus); +void usage(); /** * Start the driver. @@ -1511,10 +1598,8 @@ info(int bus) exit(0); } -} // namespace - void -hmc5883_usage() +usage() { warnx("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'"); warnx("options:"); @@ -1526,6 +1611,8 @@ hmc5883_usage() #endif } +} // namespace + int hmc5883_main(int argc, char *argv[]) { @@ -1551,7 +1638,7 @@ hmc5883_main(int argc, char *argv[]) calibrate = true; break; default: - hmc5883_usage(); + hmc5883::usage(); exit(0); } } |