From 2d855785995fcc58dcce13cab13fbf30381dadaf Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 28 May 2015 11:20:30 -0700 Subject: [PATCH] Gyrosim: Fixed constant looping Gyrosim would call measure continuously because the write_checked_reg was failing. There is no need to check faked reg writes in the simulator so that code was removed. The delay that was added to the simulator to pace the gyrosim reads was also removed now that the source of the problem was determined. Signed-off-by: Mark Charlebois --- src/modules/simulator/simulator.cpp | 3 - .../posix/drivers/gyrosim/gyrosim.cpp | 127 +----------------- 2 files changed, 4 insertions(+), 126 deletions(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index f51439faff..83e3fd8d76 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -63,9 +63,6 @@ Simulator *Simulator::getInstance() bool Simulator::getMPUReport(uint8_t *buf, int len) { - // Reads are paced from reading gyrosim and if - // we don't delay here we read too fast - usleep(50000); return _mpu.copyData(buf, len); } diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 3b59673135..1ccaecf9a6 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -259,14 +259,6 @@ private: enum Rotation _rotation; - // this is used to support runtime checking of key - // configuration registers to detect SPI bus errors and sensor - // reset -#define GYROSIM_NUM_CHECKED_REGISTERS 9 - static const uint8_t _checked_registers[GYROSIM_NUM_CHECKED_REGISTERS]; - uint8_t _checked_values[GYROSIM_NUM_CHECKED_REGISTERS]; - uint8_t _checked_next; - // last temperature reading for print_info() float _last_temperature; @@ -331,14 +323,6 @@ private: */ void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - /** - * Write a register in the GYROSIM, 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 GYROSIM measurement range. * @@ -383,11 +367,6 @@ private: */ void _set_sample_rate(unsigned 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 */ GYROSIM(const GYROSIM&); GYROSIM operator=(const GYROSIM&); @@ -413,22 +392,6 @@ private: uint8_t _regdata[108]; }; -/* - list of registers that will be checked in check_registers(). Note - that MPUREG_PRODUCT_ID must be first in the list. - */ -const uint8_t GYROSIM::_checked_registers[GYROSIM_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. */ @@ -498,7 +461,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _gyro_filter_y(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_z(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), - _checked_next(0), _last_temperature(0) { // disable debug() calls @@ -527,7 +489,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _gyro_scale.z_scale = 1.0f; memset(&_call, 0, sizeof(_call)); - _checked_values[0] = _product; } GYROSIM::~GYROSIM() @@ -695,7 +656,7 @@ GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) uint8_t div = 1000 / desired_sample_rate_hz; if(div>200) div=200; if(div<1) div=1; - write_checked_reg(MPUREG_SMPLRT_DIV, div-1); + write_reg(MPUREG_SMPLRT_DIV, div-1); _sample_rate = 1000 / div; } @@ -729,7 +690,7 @@ GYROSIM::_set_dlpf_filter(uint16_t frequency_hz) } else { filter = BITS_DLPF_CFG_2100HZ_NOLPF; } - write_checked_reg(MPUREG_CONFIG, filter); + write_reg(MPUREG_CONFIG, filter); } ssize_t @@ -1156,17 +1117,6 @@ GYROSIM::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) write_reg(reg, val); } -void -GYROSIM::write_checked_reg(unsigned reg, uint8_t value) -{ - write_reg(reg, value); - for (uint8_t i=0; imeasure(); } -void -GYROSIM::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 VDev 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], GYROSIM_HIGH_BUS_SPEED)) != - _checked_values[_checked_next]) { - /* - if we get the wrong value then we know the VDev 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 gyrosim 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]); - // waiting 3ms between register writes seems - // to raise the chance of the sensor - // recovering considerably - _reset_wait = hrt_absolute_time() + 3000; - } - _register_wait = 20; - } - _checked_next = (_checked_next+1) % GYROSIM_NUM_CHECKED_REGISTERS; -} - void GYROSIM::measure() { - 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; @@ -1319,8 +1212,6 @@ GYROSIM::measure() */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - check_registers(); - // sensor transfer at high clock speed //set_frequency(GYROSIM_HIGH_BUS_SPEED); @@ -1509,16 +1400,6 @@ GYROSIM::print_info() perf_print_counter(_reset_retries); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); - PX4_WARN("checked_next: %u", _checked_next); - for (uint8_t i=0; i