fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'accelfail' of github.com:thomasgubler/Firmware

Browse Source
This commit is contained in:
Lorenz Meier 2014-12-31 11:42:58 +01:00
commit 3aacaf036c
3 changed files with 767 additions and 296 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

196
src/drivers/l3gd20/l3gd20.cpp
View File

@ -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.
*/
@ -266,6 +281,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.
*/
@ -298,6 +318,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.
*
@ -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;
}
@ -672,6 +715,18 @@ L3GD20::write_reg(unsigned reg, uint8_t value)
transfer(cmd, nullptr, sizeof(cmd));
}
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)
{
@ -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);
@ -839,6 +899,41 @@ L3GD20::measure_trampoline(void *arg)
# define L3GD20_USE_DRDY 0
#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()
{
@ -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'");
}

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

@ -77,10 +77,6 @@
#endif
static const int ERROR = -1;
// enable this to debug the buggy lsm303d sensor in very early
// prototype pixhawk boards
#define CHECK_EXTREMES 0
/* SPI protocol address bits */
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
@ -241,16 +237,6 @@ public:
*/
void print_registers();
/**
* toggle logging
*/
void toggle_logging();
/**
* check for extreme accel values
*/
void check_extremes(const accel_report *arb);
protected:
virtual int probe();
@ -292,30 +278,25 @@ private:
perf_counter_t _accel_sample_perf;
perf_counter_t _mag_sample_perf;
perf_counter_t _reg1_resets;
perf_counter_t _reg7_resets;
perf_counter_t _extreme_values;
perf_counter_t _accel_reschedules;
perf_counter_t _bad_registers;
uint8_t _register_wait;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
math::LowPassFilter2p _accel_filter_z;
// expceted values of reg1 and reg7 to catch in-flight
// brownouts of the sensor
uint8_t _reg1_expected;
uint8_t _reg7_expected;
// accel logging
int _accel_log_fd;
bool _accel_logging_enabled;
uint64_t _last_extreme_us;
uint64_t _last_log_us;
uint64_t _last_log_sync_us;
uint64_t _last_log_reg_us;
uint64_t _last_log_alarm_us;
enum Rotation _rotation;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define LSM303D_NUM_CHECKED_REGISTERS 6
static const uint8_t _checked_registers[LSM303D_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[LSM303D_NUM_CHECKED_REGISTERS];
uint8_t _checked_next;
/**
* Start automatic measurement.
*/
@ -356,6 +337,11 @@ private:
*/
static void mag_measure_trampoline(void *arg);
/**
* check key registers for correct values
*/
void check_registers(void);
/**
* Fetch accel measurements from the sensor and update the report ring.
*/
@ -407,6 +393,14 @@ private:
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Write a register in the LSM303D, 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 LSM303D accel measurement range.
*
@ -468,6 +462,17 @@ private:
LSM303D operator=(const LSM303D&);
};
/*
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 LSM303D::_checked_registers[LSM303D_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I,
ADDR_CTRL_REG1,
ADDR_CTRL_REG2,
ADDR_CTRL_REG5,
ADDR_CTRL_REG6,
ADDR_CTRL_REG7 };
/**
* Helper class implementing the mag driver node.
*/
@ -528,23 +533,14 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
_mag_read(0),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_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")),
_extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")),
_accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")),
_bad_registers(perf_alloc(PC_COUNT, "lsm303d_bad_registers")),
_register_wait(0),
_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_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_reg1_expected(0),
_reg7_expected(0),
_accel_log_fd(-1),
_accel_logging_enabled(false),
_last_extreme_us(0),
_last_log_us(0),
_last_log_sync_us(0),
_last_log_reg_us(0),
_last_log_alarm_us(0),
_rotation(rotation)
_rotation(rotation),
_checked_next(0)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
@ -586,9 +582,7 @@ LSM303D::~LSM303D()
/* delete the perf counter */
perf_free(_accel_sample_perf);
perf_free(_mag_sample_perf);
perf_free(_reg1_resets);
perf_free(_reg7_resets);
perf_free(_extreme_values);
perf_free(_bad_registers);
perf_free(_accel_reschedules);
}
@ -703,13 +697,12 @@ LSM303D::reset()
disable_i2c();
/* enable accel*/
_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);
write_checked_reg(ADDR_CTRL_REG1,
REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A);
/* enable mag */
_reg7_expected = REG7_CONT_MODE_M;
write_reg(ADDR_CTRL_REG7, _reg7_expected);
write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
@ -739,128 +732,14 @@ LSM303D::probe()
/* verify that the device is attached and functioning */
bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM);
if (success)
if (success) {
_checked_values[0] = WHO_I_AM;
return OK;
}
return -EIO;
}
#define ACCEL_LOGFILE "/fs/microsd/lsm303d.log"
/**
check for extreme accelerometer values and log to a file on the SD card
*/
void
LSM303D::check_extremes(const accel_report *arb)
{
const float extreme_threshold = 30;
static bool boot_ok = false;
bool is_extreme = (fabsf(arb->x) > extreme_threshold &&
fabsf(arb->y) > extreme_threshold &&
fabsf(arb->z) > extreme_threshold);
if (is_extreme) {
perf_count(_extreme_values);
// force accel logging on if we see extreme values
_accel_logging_enabled = true;
} else {
boot_ok = true;
}
if (! _accel_logging_enabled) {
// logging has been disabled by user, close
if (_accel_log_fd != -1) {
::close(_accel_log_fd);
_accel_log_fd = -1;
}
return;
}
if (_accel_log_fd == -1) {
// keep last 10 logs
::unlink(ACCEL_LOGFILE ".9");
for (uint8_t i=8; i>0; i--) {
uint8_t len = strlen(ACCEL_LOGFILE)+3;
char log1[len], log2[len];
snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i);
snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1));
::rename(log1, log2);
}
::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1");
// open the new logfile
_accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666);
if (_accel_log_fd == -1) {
return;
}
}
uint64_t now = hrt_absolute_time();
// log accels at 1Hz
if (_last_log_us == 0 ||
now - _last_log_us > 1000*1000) {
_last_log_us = now;
::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n",
(unsigned long long)arb->timestamp,
(double)arb->x, (double)arb->y, (double)arb->z,
(int)arb->x_raw,
(int)arb->y_raw,
(int)arb->z_raw,
(unsigned)boot_ok);
}
const uint8_t reglist[] = { ADDR_WHO_AM_I, 0x02, 0x15, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1,
ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6,
ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M,
ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A,
ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL,
ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2,
ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC,
ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW,
ADDR_ACT_THS, ADDR_ACT_DUR,
ADDR_OUT_X_L_M, ADDR_OUT_X_H_M,
ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M, 0x02, 0x15, ADDR_WHO_AM_I};
uint8_t regval[sizeof(reglist)];
for (uint8_t i=0; i<sizeof(reglist); i++) {
regval[i] = read_reg(reglist[i]);
}
// log registers at 10Hz when we have extreme values, or 0.5 Hz without
if (_last_log_reg_us == 0 ||
(is_extreme && (now - _last_log_reg_us > 250*1000)) ||
(now - _last_log_reg_us > 10*1000*1000)) {
_last_log_reg_us = now;
::dprintf(_accel_log_fd, "XREG %llu", (unsigned long long)hrt_absolute_time());
for (uint8_t i=0; i<sizeof(reglist); i++) {
::dprintf(_accel_log_fd, " %02x:%02x", (unsigned)reglist[i], (unsigned)regval[i]);
}
::dprintf(_accel_log_fd, "\n");
}
// fsync at 0.1Hz
if (now - _last_log_sync_us > 10*1000*1000) {
_last_log_sync_us = now;
::fsync(_accel_log_fd);
}
// play alarm every 10s if we have had an extreme value
if (perf_event_count(_extreme_values) != 0 &&
(now - _last_log_alarm_us > 10*1000*1000)) {
_last_log_alarm_us = now;
int tfd = ::open(TONEALARM_DEVICE_PATH, 0);
if (tfd != -1) {
uint8_t tone = 3;
if (!is_extreme) {
tone = 3;
} else if (boot_ok) {
tone = 4;
} else {
tone = 5;
}
::ioctl(tfd, TONE_SET_ALARM, tone);
::close(tfd);
}
}
}
ssize_t
LSM303D::read(struct file *filp, char *buffer, size_t buflen)
{
@ -879,9 +758,6 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
*/
while (count--) {
if (_accel_reports->get(arb)) {
#if CHECK_EXTREMES
check_extremes(arb);
#endif
ret += sizeof(*arb);
arb++;
}
@ -1262,6 +1138,17 @@ LSM303D::write_reg(unsigned reg, uint8_t value)
transfer(cmd, nullptr, sizeof(cmd));
}
void
LSM303D::write_checked_reg(unsigned reg, uint8_t value)
{
write_reg(reg, value);
for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) {
if (reg == _checked_registers[i]) {
_checked_values[i] = value;
}
}
}
void
LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
{
@ -1270,7 +1157,7 @@ LSM303D::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
@ -1439,7 +1326,6 @@ LSM303D::accel_set_samplerate(unsigned frequency)
}
modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
_reg1_expected = (_reg1_expected & ~clearbits) | setbits;
return OK;
}
@ -1514,6 +1400,41 @@ LSM303D::mag_measure_trampoline(void *arg)
dev->mag_measure();
}
void
LSM303D::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("LSM303D: %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) % LSM303D_NUM_CHECKED_REGISTERS;
}
void
LSM303D::measure()
{
@ -1522,16 +1443,13 @@ LSM303D::measure()
// read a value and then miss the next value.
// Note that DRDY is not available when the lsm303d is
// connected on the external bus
#ifdef GPIO_EXTI_ACCEL_DRDY
if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
perf_count(_accel_reschedules);
hrt_call_delay(&_accel_call, 100);
return;
}
if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
perf_count(_reg1_resets);
reset();
return;
}
#endif
/* status register and data as read back from the device */
@ -1550,6 +1468,16 @@ LSM303D::measure()
/* start the performance counter */
perf_begin(_accel_sample_perf);
check_registers();
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again.
_register_wait--;
perf_end(_accel_sample_perf);
return;
}
/* fetch data from the sensor */
memset(&raw_accel_report, 0, sizeof(raw_accel_report));
raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
@ -1572,7 +1500,12 @@ LSM303D::measure()
accel_report.timestamp = hrt_absolute_time();
accel_report.error_count = 0; // not reported
// report the error count as the sum of the number of bad bad
// register reads. This allows the higher level code to decide
// if it should use this sensor based on whether it has had
// failures
accel_report.error_count = perf_event_count(_bad_registers);
accel_report.x_raw = raw_accel_report.x;
accel_report.y_raw = raw_accel_report.y;
@ -1611,12 +1544,6 @@ LSM303D::measure()
void
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 */
#pragma pack(push, 1)
struct {
@ -1691,8 +1618,19 @@ LSM303D::print_info()
printf("accel reads: %u\n", _accel_read);
printf("mag reads: %u\n", _mag_read);
perf_print_counter(_accel_sample_perf);
perf_print_counter(_bad_registers);
_accel_reports->print_info("accel reports");
_mag_reports->print_info("mag reports");
::printf("checked_next: %u\n", _checked_next);
for (uint8_t i=0; i<LSM303D_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
@ -1750,20 +1688,6 @@ LSM303D::print_registers()
for (uint8_t i=0; i<sizeof(regmap)/sizeof(regmap[0]); i++) {
printf("0x%02x %s\n", read_reg(regmap[i].reg), regmap[i].name);
}
printf("_reg1_expected=0x%02x\n", _reg1_expected);
printf("_reg7_expected=0x%02x\n", _reg7_expected);
}
void
LSM303D::toggle_logging()
{
if (! _accel_logging_enabled) {
_accel_logging_enabled = true;
printf("Started logging to %s\n", ACCEL_LOGFILE);
} else {
_accel_logging_enabled = false;
printf("Stopped logging\n");
}
}
LSM303D_mag::LSM303D_mag(LSM303D *parent) :
@ -1839,7 +1763,6 @@ void test();
void reset();
void info();
void regdump();
void logging();
void usage();
/**
@ -2046,24 +1969,10 @@ regdump()
exit(0);
}
/**
* toggle logging
*/
void
logging()
{
if (g_dev == nullptr)
errx(1, "driver not running\n");
g_dev->toggle_logging();
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'logging'");
warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -R rotation");
@ -2126,11 +2035,5 @@ lsm303d_main(int argc, char *argv[])
if (!strcmp(verb, "regdump"))
lsm303d::regdump();
/*
* dump device registers
*/
if (!strcmp(verb, "logging"))
lsm303d::logging();
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'");
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', or 'regdump'");
}

520
src/drivers/mpu6000/mpu6000.cpp
View File

@ -113,6 +113,10 @@
#define MPUREG_FIFO_COUNTL 0x73
#define MPUREG_FIFO_R_W 0x74
#define MPUREG_PRODUCT_ID 0x0C
#define MPUREG_TRIM1 0x0D
#define MPUREG_TRIM2 0x0E
#define MPUREG_TRIM3 0x0F
#define MPUREG_TRIM4 0x10
// Configuration bits MPU 3000 and MPU 6000 (not revised)?
#define BIT_SLEEP 0x40
@ -121,6 +125,9 @@
#define MPU_CLK_SEL_PLLGYROX 0x01
#define MPU_CLK_SEL_PLLGYROZ 0x03
#define MPU_EXT_SYNC_GYROX 0x02
#define BITS_GYRO_ST_X 0x80
#define BITS_GYRO_ST_Y 0x40
#define BITS_GYRO_ST_Z 0x20
#define BITS_FS_250DPS 0x00
#define BITS_FS_500DPS 0x08
#define BITS_FS_1000DPS 0x10
@ -196,6 +203,16 @@ public:
void print_registers();
/**
* Test behaviour against factory offsets
*
* @return 0 on success, 1 on failure
*/
int factory_self_test();
// deliberately cause a sensor error
void test_error();
protected:
virtual int probe();
@ -231,7 +248,13 @@ private:
perf_counter_t _gyro_reads;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
perf_counter_t _bad_registers;
perf_counter_t _good_transfers;
perf_counter_t _reset_retries;
uint8_t _register_wait;
uint64_t _reset_wait;
uint64_t _printf_wait;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
@ -242,6 +265,18 @@ private:
enum Rotation _rotation;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define MPU6000_NUM_CHECKED_REGISTERS 9
static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS];
uint8_t _checked_next;
// use this to avoid processing measurements when in factory
// self test
volatile bool _in_factory_test;
/**
* Start automatic measurement.
*/
@ -257,7 +292,7 @@ private:
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
void reset();
int reset();
/**
* Static trampoline from the hrt_call context; because we don't have a
@ -281,7 +316,7 @@ private:
* @param The register to read.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg);
uint8_t read_reg(unsigned reg, uint32_t speed=MPU6000_LOW_BUS_SPEED);
uint16_t read_reg16(unsigned reg);
/**
@ -303,6 +338,14 @@ private:
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Write a register in the MPU6000, 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 MPU6000 measurement range.
*
@ -347,11 +390,50 @@ private:
*/
void _set_sample_rate(uint16_t desired_sample_rate_hz);
/*
check that key registers still have the right value
*/
void check_registers(void);
/* do not allow to copy this class due to pointer data members */
MPU6000(const MPU6000&);
MPU6000 operator=(const MPU6000&);
#pragma pack(push, 1)
/**
* Report conversation within the MPU6000, including command byte and
* interrupt status.
*/
struct MPUReport {
uint8_t cmd;
uint8_t status;
uint8_t accel_x[2];
uint8_t accel_y[2];
uint8_t accel_z[2];
uint8_t temp[2];
uint8_t gyro_x[2];
uint8_t gyro_y[2];
uint8_t gyro_z[2];
};
#pragma pack(pop)
};
/*
list of registers that will be checked in check_registers(). Note
that MPUREG_PRODUCT_ID must be first in the list.
*/
const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID,
MPUREG_PWR_MGMT_1,
MPUREG_USER_CTRL,
MPUREG_SMPLRT_DIV,
MPUREG_CONFIG,
MPUREG_GYRO_CONFIG,
MPUREG_ACCEL_CONFIG,
MPUREG_INT_ENABLE,
MPUREG_INT_PIN_CFG };
/**
* Helper class implementing the gyro driver node.
*/
@ -407,14 +489,21 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
_gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
_bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")),
_bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")),
_good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")),
_reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")),
_register_wait(0),
_reset_wait(0),
_printf_wait(0),
_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_rotation(rotation)
_rotation(rotation),
_checked_next(0),
_in_factory_test(false)
{
// disable debug() calls
_debug_enabled = false;
@ -460,6 +549,7 @@ MPU6000::~MPU6000()
perf_free(_accel_reads);
perf_free(_gyro_reads);
perf_free(_bad_transfers);
perf_free(_bad_registers);
perf_free(_good_transfers);
}
@ -486,7 +576,8 @@ MPU6000::init()
if (_gyro_reports == nullptr)
goto out;
reset();
if (reset() != OK)
goto out;
/* Initialize offsets and scales */
_accel_scale.x_offset = 0;
@ -571,27 +662,39 @@ out:
return ret;
}
void MPU6000::reset()
int MPU6000::reset()
{
// if the mpu6000 is initialised after the l3gd20 and lsm303d
// then if we don't do an irqsave/irqrestore here the mpu6000
// frequenctly comes up in a bad state where all transfers
// come as zero
irqstate_t state;
state = irqsave();
uint8_t tries = 5;
while (--tries != 0) {
irqstate_t state;
state = irqsave();
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
up_udelay(10000);
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
up_udelay(10000);
// Wake up device and select GyroZ clock. Note that the
// MPU6000 starts up in sleep mode, and it can take some time
// for it to come out of sleep
write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
up_udelay(1000);
// Wake up device and select GyroZ clock. Note that the
// MPU6000 starts up in sleep mode, and it can take some time
// for it to come out of sleep
write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
up_udelay(1000);
// Disable I2C bus (recommended on datasheet)
write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
irqrestore(state);
// Disable I2C bus (recommended on datasheet)
write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
irqrestore(state);
if (read_reg(MPUREG_PWR_MGMT_1) == MPU_CLK_SEL_PLLGYROZ) {
break;
}
perf_count(_reset_retries);
usleep(2000);
}
if (read_reg(MPUREG_PWR_MGMT_1) != MPU_CLK_SEL_PLLGYROZ) {
return -EIO;
}
usleep(1000);
@ -605,7 +708,7 @@ void MPU6000::reset()
_set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ);
usleep(1000);
// Gyro scale 2000 deg/s ()
write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
usleep(1000);
// correct gyro scale factors
@ -624,7 +727,7 @@ void MPU6000::reset()
case MPU6000_REV_C5:
// Accel scale 8g (4096 LSB/g)
// Rev C has different scaling than rev D
write_reg(MPUREG_ACCEL_CONFIG, 1 << 3);
write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3);
break;
case MPU6000ES_REV_D6:
@ -639,7 +742,7 @@ void MPU6000::reset()
// presumably won't have the accel scaling bug
default:
// Accel scale 8g (4096 LSB/g)
write_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
break;
}
@ -651,15 +754,16 @@ void MPU6000::reset()
usleep(1000);
// INT CFG => Interrupt on Data Ready
write_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready
write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready
usleep(1000);
write_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
usleep(1000);
// Oscillator set
// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
usleep(1000);
return OK;
}
int
@ -684,6 +788,7 @@ MPU6000::probe()
case MPU6000_REV_D9:
case MPU6000_REV_D10:
debug("ID 0x%02x", _product);
_checked_values[0] = _product;
return OK;
}
@ -700,7 +805,7 @@ MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz)
uint8_t div = 1000 / desired_sample_rate_hz;
if(div>200) div=200;
if(div<1) div=1;
write_reg(MPUREG_SMPLRT_DIV, div-1);
write_checked_reg(MPUREG_SMPLRT_DIV, div-1);
_sample_rate = 1000 / div;
}
@ -734,7 +839,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
} else {
filter = BITS_DLPF_CFG_2100HZ_NOLPF;
}
write_reg(MPUREG_CONFIG, filter);
write_checked_reg(MPUREG_CONFIG, filter);
}
ssize_t
@ -833,6 +938,173 @@ MPU6000::gyro_self_test()
return 0;
}
/*
perform a self-test comparison to factory trim values. This takes
about 200ms and will return OK if the current values are within 14%
of the expected values (as per datasheet)
*/
int
MPU6000::factory_self_test()
{
_in_factory_test = true;
uint8_t saved_gyro_config = read_reg(MPUREG_GYRO_CONFIG);
uint8_t saved_accel_config = read_reg(MPUREG_ACCEL_CONFIG);
const uint16_t repeats = 100;
int ret = OK;
// gyro self test has to be done at 250DPS
write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS);
struct MPUReport mpu_report;
float accel_baseline[3];
float gyro_baseline[3];
float accel[3];
float gyro[3];
float accel_ftrim[3];
float gyro_ftrim[3];
// get baseline values without self-test enabled
set_frequency(MPU6000_HIGH_BUS_SPEED);
memset(accel_baseline, 0, sizeof(accel_baseline));
memset(gyro_baseline, 0, sizeof(gyro_baseline));
memset(accel, 0, sizeof(accel));
memset(gyro, 0, sizeof(gyro));
for (uint8_t i=0; i<repeats; i++) {
up_udelay(1000);
mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report));
accel_baseline[0] += int16_t_from_bytes(mpu_report.accel_x);
accel_baseline[1] += int16_t_from_bytes(mpu_report.accel_y);
accel_baseline[2] += int16_t_from_bytes(mpu_report.accel_z);
gyro_baseline[0] += int16_t_from_bytes(mpu_report.gyro_x);
gyro_baseline[1] += int16_t_from_bytes(mpu_report.gyro_y);
gyro_baseline[2] += int16_t_from_bytes(mpu_report.gyro_z);
}
#if 1
write_reg(MPUREG_GYRO_CONFIG,
BITS_FS_250DPS |
BITS_GYRO_ST_X |
BITS_GYRO_ST_Y |
BITS_GYRO_ST_Z);
// accel 8g, self-test enabled all axes
write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config | 0xE0);
#endif
up_udelay(20000);
// get values with self-test enabled
set_frequency(MPU6000_HIGH_BUS_SPEED);
for (uint8_t i=0; i<repeats; i++) {
up_udelay(1000);
mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report));
accel[0] += int16_t_from_bytes(mpu_report.accel_x);
accel[1] += int16_t_from_bytes(mpu_report.accel_y);
accel[2] += int16_t_from_bytes(mpu_report.accel_z);
gyro[0] += int16_t_from_bytes(mpu_report.gyro_x);
gyro[1] += int16_t_from_bytes(mpu_report.gyro_y);
gyro[2] += int16_t_from_bytes(mpu_report.gyro_z);
}
for (uint8_t i=0; i<3; i++) {
accel_baseline[i] /= repeats;
gyro_baseline[i] /= repeats;
accel[i] /= repeats;
gyro[i] /= repeats;
}
// extract factory trim values
uint8_t trims[4];
trims[0] = read_reg(MPUREG_TRIM1);
trims[1] = read_reg(MPUREG_TRIM2);
trims[2] = read_reg(MPUREG_TRIM3);
trims[3] = read_reg(MPUREG_TRIM4);
uint8_t atrim[3];
uint8_t gtrim[3];
atrim[0] = ((trims[0]>>3)&0x1C) | ((trims[3]>>4)&0x03);
atrim[1] = ((trims[1]>>3)&0x1C) | ((trims[3]>>2)&0x03);
atrim[2] = ((trims[2]>>3)&0x1C) | ((trims[3]>>0)&0x03);
gtrim[0] = trims[0] & 0x1F;
gtrim[1] = trims[1] & 0x1F;
gtrim[2] = trims[2] & 0x1F;
// convert factory trims to right units
for (uint8_t i=0; i<3; i++) {
accel_ftrim[i] = 4096 * 0.34f * powf(0.92f/0.34f, (atrim[i]-1)/30.0f);
gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i]-1);
}
// Y gyro trim is negative
gyro_ftrim[1] *= -1;
for (uint8_t i=0; i<3; i++) {
float diff = accel[i]-accel_baseline[i];
float err = 100*(diff - accel_ftrim[i]) / accel_ftrim[i];
::printf("ACCEL[%u] baseline=%d accel=%d diff=%d ftrim=%d err=%d\n",
(unsigned)i,
(int)(1000*accel_baseline[i]),
(int)(1000*accel[i]),
(int)(1000*diff),
(int)(1000*accel_ftrim[i]),
(int)err);
if (fabsf(err) > 14) {
::printf("FAIL\n");
ret = -EIO;
}
}
for (uint8_t i=0; i<3; i++) {
float diff = gyro[i]-gyro_baseline[i];
float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i];
::printf("GYRO[%u] baseline=%d gyro=%d diff=%d ftrim=%d err=%d\n",
(unsigned)i,
(int)(1000*gyro_baseline[i]),
(int)(1000*gyro[i]),
(int)(1000*(gyro[i]-gyro_baseline[i])),
(int)(1000*gyro_ftrim[i]),
(int)err);
if (fabsf(err) > 14) {
::printf("FAIL\n");
ret = -EIO;
}
}
write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config);
write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config);
_in_factory_test = false;
if (ret == OK) {
::printf("PASSED\n");
}
return ret;
}
/*
deliberately trigger an error in the sensor to trigger recovery
*/
void
MPU6000::test_error()
{
_in_factory_test = true;
// deliberately trigger an error. This was noticed during
// development as a handy way to test the reset logic
uint8_t data[16];
memset(data, 0, sizeof(data));
transfer(data, data, sizeof(data));
::printf("error triggered\n");
print_registers();
_in_factory_test = false;
}
ssize_t
MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
{
@ -874,8 +1146,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
switch (cmd) {
case SENSORIOCRESET:
reset();
return OK;
return reset();
case SENSORIOCSPOLLRATE: {
switch (arg) {
@ -1094,12 +1365,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
}
uint8_t
MPU6000::read_reg(unsigned reg)
MPU6000::read_reg(unsigned reg, uint32_t speed)
{
uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
// general register transfer at low clock speed
set_frequency(MPU6000_LOW_BUS_SPEED);
set_frequency(speed);
transfer(cmd, cmd, sizeof(cmd));
@ -1144,6 +1415,17 @@ MPU6000::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
write_reg(reg, val);
}
void
MPU6000::write_checked_reg(unsigned reg, uint8_t value)
{
write_reg(reg, value);
for (uint8_t i=0; i<MPU6000_NUM_CHECKED_REGISTERS; i++) {
if (reg == _checked_registers[i]) {
_checked_values[i] = value;
}
}
}
int
MPU6000::set_range(unsigned max_g)
{
@ -1215,27 +1497,104 @@ MPU6000::measure_trampoline(void *arg)
dev->measure();
}
void
MPU6000::check_registers(void)
{
/*
we read the register at full speed, even though it isn't
listed as a high speed register. The low speed requirement
for some registers seems to be a propgation delay
requirement for changing sensor configuration, which should
not apply to reading a single register. It is also a better
test of SPI bus health to read at the same speed as we read
the data registers.
*/
uint8_t v;
if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) !=
_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.
*/
if (_register_wait == 0 || _checked_next == 0) {
// if the product_id is wrong then reset the
// sensor completely
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
// after doing a reset we need to wait a long
// time before we do any other register writes
// or we will end up with the mpu6000 in a
// bizarre state where it has all correct
// register values but large offsets on the
// accel axes
_reset_wait = hrt_absolute_time() + 10000;
_checked_next = 0;
} else {
write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
_reset_wait = hrt_absolute_time() + 1000;
if (_checked_next == 1 && hrt_absolute_time() > _printf_wait) {
/*
rather bizarrely this printf() seems
to be critical for successfully
resetting the sensor. If you take
this printf out then all the
registers do get successfully reset,
but the sensor ends up with a large
fixed offset on the
accelerometers. Try a up_udelay()
of various sizes instead of a
printf() doesn't work. That makes no
sense at all, and investigating why
this is would be worthwhile.
The rate limit on the printf to 5Hz
prevents this from causing enough
delays in interrupt context to cause
the vehicle to not be able to fly
correctly.
*/
_printf_wait = hrt_absolute_time() + 200*1000UL;
::printf("Setting %u %02x to %02x\n",
(unsigned)_checked_next,
(unsigned)_checked_registers[_checked_next],
(unsigned)_checked_values[_checked_next]);
}
}
#if 0
if (_register_wait == 0) {
::printf("MPU6000: %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) % MPU6000_NUM_CHECKED_REGISTERS;
}
void
MPU6000::measure()
{
#pragma pack(push, 1)
/**
* Report conversation within the MPU6000, including command byte and
* interrupt status.
*/
struct MPUReport {
uint8_t cmd;
uint8_t status;
uint8_t accel_x[2];
uint8_t accel_y[2];
uint8_t accel_z[2];
uint8_t temp[2];
uint8_t gyro_x[2];
uint8_t gyro_y[2];
uint8_t gyro_z[2];
} mpu_report;
#pragma pack(pop)
if (_in_factory_test) {
// don't publish any data while in factory test mode
return;
}
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return;
}
struct MPUReport mpu_report;
struct Report {
int16_t accel_x;
int16_t accel_y;
@ -1254,6 +1613,8 @@ MPU6000::measure()
*/
mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
check_registers();
// sensor transfer at high clock speed
set_frequency(MPU6000_HIGH_BUS_SPEED);
@ -1292,6 +1653,14 @@ MPU6000::measure()
}
perf_count(_good_transfers);
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again. We still increment
// _good_transfers, but don't return any data yet
_register_wait--;
return;
}
/*
@ -1321,7 +1690,12 @@ MPU6000::measure()
* Adjust and scale results to m/s^2.
*/
grb.timestamp = arb.timestamp = hrt_absolute_time();
grb.error_count = arb.error_count = 0; // not reported
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
// level code to decide if it should use this sensor based on
// whether it has had failures
grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
/*
* 1) Scale raw value to SI units using scaling from datasheet.
@ -1411,9 +1785,21 @@ MPU6000::print_info()
perf_print_counter(_accel_reads);
perf_print_counter(_gyro_reads);
perf_print_counter(_bad_transfers);
perf_print_counter(_bad_registers);
perf_print_counter(_good_transfers);
perf_print_counter(_reset_retries);
_accel_reports->print_info("accel queue");
_gyro_reports->print_info("gyro queue");
::printf("checked_next: %u\n", _checked_next);
for (uint8_t i=0; i<MPU6000_NUM_CHECKED_REGISTERS; i++) {
uint8_t v = read_reg(_checked_registers[i], MPU6000_HIGH_BUS_SPEED);
if (v != _checked_values[i]) {
::printf("reg %02x:%02x should be %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_values[i]);
}
}
}
void
@ -1497,6 +1883,8 @@ void test(bool);
void reset(bool);
void info(bool);
void regdump(bool);
void testerror(bool);
void factorytest(bool);
void usage();
/**
@ -1688,10 +2076,40 @@ regdump(bool external_bus)
exit(0);
}
/**
* deliberately produce an error to test recovery
*/
void
testerror(bool external_bus)
{
MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
if (*g_dev_ptr == nullptr)
errx(1, "driver not running");
(*g_dev_ptr)->test_error();
exit(0);
}
/**
* Dump the register information
*/
void
factorytest(bool external_bus)
{
MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
if (*g_dev_ptr == nullptr)
errx(1, "driver not running");
(*g_dev_ptr)->factory_self_test();
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'");
warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'factorytest', 'testerror'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -R rotation");
@ -1754,5 +2172,11 @@ mpu6000_main(int argc, char *argv[])
if (!strcmp(verb, "regdump"))
mpu6000::regdump(external_bus);
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
if (!strcmp(verb, "factorytest"))
mpu6000::factorytest(external_bus);
if (!strcmp(verb, "testerror"))
mpu6000::testerror(external_bus);
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'regdump', 'factorytest' or 'testerror'");
}