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

hmc5883: periodically check the config and range registers 

this copes with I2C comms errors causing the range or config registers
to become corrupted, leading to bad reading. This is easily
reproducible with a 1.3m I2C cable in the same run of cable as a GPS
UART cable. The error happens every half hour or so.

Conflicts:
	mavlink/include/mavlink/v1.0
	src/drivers/hmc5883/hmc5883.cpp
This commit is contained in:
Andrew Tridgell 2014-07-12 09:51:22 +10:00 committed by Lorenz Meier
parent 8a3a87331d
commit 005dd206d1
1 changed files with 114 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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.
@ -233,6 +238,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);
}
}