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 <charlebm@gmail.com>

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);
 }
 

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; i<GYROSIM_NUM_CHECKED_REGISTERS; i++) {
-		if (reg == _checked_registers[i]) {
-			_checked_values[i] = value;
-		}
-	}
-}
-
 int
 GYROSIM::set_accel_range(unsigned max_g_in)
 {
@@ -1176,7 +1126,7 @@ GYROSIM::set_accel_range(unsigned max_g_in)
 		case GYROSIMES_REV_C5:
 		case GYROSIM_REV_C4:
 		case GYROSIM_REV_C5:
-			write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3);
+			write_reg(MPUREG_ACCEL_CONFIG, 1 << 3);
 			_accel_range_scale = (GYROSIM_ONE_G / 4096.0f);
 			_accel_range_m_s2 = 8.0f * GYROSIM_ONE_G;
 			return OK;
@@ -1204,7 +1154,7 @@ GYROSIM::set_accel_range(unsigned max_g_in)
 		max_accel_g = 2;
 	}
 
-	write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3);
+	write_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3);
 	_accel_range_scale = (GYROSIM_ONE_G / lsb_per_g);
 	_accel_range_m_s2 = max_accel_g * GYROSIM_ONE_G;
 
@@ -1240,66 +1190,9 @@ GYROSIM::measure_trampoline(void *arg)
 	dev->measure();
 }
 
-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<GYROSIM_NUM_CHECKED_REGISTERS; i++) {
-            uint8_t v = read_reg(_checked_registers[i], GYROSIM_HIGH_BUS_SPEED);
-            if (v != _checked_values[i]) {
-                PX4_WARN("reg %02x:%02x should be %02x",
-                         (unsigned)_checked_registers[i],
-                         (unsigned)v,
-                         (unsigned)_checked_values[i]);
-            }
-        }
 	PX4_WARN("temperature: %.1f", (double)_last_temperature);
 }