Diff: akmakd.cpp

Revision:

20:2fca76521680

Parent:

19:8dcc4f323bdc

Child:

27:41aa9fb23a2f

--- a/akmakd.cpp	Wed Feb 22 18:47:01 2017 +0000
+++ b/akmakd.cpp	Wed Feb 22 21:56:05 2017 +0000
@@ -24,20 +24,20 @@
     if (drdy) delete drdy;
 }
 
-AkmSensor::Status AkmAkd::checkSensor( const uint8_t id, const uint8_t subid, AkmECompass::SensorType* type){
+AkmSensor::Status AkmAkd::checkSensor( const uint8_t primaryid, const uint8_t subid, AkmECompass::DeviceId* devid){
     AK099XX* ak099xx;
     AK8963* ak8963;
     
     switch(subid){
         case AkmAkd::SUB_ID_AK8963N:
         case AkmAkd::SUB_ID_AK8963C:
-            *type = AkmECompass::AK8963;
+            *devid = AkmECompass::AK8963;
             AkmAkd::sensorName = "AK8963";
             ak8963 = new AK8963();
             compass = ak8963;
             break;
         case AkmAkd::SUB_ID_AK09911C:
-            *type = AkmECompass::AK09911;
+            *devid = AkmECompass::AK09911;
             AkmAkd::sensorName = "AK09911C";
             if(primaryId == AKM_PRIMARY_ID_AKD_SPI) 
                 return AkmSensor::ERROR;    // No SPI support
@@ -45,25 +45,25 @@
             compass = ak099xx;
             break;
         case AkmAkd::SUB_ID_AK09912C:
-            *type = AkmECompass::AK09912;
+            *devid = AkmECompass::AK09912;
             AkmAkd::sensorName = "AK09912C";
             ak099xx = new AK099XX();
             compass = ak099xx;
             break;
         case AkmAkd::SUB_ID_AK09915C:
-            *type = AkmECompass::AK09915;
+            *devid = AkmECompass::AK09915;
             AkmAkd::sensorName = "AK09915C";
             ak099xx = new AK099XX();
             compass = ak099xx;
             break;
         case AkmAkd::SUB_ID_AK09915D:
-            *type = AkmECompass::AK09915;
+            *devid = AkmECompass::AK09915;
             AkmAkd::sensorName = "AK09915D";
             ak099xx = new AK099XX();
             compass = ak099xx;
             break;
         case AkmAkd::SUB_ID_AK09916C:
-            *type = AkmECompass::AK09916C;
+            *devid = AkmECompass::AK09916C;
             AkmAkd::sensorName = "AK09916C";
             if(primaryId == AKM_PRIMARY_ID_AKD_SPI) 
                 return AkmSensor::ERROR;    // No SPI support
@@ -71,7 +71,7 @@
             compass = ak099xx;
             break;
         case AkmAkd::SUB_ID_AK09916D:
-            *type = AkmECompass::AK09916D;
+            *devid = AkmECompass::AK09916D;
             AkmAkd::sensorName = "AK09916D";
             if(primaryId == AKM_PRIMARY_ID_AKD_SPI) 
                 return AkmSensor::ERROR;    // No SPI support
@@ -79,7 +79,7 @@
             compass = ak099xx;
             break;
         case AkmAkd::SUB_ID_AK09918:
-            *type = AkmECompass::AK09918;
+            *devid = AkmECompass::AK09918;
             AkmAkd::sensorName = "AK09918";
             if(primaryId == AKM_PRIMARY_ID_AKD_SPI) 
                 return AkmSensor::ERROR;    // No SPI support
@@ -92,11 +92,11 @@
     return AkmSensor::SUCCESS;    
 }
 
-AkmSensor::Status AkmAkd::init(const uint8_t id, const uint8_t subid){
-    primaryId = id;
+AkmSensor::Status AkmAkd::init(const uint8_t primaryid, const uint8_t subid){
+    primaryId = primaryid;
     subId = subid;
     
-    AkmECompass::SensorType type;
+    AkmECompass::DeviceId devid;
 
     mode = AkmECompass::MODE_POWER_DOWN;
     nsf = AkmECompass::NSF_DISABLE;
@@ -110,7 +110,7 @@
         I2C* i2c = new I2C(I2C_SDA,I2C_SCL);
         i2c->frequency(I2C_SPEED_100KHZ);
 
-        if( checkSensor(id,subid,&type) != AkmSensor::SUCCESS ){
+        if( checkSensor(primaryId,subId,&devid) != AkmSensor::SUCCESS ){
             return AkmSensor::ERROR;
         }
         
@@ -122,7 +122,7 @@
 
         for(int i=0; i<sizeof(slaveAddr); i++)
         {
-            compass->init(i2c, slaveAddr[i], type);
+            compass->init(i2c, slaveAddr[i], devid);
             // Checks connectivity
             if(compass->checkConnection() == AkmECompass::SUCCESS) {
                 MSG("#%s detected.\r\n", AkmAkd::sensorName);
@@ -140,11 +140,11 @@
         
         DigitalOut* cs = new DigitalOut(SPI_CS);
 
-        if( checkSensor(id,subid,&type) != AkmSensor::SUCCESS ){
+        if( checkSensor(primaryId,subId,&devid) != AkmSensor::SUCCESS ){
             return AkmSensor::ERROR;
         }
         
-        compass->init(spi, cs, type);
+        compass->init(spi, cs, devid);
         if(compass->checkConnection() == AkmECompass::SUCCESS) {
             MSG("#%s detected.\r\n", AkmAkd::sensorName);
             return AkmSensor::SUCCESS;
@@ -190,7 +190,6 @@
     return ret;
 }
 
-
 AkmSensor::Status AkmAkd::startSensor(){
     // read one data to clear DRDY 
     AkmECompass::MagneticVector mag;
@@ -208,7 +207,7 @@
     }
     else{
         // No DRDY
-        ticker.attach(callback(this, &AkmAkd::checkDRDY), 0.005);  // 200Hz
+        ticker.attach(callback(this, &AkmAkd::checkDRDY), 1.0/AKDP_POLLING_FREQUENCY);
     }
     
     // set operation mode
@@ -255,11 +254,18 @@
     else{
         // No DRDY
     }
-    
+
     // Puts the device into power down mode.
-    if(compass->setOperationMode(AkmECompass::MODE_POWER_DOWN) != AkmECompass::SUCCESS) {
-        return AkmSensor::ERROR;
+    if( subId == AkmAkd::SUB_ID_AK09915C || subId == AkmAkd::SUB_ID_AK09915D ){
+        if(compass->setOperationMode(AkmECompass::MODE_POWER_DOWN,nsf,sdr) != AkmECompass::SUCCESS) {
+            return AkmSensor::ERROR;
+        }
+    }else{
+        if(compass->setOperationMode(AkmECompass::MODE_POWER_DOWN) != AkmECompass::SUCCESS) {
+            return AkmSensor::ERROR;
+        }        
     }
+    
     // read one data to clear DRDY 
     AkmECompass::MagneticVector mag;
     compass->getMagneticVector(&mag);
@@ -277,12 +283,12 @@
     }
     msg->setCommand(Message::CMD_START_MEASUREMENT);
     msg->setArgument( 0, (mag.isOverflow ? 1 : 0) );
-    msg->setArgument( 1,(char)((int32_t)(mag.mx/0.15) >> 8));
-    msg->setArgument( 2, (char)((int32_t)(mag.mx/0.15) & 0x00FF) );
-    msg->setArgument( 3, (char)((int32_t)(mag.my/0.15) >> 8) );
-    msg->setArgument( 4, (char)((int32_t)(mag.my/0.15) & 0x00FF) );
-    msg->setArgument( 5, (char)((int32_t)(mag.mz/0.15) >> 8) );
-    msg->setArgument( 6, (char)((int32_t)(mag.mz/0.15) & 0x00FF) );
+    msg->setArgument( 1,(char)((int32_t)(mag.mx/AKDP_MAG_SENSITIVITY) >> 8));
+    msg->setArgument( 2, (char)((int32_t)(mag.mx/AKDP_MAG_SENSITIVITY) & 0x00FF) );
+    msg->setArgument( 3, (char)((int32_t)(mag.my/AKDP_MAG_SENSITIVITY) >> 8) );
+    msg->setArgument( 4, (char)((int32_t)(mag.my/AKDP_MAG_SENSITIVITY) & 0x00FF) );
+    msg->setArgument( 5, (char)((int32_t)(mag.mz/AKDP_MAG_SENSITIVITY) >> 8) );
+    msg->setArgument( 6, (char)((int32_t)(mag.mz/AKDP_MAG_SENSITIVITY) & 0x00FF) );
     return AkmSensor::SUCCESS;
 }