Diff: ak09970ctrl.cpp

Revision:

15:1238993fd75f

Parent:

11:cef8dc1cf010

Child:

16:d85be9bafb80

--- a/ak09970ctrl.cpp	Mon Sep 12 17:24:11 2016 +0000
+++ b/ak09970ctrl.cpp	Fri Oct 28 21:27:33 2016 +0000
@@ -276,8 +276,8 @@
             }    
 
             if(ak09970->getSwitchConfig(&switchConfig) != AK09970::SUCCESS) {
-                status =  AkmSensor::ERROR;
                 MSG("#Error getSwitchConfig. AK09970.\r\n");
+                return AkmSensor::ERROR;
             }
             out->setArgument(0,switchConfig.enabledODINTEN);
             out->setArgument(1,switchConfig.enabledINTEN);
@@ -328,11 +328,11 @@
             if(in->getArgNum() != 0){
                 MSG("#Error argument num. AK09970. Args=%d\r\n",in->getArgNum());
                 return AkmSensor::ERROR;
-            }    
+            }
 
             if(ak09970->getOperationMode(&mode, &sensorDriveMode, &sensorMeasurementRange) != AK09970::SUCCESS) {
-                status =  AkmSensor::ERROR;
                 MSG("#Error getOperationMode. AK09970.\r\n");
+                return AkmSensor::ERROR;
             }
             out->setArgument(0,(char)sensorMeasurementRange);
             out->setArgument(1,(char)sensorDriveMode);
@@ -359,31 +359,20 @@
             break;
         }
         case Message::CMD_REG_WRITE:
-        {
-            if(in->getArgNum() != 3){
-                MSG("#Error argument num. AK09970. Args=%d\r\n",in->getArgNum());
-                status = AkmSensor::ERROR;
-                out->setArgument(0,(char)status);
-                return status;
-            }    
-
-            char address = in->getArgument(0);
-            int len = (int)in->getArgument(1);
-            const char data = in->getArgument(2);
-            if( ak09970->write(address, &data, len) != AK09970::SUCCESS) {
-                status =  AkmSensor::ERROR;
-                MSG("#Error register write.\r\n");
-            }
-            out->setArgument(0,(char)status);
-            break;
-        }        
         case Message::CMD_REG_WRITEN:
         {
             char address = in->getArgument(0);
             int len = (int)in->getArgument(1);
+            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);    
+                data[i] = in->getArgument(i+2);    
             }
             if( ak09970->write(address, data, len) != AK09970::SUCCESS) {
                 status =  AkmSensor::ERROR;
@@ -393,37 +382,19 @@
             break;
         }        
         case Message::CMD_REG_READ:
-        {
-            if(in->getArgNum() != 2){
-                MSG("#Error argument num. AK09970. Args=%d\r\n",in->getArgNum());
-                return AkmSensor::ERROR;
-            }    
-            
-            char address = in->getArgument(0);
-            int len = (int)in->getArgument(1);
-            char data;
-            if( ak09970->read(address, &data, len) != AK09970::SUCCESS) {
-                status =  AkmSensor::ERROR;
-                MSG("#Error register read.\r\n");
-            }
-            out->setArgument(0,data);
-            break;
-        }
         case Message::CMD_REG_READN:
         {
             if(in->getArgNum() != 2){
-                MSG("#Error argument num. AK09970. Args=%d\r\n",in->getArgNum());
-                status = AkmSensor::ERROR;
-                out->setArgument(0,(char)status);
-                return status;
-            }    
+                MSG("#Error argument num. Args=%d\r\n",in->getArgNum());
+                return AkmSensor::ERROR;
+            }
 
             char address = in->getArgument(0);
             int len = (int)in->getArgument(1);
             char data[len];
             if( ak09970->read(address, data, len) != AK09970::SUCCESS) {
-                status =  AkmSensor::ERROR;
                 MSG("#Error register read.\r\n");
+                return AkmSensor::ERROR;
             }
             for(int i=0; i<len; i++){
                 out->setArgument(i, data[i]);