Add calibartion

etc/arduino/arduino.ino

@@ -26,6 +26,15 @@
 #define TX_PIN 6
 #define ECHO_PACKAGE_SIZE 4
 
+//compass
+enum CompassState{
+  WORKING,
+  CALIBRATION
+};
+byte READ_DATA_COMMAND[] =  { 0x68, 0x04, 0x00, 0x04, 0x08 };
+byte START_CALIBARTION_COMMAND[] = {0x68, 0x04, 0x00, 0x08, 0x0C};
+byte STOP_CALIBRATION_COMMAND [] = {0x68, 0x04, 0x00, 0x0A, 0x0E};
+
 //Modbus registers
 #define LEFT_THRUSTER_REG 1
 #define RIGHT_THRUSTER_REG 2 
@@ -33,6 +42,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 +51,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 +77,62 @@ 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);
   
 
+}
+float ConvertFloatValue(uint16_t *buffer)
+{
+	float wholePart      = (buffer[0] & 0x0F) * 100 + (buffer[1] >> 4) * 10 + (buffer[1] & 0x0F) * 1,
+		  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, 5);
+    while(index < 14){
+      if (softSerial.available()) {
+          buffer[index] = softSerial.read();
+      }
+      index ++;
+    }
+      int heading = ConvertFloatValue(&buffer[10]);
+      mb.setHreg(HEADING_REG, heading);
+    
+    index = 0;
+  }
+   
+}
+void StartCalibrationCompass(){
+  if(state == CALIBRATION){
+    softSerial.write(START_CALIBARTION_COMMAND, 5);
+  }
+}
+void StopCalibrationCompass(){
+  if(state == CALIBRATION){
+    softSerial.write(STOP_CALIBRATION_COMMAND, 5);
+  }
+}
+void CheckCompassState(){
+  if(mb.Hreg(CALIBRATION_REG) == 1 && state == WORKING){
+    state = CALIBRATION;
+    StartCalibrationCompass();
+  }
+  else if(mb.Hreg(CALIBRATION_REG) == 0 && state == CALIBRATION){
+    StopCalibrationCompass();
+    state = WORKING;
+  }
+  else{
+    ReadCompassData();
+  }
+  
 }
 void ping(){
    if(millis() - lastPing > TIMEOUT){
@@ -103,8 +169,10 @@ void BatteryRead(){
 
 
 void loop() {
-  ping();
-  ReadDepth();
+//  ping();
+//  ReadDepth();
+  CheckCompassState();
+
   BatteryRead();
   mb.task();
   thruster_left.writeMicroseconds(constrain(mb.Hreg(LEFT_THRUSTER_REG), PWM_LOW, PWM_HIGH));