diff --git a/rover/include/roverapi/rover_sensor.hpp b/rover/include/roverapi/rover_sensor.hpp
new file mode 100644
index 0000000..85173c7
--- /dev/null
+++ b/rover/include/roverapi/rover_sensor.hpp
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2017 FH Dortmund and others
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * which accompanies this distribution, and is available at
+ * http://www.eclipse.org/legal/epl-v10.html
+ *
+ * Description:
+ *    Rover Sensor API - Interfaces for Rover sensors application development
+ *    Header file
+ *
+ * Contributors:
+ *    M.Ozcelikors <mozcelikors@gmail.com>, modified C++ API 04.12.2017
+ *    										created C++ API 17.11.2017
+ *    										initial QMC5883L driver implemented 30.11.2017
+ *    David Schmelter, Fraunhofer IEM - compass sensor initial implementation
+ *    Gael Blondelle, Eclipse Foundation - initial API and parameters
+ *
+ */
+
+#ifndef API_ROVER_SENSORS_HPP_
+#define API_ROVER_SENSORS_HPP_
+
+#include <stdint.h>
+#include <stdio.h>
+#include <unistd.h>
+
+
+//To debug the application, uncomment the following
+//#define DEBUG 1
+
+
+namespace rover
+{
+	/**
+	 * @brief RoverSensorID_t defines the sensor location ID of the proximity sensors on the rover
+	 */
+	enum RoverSensorID_t {	ROVER_REAR_RIGHT = 0,
+							ROVER_REAR_LEFT = 1,
+							ROVER_FRONT_RIGHT = 2,
+							ROVER_FRONT_LEFT = 3,
+							ROVER_FRONT = 4,
+							ROVER_REAR = 5};
+
+	/**
+	 * @brief RoverSensor class is an abstract class to define interface between common sensors used in the rover.
+	 */
+	class RoverSensor
+	{
+		public:
+			/**
+			 * @brief Virtual function (interface) for setting up sensors
+			 * @return void
+			 */
+			virtual void initialize (void) = 0;
+
+			/**
+			 * @brief Virtual function (interface) for reading from sensors
+			 * @return sensor_val Sensor value read from sensor
+			 */
+			virtual float read (void) = 0;
+
+			/**
+			 * @brief Destructor for the RoverSensor class
+			 */
+			virtual ~RoverSensor() {};
+
+	};
+}
+
+
+
+#endif /* API_ROVER_SENSORS_HPP_ */
diff --git a/rover/include/roverapi/rover_sensors.hpp b/rover/include/roverapi/rover_sensors.hpp
deleted file mode 100644
index 4eddcee..0000000
--- a/rover/include/roverapi/rover_sensors.hpp
+++ /dev/null
@@ -1,243 +0,0 @@
-/*
- * Copyright (c) 2017 FH Dortmund and others
- * All rights reserved. This program and the accompanying materials
- * are made available under the terms of the Eclipse Public License v1.0
- * which accompanies this distribution, and is available at
- * http://www.eclipse.org/legal/epl-v10.html
- *
- * Description:
- *    Rover Sensors API - Interfaces for Rover sensors application development
- *    Header file
- *
- * Contributors:
- *    M.Ozcelikors <mozcelikors@gmail.com>, created C++ API 17.11.2017
- *    										initial QMC5883L driver implemented 30.11.2017
- *    David Schmelter, Fraunhofer IEM - compass sensor initial implementation
- *    Gael Blondelle, Eclipse Foundation - initial API and parameters
- *
- */
-
-#ifndef API_ROVER_SENSORS_HPP_
-#define API_ROVER_SENSORS_HPP_
-
-#include <stdint.h>
-#include <stdio.h>
-#include <unistd.h>
-
-
-//To debug the application, uncomment the following
-//#define DEBUG 1
-
-
-namespace rover
-{
-	/**
-	 * @brief RoverSensors class contains member functions to setup and read from embedded sensors from rover's RoverSenseLayer sensor layer.
-	 */
-	class RoverSensors
-	{
-		private:
-			/* Pins of ultrasonic sensors */
-			/**
-			 * @brief Groove Sensor Pin for Front Sensor Socket (in wiringPi format)
-			 */
-			static const int SIG0 = 0; //BCM-17   ->  WiringPi 0   //Same as ECHO0 pin, if some one wants to replace front sr04 with groove sensor
-
-			/**
-			 *  @brief Groove Sensor Pin for Rear Sensor Socket (in wiringPi format)
-			 */
-			static const int SIG1 = 2; //BCM-27   ->  WiringPi 2   //Same as ECHO1 pin, if some one wants to replace back sr04 with groove sensor
-
-			/**
-			 *  @brief HC-SR04 Sensor Trigger Pin for Front Sensor Socket (in wiringPi format)
-			 */
-			static const int TRIG0 = 7; //BCM-4   ->  WiringPi 7
-			/**
-			 *  @brief HC-SR04 Sensor Echo Pin for Front Sensor Socket (in wiringPi format)
-			 */
-			static const int ECHO0 = 0; //BCM-17   ->  WiringPi 0
-
-			/**
-			 *  @brief HC-SR04 Sensor Trigger Pin for Rear Sensor Socket (in wiringPi format)
-			 */
-			static const int TRIG1 = 1; //BCM-18   ->  WiringPi 1
-			/**
-			 *  @brief HC-SR04 Sensor Echo Pin for Rear Sensor Socket (in wiringPi format)
-			 */
-			static const int ECHO1 = 2; //BCM-27   ->  WiringPi 2
-
-			/* Definitions related to DHT22 sensor */
-			/**
-			 *  @brief DHT22 sensor pin in wiringPi format
-			 */
-			static const int DHT22_RPI_PIN = 24;  //BCM19  -> wiringPi 24
-			static const int MAX_TIMINGS = 85;
-
-			/* Definitions related to compass sensor */
-			/**
-			 *  @brief Address for compass sensor
-			 */
-			int HMC588L_ADDRESS = 0x1E; //0x1E;
-			/**
-			 *  @brief Calibration duration for compass sensor
-			 */
-			int CALIBRATION_DURATION = 10000; //compass calibration has a duration of 5 seconds
-			/**
-			 *  @brief Declination angle / correction factor for compass sensor
-			 */
-			float DECLINATION_ANGLE = 0.0413; //correction factor for location Paderborn
-
-			/**
-			 * @brief Calibration variable for HMC588L
-			 */
-			unsigned int calibration_start;
-
-		public:
-
-			/* Rover Sensor IDs */
-			/**
-			 * @brief Static definition to indicate rover's front ultrasonic sensor socket
-			 */
-			static const int ROVER_FRONT = 0;
-
-			/**
-			 * @brief Static definition to indicate rover's rear ultrasonic sensor socket
-			 */
-			static const int ROVER_REAR = 1;
-
-			/* Rover Infrared Sensor IDs */
-			/**
-			 * @brief Static definition to indicate rover's rear-right infrared sensor
-			 */
-			static const int ROVER_REAR_RIGHT = 0;
-
-			/**
-			 * @brief Static definition to indicate rover's rear-left infrared sensor
-			 */
-			static const int ROVER_REAR_LEFT = 1;
-
-			/**
-			 * @brief Static definition to indicate rover's front-right infrared sensor
-			 */
-			static const int ROVER_FRONT_RIGHT = 2;
-
-			/**
-			 * @brief Static definition to indicate rover's front-left infrared sensor
-			 */
-			static const int ROVER_FRONT_LEFT = 3;
-
-			/**
-			 * @brief Initializes the sensors
-			 * @return void
-			 */
-			void initialize();
-
-			/***
-			 * @brief Constructor for the RoverSensors class.
-			 */
-			explicit RoverSensors();
-
-			/**
-			 * @brief Sets up the HC-SR04 ultrasonic sensor
-			 * @return void
-			 */
-			void setupHCSR04UltrasonicSensor(int sensor_id);
-
-			/**
-			 * @brief Reads from HC-SR04 ultrasonic sensor in centimeters. sensor_id RoverSensors::ROVER_FRONT: Front ultrasonic sensor, RoverSensors::ROVER_REAR: Rear ultrasonic sensor.
-			 * @param sensor_id RoverSensors::ROVER_FRONT: Front ultrasonic sensor, RoverSensors::ROVER_REAR: Rear ultrasonic sensor.
-			 * @return sensor_val in centimeters
-			 */
-			int readHCSR04UltrasonicSensor (int sensor_id);
-
-			/**
-			 * @brief Sets up the groove ultrasonic sensor
-			 * @return void
-			 */
-			void setupGrooveUltrasonicSensor (void);
-
-			/**
-			 * @brief Reads from groove ultrasonic sensor in centimeters. sensor_id RoverSensors::ROVER_FRONT: Front ultrasonic sensor, RoverSensors::ROVER_REAR: Rear ultrasonic sensor.
-			 * @param sensor_id RoverSensors::ROVER_FRONT: Front ultrasonic sensor, RoverSensors::ROVER_REAR: Rear ultrasonic sensor.
-			 * @return sensor_val in centimeters
-			 */
-			int readGrooveUltrasonicSensor (int sensor_id);
-
-			/**
-			 * @brief Sets up the infrared sensors and Analog to digital converter
-			 * @return void
-			 */
-			void setupInfraredSensors (void);
-
-			/**
-			 * @brief Reads from infrared sensor in centimeters (float).
-			 * @param infrared_sensor_id Indicates the channel of which on-board sensor is addressed. RoverSensors::ROVER_REAR_RIGHT: Rear-right, RoverSensors::ROVER_REAR_LEFT: Rear-left, RoverSensors::ROVER_FRONT_RIGHT: Front-right, RoverSensors::ROVER_FRONT_LEFT: Front-left.
-			 * @return sensor_val in centimeters (float).
-			 */
-			float readInfraredSensor (int infrared_sensor_id);
-
-			/**
-			 * @brief Sets up the bearing sensor Sunfounder HMC5883L and its I2C interface
-			 * @return void
-			 */
-			void setupBearingHMC5883L (void);
-
-			/**
-			 * @brief Starts calibration for the bearing sensor
-			 * @return void
-			 */
-			void calibrateBearingSensor (void);
-
-			/**
-			 * @brief Sets the HMC588L Address
-			 * @return void
-			 */
-			void setHMC588LAddress (int address);
-
-			/**
-			 * @brief Sets the HMC588L Caliration Duration
-			 * @return void
-			 */
-			void setHMC588LCalibrationPeriod(int period);
-
-			/**
-			 * @brief Sets the HMC588L Declination angle
-			 * @return void
-			 */
-			void setHMC588LDeclinationAngle (float angle);
-
-			/**
-			 * @brief Reads the bearing value from Bearing sensor Sunfounder HMC5883L (float).
-			 * @return bearing_val in degrees (float).
-			 */
-			float readBearingHMC5883L (void);
-
-			/**
-			 * @brief Reads the temperature value in Celcius degrees from DHT22 temperature and humidity sensor (float).
-			 * @return temperature_val in Celcius degrees (float).
-			 */
-			float readTemperature (void);
-
-			/**
-			 * @brief Reads the humidity value in percentage from DHT22 temperature and humidity sensor (float).
-			 * @return humidity_val in percentage (float).
-			 */
-			float readHumidity (void);
-
-			/**
-			 * @brief Sets up the bearing sensor with QMC5883L chip and its I2C interface
-			 * @return void
-			 */
-			void setupBearingQMC5883L (void);
-
-			/**
-			 * @brief Reads the bearing value from Bearing sensor with QMC5883L chip (float).
-			 * @return bearing_val in degrees (float).
-			 */
-			float readBearingQMC5883L (void);
-	};
-}
-
-
-
-#endif /* API_ROVER_SENSORS_HPP_ */
diff --git a/rover/src/roverapi/rover_sensors.cpp b/rover/src/roverapi/rover_sensors.cpp
deleted file mode 100644
index 0e96497..0000000
--- a/rover/src/roverapi/rover_sensors.cpp
+++ /dev/null
@@ -1,607 +0,0 @@
-/*
- * Copyright (c) 2017 FH Dortmund and others
- * All rights reserved. This program and the accompanying materials
- * are made available under the terms of the Eclipse Public License v1.0
- * which accompanies this distribution, and is available at
- * http://www.eclipse.org/legal/epl-v10.html
- *
- * Description:
- *    Rover Sensors API - Interfaces for Rover sensors application development
- *
- * Contributors:
- *    M.Ozcelikors <mozcelikors@gmail.com>, created C++ API 17.11.2017
- *    										initial QMC5883L driver implemented 30.11.2017
- *    David Schmelter, Fraunhofer IEM - compass sensor initial implementation
- *    Gael Blondelle - initial API and parameters
- *
- */
-
-#include <roverapi/basic_psys_rover.h>
-#include <roverapi/rover_sensors.hpp>
-#include <mcp3004.h>
-#include <wiringPiI2C.h>
-#include <math.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <unistd.h>
-
-static int i2c_hmc588l_fd = -1;
-
-static int16_t xMinRaw = 0;
-static int16_t xMaxRaw = 0;
-static int16_t yMaxRaw = 0;
-static int16_t yMinRaw = 0;
-
-rover::RoverSensors::RoverSensors()
-{
-	this->calibration_start = 0;
-}
-
-void rover::RoverSensors::initialize (void)
-{
-	//wiringPiSetup();
-
-	this->setupHCSR04UltrasonicSensor(this->ROVER_FRONT);
-	this->setupHCSR04UltrasonicSensor(this->ROVER_REAR);
-	this->setupInfraredSensors();
-	this->setupBearingHMC5883L();
-	this->setupBearingQMC5883L();
-}
-
-void rover::RoverSensors::setupHCSR04UltrasonicSensor (int sensor_id)
-{
-	int trig_pin, echo_pin;
-
-	if (sensor_id == this->ROVER_FRONT)
-	{
-		trig_pin = this->TRIG1;
-		echo_pin = this->ECHO1;
-	}
-	else if (sensor_id == this->ROVER_REAR)
-	{
-		trig_pin = this->TRIG0;
-		echo_pin = this->ECHO0;
-	}
-	else
-	{
-		printf ("Invalid Sensor ID for the Ultrasonic Sensor to function setupHCSR04UltrasonicSensor.\n");
-	}
-
-    pinMode(trig_pin, OUTPUT);
-    pinMode(echo_pin, INPUT);
-
-    //TRIG pin must start LOW
-    digitalWrite(trig_pin, LOW);
-    delayMicroseconds(2);
-}
-
-int rover::RoverSensors::readHCSR04UltrasonicSensor (int sensor_id)
-{
-	int trig_pin, echo_pin;
-
-	if (sensor_id == this->ROVER_FRONT)
-	{
-		trig_pin = this->TRIG1;
-		echo_pin = this->ECHO1;
-	}
-	else if (sensor_id == this->ROVER_REAR)
-	{
-		trig_pin = this->TRIG0;
-		echo_pin = this->ECHO0;
-	}
-	else
-	{
-		printf ("Invalid Sensor ID for the Ultrasonic Sensor to function readHCSR04UltrasonicSensor.\n");
-	}
-
-	int distance = 0;
-    //Send trig pulse
-    digitalWrite(trig_pin, HIGH);
-    delayMicroseconds(10);
-    digitalWrite(trig_pin, LOW);
-
-    //Wait for echo start
-    long startTime = micros();
-    while(digitalRead(echo_pin) == LOW && micros() < startTime + 100000);
-
-    //Wait for echo end
-    startTime = micros();
-    while(digitalRead(echo_pin) == HIGH);
-    long travelTime = micros() - startTime;
-
-    //Get distance in cm
-    distance = travelTime * 34300;
-	distance = distance / 1000000;
-	distance = distance / 2;
-	// The below protection is to ensure there is no value fluctuation due to timeout
-	if (distance > 40 )
-		distance = 40;
-
-	//	printf("dist=%d\n",distance);
-    return distance;
-}
-
-void rover::RoverSensors::setupGrooveUltrasonicSensor(void) {
-	//wiringPiSetup();   //Since this can only be used once in a program, we do it in main and comment this.
-}
-
-int rover::RoverSensors::readGrooveUltrasonicSensor (int sensor_id)
-{
-	int sig_pin;
-
-	if (sensor_id == this->ROVER_FRONT)
-	{
-		sig_pin = this->SIG1;
-	}
-	else if (sensor_id == this->ROVER_REAR)
-	{
-		sig_pin = this->SIG0;
-	}
-	else
-	{
-		printf ("Invalid Sensor ID for the Ultrasonic Sensor to function readGrooveUltrasonicSensor.\n");
-	}
-
-	long startTime, stopTime, elapsedTime, distance = 0;
-	pinMode(sig_pin, OUTPUT);
-	digitalWrite(sig_pin, LOW);
-	delayMicroseconds(2);
-	digitalWrite(sig_pin, HIGH);
-	delayMicroseconds(5);
-	digitalWrite(sig_pin, LOW);
-	pinMode(sig_pin,INPUT);
-
-	startTime = micros();
-	while (digitalRead(sig_pin) == LOW  );
-	startTime = micros();
-	// For values above 40cm, groove sensor is unable to receive signals which causes it to stuck
-	// This is resolved by adding the timeout below.
-	// However, this timeout cause values bigger than 40 to fluctuate
-	while (digitalRead(sig_pin) == HIGH && micros() < startTime + 100000);
-	stopTime = micros();
-	elapsedTime = stopTime - startTime;
-	distance = elapsedTime / 29 /2;
-	// The below protection is to ensure there is no value fluctuation
-	if (distance > 40 )
-		distance = 40;
-	return distance;
-}
-
-void rover::RoverSensors::setupInfraredSensors (void)
-{
-	/* Init the analog digital converter */
-	mcp3004Setup (BASE, SPI_CHAN); // 3004 and 3008 are the same 4/8 channels
-}
-
-float rover::RoverSensors::readInfraredSensor (int infrared_sensor_id)
-{
-	float x;
-	float y = analogRead (BASE+infrared_sensor_id);
-
-	if (infrared_sensor_id != this->ROVER_FRONT_LEFT &&
-			infrared_sensor_id != this->ROVER_FRONT_RIGHT &&
-			infrared_sensor_id != this->ROVER_REAR_LEFT &&
-			infrared_sensor_id != this->ROVER_REAR_RIGHT)
-	{
-		printf ("Invalid Sensor ID for the Infrared Sensor to function readInfraredSensor.\n");
-	}
-
-	// 1/cm to output voltage is almost linear between
-	// 80cm->0,4V->123
-	// 6cm->3,1V->961
-	// => y=5477*x+55 => x= (y-55)/5477
-	if (y<123){
-		x=100.00;
-	} else {
-		float inverse = (y-55)/5477;
-		//printf("inverse=%f\n",inverse);
-	// x is the distance in cm
-		x = 1/inverse;
-	}
-
-	return x;
-}
-
-void rover::RoverSensors::calibrateBearingSensor (void)
-{
-	this->calibration_start = millis();
-}
-
-void rover::RoverSensors::setupBearingQMC5883L(void)
-{
-	this->setHMC588LAddress(0x0D);
-	this->setHMC588LCalibrationPeriod(10000);
-	this->setHMC588LDeclinationAngle(0.0413);
-
-	if ((i2c_hmc588l_fd = wiringPiI2CSetup(this->HMC588L_ADDRESS)) < 0)
-	{
-		printf("Failed to initialize HMC588L compass sensor");
-	}
-
-	if (i2c_hmc588l_fd >= 0)
-	{
-		wiringPiI2CWriteReg8 (i2c_hmc588l_fd, 0x0B, 0x01); //init SET/PERIOD register
-
-		/*
-		Define
-		OSR = 512
-		Full Scale Range = 8G(Gauss)
-		ODR = 200HZ
-		set continuous measurement mode
-		*/
-		wiringPiI2CWriteReg8 (i2c_hmc588l_fd, 0x09, 0x1D);
-	}
-
-	this->calibration_start = millis();
-
-	//
-	// To do a software reset
-	// wiringPiI2CWriteReg8 (i2c_hmc588l_fd, 0x0A, 0x80);
-	//
-
-}
-
-float rover::RoverSensors::readBearingQMC5883L(void)
-{
-	int8_t buffer[6];
-
-	//wiringPiI2CWrite (i2c_hmc588l_fd, 0x00); //Start with register 3
-	//delay(25);
-
-	buffer[0] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x00); //LSB x
-	buffer[1] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x01); //MSB x
-	buffer[2] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x02); //LSB y
-	buffer[3] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x03); //MSB y
-	buffer[4] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x04); //LSB z
-	buffer[5] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x05); //MSB z
-
-	int16_t xRaw = (((int16_t) buffer[1] << 8) & 0xff00) | buffer[0];
-	int16_t yRaw = (((int16_t) buffer[3] << 8) & 0xff00) | buffer[2];
-#ifdef DEBUG
-	printf ("%d %d\n",xRaw,yRaw);
-#endif
-
-	//if calibration is active calculate minimum and maximum x/y values for calibration
-	if (millis() <= this->calibration_start + this->CALIBRATION_DURATION) {
-		if (xRaw < xMinRaw) {
-			xMinRaw = xRaw;
-		}
-		if (xRaw > xMaxRaw) {
-			xMaxRaw = xRaw;
-		}
-		if (yRaw < yMinRaw) {
-			yMinRaw = yRaw;
-		}
-		if (yRaw > yMaxRaw) {
-			yMaxRaw = yRaw;
-		}
-	}
-
-	//calibration: move and scale x coordinates based on minimum and maximum values to get a unit circle
-	float xf = xRaw - (float) (xMinRaw + xMaxRaw) / 2.0f;
-	xf = xf / (xMinRaw + xMaxRaw) * 2.0f;
-
-	//calibration: move and scale y coordinates based on minimum and maximum values to get a unit circle
-	float yf = yRaw - (float) (yMinRaw + yMaxRaw) / 2.0f;
-	yf = yf / (yMinRaw + yMaxRaw) * 2.0f;
-
-	float bearing = atan2(yf, xf);
-#ifdef DEBUG
-	printf("%f, bearing\n", bearing);
-#endif
-
-	//location specific magnetic field correction
-	bearing += this->DECLINATION_ANGLE;
-
-	if (bearing < 0) {
-		bearing += 2 * M_PI;
-	}
-
-	if (bearing > 2 * M_PI) {
-		bearing -= 2 * M_PI;
-	}
-
-	float headingDegrees = bearing * (180.0 / M_PI);
-#ifdef DEBUG
-	printf("%lf, headingDegrees\n", headingDegrees);
-#endif
-	return headingDegrees;
-
-
-}
-
-void rover::RoverSensors::setupBearingHMC5883L(void)
-{
-	this->setHMC588LAddress(0x1E);
-	this->setHMC588LCalibrationPeriod(10000);
-	this->setHMC588LDeclinationAngle(0.0413);
-
-#ifdef DEBUG
-	printf ("HMC588L Address is: %x\n", this->HMC588L_ADDRESS);
-	printf ("HMC588L Calibration period is %d\n", this->CALIBRATION_DURATION);
-	printf ("HMC588L Declination angle is %f\n", this->DECLINATION_ANGLE);
-#endif
-
-	if ((i2c_hmc588l_fd = wiringPiI2CSetup(this->HMC588L_ADDRESS)) < 0) {
-		printf("Failed to initialize HMC588L compass sensor");
-	}
-
-	if (i2c_hmc588l_fd >= 0) {
-		int8_t gain = 5;
-
-		wiringPiI2CWriteReg8(i2c_hmc588l_fd, 0x00, 0x70); // 8-average, 15 Hz default, positive self test measurement
-		wiringPiI2CWriteReg8(i2c_hmc588l_fd, 0x01, gain << 5); // Gain
-		wiringPiI2CWriteReg8(i2c_hmc588l_fd, 0x02, 0x00); // Continuous-measurement mode
-	}
-
-	this->calibration_start = millis();
-}
-
-float rover::RoverSensors::readBearingHMC5883L(void)
-{
-	int8_t buffer[6];
-
-	//potential optimization: wiringPiI2CReadReg16
-	buffer[0] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x03); //MSB x
-	buffer[1] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x04); //LSB x
-	buffer[2] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x05); //MSB z
-	buffer[3] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x06); //LSB z
-	buffer[4] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x07); //MSB y
-	buffer[5] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x08); //LSB y
-
-	int16_t xRaw = (((int16_t) buffer[0] << 8) & 0xff00) | buffer[1];
-	//int16_t zRaw = (((int16_t) buffer[2] << 8) & 0xff00) | buffer[3];
-	int16_t yRaw = (((int16_t) buffer[4] << 8) & 0xff00) | buffer[5];
-
-	//if calibration is active calculate minimum and maximum x/y values for calibration
-	if (millis() <= this->calibration_start + this->CALIBRATION_DURATION) {
-		if (xRaw < xMinRaw) {
-			xMinRaw = xRaw;
-		}
-		if (xRaw > xMaxRaw) {
-			xMaxRaw = xRaw;
-		}
-		if (yRaw < yMinRaw) {
-			yMinRaw = yRaw;
-		}
-		if (yRaw > yMaxRaw) {
-			yMaxRaw = yRaw;
-		}
-	}
-
-	//calibration: move and scale x coordinates based on minimum and maximum values to get a unit circle
-	float xf = xRaw - (float) (xMinRaw + xMaxRaw) / 2.0f;
-	xf = xf / (xMinRaw + xMaxRaw) * 2.0f;
-
-	//calibration: move and scale y coordinates based on minimum and maximum values to get a unit circle
-	float yf = yRaw - (float) (yMinRaw + yMaxRaw) / 2.0f;
-	yf = yf / (yMinRaw + yMaxRaw) * 2.0f;
-
-	float bearing = atan2(yf, xf);
-#ifdef DEBUG
-	printf("%f, bearing\n", bearing);
-#endif
-
-	//location specific magnetic field correction
-	bearing += this->DECLINATION_ANGLE;
-
-	if (bearing < 0) {
-		bearing += 2 * M_PI;
-	}
-
-	if (bearing > 2 * M_PI) {
-		bearing -= 2 * M_PI;
-	}
-
-	float headingDegrees = bearing * (180.0 / M_PI);
-#ifdef DEBUG
-	printf("%lf, headingDegrees\n", headingDegrees);
-#endif
-	return headingDegrees;
-}
-
-float rover::RoverSensors::readTemperature (void)
-{
-	int data[5] = { 0, 0, 0, 0, 0 };
-
-	uint8_t laststate;
-	uint8_t counter;
-	uint8_t j;
-	uint8_t i;
-
-	int try_again = 1;
-	float f, h, c;
-
-	while (try_again == 1)
-	{
-		data[0] = data[1] = data[2] = data[3] = data[4] = 0;
-		laststate = HIGH;
-		counter = 0;
-		j = 0;
-
-		/* pull pin down for 18 milliseconds */
-		pinMode( this->DHT22_RPI_PIN, OUTPUT );
-		digitalWrite( this->DHT22_RPI_PIN, LOW );
-		delay( 18 );
-
-		/* prepare to read the pin */
-		pinMode( this->DHT22_RPI_PIN, INPUT );
-
-		/* detect change and read data */
-		for ( i = 0; i < this->MAX_TIMINGS; i++ )
-		{
-			counter = 0;
-			while ( digitalRead( this->DHT22_RPI_PIN ) == laststate )
-			{
-				counter++;
-				delayMicroseconds( 1 );
-				if ( counter == 255 )
-				{
-					break;
-				}
-			}
-			laststate = digitalRead( this->DHT22_RPI_PIN );
-
-			if ( counter == 255 )
-				break;
-
-			/* ignore first 3 transitions */
-			if ( (i >= 4) && (i % 2 == 0) )
-			{
-				/* shove each bit into the storage bytes */
-				data[j / 8] <<= 1;
-				if ( counter > 16 )
-					data[j / 8] |= 1;
-				j++;
-			}
-		}
-
-		/*
-		 * check we read 40 bits (8bit x 5 ) + verify checksum in the last byte
-		 * print it out if data is good
-		 */
-		if ( (j >= 40) &&
-			 (data[4] == ( (data[0] + data[1] + data[2] + data[3]) & 0xFF) ) )
-		{
-			h = (float)((data[0] << 8) + data[1]) / 10;
-			if ( h > 100 )
-			{
-				h = data[0];	// for DHT11
-			}
-			c = (float)(((data[2] & 0x7F) << 8) + data[3]) / 10;
-			if ( c > 125 )
-			{
-				c = data[2];	// for DHT11
-			}
-			if ( data[2] & 0x80 )
-			{
-				c = -c;
-			}
-			f = c * 1.8f + 32;
-#ifdef DEBUG
-			printf( "Humidity = %.1f %% Temperature = %.1f *C (%.1f *F)\n", h, c, f );
-#endif
-			try_again = 0;
-		}
-		else
-		{
-			/* Data not good */
-			try_again = 1;
-			//printf ("Data not good, skipping\n");
-
-		}
-	}
-
-	/* Return temperature */
-	return c;
-}
-
-float rover::RoverSensors::readHumidity (void)
-{
-	int data[5] = { 0, 0, 0, 0, 0 };
-
-	uint8_t laststate;
-	uint8_t counter;
-	uint8_t j;
-	uint8_t i;
-
-	int try_again = 1;
-	float f, h, c;
-
-	while (try_again == 1)
-	{
-		data[0] = data[1] = data[2] = data[3] = data[4] = 0;
-		laststate = HIGH;
-		counter = 0;
-		j = 0;
-
-		/* pull pin down for 18 milliseconds */
-		pinMode( this->DHT22_RPI_PIN, OUTPUT );
-		digitalWrite( this->DHT22_RPI_PIN, LOW );
-		delay( 18 );
-
-		/* prepare to read the pin */
-		pinMode( this->DHT22_RPI_PIN, INPUT );
-
-		/* detect change and read data */
-		for ( i = 0; i < this->MAX_TIMINGS; i++ )
-		{
-			counter = 0;
-			while ( digitalRead( this->DHT22_RPI_PIN ) == laststate )
-			{
-				counter++;
-				delayMicroseconds( 1 );
-				if ( counter == 255 )
-				{
-					break;
-				}
-			}
-			laststate = digitalRead( this->DHT22_RPI_PIN );
-
-			if ( counter == 255 )
-				break;
-
-			/* ignore first 3 transitions */
-			if ( (i >= 4) && (i % 2 == 0) )
-			{
-				/* shove each bit into the storage bytes */
-				data[j / 8] <<= 1;
-				if ( counter > 16 )
-					data[j / 8] |= 1;
-				j++;
-			}
-		}
-
-		/*
-		 * check we read 40 bits (8bit x 5 ) + verify checksum in the last byte
-		 * print it out if data is good
-		 */
-		if ( (j >= 40) &&
-			 (data[4] == ( (data[0] + data[1] + data[2] + data[3]) & 0xFF) ) )
-		{
-			h = (float)((data[0] << 8) + data[1]) / 10;
-			if ( h > 100 )
-			{
-				h = data[0];	// for DHT11
-			}
-			c = (float)(((data[2] & 0x7F) << 8) + data[3]) / 10;
-			if ( c > 125 )
-			{
-				c = data[2];	// for DHT11
-			}
-			if ( data[2] & 0x80 )
-			{
-				c = -c;
-			}
-			f = c * 1.8f + 32;
-#ifdef DEBUG
-			printf( "Humidity = %.1f %% Temperature = %.1f *C (%.1f *F)\n", h, c, f );
-#endif
-			try_again = 0;
-		}
-		else
-		{
-			/* Data not good */
-			try_again = 1;
-			//printf ("Data not good, skipping\n");
-		}
-	}
-
-	/* Return humidity */
-	return h;
-}
-
-void rover::RoverSensors::setHMC588LAddress (int address)
-{
-	this->HMC588L_ADDRESS = address;
-}
-
-void rover::RoverSensors::setHMC588LDeclinationAngle (float angle)
-{
-	this->DECLINATION_ANGLE = angle;
-}
-
-void rover::RoverSensors::setHMC588LCalibrationPeriod(int period)
-{
-	this->CALIBRATION_DURATION = period;
-}