Diff: ak7401ctrl.cpp

Revision:

14:21e177fc308a

Parent:

13:d008249f0359

Child:

16:d85be9bafb80

--- a/ak7401ctrl.cpp	Fri Aug 12 00:04:48 2016 +0000
+++ b/ak7401ctrl.cpp	Mon Sep 12 17:24:11 2016 +0000
@@ -37,25 +37,32 @@
 
         status = ak7401->setOperationMode(AK7401::AK7401_USER_MODE);
         if( status != AK7401::SUCCESS ){
-            MSG("#AK7401 user mode failed.\r\n"); 
+            MSG("#AK7401 set USER mode failed.\r\n"); 
             return AkmSensor::ERROR;
         }
-        MSG("#AK7401 user mode succeed.\r\n"); 
-                
+
+        // check connection
+        status = ak7401->checkConnection();
+        if( status != AK7401::SUCCESS ){
+            MSG("#AK7401 check connection failed.\r\n"); 
+            return AkmSensor::ERROR;
+        }
+        
+        // change rotation direction to CW from CCW
         char data[2] = {0x00,0x07};                 // set clockwise rotation
         status = ak7401->writeEEPROM(0x05,data);    // set clockwise
         if( status != AK7401::SUCCESS ){
-            MSG("#AK7401 write EEPROM failed.\r\n"); 
+            MSG("#AK7401 set rotation failed.\r\n"); 
             return AkmSensor::ERROR;
         }
-        MSG("#AK7401 write EEPROM succeed.\r\n"); 
         
         status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
         if( status != AK7401::SUCCESS ){
-            MSG("#AK7401 normal mode failed.\r\n"); 
+            MSG("#AK7401 set NORMAL mode failed.\r\n"); 
             return AkmSensor::ERROR;
         }
-        MSG("#AK7401 normal mode succeed.\r\n");
+
+        MSG("#AK7401 init succeed.\r\n"); 
         
         interval = SENSOR_SAMPLING_RATE;  // 10Hz
     }
@@ -109,41 +116,12 @@
 
 AkmSensor::Status Ak7401Ctrl::requestCommand(Message* in, Message* out){
     AkmSensor::Status status = AkmSensor::SUCCESS;
-    
     Message::Command cmd = in->getCommand();
-
     switch(cmd){
          case Message::CMD_ANGLE_ZERO_RESET:
          {
-             AK7401::Status st;
-            if( Ak7401Ctrl::stopSensor() != AkmSensor::SUCCESS ){
-                MSG("#Error stop sensor\r\n");
-                status = AkmSensor::ERROR;
-            }
-            
-            st = ak7401->setOperationMode(AK7401::AK7401_USER_MODE);
-            if( st != AK7401::SUCCESS ){
-                MSG("#Error when set user mode\r\n");
-                status = AkmSensor::ERROR;
-            }
-            char temp[2] = {0x00,0x00};
-            st = ak7401->writeRegister(0x06,temp);
-            if( st != AK7401::SUCCESS ){
-                MSG("#Error temp read: code=%d\r\n",st);
-                status =  AkmSensor::ERROR;
-            }
-            st = ak7401->setAngleZero();    // reset ZP data
-            if( st != AK7401::SUCCESS ){
-                MSG("#Error setAngleZero: code=%d\r\n",st);
-                status =  AkmSensor::ERROR;
-            }
-            st = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
-            if( st != AK7401::SUCCESS ){
-                MSG("#Error when set normal mode\r\n");
-                status =  AkmSensor::ERROR;
-            }
-            if( Ak7401Ctrl::startSensor(interval) != AkmSensor::SUCCESS ){
-                MSG("#Error start sensor\r\n");
+            if( ak7401->setAngleZero() != AK7401::SUCCESS ){
+                MSG("#Error set angle zero\r\n");
                 status =  AkmSensor::ERROR;
             }
             if( status == AkmSensor::ERROR ){