Diff: ak7451ctrl.cpp

Revision:

30:5a241d9b3262

Parent:

29:b488d2c89fba

Child:

34:1ea3357c8d9a

--- a/ak7451ctrl.cpp	Fri Mar 17 23:29:20 2017 +0000
+++ b/ak7451ctrl.cpp	Fri Apr 28 20:32:31 2017 +0000
@@ -39,22 +39,27 @@
             return AkmSensor::ERROR;
         }
         MSG("#AK7451 user mode succeed.\r\n"); 
-                
-        char data[2] = {0x02,0x00};                 // set clockwise rotation
+        
+        // E_RD         0x1    clockwise rotation
+        // E_Z_MODE     0x0    normal output
+        // E_ABZ_E      0x1    ABZ Output enable
+        // E_ABZ_HYS    0x2    ABZ Hysteresis=1LSB
+        // ABZ_RES      0x0    ABZ phase resolution = 1024ppr
+        char data[2] = {0x02,0xA0};
         status = ak7451->writeEEPROM(0x07,data);    // set clockwise
         if( status != AK7451::SUCCESS ){
             MSG("#AK7451 write EEPROM failed.\r\n"); 
             return AkmSensor::ERROR;
         }
         MSG("#AK7451 write EEPROM succeed.\r\n"); 
-        
+/*        
         status = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE);
         if( status != AK7451::SUCCESS ){
             MSG("#AK7451 normal mode failed.\r\n"); 
             return AkmSensor::ERROR;
         }
         MSG("#AK7451 normal mode succeed.\r\n"); 
-        
+*/        
         interval = SENSOR_SAMPLING_RATE; // 10Hz
     }
     else{
@@ -65,11 +70,22 @@
 }
 
 AkmSensor::Status Ak7451Ctrl::startSensor(){
+    AK7451::Status status = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE);
+    if( status != AK7451::SUCCESS ){
+        MSG("#AK7451 normal mode failed.\r\n");
+        return AkmSensor::ERROR;
+    }
     ticker.attach(callback(this, &AkmSensor::setEvent), interval);
     return AkmSensor::SUCCESS;
 }
 
 AkmSensor::Status Ak7451Ctrl::startSensor(const float sec){
+    AK7451::Status status = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE);
+    if( status != AK7451::SUCCESS ){
+        MSG("#AK7451 normal mode failed.\r\n");
+        return AkmSensor::ERROR;
+    }
+
     interval = sec;
     ticker.attach(callback(this, &AkmSensor::setEvent), interval);
     MSG("#Start sensor %s.\r\n",sensorName);
@@ -77,6 +93,12 @@
 }
 
 AkmSensor::Status Ak7451Ctrl::stopSensor(){
+    AK7451::Status status;
+    status = ak7451->setOperationMode(AK7451::AK7451_USER_MODE);
+    if( status != AK7451::SUCCESS ){
+        MSG("#AK7451 user mode failed.\r\n"); 
+        return AkmSensor::ERROR;
+    }
     ticker.detach();
     AkmSensor::clearEvent();
     return AkmSensor::SUCCESS;
@@ -105,26 +127,54 @@
     Message::Command cmd = in->getCommand();
 
     switch(cmd){
+         case Message::CMD_ANGLE_READ:
+         {
+            // read angle
+            char angle[2] = {0x00,0x00};
+            char density = 0x00;
+            char abnormal = 0x00;
+            
+            if( ak7451->readAngleMeasureCommand(angle, &density, &abnormal) != AK7451::SUCCESS ){
+                MSG("#Error read angle\r\n");
+                status =  AkmSensor::ERROR;
+            }
+            out->setCommand(Message::CMD_ANGLE_READ);
+            out->setArgument( 0, (abnormal != 0x03)|(status == AkmSensor::ERROR) ? 0x01 : 0x00 );
+            out->setArgument( 1, angle[0] );
+            out->setArgument( 2, angle[1] );
+            out->setArgument( 3, density );
+            break;
+        }
          case Message::CMD_ANGLE_ZERO_RESET:
          {
-             AK7451::Status st;
+            AK7451::Status st;
+/*
+            AK7451::OperationMode mode = AK7451::AK7451_USER_MODE;
             
-            st = ak7451->setOperationMode(AK7451::AK7451_USER_MODE);
-            if( st != AK7451::SUCCESS ){
-                MSG("#Error when set user mode\r\n");
-                status = AkmSensor::ERROR;
+            // check the mode
+            char data[1];
+            if( ak7451->readRegister(0x00, data) != AK7451::SUCCESS) {
+                MSG("#NORMAL MODE detected. Set into USER mode.\r\n");
+                mode = AK7451::AK7451_NORMAL_MODE;
+                st = ak7451->setOperationMode(AK7451::AK7451_USER_MODE);
+                if( st != AK7451::SUCCESS ){
+                    MSG("#Error when set user mode\r\n");
+                    status = AkmSensor::ERROR;
+                }
             }
+*/            
             st = ak7451->setAngleZero();    // reset ZP data
             if( st != AK7451::SUCCESS ){
                 MSG("#Error setAngleZero: code=%d\r\n",st);
                 status =  AkmSensor::ERROR;
             }
-            st = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE);
+/*
+            st = ak7451->setOperationMode(mode);
             if( st != AK7451::SUCCESS ){
-                MSG("#Error when set normal mode\r\n");
+                MSG("#Error when set mode:%d\r\n",mode);
                 status =  AkmSensor::ERROR;
             }
-            
+*/                        
             if( status == AkmSensor::ERROR ){
                 out->setArgument(0,1);
             }else{
@@ -132,6 +182,64 @@
             }
             break;
         }
+        case Message::CMD_REG_WRITE:
+        case Message::CMD_REG_WRITEN:
+        {
+            char address = in->getArgument(0);
+            int len = in->getArgument(1);
+            if(len != 2){
+                MSG("#Error length=%d. Only support 2byte length\r\n",len);
+                status = AkmSensor::ERROR;
+                return status;                
+            }
+            if(in->getArgNum() != len+2){
+                MSG("#Error argument num. Args=%d\r\n",in->getArgNum());
+                status = AkmSensor::ERROR;
+                out->setArgument(0,(char)status);
+                return status;
+            }
+
+            char data[len];
+            for(int i=0; i<len; i++){
+                data[i] = in->getArgument(i+2);    
+            }
+            if( ak7451->writeRegister(address, data) != AK7451::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Error register write.\r\n");
+            }
+            if( ak7451->writeEEPROM(address, data) != AK7451::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Error EEPROM write.\r\n");
+            }
+            out->setArgument(0,(char)status);
+            break;
+        }
+        case Message::CMD_REG_READ:
+        case Message::CMD_REG_READN:
+        {
+            if(in->getArgNum() != 2){
+                MSG("#Error argument num. Args=%d\r\n",in->getArgNum());
+                status = AkmSensor::ERROR;
+                return status;
+            }
+            
+            char address = in->getArgument(0);
+            int len = in->getArgument(1);
+            if(len != 2){
+                MSG("#Error length=%d. Only support 2byte length\r\n",len);
+                status = AkmSensor::ERROR;
+                return status;                
+            }
+            char data[len];
+            if( ak7451->readRegister(address, data) != AK7451::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Error register read.\r\n");
+            }
+            for(int i=0; i<len; i++){
+                out->setArgument(i, data[i]);
+            }
+            break;
+        }
         default:
         {
             MSG("#Error no command.\r\n");