Diff: ak09970ctrl.cpp

Revision:

29:b488d2c89fba

Parent:

27:41aa9fb23a2f

Child:

39:3821886c902e

--- a/ak09970ctrl.cpp	Sat Mar 11 02:08:32 2017 +0000
+++ b/ak09970ctrl.cpp	Fri Mar 17 23:29:20 2017 +0000
@@ -1,6 +1,10 @@
 #include "ak09970ctrl.h"
 #include "debug.h"
 
+#ifndef PI
+#define PI           3.14159265358979323846
+#endif
+
 #define CONV16I(high,low)  ((int16_t)(((high) << 8) | (low)))
 /**
  * Constructor.
@@ -8,7 +12,7 @@
  */
 Ak09970Ctrl::Ak09970Ctrl() : AkmSensor(){
     ak09970 = NULL;
-    sw = NULL;
+//    interrupt = NULL;
 }
 
 /**
@@ -16,9 +20,10 @@
  *
  */
 Ak09970Ctrl::~Ak09970Ctrl(){
-    sw->rise(0);
+//    interrupt->rise(NULL);
+//    interrupt->fall(NULL);
     if (ak09970) delete ak09970;
-    if (sw) delete sw;
+//    if (interrupt) delete interrupt;
 }
 
 AkmSensor::Status Ak09970Ctrl::init(const uint8_t id, const uint8_t subid){
@@ -33,8 +38,8 @@
     
     if(primaryId == AKM_PRIMARY_ID_AKD_I2C){
 
-        sw = new InterruptIn(I2C_DRDY);    
-        sw->rise(0);
+//        interrupt = new InterruptIn(I2C_DRDY);    
+//        interrupt->rise(0);
 
         I2C* i2c = new I2C(I2C_SDA,I2C_SCL);
         i2c->frequency(I2C_SPEED_100KHZ);
@@ -73,8 +78,8 @@
 
     }else if(primaryId == AKM_PRIMARY_ID_AKD_SPI){
         
-        sw = new InterruptIn(SPI_DRDY);    
-        sw->rise(0);
+//        interrupt = new InterruptIn(SPI_DRDY);    
+//        sw->rise(0);
         
         SPI* spi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK);
         spi->format(8,3);    // 8bit, Mode=3
@@ -112,15 +117,10 @@
     return AkmSensor::SUCCESS;
 }
 
-
-void Ak09970Ctrl::checkINT(){
-//    AK09970::Status  status = ak09970->isDataReady();
-//    if( status == AK09970::DATA_READY || 
-//        status == AK09970::DATA_OVER_RUN ) event = true;
-}
-
-void Ak09970Ctrl::detectINT(){
-    event = true;
+void Ak09970Ctrl::setEvent(){
+//    MSG("#setEvent() in %s.\r\n",sensorName);
+    AK09970::Status  status = ak09970->isDataReady();
+    if( status == AK09970::DATA_READY ) base::setEvent();
 }
 
 AkmSensor::Status Ak09970Ctrl::startSensor(){
@@ -129,7 +129,14 @@
     ak09970->getSwitchStatus(&sw_status, readConfig);
 
     // enable interrupt
-    sw->rise(callback(this, &Ak09970Ctrl::detectINT));
+    if(switchConfig.enabledINTEN){
+//        interrupt->rise(callback(this, &AkmSensor::setEvent));  // Push-pull
+        MSG("#INT Enable.\r\n");
+    }
+    else if(switchConfig.enabledODINTEN){
+//        interrupt->fall(callback(this, &AkmSensor::setEvent));  // Opnen drain
+        MSG("#ODINT Enable.\r\n");
+    }
 
     // set operation mode
     if(ak09970->setOperationMode(mode,sensorDriveMode,sensorMeasurementRange) != AK09970::SUCCESS) {
@@ -137,17 +144,23 @@
         return AkmSensor::ERROR;
     }
     
-    MSG("#Start sensor succceeded.\r\n");
+    MSG("#Start sensor %s.\r\n", sensorName);
     return AkmSensor::SUCCESS;
 }
 
 AkmSensor::Status Ak09970Ctrl::startSensor(const float sec){
+    MSG("#Start sensor %s.\r\n", sensorName);
     return AkmSensor::ERROR;
 }
 
 AkmSensor::Status Ak09970Ctrl::stopSensor(){
-    sw->rise(NULL);
-    event = false;
+    if(switchConfig.enabledINTEN){
+//        interrupt->rise(NULL);
+    }
+    else if(switchConfig.enabledODINTEN){
+//        interrupt->fall(NULL);
+    }
+    AkmSensor::clearEvent();
     
     if(ak09970->setOperationMode(AK09970::MODE_POWER_DOWN, sensorDriveMode, sensorMeasurementRange) != AK09970::SUCCESS) {
         return AkmSensor::ERROR;
@@ -161,7 +174,7 @@
 }
 
 AkmSensor::Status Ak09970Ctrl::readSensorData(Message* msg){
-    event = false;
+    AkmSensor::clearEvent();
         
     AK09970::SwitchStatus sw_status;
     if( ak09970->getSwitchStatus(&sw_status, readConfig) != AkmSensor::SUCCESS) {
@@ -179,6 +192,11 @@
     msg->setArgument(7, (char)(sw_status.mag.mz>>8));
     msg->setArgument(8, (char)(sw_status.mag.mz & 0x00FF));
     
+    // test
+//    double angle=0;
+//    angle = atan2((double)sw_status.mag.mx, (double)sw_status.mag.my)*180.0/PI+180.0;
+//    MSG("#Angle=%3.1f\r\n",angle);
+    
     return AkmSensor::SUCCESS;
 }