Diff: akmakd.cpp

Revision:

11:cef8dc1cf010

Parent:

10:5c69b067d88a

Child:

15:1238993fd75f

--- a/akmakd.cpp	Fri Jul 08 22:26:26 2016 +0000
+++ b/akmakd.cpp	Fri Jul 22 22:54:11 2016 +0000
@@ -179,7 +179,7 @@
             foundSensor = true;
         }
         if(foundSensor != true){
-            MSG("#failed checkConnetion(SPI). %s\n",AkmAkd::sensorName);
+            MSG("#failed checkConnetion(SPI). %s\r\n",AkmAkd::sensorName);
             return AkmSensor::ERROR;
         }
         // read a data to reset DRDY 
@@ -187,7 +187,7 @@
         compass->getMagneticVector(&mag);
     }
     
-    MSG("#%s detected.\n", AkmAkd::sensorName);
+    MSG("#%s detected.\r\n", AkmAkd::sensorName);
     return AkmSensor::SUCCESS;
 }
 
@@ -207,28 +207,11 @@
 }
 
 AkmSensor::Status AkmAkd::startSensor(){
-
-    // read once to clear DRDY pin
+    // read one data to clear DRDY 
     AkmECompass::MagneticVector mag;
-    AkmECompass::Status status = compass->getMagneticVector(&mag);
+    compass->getMagneticVector(&mag);
 
-    if( subId == AkmAkd::SUB_ID_AK09915C || subId == AkmAkd::SUB_ID_AK09915D ){
-        if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
-            return AkmSensor::ERROR;
-        }
-    }
-    else if( subId == AkmAkd::SUB_ID_AK09912C ){
-        if(compass->setOperationMode(mode,nsf) != AkmECompass::SUCCESS) {
-            return AkmSensor::ERROR;
-        }
-    }
-    else{
-        if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
-            return AkmSensor::ERROR;
-        }
-    }
-    
-    // DRDY interrupt enable
+    // enable interrupt
     if( primaryId == AKM_PRIMARY_ID_AKD_I2C && 
         (subId == AkmAkd::SUB_ID_AK8963N ||
          subId == AkmAkd::SUB_ID_AK8963C ||
@@ -247,20 +230,62 @@
         // No DRDY
         ticker.attach(this, &AkmAkd::checkDRDY, 0.005);  // 200Hz
     }
+    
+    // set operation mode
+    if( subId == AkmAkd::SUB_ID_AK09915C || subId == AkmAkd::SUB_ID_AK09915D ){
+        if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
+            MSG("#Start sensor failed.\r\n");
+            return AkmSensor::ERROR;
+        }
+    }
+    else if( subId == AkmAkd::SUB_ID_AK09912C ){
+        if(compass->setOperationMode(mode,nsf) != AkmECompass::SUCCESS) {
+            MSG("#Start sensor failed.\r\n");
+            return AkmSensor::ERROR;
+        }
+    }
+    else{
+        if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
+            MSG("#Start sensor failed.\r\n");
+            return AkmSensor::ERROR;
+        }
+    }
+        
+    MSG("#Start sensor succceeded.\r\n");
     return AkmSensor::SUCCESS;
 }
 
 AkmSensor::Status AkmAkd::startSensor(const float sec){
-    return AkmSensor::ERROR;    // doesn't support
+    return AkmSensor::ERROR;
 }
 
 AkmSensor::Status AkmAkd::stopSensor(){
     ticker.detach();
- 
+    event = false;
+
+    // disable DRDY interrupt
+    if( primaryId == AKM_PRIMARY_ID_AKD_I2C && 
+        (subId == AkmAkd::SUB_ID_AK8963N ||
+         subId == AkmAkd::SUB_ID_AK8963C ||
+         subId == AkmAkd::SUB_ID_AK09912C ||
+         subId == AkmAkd::SUB_ID_AK09915C ) ){       
+         drdy->rise(NULL);
+    }
+    else if( primaryId == AKM_PRIMARY_ID_AKD_I2C && 
+        (subId == AkmAkd::SUB_ID_AK09915D ||
+         subId == AkmAkd::SUB_ID_AK09916D ) ){       
+        // Open Drain DRDY
+        drdy->fall(NULL);
+    }
+    
     // Puts the device into power down mode.
     if(compass->setOperationMode(AkmECompass::MODE_POWER_DOWN) != AkmECompass::SUCCESS) {
         return AkmSensor::ERROR;
     }
+    // read one data to clear DRDY 
+    AkmECompass::MagneticVector mag;
+    compass->getMagneticVector(&mag);
+    
     return AkmSensor::SUCCESS;
 }
 
@@ -293,7 +318,7 @@
         {
             if(compass->getOperationMode(&mode) != AkmECompass::SUCCESS) {
                 status =  AkmSensor::ERROR;
-                MSG("#Error set operation mode.\n");
+                MSG("#Error set operation mode.\r\n");
             }
             out->setArgument(0,mode);
             break;
@@ -306,20 +331,20 @@
                 sdr = (AkmECompass::Sdr)in->getArgument(2);
                 if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
                     status =  AkmSensor::ERROR;
-                    MSG("#Error set operation mode.\n");
+                    MSG("#Error set operation mode.\r\n");
                 }
             }
             else if(subId == AkmAkd::SUB_ID_AK09912C){
                 nsf = (AkmECompass::Nsf)in->getArgument(1);
                 if(compass->setOperationMode(mode,(AkmECompass::Nsf)nsf) != AkmECompass::SUCCESS) {
                     status =  AkmSensor::ERROR;
-                    MSG("#Error set operation mode.\n");
+                    MSG("#Error set operation mode.\r\n");
                 }
             }
             else{
                 if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
                     status =  AkmSensor::ERROR;
-                    MSG("#Error set operation mode.\n");
+                    MSG("#Error set operation mode.\r\n");
                 }
             }
             out->setArgument(0,(char)status);                
@@ -335,7 +360,7 @@
             }
             if( compass->write(address, data, len) != AkmECompass::SUCCESS) {
                 status =  AkmSensor::ERROR;
-                MSG("#Error register write.\n");
+                MSG("#Error register write.\r\n");
             }
             out->setArgument(0,(char)status);
             break;
@@ -347,7 +372,7 @@
             char data;
             if( compass->read(address, &data, len) != AkmECompass::SUCCESS) {
                 status =  AkmSensor::ERROR;
-                MSG("#Error register read.\n");
+                MSG("#Error register read.\r\n");
             }
             out->setArgument(0,data);
             break;
@@ -359,7 +384,7 @@
             char data[len];
             if( compass->read(address, data, len) != AkmECompass::SUCCESS) {
                 status =  AkmSensor::ERROR;
-                MSG("#Error register read.\n");
+                MSG("#Error register read.\r\n");
             }
             for(int i=0; i<len; i++){
                 out->setArgument(i, data[i]);
@@ -368,7 +393,7 @@
         }
         default:
         {
-            MSG("#Error no command.\n");
+            MSG("#Error no command.\r\n");
             status =  AkmSensor::ERROR;
             break;
         }