Diff: akmsensormanager.cpp

Revision:

39:3821886c902e

Parent:

37:c76d2edf3426

Child:

41:a3ea80c594ec

diff -r e865dadfe54d -r 3821886c902e akmsensormanager.cpp
--- a/akmsensormanager.cpp	Thu Jun 08 19:09:18 2017 +0000
+++ b/akmsensormanager.cpp	Tue Jul 18 23:20:26 2017 +0000
@@ -88,7 +88,7 @@
 //        delete(sensor[i]);
 //        i++;
 //    }
-    if(interrupt) delete interrupt;
+    detachInterrupt();
     drdyType = AkmAkd::INTERRUPT_DISABLED;
     
     switch(primaryId){
@@ -149,14 +149,14 @@
                 Ak09970Ctrl* ak09970 = new Ak09970Ctrl();
                 sensor[0] = ak09970;
                 if(sensor[0]->init(AkmSensor::AKM_PRIMARY_ID_AKD_I2C, Ak09970Ctrl::SUB_ID_AK09970) != AkmSensor::SUCCESS){
-                    MSG("#sensor[0]->init failed.\r\n");    
+                    MSG("#Error: sensor[0]->init failed.\r\n");    
                     return false;    // couldn't find
                 }
     
                 Ak9752Ctrl* ak9752ctrl = new Ak9752Ctrl();
                 sensor[1] = ak9752ctrl;
                 if(sensor[1]->init(AkmSensor::AKM_PRIMARY_ID_IR_SENSOR, Ak9752Ctrl::SUB_ID_AK9752) != AkmSensor::SUCCESS){
-                    MSG("#sensor[1]->init failed.\r\n");    
+                    MSG("#Error: sensor[1]->init failed.\r\n");    
                     return false;    // couldn't find
                 }
                 sensorNum = 2;
@@ -208,7 +208,7 @@
         }
         default:
         {
-            MSG("#Can't find ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]);
+            MSG("#Error: Can't find ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]);
             return false;    // couldn't find
         }
     }
@@ -216,13 +216,15 @@
     if(primaryId != AkmSensor::AKM_PRIMARY_ID_DEMO){       
         for(int i=0; i<sensorNum; i++){
             if(sensor[i]->init(primaryId, subId) != AkmSensor::SUCCESS){
-                MSG("#sensor[i]->init failed. ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]);    
+                MSG("#Error: sensor[i]->init failed. ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]);    
                 return false;    // couldn't find
             }
         }
         MSG("#ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]);    
     }
     
+    attachInterrupt();
+    
     return true;
 }
 
@@ -242,7 +244,7 @@
     Status status = SUCCESS;
     
     if ((Message::parse(&msg, buf)) != Message::SUCCESS) {
-        MSG("#Failed to parse message. %s\r\n", buf);
+        MSG("#Error: Failed to parse message. %s\r\n", buf);
         status = ERROR;
         eventCommandReceived = false;           // Reset flag
     }else{       
@@ -398,6 +400,9 @@
                 resMsg.setArgument(0, 0);
             }
             throwMessage(&resMsg);
+
+            detachInterrupt();
+            
             MSG("#Stop measurement:%s.\r\n",sensor[sensorIndex]->getSensorName());
             break;
         }
@@ -407,21 +412,16 @@
             if(msg.getArgNum() == 0){
                 error_code = sensor[sensorIndex]->startSensor();
                 if( error_code != AkmSensor::SUCCESS ){
-                    MSG("#StartSensor Error. Code=%d\r\n",error_code);            
-                }
-                else{
+                    MSG("#Error: StartSensor Error. Code=%d\r\n",error_code);            
+                }else{
                     switch(drdyType){
                         case AkmAkd::INTERRUPT_ENABLED_PP:
                         {
-                            if(primaryId == AkmSensor::AKM_PRIMARY_ID_AKD_SPI) interrupt = new InterruptIn(SPI_DRDY);
-                            else interrupt = new InterruptIn(I2C_DRDY);
                             interrupt->rise(callback(this, &AkmSensorManager::detectDRDY));                            
                             break;
                         }
                         case AkmAkd::INTERRUPT_ENABLED_OD:
                         {
-                            if(primaryId == AkmSensor::AKM_PRIMARY_ID_AKD_SPI) interrupt = new InterruptIn(SPI_DRDY);
-                            else interrupt = new InterruptIn(I2C_DRDY);
                             interrupt->fall(callback(this, &AkmSensorManager::detectDRDY));                            
                             break;
                         }
@@ -435,14 +435,15 @@
                 float interval = (float)(1.0 / (float)msg.getArgument(0));
                 error_code = sensor[sensorIndex]->startSensor(interval);
                 if( error_code != AkmSensor::SUCCESS ){
-                    MSG("#StartSensor Error. Code=%d\r\n",error_code);            
+                    MSG("#Error: StartSensor Error. Code=%d\r\n",error_code);            
                 }
                 else{
-                    if(interrupt) delete interrupt;
+                    detachInterrupt();
                 }
             }else{
-                MSG("#StartSensor Error. Wrong Argument num.\r\n");            
+                MSG("#Error: StartSensor Error. Wrong Argument num.\r\n");            
             }
+            
             if(error_code == AkmSensor::SUCCESS){
                 // get initial sensor state for switch type sensors
                 if( primaryId == AkmSensor::AKM_PRIMARY_ID_UNIPOLAR ||
@@ -455,6 +456,8 @@
                     throwMessage(&temp);
                 }
             }
+            
+            MSG("#Start measurement:%s.\r\n",sensor[sensorIndex]->getSensorName());
             break;
         }
         case Message::CMD_MOTOR_START_MOTOR:
@@ -497,7 +500,7 @@
             AkmSensor::Status st = sensor[sensorIndex]->requestCommand(&msg,&resMsg);
             if( (resMsg.getArgNum() == 0) && (st != AkmSensor::SUCCESS) )
             {
-                MSG("#Command failed.\r\n");            
+                MSG("#Error: Command failed.\r\n");            
             }else{
                 throwMessage(&resMsg);
             }
@@ -505,7 +508,7 @@
         }
         default:
         {
-            MSG("#Can't find command.\r\n");
+            MSG("#Error: Can't find command.\r\n");
             break;
         }
     }
@@ -605,4 +608,44 @@
     }
     MSG("#Sensor Name='%s'.\r\n",name);
     return name;
+}
+
+void AkmSensorManager::attachInterrupt(){
+    switch(drdyType){
+        case AkmAkd::INTERRUPT_ENABLED_PP:
+        {
+            if(primaryId == AkmSensor::AKM_PRIMARY_ID_AKD_SPI) interrupt = new InterruptIn(SPI_DRDY);
+            else interrupt = new InterruptIn(I2C_DRDY);
+            break;
+        }
+        case AkmAkd::INTERRUPT_ENABLED_OD:
+        {
+            if(primaryId == AkmSensor::AKM_PRIMARY_ID_AKD_SPI) interrupt = new InterruptIn(SPI_DRDY);
+            else interrupt = new InterruptIn(I2C_DRDY);
+            break;
+        }
+        default:
+        {
+            // nothing.
+        }
+    }
+}
+
+void AkmSensorManager::detachInterrupt(){
+    switch(drdyType){
+        case AkmAkd::INTERRUPT_ENABLED_PP:
+        {
+            interrupt->rise(NULL);
+            break;
+        }
+        case AkmAkd::INTERRUPT_ENABLED_OD:
+        {
+            interrupt->fall(NULL);
+            break;
+        }
+        default:
+        {
+            // nothing.
+        }
+    }
 }
\ No newline at end of file