Diff: ak7401ctrl.cpp

Revision:

30:5a241d9b3262

Parent:

29:b488d2c89fba

Child:

39:3821886c902e

--- a/ak7401ctrl.cpp	Fri Mar 17 23:29:20 2017 +0000
+++ b/ak7401ctrl.cpp	Fri Apr 28 20:32:31 2017 +0000
@@ -53,13 +53,13 @@
             MSG("#AK7401 set rotation failed.\r\n"); 
             return AkmSensor::ERROR;
         }
-        
+/*        
         status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
         if( status != AK7401::SUCCESS ){
             MSG("#AK7401 set NORMAL mode failed.\r\n"); 
             return AkmSensor::ERROR;
         }
-
+*/
         MSG("#AK7401 init succeed.\r\n"); 
         
         interval = SENSOR_SAMPLING_RATE;  // 10Hz
@@ -72,11 +72,21 @@
 }
 
 AkmSensor::Status Ak7401Ctrl::startSensor(){
+    AK7401::Status status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
+    if( status != AK7401::SUCCESS ){
+        MSG("#AK7401 set NORMAL mode failed.\r\n"); 
+        return AkmSensor::ERROR;
+    }
     ticker.attach(callback(this, &base::setEvent), interval);
     return AkmSensor::SUCCESS;
 }
 
 AkmSensor::Status Ak7401Ctrl::startSensor(const float sec){
+    AK7401::Status status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
+    if( status != AK7401::SUCCESS ){
+        MSG("#AK7401 set NORMAL mode failed.\r\n"); 
+        return AkmSensor::ERROR;
+    }
     interval = sec;
     ticker.attach(callback(this, &base::setEvent), interval);
     MSG("#Start sensor %s.\r\n",sensorName);
@@ -84,6 +94,11 @@
 }
 
 AkmSensor::Status Ak7401Ctrl::stopSensor(){
+    AK7401::Status status = ak7401->setOperationMode(AK7401::AK7401_USER_MODE);
+    if( status != AK7401::SUCCESS ){
+        MSG("#AK7401 set USER mode failed.\r\n"); 
+        return AkmSensor::ERROR;
+    }
     ticker.detach();
     AkmSensor::clearEvent();
     return AkmSensor::SUCCESS;
@@ -110,6 +125,24 @@
     AkmSensor::Status status = AkmSensor::SUCCESS;
     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( ak7401->readAngleMeasureCommand(angle, &density, &abnormal) != AK7401::SUCCESS ){
+                MSG("#Error read angle\r\n");
+                status =  AkmSensor::ERROR;
+            }
+            out->setCommand(Message::CMD_ANGLE_READ);
+            out->setArgument( 0, ((abnormal&0x01)==0x00)|(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:
          {
             if( ak7401->setAngleZero() != AK7401::SUCCESS ){
@@ -123,6 +156,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( ak7401->writeRegister(address, data) != AK7401::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Error register write.\r\n");
+            }
+            if( ak7401->writeEEPROM(address, data) != AK7401::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( ak7401->readRegister(address, data) != AK7401::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");