diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 57 |
1 files changed, 24 insertions, 33 deletions
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 768723640..0319b601e 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -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++) { |