diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index e95034e8e3..3de5625fd7 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -90,28 +90,37 @@ ORB_DECLARE(sensor_mag);
 /** set the mag internal sample rate to at least (arg) Hz */
 #define MAGIOCSSAMPLERATE	_MAGIOC(0)
 
+/** return the mag internal sample rate in Hz */
+#define MAGIOCGSAMPLERATE	_MAGIOC(1)
+
 /** set the mag internal lowpass filter to no lower than (arg) Hz */
-#define MAGIOCSLOWPASS		_MAGIOC(1)
+#define MAGIOCSLOWPASS		_MAGIOC(2)
+
+/** return the mag internal lowpass filter in Hz */
+#define MAGIOCGLOWPASS		_MAGIOC(3)
 
 /** set the mag scaling constants to the structure pointed to by (arg) */
-#define MAGIOCSSCALE		_MAGIOC(2)
+#define MAGIOCSSCALE		_MAGIOC(4)
 
 /** copy the mag scaling constants to the structure pointed to by (arg) */
-#define MAGIOCGSCALE		_MAGIOC(3)
+#define MAGIOCGSCALE		_MAGIOC(5)
 
 /** set the measurement range to handle (at least) arg Gauss */
-#define MAGIOCSRANGE		_MAGIOC(4)
+#define MAGIOCSRANGE		_MAGIOC(6)
+
+/** return the current mag measurement range in Gauss */
+#define MAGIOCGRANGE		_MAGIOC(7)
 
 /** perform self-calibration, update scale factors to canonical units */
-#define MAGIOCCALIBRATE		_MAGIOC(5)
+#define MAGIOCCALIBRATE		_MAGIOC(8)
 
 /** excite strap */
-#define MAGIOCEXSTRAP		_MAGIOC(6)
+#define MAGIOCEXSTRAP		_MAGIOC(9)
 
 /** perform self test and report status */
-#define MAGIOCSELFTEST		_MAGIOC(7)
+#define MAGIOCSELFTEST		_MAGIOC(10)
 
 /** determine if mag is external or onboard */
-#define MAGIOCGEXTERNAL		_MAGIOC(8)
+#define MAGIOCGEXTERNAL		_MAGIOC(11)
 
 #endif /* _DRV_MAG_H */
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 692f890bdd..d77f03bb72 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -77,8 +77,8 @@
 
 #define HMC5883L_ADDRESS		PX4_I2C_OBDEV_HMC5883
 
-/* Max measurement rate is 160Hz */
-#define HMC5883_CONVERSION_INTERVAL	(1000000 / 160)	/* microseconds */
+/* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
+#define HMC5883_CONVERSION_INTERVAL	(1000000 / 150)	/* microseconds */
 
 #define ADDR_CONF_A			0x00
 #define ADDR_CONF_B			0x01
@@ -191,6 +191,11 @@ private:
 	 */
 	void			stop();
 
+	/**
+	 * Reset the device
+	 */
+	int			reset();
+
 	/**
 	 * Perform the on-sensor scale calibration routine.
 	 *
@@ -365,6 +370,9 @@ HMC5883::init()
 	if (I2C::init() != OK)
 		goto out;
 
+	/* reset the device configuration */
+	reset();
+
 	/* allocate basic report buffers */
 	_num_reports = 2;
 	_reports = new struct mag_report[_num_reports];
@@ -381,9 +389,6 @@ HMC5883::init()
 	if (_mag_topic < 0)
 		debug("failed to create sensor_mag object");
 
-	/* set range */
-	set_range(_range_ga);
-
 	ret = OK;
 	/* sensor is ok, but not calibrated */
 	_sensor_ok = true;
@@ -542,68 +547,67 @@ int
 HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
-
 	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
+		switch (arg) {
+
+			/* switching to manual polling */
+		case SENSOR_POLLRATE_MANUAL:
+			stop();
+			_measure_ticks = 0;
+			return OK;
+
+			/* external signalling (DRDY) not supported */
+		case SENSOR_POLLRATE_EXTERNAL:
+
+			/* zero would be bad */
+		case 0:
+			return -EINVAL;
+
+			/* set default/max polling rate */
+		case SENSOR_POLLRATE_MAX:
+		case SENSOR_POLLRATE_DEFAULT: {
+				/* do we need to start internal polling? */
+				bool want_start = (_measure_ticks == 0);
+
+				/* set interval for next measurement to minimum legal value */
+				_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
+
+				/* if we need to start the poll state machine, do it */
+				if (want_start)
+					start();
 
-				/* switching to manual polling */
-			case SENSOR_POLLRATE_MANUAL:
-				stop();
-				_measure_ticks = 0;
 				return OK;
+			}
 
-				/* external signalling (DRDY) not supported */
-			case SENSOR_POLLRATE_EXTERNAL:
+			/* adjust to a legal polling interval in Hz */
+		default: {
+				/* do we need to start internal polling? */
+				bool want_start = (_measure_ticks == 0);
 
-				/* zero would be bad */
-			case 0:
-				return -EINVAL;
+				/* convert hz to tick interval via microseconds */
+				unsigned ticks = USEC2TICK(1000000 / arg);
 
-				/* set default/max polling rate */
-			case SENSOR_POLLRATE_MAX:
-			case SENSOR_POLLRATE_DEFAULT: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
+				/* check against maximum rate */
+				if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
+					return -EINVAL;
 
-					/* set interval for next measurement to minimum legal value */
-					_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
+				/* update interval for next measurement */
+				_measure_ticks = ticks;
 
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
+				/* if we need to start the poll state machine, do it */
+				if (want_start)
+					start();
 
-					return OK;
-				}
-
-				/* adjust to a legal polling interval in Hz */
-			default: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
-
-					/* convert hz to tick interval via microseconds */
-					unsigned ticks = USEC2TICK(1000000 / arg);
-
-					/* check against maximum rate */
-					if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
-						return -EINVAL;
-
-					/* update interval for next measurement */
-					_measure_ticks = ticks;
-
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
-
-					return OK;
-				}
+				return OK;
 			}
 		}
+	}
 
 	case SENSORIOCGPOLLRATE:
 		if (_measure_ticks == 0)
 			return SENSOR_POLLRATE_MANUAL;
 
-		return (1000 / _measure_ticks);
+		return 1000000/TICK2USEC(_measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
 			/* add one to account for the sentinel in the ring */
@@ -633,17 +637,24 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _num_reports - 1;
 
 	case SENSORIOCRESET:
-		/* XXX implement this */
-		return -EINVAL;
+		return reset();
 
 	case MAGIOCSSAMPLERATE:
-		/* not supported, always 1 sample per poll */
-		return -EINVAL;
+		/* same as pollrate because device is in single measurement mode*/
+		return ioctl(filp, SENSORIOCSPOLLRATE, arg);
+
+	case MAGIOCGSAMPLERATE:
+		/* same as pollrate because device is in single measurement mode*/
+		return 1000000/TICK2USEC(_measure_ticks);
 
 	case MAGIOCSRANGE:
 		return set_range(arg);
 
+	case MAGIOCGRANGE:
+		return _range_ga;
+
 	case MAGIOCSLOWPASS:
+	case MAGIOCGLOWPASS:
 		/* not supported, no internal filtering */
 		return -EINVAL;
 
@@ -697,6 +708,13 @@ HMC5883::stop()
 	work_cancel(HPWORK, &_work);
 }
 
+int
+HMC5883::reset()
+{
+	/* set range */
+	return set_range(_range_ga);
+}
+
 void
 HMC5883::cycle_trampoline(void *arg)
 {
@@ -861,10 +879,10 @@ HMC5883::collect()
 	} else {
 #endif
 		/* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
-		 * therefore switch and invert x and y */
+		 * therefore switch x and y and invert y */
 		_reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale;
 		/* flip axes and negate value for y */
-		_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
+		_reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
 		/* z remains z */
 		_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
 #ifdef PX4_I2C_BUS_ONBOARD
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index de6b753f12..5e0a2119a1 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;
@@ -210,6 +215,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.
@@ -273,6 +283,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
 	 *
@@ -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;
@@ -682,6 +684,14 @@ L3GD20::set_samplerate(unsigned frequency)
 	return OK;
 }
 
+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()
 {
@@ -701,6 +711,30 @@ L3GD20::stop()
 	hrt_cancel(&_call);
 }
 
+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)
 {
@@ -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 3e6ce64b81..efe7cf8eb6 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -54,6 +54,7 @@
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
+#include <systemlib/geo/geo.h>
 
 #include <nuttx/arch.h>
 #include <nuttx/clock.h>
@@ -168,6 +169,14 @@ static const int ERROR = -1;
 #define INT_CTRL_M              0x12
 #define INT_SRC_M               0x13
 
+/* default values for this device */
+#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 LSM303D_MAG_DEFAULT_RANGE_GA			2
+#define LSM303D_MAG_DEFAULT_RATE			100
 
 extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
 
@@ -180,56 +189,61 @@ public:
 	LSM303D(int bus, const char* path, spi_dev_e device);
 	virtual ~LSM303D();
 
-	virtual int			init();
+	virtual int		init();
 
 	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
-	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
 
 	/**
 	 * Diagnostics - print some basic information about the driver.
 	 */
-	void				print_info();
+	void			print_info();
 
 protected:
-	virtual int			probe();
+	virtual int		probe();
 
 	friend class 		LSM303D_mag;
 
 	virtual ssize_t		mag_read(struct file *filp, char *buffer, size_t buflen);
-	virtual int			mag_ioctl(struct file *filp, int cmd, unsigned long arg);
+	virtual int		mag_ioctl(struct file *filp, int cmd, unsigned long arg);
 
 private:
 
-	LSM303D_mag			*_mag;
+	LSM303D_mag		*_mag;
 
 	struct hrt_call		_accel_call;
 	struct hrt_call		_mag_call;
 
-	unsigned			_call_accel_interval;
-	unsigned			_call_mag_interval;
+	unsigned		_call_accel_interval;
+	unsigned		_call_mag_interval;
 
-	unsigned			_num_accel_reports;
+	unsigned		_num_accel_reports;
 	volatile unsigned	_next_accel_report;
 	volatile unsigned	_oldest_accel_report;
 	struct accel_report	*_accel_reports;
 
-	struct accel_scale	_accel_scale;
-	float				_accel_range_scale;
-	float				_accel_range_m_s2;
-	orb_advert_t		_accel_topic;
-
-	unsigned			_current_samplerate;
-
-	unsigned			_num_mag_reports;
+	unsigned		_num_mag_reports;
 	volatile unsigned	_next_mag_report;
 	volatile unsigned	_oldest_mag_report;
 	struct mag_report	*_mag_reports;
 
+	struct accel_scale	_accel_scale;
+	unsigned		_accel_range_m_s2;
+	float			_accel_range_scale;
+	unsigned		_accel_samplerate;
+	unsigned		_accel_onchip_filter_bandwith;
+
 	struct mag_scale	_mag_scale;
-	float				_mag_range_scale;
-	float				_mag_range_ga;
+	unsigned		_mag_range_ga;
+	float			_mag_range_scale;
+	unsigned		_mag_samplerate;
+
+	orb_advert_t		_accel_topic;
 	orb_advert_t		_mag_topic;
 
+	unsigned		_accel_read;
+	unsigned		_mag_read;
+
 	perf_counter_t		_accel_sample_perf;
 	perf_counter_t		_mag_sample_perf;
 
@@ -240,12 +254,19 @@ private:
 	/**
 	 * Start automatic measurement.
 	 */
-	void				start();
+	void			start();
 
 	/**
 	 * Stop automatic measurement.
 	 */
-	void				stop();
+	void			stop();
+
+	/**
+	 * Reset chip.
+	 *
+	 * Resets the chip and measurements ranges, but not scale and offset.
+	 */
+	void			reset();
 
 	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
@@ -256,24 +277,38 @@ private:
 	 *
 	 * @param arg		Instance pointer for the driver that is polling.
 	 */
-	static void			measure_trampoline(void *arg);
+	static void		measure_trampoline(void *arg);
 
 	/**
 	 * Static trampoline for the mag because it runs at a lower rate
 	 *
 	 * @param arg		Instance pointer for the driver that is polling.
 	 */
-	static void			mag_measure_trampoline(void *arg);
+	static void		mag_measure_trampoline(void *arg);
 
 	/**
 	 * Fetch accel measurements from the sensor and update the report ring.
 	 */
-	void				measure();
+	void			measure();
 
 	/**
 	 * Fetch mag measurements from the sensor and update the report ring.
 	 */
-	void				mag_measure();
+	void			mag_measure();
+
+	/**
+	 * Accel self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int			accel_self_test();
+
+	/**
+	 * Mag self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int			mag_self_test();
 
 	/**
 	 * Read a register from the LSM303D
@@ -281,7 +316,7 @@ private:
 	 * @param		The register to read.
 	 * @return		The value that was read.
 	 */
-	uint8_t				read_reg(unsigned reg);
+	uint8_t			read_reg(unsigned reg);
 
 	/**
 	 * Write a register in the LSM303D
@@ -289,7 +324,7 @@ private:
 	 * @param reg		The register to write.
 	 * @param value		The new value to write.
 	 */
-	void				write_reg(unsigned reg, uint8_t value);
+	void			write_reg(unsigned reg, uint8_t value);
 
 	/**
 	 * Modify a register in the LSM303D
@@ -300,7 +335,7 @@ private:
 	 * @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);
+	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
 
 	/**
 	 * Set the LSM303D accel measurement range.
@@ -309,7 +344,7 @@ private:
 	 *			Zero selects the maximum supported range.
 	 * @return		OK if the value can be supported, -ERANGE otherwise.
 	 */
-	int					set_range(unsigned max_g);
+	int			accel_set_range(unsigned max_g);
 
 	/**
 	 * Set the LSM303D mag measurement range.
@@ -318,24 +353,25 @@ private:
 	 *			Zero selects the maximum supported range.
 	 * @return		OK if the value can be supported, -ERANGE otherwise.
 	 */
-	int					mag_set_range(unsigned max_g);
+	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					set_antialias_filter_bandwidth(unsigned bandwith);
+	int			accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
 
 	/**
-	 * Get the LSM303D accel anti-alias filter.
+	 * Set the driver lowpass filter bandwidth.
 	 *
 	 * @param bandwidth The anti-alias filter bandwidth in Hz
-	 * @return		OK if the value was read and supported, ERROR otherwise.
+	 * 			Zero selects the highest bandwidth
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
 	 */
-	int					get_antialias_filter_bandwidth(unsigned &bandwidth);
+	int			accel_set_driver_lowpass_filter(float samplerate, float bandwidth);
 
 	/**
 	 * Set the LSM303D internal accel sampling frequency.
@@ -345,7 +381,7 @@ private:
 	 *			Zero selects the maximum rate supported.
 	 * @return		OK if the value can be supported.
 	 */
-	int					set_samplerate(unsigned frequency);
+	int			accel_set_samplerate(unsigned frequency);
 
 	/**
 	 * Set the LSM303D internal mag sampling frequency.
@@ -355,7 +391,7 @@ private:
 	 *			Zero selects the maximum rate supported.
 	 * @return		OK if the value can be supported.
 	 */
-	int					mag_set_samplerate(unsigned frequency);
+	int			mag_set_samplerate(unsigned frequency);
 };
 
 /**
@@ -396,38 +432,43 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_next_accel_report(0),
 	_oldest_accel_report(0),
 	_accel_reports(nullptr),
-	_accel_range_scale(0.0f),
-	_accel_range_m_s2(0.0f),
-	_accel_topic(-1),
-	_current_samplerate(0),
 	_num_mag_reports(0),
 	_next_mag_report(0),
 	_oldest_mag_report(0),
 	_mag_reports(nullptr),
-	_mag_range_scale(0.0f),
+	_accel_range_m_s2(0.0f),
+	_accel_range_scale(0.0f),
+	_accel_samplerate(0),
+	_accel_onchip_filter_bandwith(0),
 	_mag_range_ga(0.0f),
+	_mag_range_scale(0.0f),
+	_mag_samplerate(0),
+	_accel_topic(-1),
+	_mag_topic(-1),
+	_accel_read(0),
+	_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(800, 30),
-	_accel_filter_y(800, 30),
-	_accel_filter_z(800, 30)
+	_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;
 
 	// default scale factors
-	_accel_scale.x_offset = 0;
+	_accel_scale.x_offset = 0.0f;
 	_accel_scale.x_scale  = 1.0f;
-	_accel_scale.y_offset = 0;
+	_accel_scale.y_offset = 0.0f;
 	_accel_scale.y_scale  = 1.0f;
-	_accel_scale.z_offset = 0;
+	_accel_scale.z_offset = 0.0f;
 	_accel_scale.z_scale  = 1.0f;
 
-	_mag_scale.x_offset = 0;
+	_mag_scale.x_offset = 0.0f;
 	_mag_scale.x_scale = 1.0f;
-	_mag_scale.y_offset = 0;
+	_mag_scale.y_offset = 0.0f;
 	_mag_scale.y_scale = 1.0f;
-	_mag_scale.z_offset = 0;
+	_mag_scale.z_offset = 0.0f;
 	_mag_scale.z_scale = 1.0f;
 }
 
@@ -478,27 +519,12 @@ LSM303D::init()
 	if (_mag_reports == nullptr)
 		goto out;
 
+	reset();
+
 	/* advertise mag topic */
 	memset(&_mag_reports[0], 0, sizeof(_mag_reports[0]));
 	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);
 
-	/* enable accel, XXX do this with an ioctl? */
-	write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
-
-	/* enable mag, XXX do this with an ioctl? */
-	write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
-	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
-
-	/* XXX should we enable FIFO? */
-
-	set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */
-	set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */
-	set_samplerate(400); /* max sample rate */
-
-	mag_set_range(4); /* XXX: take highest sensor range of 12GA? */
-	mag_set_samplerate(100);
-
-	/* XXX test this when another mag is used */
 	/* do CDev init for the mag device node, keep it optional */
 	mag_ret = _mag->init();
 
@@ -511,6 +537,28 @@ out:
 	return ret;
 }
 
+void
+LSM303D::reset()
+{
+	/* enable accel*/
+	write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
+
+	/* enable mag */
+	write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
+	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
+
+	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(LSM303D_MAG_DEFAULT_RANGE_GA);
+	mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
+
+	_accel_read = 0;
+	_mag_read = 0;
+}
+
 int
 LSM303D::probe()
 {
@@ -612,64 +660,55 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 	switch (cmd) {
 
 	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
+		switch (arg) {
 
-				/* switching to manual polling */
+			/* switching to manual polling */
 			case SENSOR_POLLRATE_MANUAL:
 				stop();
 				_call_accel_interval = 0;
 				return OK;
 
-				/* external signalling not supported */
+			/* external signalling not supported */
 			case SENSOR_POLLRATE_EXTERNAL:
 
-				/* zero would be bad */
+			/* zero would be bad */
 			case 0:
 				return -EINVAL;
 
-				/* set default/max polling rate */
+			/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 				return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
 
 			case SENSOR_POLLRATE_DEFAULT:
-				/* With internal low pass filters enabled, 250 Hz is sufficient */
-				/* XXX for vibration tests with 800 Hz */
-				return ioctl(filp, SENSORIOCSPOLLRATE, 800);
+				return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {
-					/* do we need to start internal polling? */
-					bool want_start = (_call_accel_interval == 0);
+				/* do we need to start internal polling? */
+				bool want_start = (_call_accel_interval == 0);
 
-					/* convert hz to hrt interval via microseconds */
-					unsigned ticks = 1000000 / arg;
+				/* convert hz to hrt interval via microseconds */
+				unsigned ticks = 1000000 / arg;
 
-					/* check against maximum sane rate */
-					if (ticks < 1000)
-						return -EINVAL;
+				/* check against maximum sane rate */
+				if (ticks < 500)
+					return -EINVAL;
 
-					/* adjust sample rate of sensor */
-					set_samplerate(arg);
+				/* adjust filters */
+				accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq());
 
-                                        // adjust filters
-                                        float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
-                                        float sample_rate = 1.0e6f/ticks;
-                                        _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);
+				/* update interval for next measurement */
+				/* XXX this is a bit shady, but no other way to adjust... */
+				_accel_call.period = _call_accel_interval = ticks;
 
-					/* update interval for next measurement */
-					/* XXX this is a bit shady, but no other way to adjust... */
-					_accel_call.period = _call_accel_interval = ticks;
+				/* if we need to start the poll state machine, do it */
+				if (want_start)
+					start();
 
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
-
-					return OK;
-				}
+				return OK;
 			}
 		}
+	}
 
 	case SENSORIOCGPOLLRATE:
 		if (_call_accel_interval == 0)
@@ -678,66 +717,77 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000 / _call_accel_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* account for sentinel in the ring */
-			arg++;
+		/* account for sentinel in the ring */
+		arg++;
 
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 2) || (arg > 100))
+			return -EINVAL;
 
-			/* allocate new buffer */
-			struct accel_report *buf = new struct accel_report[arg];
+		/* allocate new buffer */
+		struct accel_report *buf = new struct accel_report[arg];
 
-			if (nullptr == buf)
-				return -ENOMEM;
+		if (nullptr == buf)
+			return -ENOMEM;
 
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _accel_reports;
-			_num_accel_reports = arg;
-			_accel_reports = buf;
-			start();
+		/* reset the measurement state machine with the new buffer, free the old */
+		stop();
+		delete[] _accel_reports;
+		_num_accel_reports = arg;
+		_accel_reports = buf;
+		start();
 
-			return OK;
-		}
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
 		return _num_accel_reports - 1;
 
 	case SENSORIOCRESET:
-		/* XXX implement */
-		return -EINVAL;
+		reset();
+		return OK;
+
+	case ACCELIOCSSAMPLERATE:
+		return accel_set_samplerate(arg);
+
+	case ACCELIOCGSAMPLERATE:
+		return _accel_samplerate;
 
 	case ACCELIOCSLOWPASS: {
-                float cutoff_freq_hz = arg;
-                float sample_rate = 1.0e6f / _call_accel_interval;
-                _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);
-                return OK;
-        }
+		return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg);
+	}
 
 	case ACCELIOCGLOWPASS:
-                return _accel_filter_x.get_cutoff_freq();
+		return _accel_filter_x.get_cutoff_freq();
 
-	case ACCELIOCSSCALE:
-		{
-			/* copy scale, but only if off by a few percent */
-			struct accel_scale *s = (struct accel_scale *) arg;
-			float sum = s->x_scale + s->y_scale + s->z_scale;
-			if (sum > 2.0f && sum < 4.0f) {
-				memcpy(&_accel_scale, s, sizeof(_accel_scale));
-				return OK;
-			} else {
-				return -EINVAL;
-			}
+	case ACCELIOCSSCALE: {
+		/* copy scale, but only if off by a few percent */
+		struct accel_scale *s = (struct accel_scale *) arg;
+		float sum = s->x_scale + s->y_scale + s->z_scale;
+		if (sum > 2.0f && sum < 4.0f) {
+			memcpy(&_accel_scale, s, sizeof(_accel_scale));
+			return OK;
+		} else {
+			return -EINVAL;
 		}
+	}
+
+	case ACCELIOCSRANGE:
+		/* arg needs to be in G */
+		return accel_set_range(arg);
+
+	case ACCELIOCGRANGE:
+		/* convert to m/s^2 and return rounded in G */
+		return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f);
 
 	case ACCELIOCGSCALE:
 		/* copy scale out */
 		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
 		return OK;
 
+	case ACCELIOCSELFTEST:
+		return accel_self_test();
+
 	default:
 		/* give it to the superclass */
 		return SPI::ioctl(filp, cmd, arg);
@@ -768,7 +818,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 			/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT:
-				/* 50 Hz is max for mag */
+				/* 100 Hz is max for mag */
 				return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100);
 
 			/* adjust to a legal polling interval in Hz */
@@ -783,9 +833,6 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 					if (ticks < 1000)
 						return -EINVAL;
 
-					/* adjust sample rate of sensor */
-					mag_set_samplerate(arg);
-
 					/* update interval for next measurement */
 					/* XXX this is a bit shady, but no other way to adjust... */
 					_mag_call.period = _call_mag_interval = ticks;
@@ -833,17 +880,18 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _num_mag_reports - 1;
 
 	case SENSORIOCRESET:
-		return ioctl(filp, cmd, arg);
+		reset();
+		return OK;
 
 	case MAGIOCSSAMPLERATE:
-//	case MAGIOCGSAMPLERATE:
-		/* XXX not implemented */
-		return -EINVAL;
+		return mag_set_samplerate(arg);
+
+	case MAGIOCGSAMPLERATE:
+		return _mag_samplerate;
 
 	case MAGIOCSLOWPASS:
-//	case MAGIOCGLOWPASS:
-		/* XXX not implemented */
-//		_set_dlpf_filter((uint16_t)arg);
+	case MAGIOCGLOWPASS:
+		/* not supported, no internal filtering */
 		return -EINVAL;
 
 	case MAGIOCSSCALE:
@@ -857,17 +905,13 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 		return OK;
 
 	case MAGIOCSRANGE:
-//	case MAGIOCGRANGE:
-		/* XXX not implemented */
-		// XXX change these two values on set:
-		// _mag_range_scale = xx
-		// _mag_range_ga = xx
-		return -EINVAL;
+		return mag_set_range(arg);
+
+	case MAGIOCGRANGE:
+		return _mag_range_ga;
 
 	case MAGIOCSELFTEST:
-		/* XXX not implemented */
-//		return self_test();
-		return -EINVAL;
+		return mag_self_test();
 
 	case MAGIOCGEXTERNAL:
 		/* no external mag board yet */
@@ -879,6 +923,53 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 	}
 }
 
+int
+LSM303D::accel_self_test()
+{
+	if (_accel_read == 0)
+		return 1;
+
+	/* inspect accel offsets */
+	if (fabsf(_accel_scale.x_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.y_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.z_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	return 0;
+}
+
+int
+LSM303D::mag_self_test()
+{
+	if (_mag_read == 0)
+		return 1;
+
+	/**
+	 * inspect mag offsets
+	 * don't check mag scale because it seems this is calibrated on chip
+	 */
+	if (fabsf(_mag_scale.x_offset) < 0.000001f)
+		return 1;
+
+	if (fabsf(_mag_scale.y_offset) < 0.000001f)
+		return 1;
+
+	if (fabsf(_mag_scale.z_offset) < 0.000001f)
+		return 1;
+
+	return 0;
+}
+
 uint8_t
 LSM303D::read_reg(unsigned reg)
 {
@@ -914,38 +1005,37 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 }
 
 int
-LSM303D::set_range(unsigned max_g)
+LSM303D::accel_set_range(unsigned max_g)
 {
 	uint8_t setbits = 0;
 	uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
-	float new_range_g = 0.0f;
 	float new_scale_g_digit = 0.0f;
 
 	if (max_g == 0)
 		max_g = 16;
 
 	if (max_g <= 2) {
-		new_range_g = 2.0f;
+		_accel_range_m_s2 = 2.0f*CONSTANTS_ONE_G;
 		setbits |= REG2_FULL_SCALE_2G_A;
 		new_scale_g_digit = 0.061e-3f;
 
 	} else if (max_g <= 4) {
-		new_range_g = 4.0f;
+		_accel_range_m_s2 = 4.0f*CONSTANTS_ONE_G;
 		setbits |= REG2_FULL_SCALE_4G_A;
 		new_scale_g_digit = 0.122e-3f;
 
 	} else if (max_g <= 6) {
-		new_range_g = 6.0f;
+		_accel_range_m_s2 = 6.0f*CONSTANTS_ONE_G;
 		setbits |= REG2_FULL_SCALE_6G_A;
 		new_scale_g_digit = 0.183e-3f;
 
 	} else if (max_g <= 8) {
-		new_range_g = 8.0f;
+		_accel_range_m_s2 = 8.0f*CONSTANTS_ONE_G;
 		setbits |= REG2_FULL_SCALE_8G_A;
 		new_scale_g_digit = 0.244e-3f;
 
 	} else if (max_g <= 16) {
-		new_range_g = 16.0f;
+		_accel_range_m_s2 = 16.0f*CONSTANTS_ONE_G;
 		setbits |= REG2_FULL_SCALE_16G_A;
 		new_scale_g_digit = 0.732e-3f;
 
@@ -953,8 +1043,7 @@ LSM303D::set_range(unsigned max_g)
 		return -EINVAL;
 	}
 
-	_accel_range_m_s2 = new_range_g * 9.80665f;
-	_accel_range_scale = new_scale_g_digit * 9.80665f;
+	_accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G;
 
 	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
 
@@ -966,29 +1055,28 @@ LSM303D::mag_set_range(unsigned max_ga)
 {
 	uint8_t setbits = 0;
 	uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
-	float new_range = 0.0f;
 	float new_scale_ga_digit = 0.0f;
 
 	if (max_ga == 0)
 		max_ga = 12;
 
 	if (max_ga <= 2) {
-		new_range = 2.0f;
+		_mag_range_ga = 2;
 		setbits |= REG6_FULL_SCALE_2GA_M;
 		new_scale_ga_digit = 0.080e-3f;
 
 	} else if (max_ga <= 4) {
-		new_range = 4.0f;
+		_mag_range_ga = 4;
 		setbits |= REG6_FULL_SCALE_4GA_M;
 		new_scale_ga_digit = 0.160e-3f;
 
 	} else if (max_ga <= 8) {
-		new_range = 8.0f;
+		_mag_range_ga = 8;
 		setbits |= REG6_FULL_SCALE_8GA_M;
 		new_scale_ga_digit = 0.320e-3f;
 
 	} else if (max_ga <= 12) {
-		new_range = 12.0f;
+		_mag_range_ga = 12;
 		setbits |= REG6_FULL_SCALE_12GA_M;
 		new_scale_ga_digit = 0.479e-3f;
 
@@ -996,7 +1084,6 @@ LSM303D::mag_set_range(unsigned max_ga)
 		return -EINVAL;
 	}
 
-	_mag_range_ga = new_range;
 	_mag_range_scale = new_scale_ga_digit;
 
 	modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
@@ -1005,7 +1092,7 @@ LSM303D::mag_set_range(unsigned max_ga)
 }
 
 int
-LSM303D::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;
@@ -1015,15 +1102,19 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth)
 
 	if (bandwidth <= 50) {
 		setbits |= REG2_AA_FILTER_BW_50HZ_A;
+		_accel_onchip_filter_bandwith = 50;
 
 	} else if (bandwidth <= 194) {
 		setbits |= REG2_AA_FILTER_BW_194HZ_A;
+		_accel_onchip_filter_bandwith = 194;
 
 	} else if (bandwidth <= 362) {
 		setbits |= REG2_AA_FILTER_BW_362HZ_A;
+		_accel_onchip_filter_bandwith = 362;
 
 	} else if (bandwidth <= 773) {
 		setbits |= REG2_AA_FILTER_BW_773HZ_A;
+		_accel_onchip_filter_bandwith = 773;
 
 	} else {
 		return -EINVAL;
@@ -1035,26 +1126,17 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth)
 }
 
 int
-LSM303D::get_antialias_filter_bandwidth(unsigned &bandwidth)
+LSM303D::accel_set_driver_lowpass_filter(float samplerate, float bandwidth)
 {
-	uint8_t readbits = read_reg(ADDR_CTRL_REG2);
-
-	if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_50HZ_A)
-		bandwidth = 50;
-	else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_194HZ_A)
-		bandwidth = 194;
-	else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_362HZ_A)
-		bandwidth = 362;
-	else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_773HZ_A)
-		bandwidth = 773;
-	else
-		return ERROR;
+	_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::set_samplerate(unsigned frequency)
+LSM303D::accel_set_samplerate(unsigned frequency)
 {
 	uint8_t setbits = 0;
 	uint8_t clearbits = REG1_RATE_BITS_A;
@@ -1064,23 +1146,23 @@ LSM303D::set_samplerate(unsigned frequency)
 
 	if (frequency <= 100) {
 		setbits |= REG1_RATE_100HZ_A;
-		_current_samplerate = 100;
+		_accel_samplerate = 100;
 
 	} else if (frequency <= 200) {
 		setbits |= REG1_RATE_200HZ_A;
-		_current_samplerate = 200;
+		_accel_samplerate = 200;
 
 	} else if (frequency <= 400) {
 		setbits |= REG1_RATE_400HZ_A;
-		_current_samplerate = 400;
+		_accel_samplerate = 400;
 
 	} else if (frequency <= 800) {
 		setbits |= REG1_RATE_800HZ_A;
-		_current_samplerate = 800;
+		_accel_samplerate = 800;
 
 	} else if (frequency <= 1600) {
 		setbits |= REG1_RATE_1600HZ_A;
-		_current_samplerate = 1600;
+		_accel_samplerate = 1600;
 
 	} else {
 		return -EINVAL;
@@ -1102,13 +1184,15 @@ LSM303D::mag_set_samplerate(unsigned frequency)
 
 	if (frequency <= 25) {
 		setbits |= REG5_RATE_25HZ_M;
+		_mag_samplerate = 25;
 
 	} else if (frequency <= 50) {
 		setbits |= REG5_RATE_50HZ_M;
+		_mag_samplerate = 50;
 
 	} else if (frequency <= 100) {
 		setbits |= REG5_RATE_100HZ_M;
-
+		_mag_samplerate = 100;
 
 	} else {
 		return -EINVAL;
@@ -1229,6 +1313,8 @@ LSM303D::measure()
 	/* publish for subscribers */
 	orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report);
 
+	_accel_read++;
+
 	/* stop the perf counter */
 	perf_end(_accel_sample_perf);
 }
@@ -1281,7 +1367,7 @@ LSM303D::mag_measure()
 	mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
 	mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
 	mag_report->scaling = _mag_range_scale;
-	mag_report->range_ga = _mag_range_ga;
+	mag_report->range_ga = (float)_mag_range_ga;
 
 	/* post a report to the ring - note, not locked */
 	INCREMENT(_next_mag_report, _num_mag_reports);
@@ -1297,6 +1383,8 @@ LSM303D::mag_measure()
 	/* publish for subscribers */
 	orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report);
 
+	_mag_read++;
+
 	/* stop the perf counter */
 	perf_end(_mag_sample_perf);
 }
@@ -1304,6 +1392,8 @@ LSM303D::mag_measure()
 void
 LSM303D::print_info()
 {
+	printf("accel reads:          %u\n", _accel_read);
+	printf("mag reads:            %u\n", _mag_read);
 	perf_print_counter(_accel_sample_perf);
 	printf("report queue:   %u (%u/%u @ %p)\n",
 	       _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports);
@@ -1466,7 +1556,7 @@ test()
 	/* check if mag is onboard or external */
 	if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)
 		errx(1, "failed to get if mag is onboard or external");
-	warnx("device active: %s", ret ? "external" : "onboard");
+	warnx("mag device active: %s", ret ? "external" : "onboard");
 
 	/* do a simple demand read */
 	sz = read(fd_mag, &m_report, sizeof(m_report));
@@ -1484,7 +1574,7 @@ test()
 
 	/* XXX add poll-rate tests here too */
 
-//	reset();
+	reset();
 	errx(0, "PASS");
 }
 
@@ -1503,7 +1593,17 @@ 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");
+
+	fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	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 pollrate reset failed");
+	}
 
 	exit(0);
 }
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index bfc74c73e9..0e65923db7 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -149,6 +149,15 @@
 #define MPU6000_REV_D9			0x59
 #define MPU6000_REV_D10			0x5A
 
+#define MPU6000_ACCEL_DEFAULT_RANGE_G			8
+#define MPU6000_ACCEL_DEFAULT_RATE			1000
+#define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ	30
+
+#define MPU6000_GYRO_DEFAULT_RANGE_G			8
+#define MPU6000_GYRO_DEFAULT_RATE			1000
+#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ		30
+
+#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ		42
 
 class MPU6000_gyro;
 
@@ -357,12 +366,12 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
 	_reads(0),
 	_sample_rate(1000),
 	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
-	_accel_filter_x(1000, 30),
-	_accel_filter_y(1000, 30),
-	_accel_filter_z(1000, 30),
-	_gyro_filter_x(1000, 30),
-        _gyro_filter_y(1000, 30),
-        _gyro_filter_z(1000, 30)
+	_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
+	_gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
+	_gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ)
 {
 	// disable debug() calls
 	_debug_enabled = false;
@@ -485,14 +494,13 @@ void MPU6000::reset()
 	up_udelay(1000);
 
 	// SAMPLE RATE
-	//write_reg(MPUREG_SMPLRT_DIV, 0x04);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
-	_set_sample_rate(_sample_rate); // default sample rate = 200Hz
+	_set_sample_rate(_sample_rate);
 	usleep(1000);
 
 	// FS & DLPF   FS=2000 deg/s, DLPF = 20Hz (low pass filter)
 	// was 90 Hz, but this ruins quality and does not improve the
 	// system response
-	_set_dlpf_filter(42);
+	_set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ);
 	usleep(1000);
 	// Gyro scale 2000 deg/s ()
 	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
@@ -535,8 +543,8 @@ void MPU6000::reset()
 
 	// Correct accel scale factors of 4096 LSB/g
 	// scale to m/s^2 ( 1g = 9.81 m/s^2)
-	_accel_range_scale = (9.81f / 4096.0f);
-	_accel_range_m_s2 = 8.0f * 9.81f;
+	_accel_range_scale = (CONSTANTS_ONE_G / 4096.0f);
+	_accel_range_m_s2 = 8.0f * CONSTANTS_ONE_G;
 
 	usleep(1000);
 
@@ -777,9 +785,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
+				return ioctl(filp, SENSORIOCSPOLLRATE, 1000);
+
 			case SENSOR_POLLRATE_DEFAULT:
-				/* set to same as sample rate per default */
-				return ioctl(filp, SENSORIOCSPOLLRATE, _sample_rate);
+				return ioctl(filp, SENSORIOCSPOLLRATE, MPU6000_ACCEL_DEFAULT_RATE);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {
@@ -867,9 +876,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		// XXX decide on relationship of both filters
 		// i.e. disable the on-chip filter
 		//_set_dlpf_filter((uint16_t)arg);
-                _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-                _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-                _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+		_accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+		_accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+		_accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		return OK;
 
 	case ACCELIOCSSCALE:
@@ -897,7 +906,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		// _accel_range_m_s2 = 8.0f * 9.81f;
 		return -EINVAL;
 	case ACCELIOCGRANGE:
-		return _accel_range_m_s2;
+		return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f);
 
 	case ACCELIOCSELFTEST:
 		return accel_self_test();
@@ -920,29 +929,29 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		return ioctl(filp, cmd, arg);
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 1) || (arg > 100))
-				return -EINVAL;
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
 
-			/* allocate new buffer */
-			GyroReportBuffer *buf = new GyroReportBuffer(arg);
+		/* allocate new buffer */
+		GyroReportBuffer *buf = new GyroReportBuffer(arg);
 
-			if (nullptr == buf)
-				return -ENOMEM;
-			if (buf->size() == 0) {
-				delete buf;
-				return -ENOMEM;
-			}
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete _gyro_reports;
-			_gyro_reports = buf;
-			start();
-
-			return OK;
+		if (nullptr == buf)
+			return -ENOMEM;
+		if (buf->size() == 0) {
+			delete buf;
+			return -ENOMEM;
 		}
 
+		/* reset the measurement state machine with the new buffer, free the old */
+		stop();
+		delete _gyro_reports;
+		_gyro_reports = buf;
+		start();
+
+		return OK;
+	}
+
 	case SENSORIOCGQUEUEDEPTH:
 		return _gyro_reports->size();
 
@@ -980,7 +989,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		// _gyro_range_rad_s = xx
 		return -EINVAL;
 	case GYROIOCGRANGE:
-		return _gyro_range_rad_s;
+		return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f);
 
 	case GYROIOCSELFTEST:
 		return gyro_self_test();
@@ -1400,7 +1409,7 @@ test()
 	warnx("acc  y:  \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
 	warnx("acc  z:  \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
 	warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
-	      (double)(a_report.range_m_s2 / 9.81f));
+	      (double)(a_report.range_m_s2 / CONSTANTS_ONE_G));
 
 	/* do a simple demand read */
 	sz = read(fd_gyro, &g_report, sizeof(g_report));
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 198da9f0ae..2ffa5f698c 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -919,11 +919,11 @@ Sensors::gyro_init()
 
 		#else
 
-		/* set the gyro internal sampling rate up to at leat 800Hz */
-		ioctl(fd, GYROIOCSSAMPLERATE, 800);
+		/* set the gyro internal sampling rate up to at least 760Hz */
+		ioctl(fd, GYROIOCSSAMPLERATE, 760);
 
-		/* set the driver to poll at 800Hz */
-		ioctl(fd, SENSORIOCSPOLLRATE, 800);
+		/* set the driver to poll at 760Hz */
+		ioctl(fd, SENSORIOCSPOLLRATE, 760);
 
 		#endif
 
@@ -936,6 +936,7 @@ void
 Sensors::mag_init()
 {
 	int	fd;
+	int	ret;
 
 	fd = open(MAG_DEVICE_PATH, 0);
 
@@ -944,13 +945,27 @@ Sensors::mag_init()
 		errx(1, "FATAL: no magnetometer found");
 	}
 
-	/* set the mag internal poll rate to at least 150Hz */
-	ioctl(fd, MAGIOCSSAMPLERATE, 150);
+	/* try different mag sampling rates */
 
-	/* set the driver to poll at 150Hz */
-	ioctl(fd, SENSORIOCSPOLLRATE, 150);
 
-	int ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
+	ret = ioctl(fd, MAGIOCSSAMPLERATE, 150);
+	if (ret == OK) {
+		/* set the pollrate accordingly */
+		ioctl(fd, SENSORIOCSPOLLRATE, 150);
+	} else {
+		ret = ioctl(fd, MAGIOCSSAMPLERATE, 100);
+		/* if the slower sampling rate still fails, something is wrong */
+		if (ret == OK) {
+			/* set the driver to poll also at the slower rate */
+			ioctl(fd, SENSORIOCSPOLLRATE, 100);
+		} else {
+			errx(1, "FATAL: mag sampling rate could not be set");
+		}
+	}
+
+	
+
+	ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
 	if (ret < 0)
 		errx(1, "FATAL: unknown if magnetometer is external or onboard");
 	else if (ret == 1)
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
index 5a02fd6206..188dafa4ef 100644
--- a/src/systemcmds/config/config.c
+++ b/src/systemcmds/config/config.c
@@ -2,6 +2,7 @@
  *
  *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Author: Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,6 +36,7 @@
 /**
  * @file config.c
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  *
  * config tool.
  */
@@ -69,27 +71,15 @@ config_main(int argc, char *argv[])
 {
 	if (argc >= 2) {
 		if (!strcmp(argv[1], "gyro")) {
-			if (argc >= 3) {
-				do_gyro(argc - 2, argv + 2);
-			} else {
-				errx(1, "not enough parameters.");
-			}
+			do_gyro(argc - 2, argv + 2);
 		}
 
 		if (!strcmp(argv[1], "accel")) {
-			if (argc >= 3) {
-				do_accel(argc - 2, argv + 2);
-			} else {
-				errx(1, "not enough parameters.");
-			}
+			do_accel(argc - 2, argv + 2);
 		}
 
 		if (!strcmp(argv[1], "mag")) {
-			if (argc >= 3) {
-				do_mag(argc - 2, argv + 2);
-			} else {
-				errx(1, "not enough parameters.");
-			}
+			do_mag(argc - 2, argv + 2);
 		}
 	}
 	
@@ -100,6 +90,7 @@ static void
 do_gyro(int argc, char *argv[])
 {
 	int	fd;
+	int	ret;
 
 	fd = open(GYRO_DEVICE_PATH, 0);
 
@@ -109,44 +100,45 @@ do_gyro(int argc, char *argv[])
 
 	} else {
 
-		if (argc >= 2) {
+		if (argc == 2 && !strcmp(argv[0], "sampling")) {
 
-			char* end;
-			int i = strtol(argv[1],&end,10);
+			/* set the gyro internal sampling rate up to at least i Hz */
+			ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
 
-			if (!strcmp(argv[0], "sampling")) {
+			if (ret)
+				errx(ret,"sampling rate could not be set");
 
-				/* set the accel internal sampling rate up to at leat i Hz */
-				ioctl(fd, GYROIOCSSAMPLERATE, i);
+		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
 
-			} else if (!strcmp(argv[0], "rate")) {
+			/* set the driver to poll at i Hz */
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
 
-				/* set the driver to poll at i Hz */
-				ioctl(fd, SENSORIOCSPOLLRATE, i);
-			} else if (!strcmp(argv[0], "range")) {
+			if (ret)
+				errx(ret,"pollrate could not be set");
 
-				/* set the range to i dps */
-				ioctl(fd, GYROIOCSRANGE, i);
-			}
+		} else if (argc == 2 && !strcmp(argv[0], "range")) {
 
-		} else if (argc > 0) {
+			/* set the range to i dps */
+			ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0));
 
-			if(!strcmp(argv[0], "check")) {
-				int ret = ioctl(fd, GYROIOCSELFTEST, 0);
+			if (ret)
+				errx(ret,"range could not be set");
 
-				if (ret) {
-					warnx("gyro self test FAILED! Check calibration:");
-					struct gyro_scale scale;
-					ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
-					warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
-					warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
-				} else {
-					warnx("gyro calibration and self test OK");
-				}
+		} else if(argc == 1 && !strcmp(argv[0], "check")) {
+			ret = ioctl(fd, GYROIOCSELFTEST, 0);
+
+			if (ret) {
+				warnx("gyro self test FAILED! Check calibration:");
+				struct gyro_scale scale;
+				ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
+				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+			} else {
+				warnx("gyro calibration and self test OK");
 			}
 
 		} else {
-			warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t");
+			errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t");
 		}
 
 		int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0);
@@ -165,6 +157,7 @@ static void
 do_mag(int argc, char *argv[])
 {
 	int fd;
+	int ret;
 
 	fd = open(MAG_DEVICE_PATH, 0);
 
@@ -174,31 +167,52 @@ do_mag(int argc, char *argv[])
 
 	} else {
 
-		if (argc > 0) {
+		if (argc == 2 && !strcmp(argv[0], "sampling")) {
 
-			if (!strcmp(argv[0], "check")) {
-				int ret = ioctl(fd, MAGIOCSELFTEST, 0);
+			/* set the mag internal sampling rate up to at least i Hz */
+			ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
 
-				if (ret) {
-					warnx("mag self test FAILED! Check calibration.");
-					struct mag_scale scale;
-					ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
-					warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
-					warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
-				} else {
-					warnx("mag calibration and self test OK");
-				}
+			if (ret)
+				errx(ret,"sampling rate could not be set");
+
+		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
+
+			/* set the driver to poll at i Hz */
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"pollrate could not be set");
+
+		} else if (argc == 2 && !strcmp(argv[0], "range")) {
+
+			/* set the range to i G */
+			ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"range could not be set");
+
+		} else if(argc == 1 && !strcmp(argv[0], "check")) {
+			ret = ioctl(fd, MAGIOCSELFTEST, 0);
+
+			if (ret) {
+				warnx("mag self test FAILED! Check calibration:");
+				struct mag_scale scale;
+				ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
+				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+			} else {
+				warnx("mag calibration and self test OK");
 			}
 
 		} else {
-			warnx("no arguments given. Try: \n\n\t'check' or 'info'\n\t");
+			errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t");
 		}
 
-		int srate = -1;//ioctl(fd, MAGIOCGSAMPLERATE, 0);
+		int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0);
 		int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
-		int range = -1;//ioctl(fd, MAGIOCGRANGE, 0);
+		int range = ioctl(fd, MAGIOCGRANGE, 0);
 
-		warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range);
+		warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range);
 
 		close(fd);
 	}
@@ -210,6 +224,7 @@ static void
 do_accel(int argc, char *argv[])
 {
 	int	fd;
+	int	ret;
 
 	fd = open(ACCEL_DEVICE_PATH, 0);
 
@@ -219,50 +234,52 @@ do_accel(int argc, char *argv[])
 
 	} else {
 
-		if (argc >= 2) {
+		if (argc == 2 && !strcmp(argv[0], "sampling")) {
 
-			char* end;
-			int i = strtol(argv[1],&end,10);
+			/* set the accel internal sampling rate up to at least i Hz */
+			ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
 
-			if (!strcmp(argv[0], "sampling")) {
+			if (ret)
+				errx(ret,"sampling rate could not be set");
 
-				/* set the accel internal sampling rate up to at leat i Hz */
-				ioctl(fd, ACCELIOCSSAMPLERATE, i);
+		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
 
-			} else if (!strcmp(argv[0], "rate")) {
+			/* set the driver to poll at i Hz */
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
 
-				/* set the driver to poll at i Hz */
-				ioctl(fd, SENSORIOCSPOLLRATE, i);
-			} else if (!strcmp(argv[0], "range")) {
+			if (ret)
+				errx(ret,"pollrate could not be set");
 
-				/* set the range to i dps */
-				ioctl(fd, ACCELIOCSRANGE, i);
-			}
-		} else if (argc > 0) {
+		} else if (argc == 2 && !strcmp(argv[0], "range")) {
 
-			if (!strcmp(argv[0], "check")) {
-				int ret = ioctl(fd, ACCELIOCSELFTEST, 0);
+			/* set the range to i G */
+			ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0));
 
-				if (ret) {
-					warnx("accel self test FAILED! Check calibration.");
-					struct accel_scale scale;
-					ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
-					warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
-					warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
-				} else {
-					warnx("accel calibration and self test OK");
-				}
+			if (ret)
+				errx(ret,"range could not be set");
+
+		} else if(argc == 1 && !strcmp(argv[0], "check")) {
+			ret = ioctl(fd, ACCELIOCSELFTEST, 0);
+
+			if (ret) {
+				warnx("accel self test FAILED! Check calibration:");
+				struct accel_scale scale;
+				ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
+				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+			} else {
+				warnx("accel calibration and self test OK");
 			}
 
 		} else {
-			warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 2 G\n\t");
+			errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 4 G\n\t");
 		}
 
 		int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
 		int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
 		int range = ioctl(fd, ACCELIOCGRANGE, 0);
 
-		warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d m/s", srate, prate, range);
+		warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range);
 
 		close(fd);
 	}