Diff: akmakd.cpp

Revision:

15:1238993fd75f

Parent:

11:cef8dc1cf010

Child:

16:d85be9bafb80

--- a/akmakd.cpp	Mon Sep 12 17:24:11 2016 +0000
+++ b/akmakd.cpp	Fri Oct 28 21:27:33 2016 +0000
@@ -317,10 +317,11 @@
         case Message::CMD_COMPASS_GET_OPERATION_MODE:
         {
             if(compass->getOperationMode(&mode) != AkmECompass::SUCCESS) {
-                status =  AkmSensor::ERROR;
+                status = AkmSensor::ERROR;
                 MSG("#Error set operation mode.\r\n");
+            }else{
+                out->setArgument(0,mode);
             }
-            out->setArgument(0,mode);
             break;
         }
         case Message::CMD_COMPASS_SET_OPERATION_MODE:
@@ -349,14 +350,22 @@
             }
             out->setArgument(0,(char)status);                
             break;
-        }        
+        }
+        case Message::CMD_REG_WRITE:
         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( compass->write(address, data, len) != AkmECompass::SUCCESS) {
                 status =  AkmSensor::ERROR;
@@ -364,21 +373,16 @@
             }
             out->setArgument(0,(char)status);
             break;
-        }        
+        }
         case Message::CMD_REG_READ:
-        {
-            char address = in->getArgument(0);
-            int len = (int)in->getArgument(1);
-            char data;
-            if( compass->read(address, &data, len) != AkmECompass::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. Args=%d\r\n",in->getArgNum());
+                status = AkmSensor::ERROR;
+                return status;
+            }
+            
             char address = in->getArgument(0);
             int len = (int)in->getArgument(1);
             char data[len];
@@ -398,8 +402,7 @@
             break;
         }
     }
-
-    return AkmSensor::SUCCESS;
+    return status;
 }
 
 char* AkmAkd::getSensorName(){