diff --git a/rover/include/roverapi/rover_sensors.hpp b/rover/include/roverapi/rover_sensors.hpp
new file mode 100644
index 0000000..8ca663d
--- /dev/null
+++ b/rover/include/roverapi/rover_sensors.hpp
@@ -0,0 +1,207 @@
+/*
+ * 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
+ *    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>
+
+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
+			 */
+			static const int HMC588L_ADDRESS = 0x1E;
+			/**
+			 *  @brief Calibration duration for compass sensor
+			 */
+			static const int CALIBRATION_DURATION = 10000; //compass calibration has a duration of 5 seconds
+			/**
+			 *  @brief Declination angle / correction factor for compass sensor
+			 */
+			static const 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.
+			 */
+			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 and its I2C interface
+			 * @return void
+			 */
+			void setupBearingSensor (void);
+
+			/**
+			 * @brief Starts calibration for the bearing sensor
+			 * @return void
+			 */
+			void calibrateBearingSensor (void);
+
+			/**
+			 * @brief Reads the bearing value from Bearing sensor (float).
+			 * @return bearing_val in degrees (float).
+			 */
+			float readBearing (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);
+	};
+}
+
+
+
+#endif /* API_ROVER_SENSORS_HPP_ */
diff --git a/rover/src/roverapi/rover_sensors.cpp b/rover/src/roverapi/rover_sensors.cpp
new file mode 100644
index 0000000..647a035
--- /dev/null
+++ b/rover/src/roverapi/rover_sensors.cpp
@@ -0,0 +1,444 @@
+/*
+ * 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
+ *    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->setupBearingSensor();
+}
+
+void rover::RoverSensors::setupHCSR04UltrasonicSensor (int sensor_id)
+{
+	int trig_pin, echo_pin;
+
+	if (sensor_id == 0)
+	{
+		trig_pin = this->TRIG1;
+		echo_pin = this->ECHO1;
+	}
+	else
+	{
+		trig_pin = this->TRIG0;
+		echo_pin = this->ECHO0;
+	}
+
+    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 == 0)
+	{
+		trig_pin = this->TRIG1;
+		echo_pin = this->ECHO1;
+	}
+	else
+	{
+		trig_pin = this->TRIG0;
+		echo_pin = this->ECHO0;
+	}
+
+	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 == 0)
+	{
+		sig_pin = this->SIG1;
+	}
+	else
+	{
+		sig_pin = this->SIG0;
+	}
+
+	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);
+
+	// 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::setupBearingSensor(void)
+{
+	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::readBearing(void)
+{
+	int8_t buffer[6];
+	//potential optimization: wiringPiI2CReadReg16
+	buffer[0] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x03);
+	buffer[1] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x04);
+	buffer[2] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x05);
+	buffer[3] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x06);
+	buffer[4] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x07);
+	buffer[5] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x08);
+
+	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);
+
+	//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);
+
+	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;
+			//printf( "Humidity = %.1f %% Temperature = %.1f *C (%.1f *F)\n", h, c, f );
+			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;
+			//printf( "Humidity = %.1f %% Temperature = %.1f *C (%.1f *F)\n", h, c, f );
+			try_again = 0;
+		}
+		else
+		{
+			/* Data not good */
+			try_again = 1;
+		}
+	}
+
+	/* Return humidity */
+	return h;
+}