aboutsummaryrefslogtreecommitdiff
path: root/src/drivers/l3gd20/l3gd20.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers/l3gd20/l3gd20.cpp')
-rw-r--r--src/drivers/l3gd20/l3gd20.cpp196
1 files changed, 170 insertions, 26 deletions
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 82fa5cc6e..3a0c05c9e 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -142,6 +142,7 @@ static const int ERROR = -1;
#define ADDR_INT1_TSH_ZH 0x36
#define ADDR_INT1_TSH_ZL 0x37
#define ADDR_INT1_DURATION 0x38
+#define ADDR_LOW_ODR 0x39
/* Internal configuration values */
@@ -200,6 +201,9 @@ public:
*/
void print_info();
+ // print register dump
+ void print_registers();
+
protected:
virtual int probe();
@@ -225,6 +229,9 @@ private:
perf_counter_t _sample_perf;
perf_counter_t _reschedules;
perf_counter_t _errors;
+ perf_counter_t _bad_registers;
+
+ uint8_t _register_wait;
math::LowPassFilter2p _gyro_filter_x;
math::LowPassFilter2p _gyro_filter_y;
@@ -235,6 +242,14 @@ private:
enum Rotation _rotation;
+ // this is used to support runtime checking of key
+ // configuration registers to detect SPI bus errors and sensor
+ // reset
+#define L3GD20_NUM_CHECKED_REGISTERS 8
+ static const uint8_t _checked_registers[L3GD20_NUM_CHECKED_REGISTERS];
+ uint8_t _checked_values[L3GD20_NUM_CHECKED_REGISTERS];
+ uint8_t _checked_next;
+
/**
* Start automatic measurement.
*/
@@ -267,6 +282,11 @@ private:
static void measure_trampoline(void *arg);
/**
+ * check key registers for correct values
+ */
+ void check_registers(void);
+
+ /**
* Fetch measurements from the sensor and update the report ring.
*/
void measure();
@@ -299,6 +319,14 @@ private:
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
+ * Write a register in the L3GD20, updating _checked_values
+ *
+ * @param reg The register to write.
+ * @param value The new value to write.
+ */
+ void write_checked_reg(unsigned reg, uint8_t value);
+
+ /**
* Set the L3GD20 measurement range.
*
* @param max_dps The measurement range is set to permit reading at least
@@ -338,6 +366,19 @@ private:
L3GD20 operator=(const L3GD20&);
};
+/*
+ list of registers that will be checked in check_registers(). Note
+ that ADDR_WHO_AM_I must be first in the list.
+ */
+const uint8_t L3GD20::_checked_registers[L3GD20_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I,
+ ADDR_CTRL_REG1,
+ ADDR_CTRL_REG2,
+ ADDR_CTRL_REG3,
+ ADDR_CTRL_REG4,
+ ADDR_CTRL_REG5,
+ ADDR_FIFO_CTRL_REG,
+ ADDR_LOW_ODR };
+
L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
_call{},
@@ -355,11 +396,14 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
+ _bad_registers(perf_alloc(PC_COUNT, "l3gd20_bad_registers")),
+ _register_wait(0),
_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_is_l3g4200d(false),
- _rotation(rotation)
+ _rotation(rotation),
+ _checked_next(0)
{
// enable debug() calls
_debug_enabled = true;
@@ -389,6 +433,7 @@ L3GD20::~L3GD20()
perf_free(_sample_perf);
perf_free(_reschedules);
perf_free(_errors);
+ perf_free(_bad_registers);
}
int
@@ -448,29 +493,27 @@ L3GD20::probe()
(void)read_reg(ADDR_WHO_AM_I);
bool success = false;
+ uint8_t v = 0;
- /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
- if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
-
+ /* verify that the device is attached and functioning, accept
+ * L3GD20, L3GD20H and L3G4200D */
+ if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM) {
_orientation = SENSOR_BOARD_ROTATION_DEFAULT;
success = true;
- }
-
-
- if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) {
+ } else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_H) {
_orientation = SENSOR_BOARD_ROTATION_180_DEG;
success = true;
- }
-
- /* Detect the L3G4200D used on AeroCore */
- if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) {
+ } else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_L3G4200D) {
+ /* Detect the L3G4200D used on AeroCore */
_is_l3g4200d = true;
_orientation = SENSOR_BOARD_ROTATION_DEFAULT;
success = true;
}
- if (success)
+ if (success) {
+ _checked_values[0] = v;
return OK;
+ }
return -EIO;
}
@@ -673,6 +716,18 @@ L3GD20::write_reg(unsigned reg, uint8_t value)
}
void
+L3GD20::write_checked_reg(unsigned reg, uint8_t value)
+{
+ write_reg(reg, value);
+ for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) {
+ if (reg == _checked_registers[i]) {
+ _checked_values[i] = value;
+ }
+ }
+}
+
+
+void
L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
{
uint8_t val;
@@ -680,7 +735,7 @@ L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
val = read_reg(reg);
val &= ~clearbits;
val |= setbits;
- write_reg(reg, val);
+ write_checked_reg(reg, val);
}
int
@@ -714,7 +769,7 @@ L3GD20::set_range(unsigned max_dps)
_gyro_range_rad_s = new_range / 180.0f * M_PI_F;
_gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F;
- write_reg(ADDR_CTRL_REG4, bits);
+ write_checked_reg(ADDR_CTRL_REG4, bits);
return OK;
}
@@ -750,7 +805,7 @@ L3GD20::set_samplerate(unsigned frequency)
return -EINVAL;
}
- write_reg(ADDR_CTRL_REG1, bits);
+ write_checked_reg(ADDR_CTRL_REG1, bits);
return OK;
}
@@ -791,6 +846,11 @@ L3GD20::disable_i2c(void)
uint8_t a = read_reg(0x05);
write_reg(0x05, (0x20 | a));
if (read_reg(0x05) == (a | 0x20)) {
+ // this sets the I2C_DIS bit on the
+ // L3GD20H. The l3gd20 datasheet doesn't
+ // mention this register, but it does seem to
+ // accept it.
+ write_checked_reg(ADDR_LOW_ODR, 0x08);
return;
}
}
@@ -804,18 +864,18 @@ L3GD20::reset()
disable_i2c();
/* set default configuration */
- write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
- write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
- write_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */
- write_reg(ADDR_CTRL_REG4, REG4_BDU);
- write_reg(ADDR_CTRL_REG5, 0);
-
- write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
+ write_checked_reg(ADDR_CTRL_REG1,
+ REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
+ write_checked_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
+ write_checked_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */
+ write_checked_reg(ADDR_CTRL_REG4, REG4_BDU);
+ write_checked_reg(ADDR_CTRL_REG5, 0);
+ write_checked_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
/* disable FIFO. This makes things simpler and ensures we
* aren't getting stale data. It means we must run the hrt
* callback fast enough to not miss data. */
- write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
+ write_checked_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
set_samplerate(0); // 760Hz or 800Hz
set_range(L3GD20_DEFAULT_RANGE_DPS);
@@ -840,6 +900,41 @@ L3GD20::measure_trampoline(void *arg)
#endif
void
+L3GD20::check_registers(void)
+{
+ uint8_t v;
+ if ((v=read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) {
+ /*
+ if we get the wrong value then we know the SPI bus
+ or sensor is very sick. We set _register_wait to 20
+ and wait until we have seen 20 good values in a row
+ before we consider the sensor to be OK again.
+ */
+ perf_count(_bad_registers);
+
+ /*
+ try to fix the bad register value. We only try to
+ fix one per loop to prevent a bad sensor hogging the
+ bus. We skip zero as that is the WHO_AM_I, which
+ is not writeable
+ */
+ if (_checked_next != 0) {
+ write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
+ }
+#if 1
+ if (_register_wait == 0) {
+ ::printf("L3GD20: %02x:%02x should be %02x\n",
+ (unsigned)_checked_registers[_checked_next],
+ (unsigned)v,
+ (unsigned)_checked_values[_checked_next]);
+ }
+#endif
+ _register_wait = 20;
+ }
+ _checked_next = (_checked_next+1) % L3GD20_NUM_CHECKED_REGISTERS;
+}
+
+void
L3GD20::measure()
{
#if L3GD20_USE_DRDY
@@ -870,6 +965,8 @@ L3GD20::measure()
/* start the performance counter */
perf_begin(_sample_perf);
+ check_registers();
+
/* fetch data from the sensor */
memset(&raw_report, 0, sizeof(raw_report));
raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
@@ -973,7 +1070,32 @@ L3GD20::print_info()
perf_print_counter(_sample_perf);
perf_print_counter(_reschedules);
perf_print_counter(_errors);
+ perf_print_counter(_bad_registers);
_reports->print_info("report queue");
+ ::printf("checked_next: %u\n", _checked_next);
+ for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) {
+ uint8_t v = read_reg(_checked_registers[i]);
+ if (v != _checked_values[i]) {
+ ::printf("reg %02x:%02x should be %02x\n",
+ (unsigned)_checked_registers[i],
+ (unsigned)v,
+ (unsigned)_checked_values[i]);
+ }
+ }
+}
+
+void
+L3GD20::print_registers()
+{
+ printf("L3GD20 registers\n");
+ for (uint8_t reg=0; reg<=0x40; reg++) {
+ uint8_t v = read_reg(reg);
+ printf("%02x:%02x ",(unsigned)reg, (unsigned)v);
+ if ((reg+1) % 16 == 0) {
+ printf("\n");
+ }
+ }
+ printf("\n");
}
int
@@ -1011,6 +1133,7 @@ void start(bool external_bus, enum Rotation rotation);
void test();
void reset();
void info();
+void regdump();
/**
* Start the driver.
@@ -1149,10 +1272,25 @@ info()
exit(0);
}
+/**
+ * Dump the register information
+ */
+void
+regdump(void)
+{
+ if (g_dev == nullptr)
+ errx(1, "driver not running");
+
+ printf("regdump @ %p\n", g_dev);
+ g_dev->print_registers();
+
+ exit(0);
+}
+
void
usage()
{
- warnx("missing command: try 'start', 'info', 'test', 'reset'");
+ warnx("missing command: try 'start', 'info', 'test', 'reset' or 'regdump'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -R rotation");
@@ -1209,5 +1347,11 @@ l3gd20_main(int argc, char *argv[])
if (!strcmp(verb, "info"))
l3gd20::info();
- errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+ /*
+ * Print register information.
+ */
+ if (!strcmp(verb, "regdump"))
+ l3gd20::regdump();
+
+ errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-06-26 19:57:31 +0000