Diff: akmakd.cpp

Revision:

9:6fa3e7b17c27

Parent:

7:e269411568c9

Child:

10:5c69b067d88a

diff -r 8fa4b81db50b -r 6fa3e7b17c27 akmakd.cpp
--- a/akmakd.cpp	Sat Jun 11 00:16:17 2016 +0000
+++ b/akmakd.cpp	Thu Jun 16 18:36:26 2016 +0000
@@ -31,7 +31,11 @@
     AkmECompass::SensorType type;
     AK099XX* ak099xx;
     AK8963* ak8963;
-
+    
+    mode = AkmECompass::MODE_POWER_DOWN;
+    nsf = AkmECompass::NSF_DISABLE;
+    sdr = AkmECompass::SDR_LOW_POWER_DRIVE;
+    
     if(primaryId == AKM_PRIMARY_ID_AKD_I2C){
 
         drdy = new InterruptIn(I2C_DRDY);    
@@ -182,20 +186,36 @@
 }
 
 AkmSensor::Status AkmAkd::startSensor(){
+    // read once to clear DRDY pin
+    AkmECompass::MagneticVector mag;
+    AkmECompass::Status status = compass->getMagneticVector(&mag);
+
     // Puts the device into continuous measurement mode.
-    if(compass->setOperationMode((AkmECompass::OperationMode)0x02) != AkmECompass::SUCCESS) {
-        return AkmSensor::ERROR;
+    if(compass->getSensorType() == AkmECompass::AK09915){
+        if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
+            return AkmSensor::ERROR;
+        }
+    }
+    else if(compass->getSensorType() == AkmECompass::AK09912){
+        if(compass->setOperationMode(mode,nsf) != AkmECompass::SUCCESS) {
+            return AkmSensor::ERROR;
+        }
+    }
+    else{
+        if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
+            return AkmSensor::ERROR;
+        }
     }
     
     // DRDY interrupt enable for AK8963/AK09912/AK09915 with I2C interface
-    if(primaryId == AKM_PRIMARY_ID_AKD_I2C && 
+    if( primaryId == AKM_PRIMARY_ID_AKD_I2C && 
         (compass->getSensorType() == AkmECompass::AK8963 ||
          compass->getSensorType() == AkmECompass::AK09912 ||
          compass->getSensorType() == AkmECompass::AK09915) ){       
         drdy->rise(this, &AkmAkd::detectDRDY);
     }
     else{
-        ticker.attach(this, &AkmAkd::checkDRDY, 0.01);  // 100Hz
+        ticker.attach(this, &AkmAkd::checkDRDY, 0.005);  // 200Hz
     }
     return AkmSensor::SUCCESS;
 }
@@ -234,6 +254,96 @@
 }
 
 AkmSensor::Status AkmAkd::requestCommand(Message* in, Message* out){    
+    AkmSensor::Status status = AkmSensor::SUCCESS;
+    Message::Command cmd = in->getCommand();
+    out->setCommand(cmd);
+    
+    switch(cmd){
+        case Message::CMD_COMPASS_GET_OPERATION_MODE:
+        {
+            if(compass->getOperationMode(&mode) != AkmECompass::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Error set operation mode.\n");
+            }
+            out->setArgument(0,mode);
+            break;
+        }
+        case Message::CMD_COMPASS_SET_OPERATION_MODE:
+        {
+            mode = (AkmECompass::OperationMode)in->getArgument(0);
+            if(compass->getSensorType() == AkmECompass::AK09915){
+                nsf = (AkmECompass::Nsf)in->getArgument(1);
+                sdr = (AkmECompass::Sdr)in->getArgument(2);
+                if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
+                    status =  AkmSensor::ERROR;
+                    MSG("#Error set operation mode.\n");
+                }
+            }
+            else if(compass->getSensorType() == AkmECompass::AK09912){
+                nsf = (AkmECompass::Nsf)in->getArgument(1);
+                if(compass->setOperationMode(mode,(AkmECompass::Nsf)nsf) != AkmECompass::SUCCESS) {
+                    status =  AkmSensor::ERROR;
+                    MSG("#Error set operation mode.\n");
+                }
+            }
+            else{
+                if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
+                    status =  AkmSensor::ERROR;
+                    MSG("#Error set operation mode.\n");
+                }
+            }
+            out->setArgument(0,(char)status);                
+            break;
+        }        
+        case Message::CMD_REG_WRITEN:
+        {
+            char address = in->getArgument(0);
+            int len = (int)in->getArgument(1);
+            char data[len];
+            for(int i=0; i<len; i++){
+                data[i] = in->getArgument(i);    
+            }
+            if( compass->write(address, data, len) != AkmECompass::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Error register write.\n");
+            }
+            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.\n");
+            }
+            out->setArgument(0,data);
+            break;
+        }
+        case Message::CMD_REG_READN:
+        {
+            char address = in->getArgument(0);
+            int len = (int)in->getArgument(1);
+            char data[len];
+            if( compass->read(address, data, len) != AkmECompass::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Error register read.\n");
+            }
+            for(int i=0; i<len; i++){
+                out->setArgument(i, data[i]);
+            }
+            break;
+        }
+        default:
+        {
+            MSG("#Error no command.\n");
+            status =  AkmSensor::ERROR;
+            break;
+        }
+    }
+
     return AkmSensor::SUCCESS;
 }