diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 32030a1f77..ff56bff17b 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -108,8 +108,10 @@ static const int ERROR = -1;
 #define ADDR_CTRL_REG3			0x22
 #define ADDR_CTRL_REG4			0x23
 #define ADDR_CTRL_REG5			0x24
+#define ADDR_CTRL_REG6			0x25
 #define ADDR_CTRL_REG7			0x26
 
+#define REG1_RATE_BITS_A		((1<<7) | (1<<6) | (1<<5) | (1<<4))
 #define REG1_POWERDOWN_A		((0<<7) | (0<<6) | (0<<5) | (0<<4))
 #define REG1_RATE_3_125HZ_A		((0<<7) | (0<<6) | (0<<5) | (1<<4))
 #define REG1_RATE_6_25HZ_A		((0<<7) | (0<<6) | (1<<5) | (0<<4))
@@ -127,11 +129,13 @@ static const int ERROR = -1;
 #define REG1_Y_ENABLE_A			(1<<1)
 #define REG1_X_ENABLE_A			(1<<0)
 
+#define REG2_ANTIALIAS_FILTER_BW_BITS_A	((1<<7) | (1<<6))
 #define REG2_AA_FILTER_BW_773HZ_A		((0<<7) | (0<<6))
 #define REG2_AA_FILTER_BW_194HZ_A		((0<<7) | (1<<6))
 #define REG2_AA_FILTER_BW_362HZ_A		((1<<7) | (0<<6))
 #define REG2_AA_FILTER_BW_50HZ_A		((1<<7) | (1<<6))
 
+#define REG2_FULL_SCALE_BITS_A	((1<<5) | (1<<4) | (1<<3))
 #define REG2_FULL_SCALE_2G_A	((0<<5) | (0<<4) | (0<<3))
 #define REG2_FULL_SCALE_4G_A	((0<<5) | (0<<4) | (1<<3))
 #define REG2_FULL_SCALE_6G_A	((0<<5) | (1<<4) | (0<<3))
@@ -143,6 +147,7 @@ static const int ERROR = -1;
 #define REG5_RES_HIGH_M			((1<<6) | (1<<5))
 #define REG5_RES_LOW_M			((0<<6) | (0<<5))
 
+#define REG5_RATE_BITS_M		((1<<4) | (1<<3) | (1<<2))
 #define REG5_RATE_3_125HZ_M		((0<<4) | (0<<3) | (0<<2))
 #define REG5_RATE_6_25HZ_M		((0<<4) | (0<<3) | (1<<2))
 #define REG5_RATE_12_5HZ_M		((0<<4) | (1<<3) | (0<<2))
@@ -151,6 +156,7 @@ static const int ERROR = -1;
 #define REG5_RATE_100HZ_M		((1<<4) | (0<<3) | (1<<2))
 #define REG5_RATE_DO_NOT_USE_M	((1<<4) | (1<<3) | (0<<2))
 
+#define REG6_FULL_SCALE_BITS_M	((1<<7) | (1<<6))
 #define REG6_FULL_SCALE_2GA_M	((0<<7) | (0<<6))
 #define REG6_FULL_SCALE_4GA_M	((0<<7) | (1<<6))
 #define REG6_FULL_SCALE_8GA_M	((1<<7) | (0<<6))
@@ -174,72 +180,66 @@ 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;
+	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;
+	float				_accel_range_scale;
+	float				_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
 
-	unsigned		_num_mag_reports;
+	unsigned			_num_mag_reports;
 	volatile unsigned	_next_mag_report;
 	volatile unsigned	_oldest_mag_report;
 	struct mag_report	*_mag_reports;
 
 	struct mag_scale	_mag_scale;
-	float			_mag_range_scale;
-	float			_mag_range_ga;
+	float				_mag_range_scale;
+	float				_mag_range_ga;
 	orb_advert_t		_mag_topic;
 
-	unsigned		_current_accel_rate;
-	unsigned		_current_accel_range;
-
-	unsigned		_current_mag_rate;
-	unsigned		_current_mag_range;
-
 	perf_counter_t		_accel_sample_perf;
 	perf_counter_t		_mag_sample_perf;
 
 	/**
 	 * Start automatic measurement.
 	 */
-	void			start();
+	void				start();
 
 	/**
 	 * Stop automatic measurement.
 	 */
-	void			stop();
+	void				stop();
 
 	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
@@ -250,24 +250,24 @@ 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();
 
 	/**
 	 * Read a register from the LSM303D
@@ -275,7 +275,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
@@ -283,7 +283,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
@@ -294,27 +294,54 @@ 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 measurement range.
+	 * Set the LSM303D accel measurement range.
 	 *
-	 * @param max_dps	The measurement range is set to permit reading at least
-	 *			this rate in degrees per second.
+	 * @param max_g	The measurement range of the accel is in g (9.81m/s^2)
 	 *			Zero selects the maximum supported range.
 	 * @return		OK if the value can be supported, -ERANGE otherwise.
 	 */
-	int			set_range(unsigned max_dps);
+	int					set_range(unsigned max_g);
 
 	/**
-	 * Set the LSM303D internal sampling frequency.
+	 * Set the LSM303D mag measurement range.
 	 *
-	 * @param frequency	The internal sampling frequency is set to not less than
+	 * @param max_ga	The measurement range of the mag is in Ga
+	 *			Zero selects the maximum supported range.
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int					mag_set_range(unsigned max_g);
+
+	/**
+	 * Set the LSM303D accel anti-alias filter.
+	 *
+	 * @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 max_g);
+
+	/**
+	 * Set the LSM303D internal accel sampling frequency.
+	 *
+	 * @param frequency	The internal accel sampling frequency is set to not less than
 	 *			this value.
 	 *			Zero selects the maximum rate supported.
 	 * @return		OK if the value can be supported.
 	 */
-	int			set_samplerate(unsigned frequency);
+	int					set_samplerate(unsigned frequency);
+
+	/**
+	 * Set the LSM303D internal mag sampling frequency.
+	 *
+	 * @param frequency	The internal mag sampling frequency is set to not less than
+	 *			this value.
+	 *			Zero selects the maximum rate supported.
+	 * @return		OK if the value can be supported.
+	 */
+	int					mag_set_samplerate(unsigned frequency);
 };
 
 /**
@@ -327,18 +354,18 @@ public:
 	~LSM303D_mag();
 
 	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);
 
 protected:
 	friend class LSM303D;
 
-	void			parent_poll_notify();
+	void				parent_poll_notify();
 private:
-	LSM303D			*_parent;
+	LSM303D				*_parent;
 
-	void			measure();
+	void				measure();
 
-	void			measure_trampoline(void *arg);
+	void				measure_trampoline(void *arg);
 };
 
 
@@ -364,19 +391,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_mag_reports(nullptr),
 	_mag_range_scale(0.0f),
 	_mag_range_ga(0.0f),
-	_current_accel_rate(0),
-	_current_accel_range(0),
-	_current_mag_rate(0),
-	_current_mag_range(0),
 	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
 	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read"))
 {
 	// enable debug() calls
 	_debug_enabled = true;
 
-	/* XXX fix this default values */
-	_accel_range_scale = 1.0f;
-	_mag_range_scale = 1.0f;
+	_mag_range_scale = 12.0f/32767.0f;
 
 	// default scale factors
 	_accel_scale.x_offset = 0;
@@ -446,18 +467,21 @@ LSM303D::init()
 	memset(&_mag_reports[0], 0, sizeof(_mag_reports[0]));
 	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);
 
-	/* XXX do this with ioctls */
-	/* set default configuration */
-	write_reg(ADDR_CTRL_REG1, REG1_RATE_400HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A);
+	/* 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);
+
+	/* enable mag, XXX do this with an ioctl? */
 	write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
-	write_reg(ADDR_CTRL_REG5, REG5_RATE_100HZ_M | REG5_RES_HIGH_M);
+	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
 
-	/* XXX should we enable FIFO */
+	/* XXX should we enable FIFO? */
 
-	set_range(500);				/* default to 500dps */
-	set_samplerate(0);			/* max sample rate */
+	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(194); /* XXX: choose bandwidth: 50, 194, 362 or 773 Hz */
+	set_samplerate(400); /* max sample rate */
 
-//	_current_accel_rate = 100;
+	mag_set_range(12); /* 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 */
@@ -590,9 +614,13 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 				/* 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 */
-				return ioctl(filp, SENSORIOCSPOLLRATE, 250);
+				/* XXX for vibration tests with 800 Hz */
+				return ioctl(filp, SENSORIOCSPOLLRATE, 800);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {
@@ -606,6 +634,9 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 					if (ticks < 1000)
 						return -EINVAL;
 
+					/* adjust sample rate of sensor */
+					set_samplerate(arg);
+
 					/* update interval for next measurement */
 					/* XXX this is a bit shady, but no other way to adjust... */
 					_accel_call.period = _call_accel_interval = ticks;
@@ -801,33 +832,111 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 }
 
 int
-LSM303D::set_range(unsigned max_dps)
+LSM303D::set_range(unsigned max_g)
 {
-	/* XXX implement this */
-//	uint8_t bits = REG4_BDU;
-//
-//	if (max_dps == 0)
-//		max_dps = 2000;
-//
-//	if (max_dps <= 250) {
-//		_current_range = 250;
-//		bits |= RANGE_250DPS;
-//
-//	} else if (max_dps <= 500) {
-//		_current_range = 500;
-//		bits |= RANGE_500DPS;
-//
-//	} else if (max_dps <= 2000) {
-//		_current_range = 2000;
-//		bits |= RANGE_2000DPS;
-//
-//	} else {
-//		return -EINVAL;
-//	}
-//
-//	_gyro_range_rad_s = _current_range / 180.0f * M_PI_F;
-//	_gyro_range_scale = _gyro_range_rad_s / 32768.0f;
-//	write_reg(ADDR_CTRL_REG4, bits);
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
+	float new_range = 0.0f;
+
+	if (max_g == 0)
+		max_g = 16;
+
+	if (max_g <= 2) {
+		new_range = 2.0f;
+		setbits |= REG2_FULL_SCALE_2G_A;
+
+	} else if (max_g <= 4) {
+		new_range = 4.0f;
+		setbits |= REG2_FULL_SCALE_4G_A;
+
+	} else if (max_g <= 6) {
+		new_range = 6.0f;
+		setbits |= REG2_FULL_SCALE_6G_A;
+
+	} else if (max_g <= 8) {
+		new_range = 8.0f;
+		setbits |= REG2_FULL_SCALE_8G_A;
+
+	} else if (max_g <= 16) {
+		new_range = 16.0f;
+		setbits |= REG2_FULL_SCALE_16G_A;
+
+	} else {
+		return -EINVAL;
+	}
+
+	_accel_range_m_s2 = new_range * 9.80665f;
+	_accel_range_scale = _accel_range_m_s2 / 32768.0f;
+
+	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
+
+	return OK;
+}
+
+int
+LSM303D::mag_set_range(unsigned max_ga)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
+	float new_range = 0.0f;
+
+	if (max_ga == 0)
+		max_ga = 12;
+
+	if (max_ga <= 2) {
+		new_range = 2.0f;
+		setbits |= REG6_FULL_SCALE_2GA_M;
+
+	} else if (max_ga <= 4) {
+		new_range = 4.0f;
+		setbits |= REG6_FULL_SCALE_4GA_M;
+
+	} else if (max_ga <= 8) {
+		new_range = 8.0f;
+		setbits |= REG6_FULL_SCALE_8GA_M;
+
+	} else if (max_ga <= 12) {
+		new_range = 12.0f;
+		setbits |= REG6_FULL_SCALE_12GA_M;
+
+	} else {
+		return -EINVAL;
+	}
+
+	_mag_range_ga = new_range;
+	_mag_range_scale = _mag_range_ga / 32768.0f;
+
+	modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
+
+	return OK;
+}
+
+int
+LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
+
+	if (bandwidth == 0)
+		bandwidth = 773;
+
+	if (bandwidth <= 50) {
+		setbits |= REG2_AA_FILTER_BW_50HZ_A;
+
+	} else if (bandwidth <= 194) {
+		setbits |= REG2_AA_FILTER_BW_194HZ_A;
+
+	} else if (bandwidth <= 362) {
+		setbits |= REG2_AA_FILTER_BW_362HZ_A;
+
+	} else if (bandwidth <= 773) {
+		setbits |= REG2_AA_FILTER_BW_773HZ_A;
+
+	} else {
+		return -EINVAL;
+	}
+
+	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
 
 	return OK;
 }
@@ -835,33 +944,60 @@ LSM303D::set_range(unsigned max_dps)
 int
 LSM303D::set_samplerate(unsigned frequency)
 {
-	/* XXX implement this */
-//	uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
-//
-//	if (frequency == 0)
-//		frequency = 760;
-//
-//	if (frequency <= 95) {
-//		_current_rate = 95;
-//		bits |= RATE_95HZ_LP_25HZ;
-//
-//	} else if (frequency <= 190) {
-//		_current_rate = 190;
-//		bits |= RATE_190HZ_LP_25HZ;
-//
-//	} else if (frequency <= 380) {
-//		_current_rate = 380;
-//		bits |= RATE_380HZ_LP_30HZ;
-//
-//	} else if (frequency <= 760) {
-//		_current_rate = 760;
-//		bits |= RATE_760HZ_LP_30HZ;
-//
-//	} else {
-//		return -EINVAL;
-//	}
-//
-//	write_reg(ADDR_CTRL_REG1, bits);
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG1_RATE_BITS_A;
+
+	if (frequency == 0)
+		frequency = 1600;
+
+	if (frequency <= 100) {
+		setbits |= REG1_RATE_100HZ_A;
+
+	} else if (frequency <= 200) {
+		setbits |= REG1_RATE_200HZ_A;
+
+	} else if (frequency <= 400) {
+		setbits |= REG1_RATE_400HZ_A;
+
+	} else if (frequency <= 800) {
+		setbits |= REG1_RATE_800HZ_A;
+
+	} else if (frequency <= 1600) {
+		setbits |= REG1_RATE_1600HZ_A;
+
+	} else {
+		return -EINVAL;
+	}
+
+	modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
+
+	return OK;
+}
+
+int
+LSM303D::mag_set_samplerate(unsigned frequency)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG5_RATE_BITS_M;
+
+	if (frequency == 0)
+		frequency = 100;
+
+	if (frequency <= 25) {
+		setbits |= REG5_RATE_25HZ_M;
+
+	} else if (frequency <= 50) {
+		setbits |= REG5_RATE_50HZ_M;
+
+	} else if (frequency <= 100) {
+		setbits |= REG5_RATE_100HZ_M;
+
+
+	} else {
+		return -EINVAL;
+	}
+
+	modify_reg(ADDR_CTRL_REG5, clearbits, setbits);
 
 	return OK;
 }
@@ -956,8 +1092,8 @@ LSM303D::measure()
 	accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
 	accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
 	accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-//	report->scaling = _gyro_range_scale;
-//	report->range_rad_s = _gyro_range_rad_s;
+	accel_report->scaling = _accel_range_scale;
+	accel_report->range_m_s2 = _accel_range_m_s2;
 
 	/* post a report to the ring - note, not locked */
 	INCREMENT(_next_accel_report, _num_accel_reports);
@@ -1183,15 +1319,13 @@ test()
 	if (sz != sizeof(a_report))
 		err(1, "immediate read failed");
 
-	/* XXX fix the test output */
-//	warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x);
-//	warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y);
-//	warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z);
+	warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x);
+	warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y);
+	warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z);
 	warnx("accel x: \t%d\traw", (int)a_report.x_raw);
 	warnx("accel y: \t%d\traw", (int)a_report.y_raw);
 	warnx("accel z: \t%d\traw", (int)a_report.z_raw);
-//	warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2);
-
+	warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2);
 
 
 	int fd_mag = -1;
@@ -1209,10 +1343,13 @@ test()
 	if (sz != sizeof(m_report))
 		err(1, "immediate read failed");
 
-	/* XXX fix the test output */
+	warnx("mag x: \t% 9.5f\tga", (double)m_report.x);
+	warnx("mag y: \t% 9.5f\tga", (double)m_report.y);
+	warnx("mag z: \t% 9.5f\tga", (double)m_report.z);
 	warnx("mag x: \t%d\traw", (int)m_report.x_raw);
 	warnx("mag y: \t%d\traw", (int)m_report.y_raw);
 	warnx("mag z: \t%d\traw", (int)m_report.z_raw);
+	warnx("mag range: %8.4f ga", (double)m_report.range_ga);
 
 	/* XXX add poll-rate tests here too */