Diff: akmakd.cpp

Revision:

41:a3ea80c594ec

Parent:

40:42e48427e4b7

Child:

42:b48b3ab8690e

--- a/akmakd.cpp	Wed Jul 19 23:30:22 2017 +0000
+++ b/akmakd.cpp	Thu Aug 17 19:59:10 2017 +0000
@@ -44,18 +44,21 @@
             AkmAkd::sensorName = "AK09912C";
             ak099xx = new AK099XX();
             compass = ak099xx;
+            lenOptions = 1;
             break;
         case AkmAkd::SUB_ID_AK09915C:
             *devid = AkmECompass::AK09915;
             AkmAkd::sensorName = "AK09915C";
             ak099xx = new AK099XX();
             compass = ak099xx;
+            lenOptions = 2;
             break;
         case AkmAkd::SUB_ID_AK09915D:
             *devid = AkmECompass::AK09915;
             AkmAkd::sensorName = "AK09915D";
             ak099xx = new AK099XX();
             compass = ak099xx;
+            lenOptions = 2;
             break;
         case AkmAkd::SUB_ID_AK09916C:
             *devid = AkmECompass::AK09916C;
@@ -88,10 +91,22 @@
                 return AkmSensor::ERROR;    // No SPI support
             ak099xx = new AK099XX();
             compass = ak099xx;
+            lenOptions = 2;
+            break;
+        case AkmAkd::SUB_ID_AK09940:
+            *devid = AkmECompass::AK09940;
+            AkmAkd::sensorName = "AK09940";
+            ak099xx = new AK099XX();
+            compass = ak099xx;
+            lenOptions = 5;
             break;
         default:
             return AkmSensor::ERROR;
     }
+    
+    uint8_t options[lenOptions];
+    mode.options = options;
+    
     return AkmSensor::SUCCESS;    
 }
 
@@ -101,9 +116,8 @@
     
     AkmECompass::DeviceId devid;
 
-    mode = AkmECompass::MODE_POWER_DOWN;
-    nsf = AkmECompass::NSF_DISABLE;
-    sdr = AkmECompass::SDR_LOW_POWER_DRIVE;
+    mode.mode = AkmECompass::MODE_POWER_DOWN;
+    lenOptions = 0;
     
     if(primaryId == AKM_PRIMARY_ID_AKD_I2C){
 
@@ -133,7 +147,7 @@
         
         SPI* spi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK);
         spi->format(8,3);    // 8bit, Mode=3
-        spi->frequency(1000000);
+        spi->frequency(SPI_SPEED);
         
         DigitalOut* cs = new DigitalOut(SPI_CS);
 
@@ -151,8 +165,8 @@
 }
 
 void AkmAkd::setEvent(){
-    AkmECompass::Status  status = compass->isDataReady();
-    if( status == AkmECompass::DATA_READY || status == AkmECompass::DATA_OVER_RUN ){
+    AkmECompass::Status status = compass->isDataReady();
+    if( status == AkmECompass::DATA_READY ){
         base::setEvent();
     }
 }
@@ -164,7 +178,8 @@
         (subId == AkmAkd::SUB_ID_AK8963N ||
          subId == AkmAkd::SUB_ID_AK8963C ||
          subId == AkmAkd::SUB_ID_AK09912C ||
-         subId == AkmAkd::SUB_ID_AK09915C ) ){
+         subId == AkmAkd::SUB_ID_AK09915C ||
+         subId == AkmAkd::SUB_ID_AK09940 ) ){
         ret = AkmAkd::INTERRUPT_ENABLED_PP;
     }
     else if( primaryId == AKM_PRIMARY_ID_AKD_I2C && 
@@ -182,8 +197,8 @@
 
 AkmSensor::Status AkmAkd::startSensor(){
     // read one data to clear DRDY 
-    AkmECompass::MagneticVector mag;
-    compass->getMagneticVector(&mag);
+    AkmECompass::MagneticVectorLsb mag;
+    compass->getMagneticVectorLsb(&mag);
 
     AkmAkd::InterruptMode int_mode = getInterrupt(primaryId,subId);
     if( int_mode == AkmAkd::INTERRUPT_DISABLED ){       
@@ -192,25 +207,9 @@
     }
     
     // set operation mode
-    if( subId == AkmAkd::SUB_ID_AK09915C || 
-        subId == AkmAkd::SUB_ID_AK09915D || 
-        subId == AkmAkd::SUB_ID_AK09917 ){
-        if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
-            MSG("#Error: Start sensor failed %s\r\n", sensorName);
-            return AkmSensor::ERROR;
-        }
-    }
-    else if( subId == AkmAkd::SUB_ID_AK09912C ){
-        if(compass->setOperationMode(mode,nsf) != AkmECompass::SUCCESS) {
-            MSG("#Error: Start sensor failed %s\r\n", sensorName);
-            return AkmSensor::ERROR;
-        }
-    }
-    else{
-        if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
-            MSG("#Error: Start sensor failed %s\r\n", sensorName);
-            return AkmSensor::ERROR;
-        }
+    if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
+        MSG("#Error: Start sensor failed %s\r\n", sensorName);
+        return AkmSensor::ERROR;
     }
         
     MSG("#Start sensor %s.\r\n",sensorName);
@@ -224,23 +223,19 @@
 AkmSensor::Status AkmAkd::stopSensor(){
     ticker.detach();
     AkmSensor::clearEvent();
-
-    // Puts the device into power down mode.
-    if( subId == AkmAkd::SUB_ID_AK09915C || 
-        subId == AkmAkd::SUB_ID_AK09915D || 
-        subId == AkmAkd::SUB_ID_AK09917 ){
-        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;
-        }        
+    
+    AkmECompass::Mode temp;
+    temp.mode = AkmECompass::MODE_POWER_DOWN;
+    for(int i=0; i<lenOptions; i++){
+        temp.options[i] = mode.options[i];
+    }
+    if(compass->setOperationMode(temp) != AkmECompass::SUCCESS) {
+        return AkmSensor::ERROR;
     }
     
     // read one data to clear DRDY 
-    AkmECompass::MagneticVector mag;
-    compass->getMagneticVector(&mag);
+    AkmECompass::MagneticVectorLsb mag;
+    compass->getMagneticVectorLsb(&mag);
     
     return AkmSensor::SUCCESS;
 }
@@ -248,19 +243,35 @@
 AkmSensor::Status AkmAkd::readSensorData(Message* msg){
     AkmSensor::clearEvent();
 
-    AkmECompass::MagneticVector mag;
-    AkmECompass::Status status = compass->getMagneticVector(&mag);
+    AkmECompass::MagneticVectorLsb lsb;
+    AkmECompass::Status status = compass->getMagneticVectorLsb(&lsb);
     if( status != AkmECompass::SUCCESS){
         return AkmSensor::ERROR;
     }
     msg->setCommand(Message::CMD_START_MEASUREMENT);
-    msg->setArgument( 0, (mag.isOverflow ? 1 : 0) );
-    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) );
+    msg->setArgument( 0, (lsb.isOverflow ? 1 : 0) );
+    if(subId == AkmAkd::SUB_ID_AK09940)
+    {
+        msg->setArgument( 1,(char)( lsb.lsbX>>16  ) );
+        msg->setArgument( 2,(char)( lsb.lsbX>>8  ) );
+        msg->setArgument( 3,(char)( lsb.lsbX ) );        
+        msg->setArgument( 4,(char)( lsb.lsbY>>16 ) );
+        msg->setArgument( 5,(char)( lsb.lsbY>>8 ) );
+        msg->setArgument( 6,(char)( lsb.lsbY ) );        
+        msg->setArgument( 7,(char)( lsb.lsbZ>>16 ) );
+        msg->setArgument( 8,(char)( lsb.lsbZ>>8 ) );
+        msg->setArgument( 9,(char)( lsb.lsbZ ) );
+        msg->setArgument(10,(char)( lsb.lsbTemp ) );
+    }
+    else
+    {
+        msg->setArgument( 1,(char)( lsb.lsbX>>8 ) );
+        msg->setArgument( 2,(char)( lsb.lsbX ) );
+        msg->setArgument( 3,(char)( lsb.lsbY>>8 ) );
+        msg->setArgument( 4,(char)( lsb.lsbY ) );
+        msg->setArgument( 5,(char)( lsb.lsbZ>>8  ) );
+        msg->setArgument( 6,(char)( lsb.lsbZ ) );        
+    }
     return AkmSensor::SUCCESS;
 }
 
@@ -271,65 +282,29 @@
     
     switch(cmd){
         case Message::CMD_COMPASS_GET_OPERATION_MODE:
-        {
-            if( subId == AkmAkd::SUB_ID_AK09915C || 
-                subId == AkmAkd::SUB_ID_AK09915D || 
-                subId == AkmAkd::SUB_ID_AK09917 ){
-                if(compass->getOperationMode(&mode, &nsf, &sdr) != AkmECompass::SUCCESS) {
-                    status = AkmSensor::ERROR;
-                    MSG("#Error: set operation mode.\r\n");
-                }else{
-                    out->setArgument(0,mode);
-                    out->setArgument(1,nsf);
-                    out->setArgument(2,sdr);
-                }                
-                
-            }else if(subId == AkmAkd::SUB_ID_AK09912C){
-                if(compass->getOperationMode(&mode, &nsf) != AkmECompass::SUCCESS) {
-                    status = AkmSensor::ERROR;
-                    MSG("#Error: set operation mode.\r\n");
-                }else{
-                    out->setArgument(0,mode);
-                    out->setArgument(1,nsf);
-                }                
+        {            
+            if(compass->getOperationMode(&mode) != AkmECompass::SUCCESS) {
+                status = AkmSensor::ERROR;
+                MSG("#Error: set operation mode.\r\n");
             }else{
-                if(compass->getOperationMode(&mode) != AkmECompass::SUCCESS) {
-                    status = AkmSensor::ERROR;
-                    MSG("#Error: set operation mode.\r\n");
-                }else{
-                    out->setArgument(0,mode);
-                }                
+                out->setArgument(0,mode.mode);
+                for(int i=0; i<lenOptions; i++){
+                    out->setArgument(i+1,mode.options[i]);                
+                }
             }
             break;
         }
         case Message::CMD_COMPASS_SET_OPERATION_MODE:
         {
-            mode = (AkmECompass::OperationMode)in->getArgument(0);
-            if( subId == AkmAkd::SUB_ID_AK09915C || 
-                subId == AkmAkd::SUB_ID_AK09915D || 
-                subId == AkmAkd::SUB_ID_AK09917 ){
-
-                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.\r\n");
-                }
+            mode.mode = (AkmECompass::OperationMode)in->getArgument(0);
+            for(int i=0; i<lenOptions; i++){
+                mode.options[i] = in->getArgument(i+1);
             }
-            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.\r\n");
-                }
+            if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Error: set operation mode.\r\n");
             }
-            else{
-                if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
-                    status =  AkmSensor::ERROR;
-                    MSG("#Error: set operation mode.\r\n");
-                }
-            }
-            out->setArgument(0,(char)status);                
+            out->setArgument(0,(char)status);
             break;
         }
         case Message::CMD_REG_WRITE: