diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index a71241cbeb..0254cf8b8e 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -311,15 +311,16 @@ then
 	then
 	fi
 
+	# Internal SPI bus
+	if lis3mdl -R 0 start
+	then
+	fi
+	
 	# Possible external compasses
 	if hmc5883 -X start
 	then
 	fi
 
-	# Internal SPI bus
-	if lis3mdl -R 2 start
-	then
-	fi
 fi
 
 if ver hwcmp PX4FMU_V5
diff --git a/src/drivers/lis3mdl/lis3mdl.cpp b/src/drivers/lis3mdl/lis3mdl.cpp
index 5db810f044..2cc0194ebc 100644
--- a/src/drivers/lis3mdl/lis3mdl.cpp
+++ b/src/drivers/lis3mdl/lis3mdl.cpp
@@ -128,6 +128,11 @@ public:
 	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
 	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
 
+	/**
+	 * Stop the automatic measurement state machine.
+	 */
+	void			stop();
+	
 	/**
 	 * Diagnostics - print some basic information about the driver.
 	 */
@@ -180,11 +185,6 @@ private:
 	 */
 	void			start();
 
-	/**
-	 * Stop the automatic measurement state machine.
-	 */
-	void			stop();
-
 	/**
 	 * Reset the device
 	 */
@@ -514,7 +514,7 @@ void LIS3MDL::check_conf(void)
 
 		if (reg_in != _cntl_reg4) {
 			perf_count(_conf_errors);
-			ret = write_reg(ADDR_CTRL_REG1, _cntl_reg4);
+			ret = write_reg(ADDR_CTRL_REG4, _cntl_reg4);
 
 			if (OK != ret) {
 				perf_count(_comms_errors);
@@ -787,7 +787,11 @@ LIS3MDL::start()
 void
 LIS3MDL::stop()
 {
-	work_cancel(HPWORK, &_work);
+	if (_measure_ticks > 0) {
+		/* ensure no new items are queued while we cancel this one */
+		_measure_ticks = 0;
+		work_cancel(HPWORK, &_work);
+	}
 }
 
 int
@@ -808,6 +812,10 @@ LIS3MDL::cycle_trampoline(void *arg)
 void
 LIS3MDL::cycle()
 {
+	if (_measure_ticks == 0) {
+		return;
+	}	
+	
 	/* collection phase? */
 	if (_collect_phase) {
 
@@ -845,13 +853,15 @@ LIS3MDL::cycle()
 
 	/* next phase is collection */
 	_collect_phase = true;
-
-	/* schedule a fresh cycle call when the measurement is done */
-	work_queue(HPWORK,
-		   &_work,
-		   (worker_t)&LIS3MDL::cycle_trampoline,
-		   this,
-		   USEC2TICK(LIS3MDL_CONVERSION_INTERVAL));
+		   
+	if (_measure_ticks > 0) {
+		/* schedule a fresh cycle call when the measurement is done */
+		work_queue(HPWORK,
+			   &_work,
+			   (worker_t)&LIS3MDL::cycle_trampoline,
+			   this,
+			   USEC2TICK(LIS3MDL_CONVERSION_INTERVAL));
+	}		   
 }
 
 int
@@ -934,32 +944,18 @@ LIS3MDL::collect()
 	/*
 	 * RAW outputs
 	 *
-	 * to align the sensor axes with the board, x and y need to be flipped
-	 * and y needs to be negated
 	 */
-	new_report.x_raw = report.y;
-	new_report.y_raw = -report.x;
-	/* z remains z */
+	new_report.x_raw = report.x;
+	new_report.y_raw = report.y;
 	new_report.z_raw = report.z;
 
-	/* scale values for output */
-
-	// XXX revisit for SPI part, might require a bus type IOCTL
-	unsigned dummy;
-	sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
-
-	if (sensor_is_onboard) {
-		// convert onboard so it matches offboard for the
-		// scaling below
-		report.y = -report.y;
-		report.x = -report.x;
-	}
-
-	/* the standard external mag by 3DR has x pointing to the
-	 * right, y pointing backwards, and z down, therefore switch x
-	 * and y and invert y */
-	xraw_f = -report.y;
-	yraw_f = report.x;
+	/* the LIS3MDL mag on Pixhawk Pro by Drotek has x pointing towards,
+	 * y pointing to the right, and z down, therefore no switch needed,
+	 * it is better to have no artificial rotation inside the 
+	 * driver and then use the startup script with -R command with the 
+	 * real rotation between the sensor and body frame */
+	xraw_f = report.x;
+	yraw_f = report.y;
 	zraw_f = report.z;
 
 	// apply user specified rotation
@@ -1011,37 +1007,12 @@ int LIS3MDL::calibrate(struct file *filp, unsigned enable)
 	struct mag_report report;
 	ssize_t sz;
 	int ret = 1;
-	uint8_t good_count = 0;
 
 	// XXX do something smarter here
 	int fd = (int)enable;
 
-	struct mag_calibration_s mscale_previous = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	struct mag_calibration_s mscale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
 	float sum_excited[3] = {0.0f, 0.0f, 0.0f};
-
-	/* expected axis scaling. The datasheet says that 766 will
-	 * be places in the X and Y axes and 713 in the Z
-	 * axis. Experiments show that in fact 766 is placed in X,
-	 * and 713 in Y and Z. This is relative to a base of 660
-	 * LSM/Ga, giving 1.16 and 1.08 */
-	float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
+	float sum_non_excited[3] = {0.0f, 0.0f, 0.0f};
 
 	/* start the sensor polling at 50 Hz */
 	if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
@@ -1051,30 +1022,14 @@ int LIS3MDL::calibrate(struct file *filp, unsigned enable)
 	}
 
 	/* Set to 4 Gauss */
-	if (OK != ioctl(filp, MAGIOCSRANGE, 4)) {
-		warnx("FAILED: MAGIOCSRANGE 4 Ga");
-		ret = 1;
-		goto out;
-	}
-
-	if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
-		warnx("FAILED: MAGIOCEXSTRAP 1");
-		ret = 1;
-		goto out;
-	}
-
-	if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
-		warn("FAILED: MAGIOCGSCALE 1");
-		ret = 1;
-		goto out;
-	}
-
-	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
-		warn("FAILED: MAGIOCSSCALE 1");
+	if (OK != ioctl(filp, MAGIOCSRANGE, 12)) {
+		warnx("FAILED: MAGIOCSRANGE 12 Ga");
 		ret = 1;
 		goto out;
 	}
 
+	usleep(20000);
+	
 	// discard 10 samples to let the sensor settle
 	for (uint8_t i = 0; i < 10; i++) {
 		struct pollfd fds;
@@ -1099,8 +1054,8 @@ int LIS3MDL::calibrate(struct file *filp, unsigned enable)
 		}
 	}
 
-	/* read the sensor up to 100x, stopping when we have 30 good values */
-	for (uint8_t i = 0; i < 100 && good_count < 30; i++) {
+	/* read the sensor up to 10x */
+	for (uint8_t i = 0; i < 10; i++) {
 		struct pollfd fds;
 
 		/* wait for data to be ready */
@@ -1122,63 +1077,106 @@ int LIS3MDL::calibrate(struct file *filp, unsigned enable)
 			goto out;
 		}
 
-		float cal[3] = {fabsf(expected_cal[0] / report.x),
-				fabsf(expected_cal[1] / report.y),
-				fabsf(expected_cal[2] / report.z)
-			       };
+		sum_non_excited[0] += report.x;
+		sum_non_excited[1] += report.y;
+		sum_non_excited[2] += report.z;	
+	}
+	
+	sum_non_excited[0] /= 10.0f;
+	sum_non_excited[1] /= 10.0f;
+	sum_non_excited[2] /= 10.0f;
+	
+	
+	if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
+		warnx("FAILED: MAGIOCEXSTRAP 1");
+		ret = 1;
+		goto out;
+	}
 
-		if (cal[0] > 0.7f && cal[0] < 1.35f &&
-		    cal[1] > 0.7f && cal[1] < 1.35f &&
-		    cal[2] > 0.7f && cal[2] < 1.35f) {
-			good_count++;
-			sum_excited[0] += cal[0];
-			sum_excited[1] += cal[1];
-			sum_excited[2] += cal[2];
+	usleep(60000);
+	
+	// discard 10 samples to let the sensor settle
+	for (uint8_t i = 0; i < 10; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = ::poll(&fds, 1, 2000);
+
+		if (ret != 1) {
+			warn("ERROR: TIMEOUT 1");
+			goto out;
+		}
+
+		/* now go get it */
+		sz = ::read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report)) {
+			warn("ERROR: READ 1");
+			ret = -EIO;
+			goto out;
 		}
 	}
 
-	if (good_count < 5) {
+	/* read the sensor up to 10x */
+	for (uint8_t i = 0; i < 10; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = ::poll(&fds, 1, 2000);
+
+		if (ret != 1) {
+			warn("ERROR: TIMEOUT 2");
+			goto out;
+		}
+
+		/* now go get it */
+		sz = ::read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report)) {
+			warn("ERROR: READ 2");
+			ret = -EIO;
+			goto out;
+		}
+
+		sum_excited[0] += report.x;
+		sum_excited[1] += report.y;
+		sum_excited[2] += report.z;	
+	}
+	
+	sum_excited[0] /= 10.0f;
+	sum_excited[1] /= 10.0f;
+	sum_excited[2] /= 10.0f;	
+
+	if (1.0f < fabsf(sum_excited[0] - sum_non_excited[0]) && fabsf(sum_excited[0] - sum_non_excited[0]) < 3.0f &&
+		1.0f < fabsf(sum_excited[1] - sum_non_excited[1]) && fabsf(sum_excited[1] - sum_non_excited[1]) < 3.0f &&
+		0.1f < fabsf(sum_excited[2] - sum_non_excited[2]) && fabsf(sum_excited[2] - sum_non_excited[2]) < 1.0f){
+		ret = OK;	
+	}
+	else
+	{
 		ret = -EIO;
 		goto out;
 	}
 
-	float scaling[3];
-
-	scaling[0] = sum_excited[0] / good_count;
-	scaling[1] = sum_excited[1] / good_count;
-	scaling[2] = sum_excited[2] / good_count;
-
-	/* set scaling in device */
-	mscale_previous.x_scale = 1.0f / scaling[0];
-	mscale_previous.y_scale = 1.0f / scaling[1];
-	mscale_previous.z_scale = 1.0f / scaling[2];
-
 	ret = OK;
 
 out:
 
-	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
-		warn("FAILED: MAGIOCSSCALE 2");
-	}
-
 	/* set back to normal mode */
 	/* Set to 4 Gauss */
 	if (OK != ::ioctl(fd, MAGIOCSRANGE, 4)) {
 		warnx("FAILED: MAGIOCSRANGE 4 Ga");
 	}
-
+	
 	if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
 		warnx("FAILED: MAGIOCEXSTRAP 0");
 	}
 
-	if (ret == OK) {
-		if (check_scale()) {
-			/* failed */
-			warnx("FAILED: SCALE");
-			ret = PX4_ERROR;
-		}
-
-	}
+	usleep(20000);
 
 	return ret;
 }
@@ -1198,16 +1196,16 @@ int LIS3MDL::check_scale()
 	}
 
 	/* return 0 if calibrated, 1 else */
-	return !scale_valid;
+	return scale_valid;
 }
 
 int LIS3MDL::check_offset()
 {
 	bool offset_valid;
 
-	if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) &&
-	    (-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) &&
-	    (-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) {
+	if ((-0.6f < _scale.x_offset && _scale.x_offset < 0.6f) &&
+	    (-0.6f < _scale.y_offset && _scale.y_offset < 0.6f) &&
+	    (-0.6f < _scale.z_offset && _scale.z_offset < 0.6f)) {
 		/* offset is zero */
 		offset_valid = false;
 
@@ -1216,7 +1214,7 @@ int LIS3MDL::check_offset()
 	}
 
 	/* return 0 if calibrated, 1 else */
-	return !offset_valid;
+	return offset_valid;
 }
 
 int LIS3MDL::check_calibration()
@@ -1342,6 +1340,7 @@ struct lis3mdl_bus_option {
 #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
 
 void	start(enum LIS3MDL_BUS busid, enum Rotation rotation);
+int		stop();
 bool	start_bus(struct lis3mdl_bus_option &bus, enum Rotation rotation);
 struct lis3mdl_bus_option &find_bus(enum LIS3MDL_BUS busid);
 void	test(enum LIS3MDL_BUS busid);
@@ -1437,6 +1436,26 @@ start(enum LIS3MDL_BUS busid, enum Rotation rotation)
 	}
 }
 
+/**
+ * Stop the driver.
+ */
+int
+stop()
+{
+	bool stopped = false;
+
+	for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
+		if (bus_options[i].dev != nullptr) {
+			bus_options[i].dev->stop();
+			delete bus_options[i].dev;
+			bus_options[i].dev = nullptr;
+			stopped = true;
+		}
+	}
+
+	return !stopped;
+}
+
 /**
  * find a bus structure for a busid
  */
@@ -1531,45 +1550,19 @@ test(enum LIS3MDL_BUS busid)
 
 
 /**
- * Automatic scale calibration.
+ * Self test check.
  *
- * Basic idea:
- *
- *   output = (ext field +- 1.1 Ga self-test) * scale factor
- *
- * and consequently:
- *
- *   1.1 Ga = (excited - normal) * scale factor
- *   scale factor = (excited - normal) / 1.1 Ga
- *
- *   sxy = (excited - normal) / 766	| for conf reg. B set to 0x60 / Gain = 3
- *   sz  = (excited - normal) / 713	| for conf reg. B set to 0x60 / Gain = 3
- *
- * By subtracting the non-excited measurement the pure 1.1 Ga reading
- * can be extracted and the sensitivity of all axes can be matched.
+ * Unlike HMC5883, self test feature cannot be used to calculate 
+ * scale.
  *
  * SELF TEST OPERATION
- * To check the LIS3MDLL for proper operation, a self test feature in incorporated
- * in which the sensor offset straps are excited to create a nominal field strength
+ * To check the LIS3MDL for proper operation, a self test feature is incorporated :
+ * sensor offset straps are excited to create a nominal field strength
  * (bias field) to be measured. To implement self test, the least significant bits
- * (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias)
- * or 10 (negetive bias), e.g. 0x11 or 0x12.
- * Then, by placing the mode register into single-measurement mode (0x01),
- * two data acquisition cycles will be made on each magnetic vector.
- * The first acquisition will be a set pulse followed shortly by measurement
- * data of the external field. The second acquisition will have the offset strap
- * excited (about 10 mA) in the positive bias mode for X, Y, and Z axes to create
- * about a ±1.1 gauss self test field plus the external field. The first acquisition
- * values will be subtracted from the second acquisition, and the net measurement
- * will be placed into the data output registers.
- * Since self test adds ~1.1 Gauss additional field to the existing field strength,
- * using a reduced gain setting prevents sensor from being saturated and data registers
- * overflowed. For example, if the configuration register B is set to 0x60 (Gain=3),
- * values around +766 LSB (1.16 Ga * 660 LSB/Ga) will be placed in the X and Y data
- * output registers and around +713 (1.08 Ga * 660 LSB/Ga) will be placed in Z data
- * output register. To leave the self test mode, change MS1 and MS0 bit of the
- * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10.
- * Using the self test method described above, the user can scale sensor
+ * (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias).
+ * A few measurements are taken and stored with and without the additional magnetic
+ * field. According to ST datasheet, those values must stay between thresholds in order 
+ * to pass the self test.
  */
 int calibrate(enum LIS3MDL_BUS busid)
 {
@@ -1699,6 +1692,13 @@ lis3mdl_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	/*
+	 * Stop the driver.
+	 */	
+	if (!strcmp(verb, "stop")) {
+		return lis3mdl::stop();
+	}
+	
 	/*
 	 * Test the driver/device.
 	 */