diff --git a/etc/arduino/arduino.ino b/etc/arduino/arduino.ino
index 610f0ff4acde1632c34e94d13e4990d026db282b..da50e08029aff058fa85015763ad8fbc4419775a 100644
--- a/etc/arduino/arduino.ino
+++ b/etc/arduino/arduino.ino
@@ -11,6 +11,7 @@
 #define MODBUS_SERIAL Serial
 #define BAUDRATE 115200 
 #define TIMEOUT 200
+#define LENGTHOF(array) (sizeof(array) / sizeof(*array))
 //Battarey parameters
 #define R_S 0.01
 #define R_L 49900
@@ -26,6 +27,15 @@
 #define TX_PIN 6
 #define ECHO_PACKAGE_SIZE 4
 
+//compass
+enum CompassState{
+  WORKING,
+  CALIBRATION
+};
+const byte READ_DATA_COMMAND[] =  { 0x68, 0x04, 0x00, 0x04, 0x08 };
+const byte START_CALIBARTION_COMMAND[] = {0x68, 0x04, 0x00, 0x08, 0x0C};
+const byte STOP_CALIBRATION_COMMAND [] = {0x68, 0x04, 0x00, 0x0A, 0x0E};
+
 //Modbus registers
 #define LEFT_THRUSTER_REG 1
 #define RIGHT_THRUSTER_REG 2 
@@ -33,6 +43,8 @@
 #define CURRENT_REG 4
 #define VOLTAGE_REG 5
 #define BATTERY_REG 6
+#define HEADING_REG 7
+#define CALIBRATION_REG 8
 
 // ModbusSerial object
 ModbusSerial mb (MODBUS_SERIAL, SLAVE_ID, -1);
@@ -40,6 +52,9 @@ ModbusSerial mb (MODBUS_SERIAL, SLAVE_ID, -1);
 // Motor objects
 Servo thruster_left;
 Servo thruster_right;
+enum CompassState state;
+uint16_t buffer[14];
+int index;
 
 SoftwareSerial softSerial =  SoftwareSerial(RX_PIN, TX_PIN);
 int echoPackage[ECHO_PACKAGE_SIZE];
@@ -63,10 +78,53 @@ void setup() {
   mb.addHreg(BATTERY_REG, 0);
   mb.addHreg(CURRENT_REG, 0);
   mb.addHreg(VOLTAGE_REG, 0);
+  mb.addHreg(HEADING_REG, 0);
+  mb.addHreg(CALIBRATION_REG, 0);
+  state = WORKING;
   pinMode(CURRENT_PIN, INPUT);
   pinMode(VOLTAGE_PIN, INPUT);
   
 
+}
+static float ConvertFloatValue(uint16_t *buffer)
+{
+	float wholePart      = (buffer[0] & 0x0F) * 100 + (buffer[1] >> 4) * 10 + (buffer[1] & 0x0F) * 1;
+	float fractionalPart = (buffer[2] >> 4) * 10 + (buffer[2] & 0x0F) * 1;
+
+	float result = wholePart + fractionalPart / 100;
+
+	return (buffer[0] >> 4) ? -result : result;
+}
+void ReadCompassData(){
+  if(state == WORKING){
+    softSerial.write(READ_DATA_COMMAND, LENGTHOF(READ_DATA_COMMAND));
+    while(index < 14){
+      if (softSerial.available()) {
+          buffer[index] = softSerial.read();
+      }
+      index ++;
+    }
+      int heading = ConvertFloatValue(&buffer[10]);
+      mb.setHreg(HEADING_REG, heading);
+    
+    index = 0;
+  }
+   
+}
+
+void CheckCompassState(){
+  if(mb.Hreg(CALIBRATION_REG) == 1 && state == WORKING){
+    state = CALIBRATION;
+    softSerial.write(START_CALIBARTION_COMMAND, LENGTHOF(START_CALIBARTION_COMMAND));
+  }
+  else if(mb.Hreg(CALIBRATION_REG) == 0 && state == CALIBRATION){
+    softSerial.write(STOP_CALIBRATION_COMMAND, LENGTHOF(STOP_CALIBRATION_COMMAND));
+    state = WORKING;
+  }
+  else{
+    ReadCompassData();
+  }
+  
 }
 void ping(){
    if(millis() - lastPing > TIMEOUT){
@@ -103,8 +161,7 @@ void BatteryRead(){
 
 
 void loop() {
-  ping();
-  ReadDepth();
+  CheckCompassState();
   BatteryRead();
   mb.task();
   thruster_left.writeMicroseconds(constrain(mb.Hreg(LEFT_THRUSTER_REG), PWM_LOW, PWM_HIGH));