forked from Archive/PX4-Autopilot
l3gd20: follow same pattern as lsm303d for duplicate rejection
This commit is contained in:
Andrew Tridgell
parent
a710159263
commit
db8dd000e3
|
|
@ -189,6 +189,14 @@ static const int ERROR = -1;
|
|||
#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG
|
||||
#endif
|
||||
|
||||
/*
|
||||
we set the timer interrupt to run a bit faster than the desired
|
||||
sample rate and then throw away duplicates using the data ready bit.
|
||||
This time reduction is enough to cope with worst case timing jitter
|
||||
due to other timers
|
||||
*/
|
||||
#define L3GD20_TIMER_REDUCTION 200
|
||||
|
||||
extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
|
||||
|
||||
class L3GD20 : public device::SPI
|
||||
|
|
@ -236,9 +244,9 @@ private:
|
|||
unsigned _read;
|
||||
|
||||
perf_counter_t _sample_perf;
|
||||
perf_counter_t _reschedules;
|
||||
perf_counter_t _errors;
|
||||
perf_counter_t _bad_registers;
|
||||
perf_counter_t _duplicates;
|
||||
|
||||
uint8_t _register_wait;
|
||||
|
||||
|
|
@ -410,9 +418,9 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
|
|||
_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
|
||||
_read(0),
|
||||
_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")),
|
||||
_duplicates(perf_alloc(PC_COUNT, "l3gd20_duplicates")),
|
||||
_register_wait(0),
|
||||
_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
|
||||
_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
|
||||
|
|
@ -449,9 +457,9 @@ L3GD20::~L3GD20()
|
|||
|
||||
/* delete the perf counter */
|
||||
perf_free(_sample_perf);
|
||||
perf_free(_reschedules);
|
||||
perf_free(_errors);
|
||||
perf_free(_bad_registers);
|
||||
perf_free(_duplicates);
|
||||
}
|
||||
|
||||
int
|
||||
|
|
@ -608,7 +616,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
|
||||
/* update interval for next measurement */
|
||||
/* XXX this is a bit shady, but no other way to adjust... */
|
||||
_call.period = _call_interval = ticks;
|
||||
_call_interval = ticks;
|
||||
|
||||
_call.period = _call_interval - L3GD20_TIMER_REDUCTION;
|
||||
|
||||
/* adjust filters */
|
||||
float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
|
||||
|
|
@ -834,7 +844,10 @@ L3GD20::start()
|
|||
_reports->flush();
|
||||
|
||||
/* start polling at the specified rate */
|
||||
hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this);
|
||||
hrt_call_every(&_call,
|
||||
1000,
|
||||
_call_interval - L3GD20_TIMER_REDUCTION,
|
||||
(hrt_callout)&L3GD20::measure_trampoline, this);
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -899,12 +912,6 @@ L3GD20::measure_trampoline(void *arg)
|
|||
dev->measure();
|
||||
}
|
||||
|
||||
#ifdef GPIO_EXTI_GYRO_DRDY
|
||||
# define L3GD20_USE_DRDY 1
|
||||
#else
|
||||
# define L3GD20_USE_DRDY 0
|
||||
#endif
|
||||
|
||||
void
|
||||
L3GD20::check_registers(void)
|
||||
{
|
||||
|
|
@ -954,33 +961,17 @@ 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;
|
||||
transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
|
||||
|
||||
#if L3GD20_USE_DRDY
|
||||
if (_bus == PX4_SPI_BUS_SENSORS && (raw_report.status & 0xF) != 0xF) {
|
||||
/*
|
||||
we waited for DRDY, but did not see DRDY on all axes
|
||||
when we captured. That means a transfer error of some sort
|
||||
*/
|
||||
perf_count(_errors);
|
||||
return;
|
||||
if (!(raw_report.status & STATUS_ZYXDA)) {
|
||||
perf_end(_sample_perf);
|
||||
perf_count(_duplicates);
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
* 1) Scale raw value to SI units using scaling from datasheet.
|
||||
* 2) Subtract static offset (in SI units)
|
||||
|
|
@ -1071,9 +1062,9 @@ L3GD20::print_info()
|
|||
{
|
||||
printf("gyro reads: %u\n", _read);
|
||||
perf_print_counter(_sample_perf);
|
||||
perf_print_counter(_reschedules);
|
||||
perf_print_counter(_errors);
|
||||
perf_print_counter(_bad_registers);
|
||||
perf_print_counter(_duplicates);
|
||||
_reports->print_info("report queue");
|
||||
::printf("checked_next: %u\n", _checked_next);
|
||||
for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue