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

lsm303d: try to reset the lsm303d if it goes bad in flight 

this is based on earlier work by Julian Oes
This commit is contained in:
Andrew Tridgell 2013-10-28 15:50:33 +11:00 committed by Lorenz Meier
parent 75a0c18a9e
commit 52ee477137
1 changed files with 29 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/drivers/lsm303d/lsm303d.cpp
View File

@ -241,11 +241,18 @@ private:
perf_counter_t _accel_sample_perf; perf_counter_t _accel_sample_perf;
perf_counter_t _mag_sample_perf; perf_counter_t _mag_sample_perf;
perf_counter_t _reg7_resets;
perf_counter_t _reg1_resets;
math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y; math::LowPassFilter2p _accel_filter_y;
math::LowPassFilter2p _accel_filter_z; math::LowPassFilter2p _accel_filter_z;
// expceted values of reg1 and reg7 to catch in-flight
// brownouts of the sensor
uint8_t _reg7_expected;
uint8_t _reg1_expected;
/** /**
* Start automatic measurement. * Start automatic measurement.
*/ */
@ -434,9 +441,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
_mag_read(0), _mag_read(0),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
_reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")),
_reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")),
_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ) _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_reg1_expected(0),
_reg7_expected(0)
{ {
// enable debug() calls // enable debug() calls
_debug_enabled = true; _debug_enabled = true;
@ -526,10 +537,12 @@ void
LSM303D::reset() LSM303D::reset()
{ {
/* enable accel*/ /* enable accel*/
write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A;
write_reg(ADDR_CTRL_REG1, _reg1_expected);
/* enable mag */ /* enable mag */
write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); _reg7_expected = REG7_CONT_MODE_M;
write_reg(ADDR_CTRL_REG7, _reg7_expected);
write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
@ -1133,6 +1146,7 @@ LSM303D::accel_set_samplerate(unsigned frequency)
} }
modify_reg(ADDR_CTRL_REG1, clearbits, setbits); modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
_reg1_expected = (_reg1_expected & ~clearbits) | setbits;
return OK; return OK;
} }
@ -1210,6 +1224,12 @@ LSM303D::mag_measure_trampoline(void *arg)
void void
LSM303D::measure() LSM303D::measure()
{ {
if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
perf_count(_reg1_resets);
reset();
return;
}
/* status register and data as read back from the device */ /* status register and data as read back from the device */
#pragma pack(push, 1) #pragma pack(push, 1)
@ -1282,6 +1302,12 @@ LSM303D::measure()
void void
LSM303D::mag_measure() LSM303D::mag_measure()
{ {
if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) {
perf_count(_reg7_resets);
reset();
return;
}
/* status register and data as read back from the device */ /* status register and data as read back from the device */
#pragma pack(push, 1) #pragma pack(push, 1)
struct { struct {