Merge pull request #2389 from mcharleb/gyrossim-cleanup-2

gyrosim: removed dead code from gyrosim

src/platforms/posix/drivers/gyrosim/gyrosim.cpp

@@ -81,69 +81,17 @@
 #define MPU_DEVICE_PATH_GYRO		"/dev/gyrosim_gyro"
 
 // MPU 6000 registers
-#define MPUREG_WHOAMI			0x75
 #define MPUREG_SMPLRT_DIV		0x19
 #define MPUREG_CONFIG			0x1A
 #define MPUREG_GYRO_CONFIG		0x1B
 #define MPUREG_ACCEL_CONFIG		0x1C
-#define MPUREG_FIFO_EN			0x23
 #define MPUREG_INT_PIN_CFG		0x37
 #define MPUREG_INT_ENABLE		0x38
 #define MPUREG_INT_STATUS		0x3A
-#define MPUREG_ACCEL_XOUT_H		0x3B
-#define MPUREG_ACCEL_XOUT_L		0x3C
-#define MPUREG_ACCEL_YOUT_H		0x3D
-#define MPUREG_ACCEL_YOUT_L		0x3E
-#define MPUREG_ACCEL_ZOUT_H		0x3F
-#define MPUREG_ACCEL_ZOUT_L		0x40
-#define MPUREG_TEMP_OUT_H		0x41
-#define MPUREG_TEMP_OUT_L		0x42
-#define MPUREG_GYRO_XOUT_H		0x43
-#define MPUREG_GYRO_XOUT_L		0x44
-#define MPUREG_GYRO_YOUT_H		0x45
-#define MPUREG_GYRO_YOUT_L		0x46
-#define MPUREG_GYRO_ZOUT_H		0x47
-#define MPUREG_GYRO_ZOUT_L		0x48
 #define MPUREG_USER_CTRL		0x6A
 #define MPUREG_PWR_MGMT_1		0x6B
 #define MPUREG_PWR_MGMT_2		0x6C
-#define MPUREG_FIFO_COUNTH		0x72
-#define MPUREG_FIFO_COUNTL		0x73
-#define MPUREG_FIFO_R_W			0x74
 #define MPUREG_PRODUCT_ID		0x0C
-#define MPUREG_TRIM1			0x0D
-#define MPUREG_TRIM2			0x0E
-#define MPUREG_TRIM3			0x0F
-#define MPUREG_TRIM4			0x10
-
-// Configuration bits MPU 3000 and MPU 6000 (not revised)?
-#define BIT_SLEEP			0x40
-#define BIT_H_RESET			0x80
-#define BITS_CLKSEL			0x07
-#define MPU_CLK_SEL_PLLGYROX		0x01
-#define MPU_CLK_SEL_PLLGYROZ		0x03
-#define MPU_EXT_SYNC_GYROX		0x02
-#define BITS_GYRO_ST_X			0x80
-#define BITS_GYRO_ST_Y			0x40
-#define BITS_GYRO_ST_Z			0x20
-#define BITS_FS_250DPS			0x00
-#define BITS_FS_500DPS			0x08
-#define BITS_FS_1000DPS			0x10
-#define BITS_FS_2000DPS			0x18
-#define BITS_FS_MASK			0x18
-#define BITS_DLPF_CFG_256HZ_NOLPF2	0x00
-#define BITS_DLPF_CFG_188HZ		0x01
-#define BITS_DLPF_CFG_98HZ		0x02
-#define BITS_DLPF_CFG_42HZ		0x03
-#define BITS_DLPF_CFG_20HZ		0x04
-#define BITS_DLPF_CFG_10HZ		0x05
-#define BITS_DLPF_CFG_5HZ		0x06
-#define BITS_DLPF_CFG_2100HZ_NOLPF	0x07
-#define BITS_DLPF_CFG_MASK		0x07
-#define BIT_INT_ANYRD_2CLEAR		0x10
-#define BIT_RAW_RDY_EN			0x01
-#define BIT_I2C_IF_DIS			0x10
-#define BIT_INT_STATUS_DATA		0x01
 
 // Product ID Description for GYROSIM
 // high 4 bits 	low 4 bits
@@ -161,16 +109,12 @@
 #define GYROSIM_REV_D9			0x59
 #define GYROSIM_REV_D10			0x5A
 
-#define GYROSIM_ACCEL_DEFAULT_RANGE_G			8
 #define GYROSIM_ACCEL_DEFAULT_RATE			1000
 #define GYROSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ	30
 
-#define GYROSIM_GYRO_DEFAULT_RANGE_G			8
 #define GYROSIM_GYRO_DEFAULT_RATE			1000
 #define GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ		30
 
-#define GYROSIM_DEFAULT_ONCHIP_FILTER_FREQ		42
-
 #define GYROSIM_ONE_G					9.80665f
 
 #ifdef PX4_SPI_BUS_EXT
@@ -184,9 +128,6 @@
   interrupt status registers. All other registers have a maximum 1MHz
   SPI speed
  */
-#define GYROSIM_LOW_BUS_SPEED				1000*1000
-#define GYROSIM_HIGH_BUS_SPEED				11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */
-
 class GYROSIM_gyro;
 
 class GYROSIM : public device::VDev
@@ -240,8 +181,6 @@ private:
 	perf_counter_t		_accel_reads;
 	perf_counter_t		_gyro_reads;
 	perf_counter_t		_sample_perf;
-	perf_counter_t		_bad_transfers;
-	perf_counter_t		_bad_registers;
 	perf_counter_t		_good_transfers;
 	perf_counter_t		_reset_retries;
 	perf_counter_t		_system_latency_perf;
@@ -301,8 +240,7 @@ private:
 	 * @param		The register to read.
 	 * @return		The value that was read.
 	 */
-	uint8_t			read_reg(unsigned reg, uint32_t speed=GYROSIM_LOW_BUS_SPEED);
+	uint8_t			read_reg(unsigned reg);
-	uint16_t		read_reg16(unsigned reg);
 
 	/**
 	 * Write a register in the GYROSIM
@@ -312,17 +250,6 @@ private:
 	 */
 	void			write_reg(unsigned reg, uint8_t value);
 
-	/**
-	 * Modify a register in the GYROSIM
-	 *
-	 * Bits are cleared before bits are set.
-	 *
-	 * @param reg		The register to modify.
-	 * @param clearbits	Bits in the register to clear.
-	 * @param setbits	Bits in the register to set.
-	 */
-	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
-
 	/**
 	 * Set the GYROSIM measurement range.
 	 *
@@ -357,11 +284,6 @@ private:
 	 */
 	 int 			gyro_self_test();
 
-	/*
-	  set low pass filter frequency
-	 */
-	void _set_dlpf_filter(uint16_t frequency_hz);
-
 	/*
 	  set sample rate (approximate) - 1kHz to 5Hz
 	*/
@@ -446,8 +368,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro
 	_accel_reads(perf_alloc(PC_COUNT, "gyrosim_accel_read")),
 	_gyro_reads(perf_alloc(PC_COUNT, "gyrosim_gyro_read")),
 	_sample_perf(perf_alloc(PC_ELAPSED, "gyrosim_read")),
-	_bad_transfers(perf_alloc(PC_COUNT, "gyrosim_bad_transfers")),
-	_bad_registers(perf_alloc(PC_COUNT, "gyrosim_bad_registers")),
 	_good_transfers(perf_alloc(PC_COUNT, "gyrosim_good_transfers")),
 	_reset_retries(perf_alloc(PC_COUNT, "gyrosim_reset_retries")),
 	_system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")),
@@ -515,8 +435,6 @@ GYROSIM::~GYROSIM()
 	perf_free(_sample_perf);
 	perf_free(_accel_reads);
 	perf_free(_gyro_reads);
-	perf_free(_bad_transfers);
-	perf_free(_bad_registers);
 	perf_free(_good_transfers);
 }
 
@@ -666,39 +584,6 @@ GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz)
 	_sample_rate = 1000 / div;
 }
 
-/*
-  set the DLPF filter frequency. This affects both accel and gyro.
- */
-void
-GYROSIM::_set_dlpf_filter(uint16_t frequency_hz)
-{
-	uint8_t filter;
-
-	/*
-	   choose next highest filter frequency available
-	 */
-        if (frequency_hz == 0) {
-		filter = BITS_DLPF_CFG_2100HZ_NOLPF;
-        } else if (frequency_hz <= 5) {
-		filter = BITS_DLPF_CFG_5HZ;
-	} else if (frequency_hz <= 10) {
-		filter = BITS_DLPF_CFG_10HZ;
-	} else if (frequency_hz <= 20) {
-		filter = BITS_DLPF_CFG_20HZ;
-	} else if (frequency_hz <= 42) {
-		filter = BITS_DLPF_CFG_42HZ;
-	} else if (frequency_hz <= 98) {
-		filter = BITS_DLPF_CFG_98HZ;
-	} else if (frequency_hz <= 188) {
-		filter = BITS_DLPF_CFG_188HZ;
-	} else if (frequency_hz <= 256) {
-		filter = BITS_DLPF_CFG_256HZ_NOLPF2;
-	} else {
-		filter = BITS_DLPF_CFG_2100HZ_NOLPF;
-	}
-	write_reg(MPUREG_CONFIG, filter);
-}
-
 ssize_t
 GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen)
 {
@@ -903,14 +788,12 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 					// adjust filters
 					float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
 					float sample_rate = 1.0e6f/ticks;
-					_set_dlpf_filter(cutoff_freq_hz);
 					_accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
 					_accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
 					_accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
 
 
 					float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
-					_set_dlpf_filter(cutoff_freq_hz_gyro);
 					_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
 					_gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
 					_gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
@@ -960,8 +843,6 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		return _accel_filter_x.get_cutoff_freq();
 
 	case ACCELIOCSLOWPASS:
-		// set hardware filtering
-		_set_dlpf_filter(arg);
 		// set software filtering
 		_accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		_accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
@@ -1038,7 +919,6 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		return _gyro_filter_x.get_cutoff_freq();
 	case GYROIOCSLOWPASS:
 		// set hardware filtering
-		_set_dlpf_filter(arg);
 		_gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		_gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		_gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
@@ -1073,31 +953,16 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 }
 
 uint8_t
-GYROSIM::read_reg(unsigned reg, uint32_t speed)
+GYROSIM::read_reg(unsigned reg)
 {
 	uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
 
-        // general register transfer at low clock speed
+	// general register transfer at low clock speed
-        //set_frequency(speed);
-
 	transfer(cmd, cmd, sizeof(cmd));
 
 	return cmd[1];
 }
 
-uint16_t
-GYROSIM::read_reg16(unsigned reg)
-{
-	uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
-
-        // general register transfer at low clock speed
-        //set_frequency(GYROSIM_LOW_BUS_SPEED);
-
-	transfer(cmd, cmd, sizeof(cmd));
-
-	return (uint16_t)(cmd[1] << 8) | cmd[2];
-}
-
 void
 GYROSIM::write_reg(unsigned reg, uint8_t value)
 {
@@ -1107,22 +972,9 @@ GYROSIM::write_reg(unsigned reg, uint8_t value)
 	cmd[1] = value;
 
         // general register transfer at low clock speed
-        //set_frequency(GYROSIM_LOW_BUS_SPEED);
-
 	transfer(cmd, nullptr, sizeof(cmd));
 }
 
-void
-GYROSIM::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
-{
-	uint8_t	val;
-
-	val = read_reg(reg);
-	val &= ~clearbits;
-	val |= setbits;
-	write_reg(reg, val);
-}
-
 int
 GYROSIM::set_accel_range(unsigned max_g_in)
 {
@@ -1234,7 +1086,7 @@ GYROSIM::measure()
 	// transfers and bad register reads. This allows the higher
 	// level code to decide if it should use this sensor based on
 	// whether it has had failures
-        grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
+        grb.error_count = arb.error_count = 0;
 
 	/*
 	 * 1) Scale raw value to SI units using scaling from datasheet.
@@ -1257,22 +1109,7 @@ GYROSIM::measure()
 	arb.x_raw = (int16_t)(mpu_report.accel_x / _accel_range_scale);
 	arb.y_raw = (int16_t)(mpu_report.accel_y / _accel_range_scale);
 	arb.z_raw = (int16_t)(mpu_report.accel_z / _accel_range_scale);
-/*
-	float xraw_f = (int16_t)(mpu_report.accel_x / _accel_range_scale);
-	float yraw_f = (int16_t)(mpu_report.accel_y / _accel_range_scale);
-	float zraw_f = (int16_t)(mpu_report.accel_z / _accel_range_scale);
 
-	// apply user specified rotation
-	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
-
-	float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-
-	arb.x = _accel_filter_x.apply(x_in_new);
-	arb.y = _accel_filter_y.apply(y_in_new);
-	arb.z = _accel_filter_z.apply(z_in_new);
-*/
 	arb.scaling = _accel_range_scale;
 	arb.range_m_s2 = _accel_range_m_s2;
 
@@ -1288,22 +1125,7 @@ GYROSIM::measure()
 	grb.x_raw = (int16_t)(mpu_report.gyro_x/_gyro_range_scale);
 	grb.y_raw = (int16_t)(mpu_report.gyro_y/_gyro_range_scale);
 	grb.z_raw = (int16_t)(mpu_report.gyro_z/_gyro_range_scale);
-/*
-	xraw_f = (int16_t)(mpu_report.gyro_x/_gyro_range_scale);
-	yraw_f = (int16_t)(mpu_report.gyro_y/_gyro_range_scale);
-	zraw_f = (int16_t)(mpu_report.gyro_z/_gyro_range_scale);
 
-	// apply user specified rotation
-	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
-
-	float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
-	float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
-	float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
-
-	grb.x = _gyro_filter_x.apply(x_gyro_in_new);
-	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
-	grb.z = _gyro_filter_z.apply(z_gyro_in_new);
-*/
 	grb.scaling = _gyro_range_scale;
 	grb.range_rad_s = _gyro_range_rad_s;
 
@@ -1344,13 +1166,11 @@ GYROSIM::print_info()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_accel_reads);
 	perf_print_counter(_gyro_reads);
-	perf_print_counter(_bad_transfers);
-	perf_print_counter(_bad_registers);
 	perf_print_counter(_good_transfers);
 	perf_print_counter(_reset_retries);
 	_accel_reports->print_info("accel queue");
 	_gyro_reports->print_info("gyro queue");
-	PX4_WARN("temperature: %.1f", (double)_last_temperature);
+	PX4_INFO("temperature: %.1f", (double)_last_temperature);
 }
 
 void
@@ -1360,18 +1180,18 @@ GYROSIM::print_registers()
 	int i=0;
 
 	buf[0] = '\0';
-	PX4_WARN("GYROSIM registers");
+	PX4_INFO("GYROSIM registers");
 	for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) {
 		uint8_t v = read_reg(reg);
 		sprintf(&buf[i*6], "%02x:%02x ",(unsigned)reg, (unsigned)v);
 		i++;
 		if ((i+1) % 13 == 0) {
-			PX4_WARN("%s", buf);
+			PX4_INFO("%s", buf);
 			i=0;
 			buf[i] = '\0';
 		}
 	}
-	PX4_WARN("%s",buf);
+	PX4_INFO("%s",buf);
 }
 
 
@@ -1677,9 +1497,9 @@ regdump()
 void
 usage()
 {
-	PX4_WARN("missing command: try 'start', 'info', 'test', 'stop', 'reset', 'regdump'");
+	PX4_INFO("missing command: try 'start', 'info', 'test', 'stop', 'reset', 'regdump'");
-	PX4_WARN("options:");
+	PX4_INFO("options:");
-	PX4_WARN("    -R rotation");
+	PX4_INFO("    -R rotation");
 }
 
 } // namespace
@@ -1697,7 +1517,7 @@ gyrosim_main(int argc, char *argv[])
 	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'R':
-			rotation = (enum Rotation)atoi(optarg);
+			rotation = (enum Rotation)atoi(myoptarg);
 			break;
 		default:
 			gyrosim::usage();