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

Merge pull request #1555 from tridge/pullrequest-regcheck-fixes

Browse Source 
this fixes errors in the recent regcheck code
This commit is contained in:
Lorenz Meier
2015-01-01 00:48:29 +01:00
parent 72eafad510 8470105f9e
commit f3b021ac05
3 changed files with 107 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/l3gd20/l3gd20.cpp
+47 -22
View File
@@ -204,6 +204,9 @@ public:
// print register dump
void print_registers();
// trigger an error
void test_error();
protected:
virtual int probe();
@@ -921,14 +924,6 @@ L3GD20::check_registers(void)
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;
@@ -937,17 +932,6 @@ L3GD20::check_registers(void)
void
L3GD20::measure()
{
#if L3GD20_USE_DRDY
// if the gyro doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
perf_count(_reschedules);
hrt_call_delay(&_call, 100);
return;
}
#endif
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
@@ -967,6 +951,18 @@ L3GD20::measure()
check_registers();
#if L3GD20_USE_DRDY
// if the gyro doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
perf_count(_reschedules);
hrt_call_delay(&_call, 100);
perf_end(_sample_perf);
return;
}
#endif
/* fetch data from the sensor */
memset(&raw_report, 0, sizeof(raw_report));
raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
@@ -997,7 +993,7 @@ L3GD20::measure()
* 74 from all measurements centers them around zero.
*/
report.timestamp = hrt_absolute_time();
report.error_count = 0; // not recorded
report.error_count = perf_event_count(_bad_registers);
switch (_orientation) {
@@ -1098,6 +1094,13 @@ L3GD20::print_registers()
printf("\n");
}
void
L3GD20::test_error()
{
// trigger a deliberate error
write_reg(ADDR_CTRL_REG3, 0);
}
int
L3GD20::self_test()
{
@@ -1134,6 +1137,7 @@ void test();
void reset();
void info();
void regdump();
void test_error();
/**
* Start the driver.
@@ -1287,10 +1291,25 @@ regdump(void)
exit(0);
}
/**
* trigger an error
*/
void
test_error(void)
{
if (g_dev == nullptr)
errx(1, "driver not running");
printf("regdump @ %p\n", g_dev);
g_dev->test_error();
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'reset' or 'regdump'");
warnx("missing command: try 'start', 'info', 'test', 'reset', 'testerror' or 'regdump'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -R rotation");
@@ -1353,5 +1372,11 @@ l3gd20_main(int argc, char *argv[])
if (!strcmp(verb, "regdump"))
l3gd20::regdump();
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
/*
* trigger an error
*/
if (!strcmp(verb, "testerror"))
l3gd20::test_error();
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'");
}
src/drivers/lsm303d/lsm303d.cpp
+56 -26
View File
@@ -237,6 +237,11 @@ public:
*/
void print_registers();
/**
* deliberately trigger an error
*/
void test_error();
protected:
virtual int probe();
@@ -292,7 +297,7 @@ private:
// 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
#define LSM303D_NUM_CHECKED_REGISTERS 8
static const uint8_t _checked_registers[LSM303D_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[LSM303D_NUM_CHECKED_REGISTERS];
uint8_t _checked_next;
@@ -469,6 +474,8 @@ private:
const uint8_t LSM303D::_checked_registers[LSM303D_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I,
ADDR_CTRL_REG1,
ADDR_CTRL_REG2,
ADDR_CTRL_REG3,
ADDR_CTRL_REG4,
ADDR_CTRL_REG5,
ADDR_CTRL_REG6,
ADDR_CTRL_REG7 };
@@ -703,8 +710,8 @@ LSM303D::reset()
/* enable mag */
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
write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
write_checked_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
@@ -1422,14 +1429,6 @@ LSM303D::check_registers(void)
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;
@@ -1438,19 +1437,6 @@ LSM303D::check_registers(void)
void
LSM303D::measure()
{
// if the accel doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// 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;
}
#endif
/* status register and data as read back from the device */
#pragma pack(push, 1)
@@ -1470,6 +1456,20 @@ LSM303D::measure()
check_registers();
// if the accel doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// 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);
perf_end(_accel_sample_perf);
return;
}
#endif
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again.
@@ -1618,7 +1618,9 @@ 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(_mag_sample_perf);
perf_print_counter(_bad_registers);
perf_print_counter(_accel_reschedules);
_accel_reports->print_info("accel reports");
_mag_reports->print_info("mag reports");
::printf("checked_next: %u\n", _checked_next);
@@ -1690,6 +1692,13 @@ LSM303D::print_registers()
}
}
void
LSM303D::test_error()
{
// trigger an error
write_reg(ADDR_CTRL_REG3, 0);
}
LSM303D_mag::LSM303D_mag(LSM303D *parent) :
CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
_parent(parent),
@@ -1764,6 +1773,7 @@ void reset();
void info();
void regdump();
void usage();
void test_error();
/**
* Start the driver.
@@ -1969,10 +1979,24 @@ regdump()
exit(0);
}
/**
* trigger an error
*/
void
test_error()
{
if (g_dev == nullptr)
errx(1, "driver not running\n");
g_dev->test_error();
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'");
warnx("missing command: try 'start', 'info', 'test', 'reset', 'testerror' or 'regdump'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -R rotation");
@@ -2035,5 +2059,11 @@ lsm303d_main(int argc, char *argv[])
if (!strcmp(verb, "regdump"))
lsm303d::regdump();
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', or 'regdump'");
/*
* trigger an error
*/
if (!strcmp(verb, "testerror"))
lsm303d::test_error();
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'");
}
src/drivers/mpu6000/mpu6000.cpp
+4 -38
View File
@@ -254,7 +254,6 @@ private:
uint8_t _register_wait;
uint64_t _reset_wait;
uint64_t _printf_wait;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
@@ -494,7 +493,6 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
_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),
@@ -1539,43 +1537,11 @@ MPU6000::check_registers(void)
_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]);
}
// waiting 3ms between register writes seems
// to raise the chance of the sensor
// recovering considerably
_reset_wait = hrt_absolute_time() + 3000;
}
#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;