Diff: ak9750ctrl.cpp

Revision:

15:1238993fd75f

Parent:

13:d008249f0359

Child:

16:d85be9bafb80

diff -r 21e177fc308a -r 1238993fd75f ak9750ctrl.cpp
--- a/ak9750ctrl.cpp	Mon Sep 12 17:24:11 2016 +0000
+++ b/ak9750ctrl.cpp	Fri Oct 28 21:27:33 2016 +0000
@@ -69,59 +69,67 @@
     if (ak9750->reset() != AK9750::SUCCESS) {
         MSG("#Failed to reset AK9750.\r\n");
     }
-    
+/*    
     // Set to EEPROM mode to EEPROM access
     if(ak9750->setOperationMode(AK9750::MODE_EEPROM_ACCESS, AK9750::DF_0P3HZ) != AK9750::SUCCESS) {
         MSG("#Error setOperationMode to EEPROM mode. AK9750.\r\n");
         return AkmSensor::ERROR;
     }
+*/    
     // Gets threshold from EEPROM
     AK9750::Threshold th;
     if (ak9750->getThresholdFromEEPROM(&th) != AK9750::SUCCESS) {
         MSG("#Failed to get threshold from EEPROM AK9750.\r\n");
     }
+    MSG("#Threshold:(0x%02X,0x%02X,0x%02X,0x%02X)\r\n",th.eth13h,th.eth13l,th.eth24h,th.eth24l);
+    
     // Gets hysteresis from EEPROM
     AK9750::Hysteresis hys;
     if (ak9750->getHysteresisFromEEPROM(&hys) != AK9750::SUCCESS) {
         MSG("#Failed to get hysteresis from EEPROM AK9750.\r\n");
     }
+    MSG("#Hysteresis:(0x%02X,0x%02X)\r\n",hys.ehys13,hys.ehys24);
+
     // Gets interrupt status from EEPROM
     AK9750::InterruptStatus intStatus;
     if ((ak9750->getInterruptEnableFromEEPROM(&intStatus)) != AK9750::SUCCESS) {
         MSG("#Failed to get interrupts of AK9750 from EEPROM.\r\n");
     }
+    MSG("#Interrupt:(0x%02X,0x%02X,0x%02X,0x%02X,0x%02X)\r\n",intStatus.ir13h,intStatus.ir13l,intStatus.ir24h,intStatus.ir24l,intStatus.drdy);
 
     // Gets operation mode from EEPROM
     if ((ak9750->getOperationModeFromEEPROM(&mode,&filter)) != AK9750::SUCCESS) {
         MSG("#Failed to get operation mode of AK9750 from EEPROM.\r\n");
     }
     MSG("#Operation Mode:(0x%02X,0x%02X)\r\n",mode, filter);
-    
+/*    
     // Back to Stand-by Mode for register access
     if(ak9750->setOperationMode(AK9750::MODE_STANDBY, filter) != AK9750::SUCCESS) {
         MSG("#Error setOperationMode to stand-by mode of AK9750.\r\n");
         return AkmSensor::ERROR;
     }
-    
+*/    
     // Sets threshold from the red EEPROM values
     if (ak9750->setThreshold(&th) != AK9750::SUCCESS) {
         MSG("#Failed to set threshold to AK9750.\r\n");
     }
-    MSG("#Threshold:(0x%02X,0x%02X,0x%02X,0x%02X)\r\n",th.eth13h,th.eth13l,th.eth24h,th.eth24l);
 
     // Sets hysteresis from the red EEPROM values
     if (ak9750->setHysteresis(&hys) != AK9750::SUCCESS) {
         MSG("#Failed to set hysteresis to AK9750.\r\n");
     }
-    MSG("#Hysteresis:(0x%02X,0x%02X)\r\n",hys.ehys13,hys.ehys24);
     
     // Sets interruput status from the red EEPROM values
     AK9750::Status status = ak9750->setInterruptEnable(&intStatus);
     if (status != AK9750::SUCCESS) {
         MSG("#Failed to set interrupts of AK9750. Status = 0x%02X\r\n", status);
     }
-    MSG("#Interrupt:(0x%02X,0x%02X,0x%02X,0x%02X,0x%02X)\r\n",intStatus.ir13h,intStatus.ir13l,intStatus.ir24h,intStatus.ir24l,intStatus.drdy);
     
+    if(ak9750->setOperationMode(mode, filter) != AK9750::SUCCESS) {
+        MSG("#Error setOperationMode of AK9750.\r\n");
+        return AkmSensor::ERROR;
+    }
+
     MSG("#Init success AK9750.\r\n");
     return AkmSensor::SUCCESS;
 }
@@ -215,8 +223,8 @@
         case Message::CMD_IR_GET_THRESHOLD:
         {
             if (ak9750->getThreshold(&th) != AK9750::SUCCESS) {
-                status =  AkmSensor::ERROR;
-                MSG("#Failed to set threshold to AK9750.\r\n");
+                MSG("#Failed to get threshold of AK9750.\r\n");
+                return AkmSensor::ERROR;
             }
             out->setArgument(0,(char)((int32_t)(th.eth13h) >> 8));
             out->setArgument(1,(char)((int32_t)(th.eth13h) & 0x00FF));
@@ -244,8 +252,8 @@
         case Message::CMD_IR_GET_HYSTERESIS:
         {
             if (ak9750->getHysteresis(&hys) != AK9750::SUCCESS) {
-                status =  AkmSensor::ERROR;
-                MSG("#Failed to set hysteresis to AK9750.\r\n");
+                MSG("#Failed to get hysteresis of AK9750.\r\n");
+                return AkmSensor::ERROR;
             }
             out->setArgument(0,hys.ehys13);
             out->setArgument(1,hys.ehys24);
@@ -265,8 +273,8 @@
         case Message::CMD_IR_GET_INTERRUPT:
         {
             if (ak9750->getInterruptEnable(&interrupt) != AK9750::SUCCESS) {
-                status =  AkmSensor::ERROR;
-                MSG("#Failed to set hysteresis to AK9750.\r\n");
+                MSG("#Failed to get interrupt of AK9750.\r\n");
+                return AkmSensor::ERROR;
             }
             out->setArgument(0, interrupt.ir13h);
             out->setArgument(1, interrupt.ir13l);
@@ -284,7 +292,7 @@
             interrupt.drdy = in->getArgument(4);
             if (ak9750->setInterruptEnable(&interrupt) != AK9750::SUCCESS) {
                 status =  AkmSensor::ERROR;
-                MSG("#Failed to set hysteresis to AK9750.\r\n");
+                MSG("#Failed to set interrupt to AK9750.\r\n");
             }
             out->setArgument(0,(char)status);
             break;            
@@ -292,8 +300,8 @@
         case Message::CMD_IR_GET_OPERATION_MODE:
         {
             if(ak9750->getOperationMode(&mode, &filter) != AK9750::SUCCESS) {
-                status =  AkmSensor::ERROR;
-                MSG("#Error setOperationMode. AK9750.\r\n");
+                MSG("#Error getOperationMode. AK9750.\r\n");
+                return AkmSensor::ERROR;
             }
             out->setArgument(0,(char)mode);
             out->setArgument(1,(char)filter);
@@ -313,8 +321,8 @@
         case Message::CMD_IR_GET_THRESHOLD_EEPROM:
         {
             if (ak9750->getThresholdFromEEPROM(&th) != AK9750::SUCCESS) {
-                status =  AkmSensor::ERROR;
-                MSG("#Failed to set threshold to AK9750(EEPROM).\r\n");
+                MSG("#Failed to get threshold of AK9750(EEPROM).\r\n");
+                return AkmSensor::ERROR;
             }
             out->setArgument(0,(char)((int32_t)(th.eth13h) >> 8));
             out->setArgument(1,(char)((int32_t)(th.eth13h) & 0x00FF));
@@ -342,8 +350,8 @@
         case Message::CMD_IR_GET_HYSTERESIS_EEPROM:
         {
             if (ak9750->getHysteresisFromEEPROM(&hys) != AK9750::SUCCESS) {
-                status =  AkmSensor::ERROR;
-                MSG("#Failed to set hysteresis to AK9750(EEPROM).\r\n");
+                MSG("#Failed to get hysteresis of AK9750(EEPROM).\r\n");
+                return AkmSensor::ERROR;
             }
             out->setArgument(0,hys.ehys13);
             out->setArgument(1,hys.ehys24);
@@ -363,8 +371,8 @@
         case Message::CMD_IR_GET_INTERRUPT_EEPROM:
         {
             if (ak9750->getInterruptEnableFromEEPROM(&interrupt) != AK9750::SUCCESS) {
-                status =  AkmSensor::ERROR;
-                MSG("#Failed to set interrupt to AK9750(EEPROM).\r\n");
+                MSG("#Failed to get interrupt of AK9750(EEPROM).\r\n");
+                return AkmSensor::ERROR;
             }
             out->setArgument(0, interrupt.ir13h);
             out->setArgument(1, interrupt.ir13l);
@@ -390,8 +398,8 @@
         case Message::CMD_IR_GET_OPERATION_MODE_EEPROM:
         {
             if(ak9750->getOperationModeFromEEPROM(&mode, &filter) != AK9750::SUCCESS) {
-                status =  AkmSensor::ERROR;
                 MSG("#Error getOperationMode. AK9750(EEPROM).\r\n");
+                return AkmSensor::ERROR;
             }
             out->setArgument(0,(char)mode);
             out->setArgument(1,(char)filter);
@@ -409,24 +417,20 @@
             break;
         }
         case Message::CMD_REG_WRITE:
-        {
-            char address = in->getArgument(0);
-            int len = (int)in->getArgument(1);
-            const char data = in->getArgument(2);
-            if( ak9750->write(address, &data, len) != AK9750::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( ak9750->write(address, data, len) != AK9750::SUCCESS) {
                 status =  AkmSensor::ERROR;
@@ -436,25 +440,19 @@
             break;
         }        
         case Message::CMD_REG_READ:
-        {
-            char address = in->getArgument(0);
-            int len = (int)in->getArgument(1);
-            char data;
-            if( ak9750->read(address, &data, len) != AK9750::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());
+                return AkmSensor::ERROR;
+            }
+
             char address = in->getArgument(0);
             int len = (int)in->getArgument(1);
             char data[len];
             if( ak9750->read(address, data, len) != AK9750::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]);