L3GD20 and LSM303D reset and range config working properly now

2 files changed, 136 insertions, 87 deletions

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index de6b753f1..5e0a2119a 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -154,6 +154,10 @@ static const int ERROR = -1;
 #define FIFO_CTRL_STREAM_TO_FIFO_MODE		(3<<5)
 #define FIFO_CTRL_BYPASS_TO_STREAM_MODE		(1<<7)
 
+#define L3GD20_DEFAULT_RATE			760
+#define L3GD20_DEFAULT_RANGE_DPS		2000
+#define L3GD20_DEFAULT_FILTER_FREQ		30
+
 extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
 
 class L3GD20 : public device::SPI
@@ -191,9 +195,10 @@ private:
 	orb_advert_t		_gyro_topic;
 
 	unsigned		_current_rate;
-	unsigned		_current_range;
 	unsigned		_orientation;
 
+	unsigned		_read;
+
 	perf_counter_t		_sample_perf;
 
 	math::LowPassFilter2p	_gyro_filter_x;
@@ -211,6 +216,11 @@ private:
 	void			stop();
 
 	/**
+	 * Reset the driver
+	 */
+	void			reset();
+
+	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
 	 *
@@ -274,6 +284,14 @@ private:
 	int			set_samplerate(unsigned frequency);
 
 	/**
+	 * Set the lowpass filter of the driver
+	 *
+	 * @param samplerate	The current samplerate
+	 * @param frequency	The cutoff frequency for the lowpass filter
+	 */
+	void			set_driver_lowpass_filter(float samplerate, float bandwidth);
+
+	/**
 	 * Self test
 	 *
 	 * @return 0 on success, 1 on failure
@@ -296,12 +314,12 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
 	_gyro_range_rad_s(0.0f),
 	_gyro_topic(-1),
 	_current_rate(0),
-	_current_range(0),
 	_orientation(SENSOR_BOARD_ROTATION_270_DEG),
-        _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
-        _gyro_filter_x(250, 30),
-        _gyro_filter_y(250, 30),
-        _gyro_filter_z(250, 30)
+	_read(0),
+	_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
+	_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
+	_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
+	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -349,22 +367,7 @@ L3GD20::init()
 	memset(&_reports[0], 0, sizeof(_reports[0]));
 	_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]);
 
-	/* set default configuration */
-	write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
-	write_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
-	write_reg(ADDR_CTRL_REG3, 0);		/* no interrupts - we don't use them */
-	write_reg(ADDR_CTRL_REG4, REG4_BDU);
-	write_reg(ADDR_CTRL_REG5, 0);
-
-	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);		/* disable wake-on-interrupt */
-
-        /* disable FIFO. This makes things simpler and ensures we
-         * aren't getting stale data. It means we must run the hrt
-         * callback fast enough to not miss data. */
-	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);	
-
-	set_range(2000);			/* default to 2000dps */
-	set_samplerate(0);			/* max sample rate */
+	reset();
 
 	ret = OK;
 out:
@@ -464,8 +467,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT:
-				/* With internal low pass filters enabled, 250 Hz is sufficient */
-				return ioctl(filp, SENSORIOCSPOLLRATE, 250);
+				return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {
@@ -483,12 +485,10 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 					/* XXX this is a bit shady, but no other way to adjust... */
 					_call.period = _call_interval = ticks;
 
-                                        // adjust filters
-                                        float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
-                                        float sample_rate = 1.0e6f/ticks;
-                                        _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-                                        _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-                                        _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+					/* adjust filters */
+					float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
+					float sample_rate = 1.0e6f/ticks;
+					set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start)
@@ -533,8 +533,8 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _num_reports - 1;
 
 	case SENSORIOCRESET:
-		/* XXX implement */
-		return -EINVAL;
+		reset();
+		return OK;
 
 	case GYROIOCSSAMPLERATE:
 		return set_samplerate(arg);
@@ -543,16 +543,15 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _current_rate;
 
 	case GYROIOCSLOWPASS: {
-                float cutoff_freq_hz = arg;
-                float sample_rate = 1.0e6f / _call_interval;
-                _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-                _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-                _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-                return OK;
-        }
+		float cutoff_freq_hz = arg;
+		float sample_rate = 1.0e6f / _call_interval;
+		set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);
+
+		return OK;
+	}
 
 	case GYROIOCGLOWPASS:
-                return _gyro_filter_x.get_cutoff_freq();
+		return _gyro_filter_x.get_cutoff_freq();
 
 	case GYROIOCSSCALE:
 		/* copy scale in */
@@ -565,10 +564,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return OK;
 
 	case GYROIOCSRANGE:
+		/* arg should be in dps */
 		return set_range(arg);
 
 	case GYROIOCGRANGE:
-		return _current_range;
+		/* convert to dps and round */
+		return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f);
 
 	case GYROIOCSELFTEST:
 		return self_test();
@@ -618,22 +619,23 @@ L3GD20::set_range(unsigned max_dps)
 {
 	uint8_t bits = REG4_BDU;
 	float new_range_scale_dps_digit;
+	float new_range;
 
 	if (max_dps == 0) {
 		max_dps = 2000;
 	}
 	if (max_dps <= 250) {
-		_current_range = 250;
+		new_range = 250;
 		bits |= RANGE_250DPS;
 		new_range_scale_dps_digit = 8.75e-3f;
 
 	} else if (max_dps <= 500) {
-		_current_range = 500;
+		new_range = 500;
 		bits |= RANGE_500DPS;
 		new_range_scale_dps_digit = 17.5e-3f;
 
 	} else if (max_dps <= 2000) {
-		_current_range = 2000;
+		new_range = 2000;
 		bits |= RANGE_2000DPS;
 		new_range_scale_dps_digit = 70e-3f;
 
@@ -641,7 +643,7 @@ L3GD20::set_range(unsigned max_dps)
 		return -EINVAL;
 	}
 
-	_gyro_range_rad_s = _current_range / 180.0f * M_PI_F;
+	_gyro_range_rad_s = new_range / 180.0f * M_PI_F;
 	_gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F;
 	write_reg(ADDR_CTRL_REG4, bits);
 
@@ -656,7 +658,7 @@ L3GD20::set_samplerate(unsigned frequency)
 	if (frequency == 0)
 		frequency = 760;
 
-        // use limits good for H or non-H models
+	/* use limits good for H or non-H models */
 	if (frequency <= 100) {
 		_current_rate = 95;
 		bits |= RATE_95HZ_LP_25HZ;
@@ -683,6 +685,14 @@ L3GD20::set_samplerate(unsigned frequency)
 }
 
 void
+L3GD20::set_driver_lowpass_filter(float samplerate, float bandwidth)
+{
+	_gyro_filter_x.set_cutoff_frequency(samplerate, bandwidth);
+	_gyro_filter_y.set_cutoff_frequency(samplerate, bandwidth);
+	_gyro_filter_z.set_cutoff_frequency(samplerate, bandwidth);
+}
+
+void
 L3GD20::start()
 {
 	/* make sure we are stopped first */
@@ -702,6 +712,30 @@ L3GD20::stop()
 }
 
 void
+L3GD20::reset()
+{
+	/* set default configuration */
+	write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
+	write_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
+	write_reg(ADDR_CTRL_REG3, 0);		/* no interrupts - we don't use them */
+	write_reg(ADDR_CTRL_REG4, REG4_BDU);
+	write_reg(ADDR_CTRL_REG5, 0);
+
+	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);		/* disable wake-on-interrupt */
+
+	/* disable FIFO. This makes things simpler and ensures we
+	 * aren't getting stale data. It means we must run the hrt
+	 * callback fast enough to not miss data. */
+	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
+
+	set_samplerate(L3GD20_DEFAULT_RATE);
+	set_range(L3GD20_DEFAULT_RANGE_DPS);
+	set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
+
+	_read = 0;
+}
+
+void
 L3GD20::measure_trampoline(void *arg)
 {
 	L3GD20 *dev = (L3GD20 *)arg;
@@ -804,6 +838,8 @@ L3GD20::measure()
 	if (_gyro_topic > 0)
 		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
 
+	_read++;
+
 	/* stop the perf counter */
 	perf_end(_sample_perf);
 }
@@ -811,6 +847,7 @@ L3GD20::measure()
 void
 L3GD20::print_info()
 {
+	printf("gyro reads:          %u\n", _read);
 	perf_print_counter(_sample_perf);
 	printf("report queue:   %u (%u/%u @ %p)\n",
 	       _num_reports, _oldest_report, _next_report, _reports);
@@ -949,7 +986,7 @@ reset()
 		err(1, "driver reset failed");
 
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		err(1, "driver poll restart failed");
+		err(1, "accel pollrate reset failed");
 
 	exit(0);
 }
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 803cd658f..efe7cf8eb 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -170,13 +170,13 @@ static const int ERROR = -1;
 #define INT_SRC_M               0x13
 
 /* default values for this device */
-#define ACCEL_DEFAULT_RANGE_G 8
-#define ACCEL_DEFAULT_SAMPLERATE 800
-#define ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50
-#define ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
+#define LSM303D_ACCEL_DEFAULT_RANGE_G			8
+#define LSM303D_ACCEL_DEFAULT_RATE			800
+#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ	50
+#define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ	30
 
-#define MAG_DEFAULT_RANGE_GA 2
-#define MAG_DEFAULT_SAMPLERATE 100
+#define LSM303D_MAG_DEFAULT_RANGE_GA			2
+#define LSM303D_MAG_DEFAULT_RATE			100
 
 extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
 
@@ -231,7 +231,7 @@ private:
 	unsigned		_accel_range_m_s2;
 	float			_accel_range_scale;
 	unsigned		_accel_samplerate;
-	unsigned		_accel_filter_bandwith;
+	unsigned		_accel_onchip_filter_bandwith;
 
 	struct mag_scale	_mag_scale;
 	unsigned		_mag_range_ga;
@@ -356,13 +356,22 @@ private:
 	int			mag_set_range(unsigned max_g);
 
 	/**
-	 * Set the LSM303D accel anti-alias filter.
+	 * Set the LSM303D on-chip anti-alias filter bandwith.
 	 *
 	 * @param bandwidth The anti-alias filter bandwidth in Hz
 	 * 			Zero selects the highest bandwidth
 	 * @return		OK if the value can be supported, -ERANGE otherwise.
 	 */
-	int			accel_set_antialias_filter_bandwidth(unsigned bandwith);
+	int			accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
+
+	/**
+	 * Set the driver lowpass filter bandwidth.
+	 *
+	 * @param bandwidth The anti-alias filter bandwidth in Hz
+	 * 			Zero selects the highest bandwidth
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			accel_set_driver_lowpass_filter(float samplerate, float bandwidth);
 
 	/**
 	 * Set the LSM303D internal accel sampling frequency.
@@ -430,7 +439,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_accel_range_m_s2(0.0f),
 	_accel_range_scale(0.0f),
 	_accel_samplerate(0),
-	_accel_filter_bandwith(0),
+	_accel_onchip_filter_bandwith(0),
 	_mag_range_ga(0.0f),
 	_mag_range_scale(0.0f),
 	_mag_samplerate(0),
@@ -440,9 +449,9 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_mag_read(0),
 	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
 	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
-	_accel_filter_x(0, 0),
-	_accel_filter_y(0, 0),
-	_accel_filter_z(0, 0)
+	_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -538,14 +547,13 @@ LSM303D::reset()
 	write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
 	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
 
-	_accel_filter_bandwith = ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
-
-	accel_set_range(ACCEL_DEFAULT_RANGE_G);
-	accel_set_samplerate(ACCEL_DEFAULT_SAMPLERATE);
-	accel_set_antialias_filter_bandwidth(ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
+	accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
+	accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
+	accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
+	accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
 
-	mag_set_range(MAG_DEFAULT_RANGE_GA);
-	mag_set_samplerate(MAG_DEFAULT_SAMPLERATE);
+	mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
+	mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
 
 	_accel_read = 0;
 	_mag_read = 0;
@@ -672,7 +680,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
 
 			case SENSOR_POLLRATE_DEFAULT:
-				return ioctl(filp, SENSORIOCSPOLLRATE, ACCEL_DEFAULT_SAMPLERATE);
+				return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {
@@ -687,11 +695,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 					return -EINVAL;
 
 				/* adjust filters */
-				float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
-
-				_accel_filter_x.set_cutoff_frequency((float)arg, cutoff_freq_hz);
-				_accel_filter_y.set_cutoff_frequency((float)arg, cutoff_freq_hz);
-				_accel_filter_z.set_cutoff_frequency((float)arg, cutoff_freq_hz);
+				accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq());
 
 				/* update interval for next measurement */
 				/* XXX this is a bit shady, but no other way to adjust... */
@@ -750,10 +754,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _accel_samplerate;
 
 	case ACCELIOCSLOWPASS: {
-		_accel_filter_x.set_cutoff_frequency((float)_accel_samplerate, (float)arg);
-		_accel_filter_y.set_cutoff_frequency((float)_accel_samplerate, (float)arg);
-		_accel_filter_z.set_cutoff_frequency((float)_accel_samplerate, (float)arg);
-		return OK;
+		return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg);
 	}
 
 	case ACCELIOCGLOWPASS:
@@ -1091,7 +1092,7 @@ LSM303D::mag_set_range(unsigned max_ga)
 }
 
 int
-LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth)
+LSM303D::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
 {
 	uint8_t setbits = 0;
 	uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
@@ -1101,19 +1102,19 @@ LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth)
 
 	if (bandwidth <= 50) {
 		setbits |= REG2_AA_FILTER_BW_50HZ_A;
-		_accel_filter_bandwith = 50;
+		_accel_onchip_filter_bandwith = 50;
 
 	} else if (bandwidth <= 194) {
 		setbits |= REG2_AA_FILTER_BW_194HZ_A;
-		_accel_filter_bandwith = 194;
+		_accel_onchip_filter_bandwith = 194;
 
 	} else if (bandwidth <= 362) {
 		setbits |= REG2_AA_FILTER_BW_362HZ_A;
-		_accel_filter_bandwith = 362;
+		_accel_onchip_filter_bandwith = 362;
 
 	} else if (bandwidth <= 773) {
 		setbits |= REG2_AA_FILTER_BW_773HZ_A;
-		_accel_filter_bandwith = 773;
+		_accel_onchip_filter_bandwith = 773;
 
 	} else {
 		return -EINVAL;
@@ -1125,6 +1126,16 @@ LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth)
 }
 
 int
+LSM303D::accel_set_driver_lowpass_filter(float samplerate, float bandwidth)
+{
+	_accel_filter_x.set_cutoff_frequency(samplerate, bandwidth);
+	_accel_filter_y.set_cutoff_frequency(samplerate, bandwidth);
+	_accel_filter_z.set_cutoff_frequency(samplerate, bandwidth);
+
+	return OK;
+}
+
+int
 LSM303D::accel_set_samplerate(unsigned frequency)
 {
 	uint8_t setbits = 0;
@@ -1582,15 +1593,16 @@ reset()
 		err(1, "driver reset failed");
 
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		err(1, "accel driver poll rate reset failed");
+		err(1, "accel pollrate reset failed");
 
-	int fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+	fd = open(MAG_DEVICE_PATH, O_RDONLY);
 
-	if (fd_mag < 0) {
-		warnx("could not open to mag " MAG_DEVICE_PATH);
+	if (fd < 0) {
+		warnx("mag could not be opened, external mag might be used");
 	} else {
+		/* no need to reset the mag as well, the reset() is the same */
 		if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-			err(1, "mag driver poll rate reset failed");
+			err(1, "mag pollrate reset failed");
 	}
 
 	exit(0);