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