Diff: akmsensormanager.cpp

Revision:

15:1238993fd75f

Parent:

13:d008249f0359

Child:

16:d85be9bafb80

Child:

21:966724730ce6

--- a/akmsensormanager.cpp	Mon Sep 12 17:24:11 2016 +0000
+++ b/akmsensormanager.cpp	Fri Oct 28 21:27:33 2016 +0000
@@ -1,4 +1,5 @@
 #include "ble/services/UARTService.h"
+#include "akdphwinfo.h"
 #include "akmsensormanager.h"
 #include "akmhallswitch.h"
 #include "akmanalogsensor.h"
@@ -10,15 +11,10 @@
 #include "akmakd.h"
 #include "debug.h"
 #include "Message.h"
-#ifdef REV_D
 #include "mcp342x.h"
 #include "I2CNano.h"
 #include "tca9554a.h"
-#endif
 
-#define CR                 '\r'
-#define LF                 '\n'
-#define FIRMWARE_VERSION   0x02
 #define MAGNETOMETER_ID    0x0A
 
 #define CONV16I(high,low)  ((int16_t)(((high) << 8) | (low)))
@@ -134,6 +130,9 @@
             if(subId == Ak9750Ctrl::SUB_ID_AK9750){
                 Ak9750Ctrl* ak9750ctrl = new Ak9750Ctrl();
                 sensor = ak9750ctrl;
+            }else if(subId == Ak9750Ctrl::SUB_ID_AK9753){
+                Ak9750Ctrl* ak9753ctrl = new Ak9750Ctrl();
+                sensor = ak9753ctrl;
             }else if(subId == Ak9752Ctrl::SUB_ID_AK9752){
                 Ak9752Ctrl* ak9752ctrl = new Ak9752Ctrl();
                 sensor = ak9752ctrl;
@@ -167,12 +166,12 @@
         MSG("#Failed to parse message. %s\r\n", buf);
         status = ERROR;
         eventCommandReceived = false;
+    }else{       
+        eventCommandReceived = true;
     }
-    eventCommandReceived = true;
     return status;
 }
 
-#ifdef REV_D
 int16_t AkmSensorManager::getAdcData(MCP342X *mcp3428, MCP342X::AdcChannel ch, MCP342X::SampleSetting s) {
     const int WAIT_ADC_MS = 1;
 
@@ -190,18 +189,9 @@
     
     return data.value;
 }
-#endif
 
 uint8_t AkmSensorManager::getId(PinName pin, uint8_t bits)
 {
-#ifndef REV_D
-    /* Rev.C */
-    AnalogIn id(pin);
-    MSG("#Voltage=%5.2f[V]\r\n",id*3.0);
-    double s = id + 1.0/(double)(pow(2.0,bits+1));
-    uint8_t value = (uint8_t)(s*pow(2.0,bits));
-#else
-    /* Rev.D */
     MSG("#GetID\r\n");
     
     I2C i2c(I2C_SDA, I2C_SCL);
@@ -223,7 +213,6 @@
     uint8_t value = (uint8_t)((val - VAL_MIN)/(float)(VAL_MAX - VAL_MIN) * (1 << bits) + 0.5);
     MSG("#ID = %d.\r\n", value);
 
-#endif    
     return value;
 }
 
@@ -238,8 +227,6 @@
 
 void AkmSensorManager::processCommand()
 {
-    AkmSensorManager::Status status = AkmSensorManager::SUCCESS;
-    
     // Gets command in the message
     Message::Command cmd = msg.getCommand();
     
@@ -281,8 +268,11 @@
         }
         case Message::CMD_STOP_MEASUREMENT:
         {
-            if( sensor->stopSensor() != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR;
-            resMsg.setArgument(0, status==AkmSensorManager::SUCCESS ? 0 : 1);
+            if( sensor->stopSensor() != AkmSensor::SUCCESS){
+                resMsg.setArgument(0, 1);
+            }else{
+                resMsg.setArgument(0, 0);                 
+            }
             throwMessage(&resMsg);
             MSG("#Stop measurement.\r\n");
             break;
@@ -356,13 +346,18 @@
         case Message::CMD_COMPASS_GET_OPERATION_MODE:
         case Message::CMD_COMPASS_SET_OPERATION_MODE:
         {
-            sensor->requestCommand(&msg,&resMsg);
-            throwMessage(&resMsg);
+            AkmSensor::Status st = sensor->requestCommand(&msg,&resMsg);
+            if( (resMsg.getArgNum() == 0) && (st != AkmSensor::SUCCESS) )
+            {
+                MSG("#Command failed.\r\n");            
+            }else{
+                throwMessage(&resMsg);
+            }
             break;
         }
         default:
         {
-            MSG("#Can't find command:%s\r\n", (char)cmd);
+            MSG("#Can't find command.\r\n");
             break;
         }
     }