Diff: akmirsensor.cpp

Revision:

1:b46b8653331f

Parent:

0:7a00359e701e

Child:

3:a817a3b3e9ae

--- a/akmirsensor.cpp	Thu Apr 28 21:12:04 2016 +0000
+++ b/akmirsensor.cpp	Thu May 05 00:19:39 2016 +0000
@@ -1,6 +1,8 @@
 #include "akmirsensor.h"
 #include "debug.h"
 
+#define CONV16I(high,low)  ((int16_t)(((high) << 8) | (low)))
+
 /**
  * Constructor.
  *
@@ -8,6 +10,7 @@
 AkmIrSensor::AkmIrSensor(){
     ak9750 = NULL;
     event = false;
+    interrupt = NULL;
 }
 
 /**
@@ -16,22 +19,102 @@
  */
 AkmIrSensor::~AkmIrSensor(){
     if (ak9750) delete ak9750;
+    if (interrupt) delete interrupt;
 }
 
 AkmSensor::Status AkmIrSensor::init(const uint8_t id, const uint8_t subid){
     primaryId = id;
     subId = subid;
 
+    interrupt = new InterruptIn(DRDY);    
+
     I2C* i2c = new I2C(I2C_SDA,I2C_SCL);
     i2c->frequency(I2C_SPEED_100KHZ);
 
     if(subId == SUB_ID_AK9750){
-//      ak9750 = new AK9750();
+      ak9750 = new AK9750();
     }
     else{
         return AkmSensor::ERROR;
     }
 
+    bool foundSensor = false;
+
+    AK9750::SlaveAddress slaveAddr[] 
+    = { AK9750::SLAVE_ADDR_1,
+        AK9750::SLAVE_ADDR_2,
+        AK9750::SLAVE_ADDR_3};
+
+    for(int i=0; i<sizeof(slaveAddr); i++)
+    {
+        ak9750->init(i2c, slaveAddr[i]);
+        // Checks connectivity
+        if(ak9750->checkConnection() == AkmIrSensor::SUCCESS) {
+            // found
+            foundSensor = true;
+            break;
+        }
+    }
+    if(foundSensor != true) return AkmSensor::ERROR;
+
+    // reset
+    if (ak9750->reset() != AK9750::SUCCESS) {
+        MSG("#Failed to reset AK9750.\n");
+    }
+    
+    // Set to EEPROM mode to EEPROM access
+    if(ak9750->setOperationMode(AK9750::MODE_EEPROM_ACCESS, AK9750::DF_0P3HZ) != AK9750::SUCCESS) {
+        MSG("#Error setOperationMode to EEPROM mode. AK9750.\n");
+        return AkmSensor::ERROR;
+    }
+    // Gets threshold from EEPROM
+    AK9750::Threshold th;
+    if (ak9750->getThresholdFromEEPROM(&th) != AK9750::SUCCESS) {
+        MSG("#Failed to get threshold from EEPROM AK9750.\n");
+    }
+    // Gets hysteresis from EEPROM
+    AK9750::Hysteresis hys;
+    if (ak9750->getHysteresisFromEEPROM(&hys) != AK9750::SUCCESS) {
+        MSG("#Failed to get hysteresis from EEPROM AK9750.\n");
+    }
+    // Gets interrupt status from EEPROM
+    AK9750::InterruptStatus intStatus;
+    if ((ak9750->getInterruptStatusFromEEPROM(&intStatus)) != AK9750::SUCCESS) {
+        MSG("#Failed to get interrupts of AK9750 from EEPROM.\n");
+    }
+
+    // Gets operation mode from EEPROM
+    if ((ak9750->getOperationModeFromEEPROM(&mode,&filter)) != AK9750::SUCCESS) {
+        MSG("#Failed to get operation mode of AK9750 from EEPROM.\n");
+    }
+    MSG("#Operation Mode:(0x%02X,0x%02X)\n",mode, filter);
+    
+    // Back to Stand-by Mode for register access
+    if(ak9750->setOperationMode(AK9750::MODE_STANDBY, filter) != AK9750::SUCCESS) {
+        MSG("#Error setOperationMode to stand-by mode of AK9750.\n");
+        return AkmSensor::ERROR;
+    }
+    
+    // Sets threshold from the red EEPROM values
+    if (ak9750->setThreshold(&th) != AK9750::SUCCESS) {
+        MSG("#Failed to set threshold to AK9750.\n");
+    }
+    MSG("#Threshold:(0x%02X,0x%02X,0x%02X,0x%02X)\n",th.eth13h,th.eth13l,th.eth24h,th.eth24l);
+
+    // Sets hysteresis from the red EEPROM values
+    if (ak9750->setHysteresis(&hys) != AK9750::SUCCESS) {
+        MSG("#Failed to set hysteresis to AK9750.\n");
+    }
+    MSG("#Hysteresis:(0x%02X,0x%02X)\n",hys.ehys13,hys.ehys24);
+    
+    // Sets interruput status from the red EEPROM values
+    AK9750::Status status;
+    if ((status=ak9750->setInterruptStatus(&intStatus)) != AK9750::SUCCESS) {
+        MSG("#Failed to set interrupts of AK9750. Status = 0x%02X\n", status);
+    }
+    MSG("#Interrupt:(0x%02X,0x%02X,0x%02X,0x%02X,0x%02X)\n",intStatus.drdy,intStatus.ir13h,intStatus.ir13l,intStatus.ir24h,intStatus.ir24l);
+    
+    MSG("#Init success AK9750.\n");
     return AkmSensor::SUCCESS;
 }
 
@@ -43,26 +126,294 @@
     return event;
 }
 
+void AkmIrSensor::checkINT(){
+    
+}
+
 AkmSensor::Status AkmIrSensor::startSensor(){
-    interrupt->rise(this, &AkmIrSensor::detectINT);
+    if(ak9750->setOperationMode(mode,filter) != AK9750::SUCCESS) {
+        MSG("#Error setOperationMode. AK9750.\n");
+        return AkmSensor::ERROR;
+    }
+
+    interrupt->fall(this, &AkmIrSensor::detectINT);
+    MSG("#Start Sensor success AK9750.\n");
     return AkmSensor::SUCCESS;
 }
 
 AkmSensor::Status AkmIrSensor::startSensor(const float sec){
+    interrupt->fall(this, &AkmIrSensor::detectINT);
     return AkmSensor::ERROR;
 }
 
 AkmSensor::Status AkmIrSensor::stopSensor(){
     event = false;
+    interrupt->fall(0);
+    if(ak9750->setOperationMode(AK9750::MODE_STANDBY, filter) != AK9750::SUCCESS) {
+        MSG("#Error setOperationMode. AK9750.\n");
+        return AkmSensor::ERROR;
+    }
+    
+    // read one data to clear INT pin
+    Message msg;
+    AkmIrSensor::readSensorData(&msg);
+    
     return AkmSensor::SUCCESS;
 }
 
 AkmSensor::Status AkmIrSensor::readSensorData(Message* msg){
     event = false;
+
+    // check interrupt status to release INT pin status
+    AK9750::InterruptStatus interrupt;
+    AK9750::Status status = ak9750->getInterruptStatus(&interrupt);
+    if( status != AK9750::SUCCESS){
+        MSG("#Error getInterruptStatus. AK9750.\n");
+        return AkmSensor::ERROR;
+    }
+    
+    AK9750::SensorData data;
+    status = ak9750->getSensorData(&data);
+    if( status != AK9750::SUCCESS){
+        MSG("#Error getSensorData. AK9750.\n");
+        return AkmSensor::ERROR;
+    }
+    msg->setCommand(Message::CMD_START_MEASUREMENT);
+    msg->setArgument( 0, interrupt.drdy);
+    msg->setArgument( 1, interrupt.ir13h);
+    msg->setArgument( 2, interrupt.ir13l);
+    msg->setArgument( 3, interrupt.ir24h);
+    msg->setArgument( 4, interrupt.ir24l);
+    msg->setArgument( 5,(char)((int32_t)(data.ir1) >> 8));
+    msg->setArgument( 6, (char)((int32_t)(data.ir1) & 0x00FF) );
+    msg->setArgument( 7,(char)((int32_t)(data.ir2) >> 8));
+    msg->setArgument( 8, (char)((int32_t)(data.ir2) & 0x00FF) );
+    msg->setArgument( 9,(char)((int32_t)(data.ir3) >> 8));
+    msg->setArgument( 10, (char)((int32_t)(data.ir3) & 0x00FF) );
+    msg->setArgument( 11,(char)((int32_t)(data.ir4) >> 8));
+    msg->setArgument( 12, (char)((int32_t)(data.ir4) & 0x00FF) );
+    msg->setArgument( 13,(char)((int32_t)(data.temperature) >> 8));
+    msg->setArgument( 14, (char)((int32_t)(data.temperature) & 0x00FF) );    
+
     return AkmSensor::SUCCESS;
 }
 
 AkmSensor::Status AkmIrSensor::requestCommand(Message* in, Message* out){
     AkmSensor::Status status = AkmSensor::SUCCESS;
+    Message::Command cmd = in->getCommand();
+    AK9750::Threshold th;
+    AK9750::Hysteresis hys;
+    AK9750::InterruptStatus interrupt;
+    
+    out->setCommand(cmd);
+    
+    switch(cmd){
+        case Message::CMD_IR_GET_THRESHOLD:
+        {
+            if (ak9750->getThreshold(&th) != AK9750::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Failed to set threshold to AK9750.\n");
+            }
+            out->setArgument(0,(char)((int32_t)(th.eth13h) >> 8));
+            out->setArgument(1,(char)((int32_t)(th.eth13h) & 0x00FF));
+            out->setArgument(2,(char)((int32_t)(th.eth13l) >> 8));
+            out->setArgument(3,(char)((int32_t)(th.eth13l) & 0x00FF));
+            out->setArgument(4,(char)((int32_t)(th.eth24h) >> 8));
+            out->setArgument(5,(char)((int32_t)(th.eth24h) & 0x00FF));
+            out->setArgument(6,(char)((int32_t)(th.eth24l) >> 8));
+            out->setArgument(7,(char)((int32_t)(th.eth24l) & 0x00FF));
+            break;
+        }
+        case Message::CMD_IR_SET_THRESHOLD:
+        {
+            th.eth13h = CONV16I(in->getArgument(0), in->getArgument(1));
+            th.eth13l = CONV16I(in->getArgument(2), in->getArgument(3));
+            th.eth24h = CONV16I(in->getArgument(4), in->getArgument(5));
+            th.eth24l = CONV16I(in->getArgument(6), in->getArgument(7));
+            if (ak9750->setThreshold(&th) != AK9750::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Failed to set threshold to AK9750.\n");
+            }
+            out->setArgument(0,(char)status);
+            break;
+        }
+        case Message::CMD_IR_GET_HYSTERESIS:
+        {
+            if (ak9750->getHysteresis(&hys) != AK9750::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Failed to set hysteresis to AK9750.\n");
+            }
+            out->setArgument(0,hys.ehys13);
+            out->setArgument(1,hys.ehys24);
+            break;
+        }
+        case Message::CMD_IR_SET_HYSTERESIS:
+        {
+            hys.ehys13 = in->getArgument(0);
+            hys.ehys24 = in->getArgument(1);
+            if (ak9750->setHysteresis(&hys) != AK9750::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Failed to set hysteresis to AK9750.\n");
+            }
+            out->setArgument(0,(char)status);
+            break;
+        }
+        case Message::CMD_IR_GET_INTERRUPT:
+        {
+            if (ak9750->getInterruptStatus(&interrupt) != AK9750::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Failed to set hysteresis to AK9750.\n");
+            }
+            out->setArgument(0, interrupt.drdy);
+            out->setArgument(1, interrupt.ir13h);
+            out->setArgument(2, interrupt.ir13l);
+            out->setArgument(3, interrupt.ir24h);
+            out->setArgument(4, interrupt.ir24l);
+            break;
+        }
+        case Message::CMD_IR_SET_INTERRUPT:
+        {
+            interrupt.drdy = in->getArgument(0);
+            interrupt.ir13h = in->getArgument(1);
+            interrupt.ir13l = in->getArgument(2);
+            interrupt.ir24h = in->getArgument(3);
+            interrupt.ir24l = in->getArgument(4);
+            if (ak9750->setInterruptStatus(&interrupt) != AK9750::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Failed to set hysteresis to AK9750.\n");
+            }
+            out->setArgument(0,(char)status);
+            break;            
+        }
+        case Message::CMD_IR_GET_OPERATION_MODE:
+        {
+            AK9750::OperationMode mode;
+            AK9750::DigitalFilter filter;
+            if(ak9750->getOperationMode(&mode, &filter) != AK9750::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Error setOperationMode. AK9750.\n");
+            }
+            out->setArgument(0,(char)mode);
+            out->setArgument(1,(char)filter);
+            break;
+        }
+        case Message::CMD_IR_SET_OPERATION_MODE:
+        {
+            char mode = in->getArgument(0);
+            char filter = in->getArgument(1);
+            if(ak9750->setOperationMode((AK9750::OperationMode)mode, (AK9750::DigitalFilter)filter) != AK9750::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Error setOperationMode. AK9750.\n");
+            }
+            out->setArgument(0,(char)status);
+            break;
+        }
+        case Message::CMD_IR_GET_THRESHOLD_EEPROM:
+        {
+            if (ak9750->getThresholdFromEEPROM(&th) != AK9750::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Failed to set threshold to AK9750(EEPROM).\n");
+            }
+            out->setArgument(0,(char)((int32_t)(th.eth13h) >> 8));
+            out->setArgument(1,(char)((int32_t)(th.eth13h) & 0x00FF));
+            out->setArgument(2,(char)((int32_t)(th.eth13l) >> 8));
+            out->setArgument(3,(char)((int32_t)(th.eth13l) & 0x00FF));
+            out->setArgument(4,(char)((int32_t)(th.eth24h) >> 8));
+            out->setArgument(5,(char)((int32_t)(th.eth24h) & 0x00FF));
+            out->setArgument(6,(char)((int32_t)(th.eth24l) >> 8));
+            out->setArgument(7,(char)((int32_t)(th.eth24l) & 0x00FF));
+            break;
+        }
+        case Message::CMD_IR_SET_THRESHOLD_EEPROM:
+        {
+            th.eth13h = CONV16I(in->getArgument(0), in->getArgument(1));
+            th.eth13l = CONV16I(in->getArgument(2), in->getArgument(3));
+            th.eth24h = CONV16I(in->getArgument(4), in->getArgument(5));
+            th.eth24l = CONV16I(in->getArgument(6), in->getArgument(7));
+            if (ak9750->setThresholdToEEPROM(&th) != AK9750::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Failed to set threshold to AK9750(EEPROM).\n");
+            }
+            out->setArgument(0,(char)status);
+            break;
+        }
+        case Message::CMD_IR_GET_HYSTERESIS_EEPROM:
+        {
+            if (ak9750->getHysteresisFromEEPROM(&hys) != AK9750::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Failed to set hysteresis to AK9750(EEPROM).\n");
+            }
+            out->setArgument(0,hys.ehys13);
+            out->setArgument(1,hys.ehys24);
+            break;
+        }
+        case Message::CMD_IR_SET_HYSTERESIS_EEPROM:
+        {
+            hys.ehys13 = in->getArgument(0);
+            hys.ehys24 = in->getArgument(1);
+            if (ak9750->setHysteresisToEEPROM(&hys) != AK9750::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Failed to set hysteresis to AK9750(EEPROM).\n");
+            }
+            out->setArgument(0,(char)status);
+            break;
+        }
+        case Message::CMD_IR_GET_INTERRUPT_EEPROM:
+        {
+            if (ak9750->getInterruptStatusFromEEPROM(&interrupt) != AK9750::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Failed to set interrupt to AK9750(EEPROM).\n");
+            }
+            out->setArgument(0, interrupt.drdy);
+            out->setArgument(1, interrupt.ir13h);
+            out->setArgument(2, interrupt.ir13l);
+            out->setArgument(3, interrupt.ir24h);
+            out->setArgument(4, interrupt.ir24l);
+            break;
+        }
+        case Message::CMD_IR_SET_INTERRUPT_EEPROM:
+        {
+            interrupt.drdy = in->getArgument(0);
+            interrupt.ir13h = in->getArgument(1);
+            interrupt.ir13l = in->getArgument(2);
+            interrupt.ir24h = in->getArgument(3);
+            interrupt.ir24l = in->getArgument(4);
+            if (ak9750->setInterruptStatusToEEPROM(&interrupt) != AK9750::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Failed to set interrupt to AK9750(EEPROM).\n");
+            }
+            out->setArgument(0,(char)status);
+            break;            
+        }
+        case Message::CMD_IR_GET_OPERATION_MODE_EEPROM:
+        {
+            AK9750::OperationMode mode;
+            AK9750::DigitalFilter filter;
+            if(ak9750->getOperationModeFromEEPROM(&mode, &filter) != AK9750::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Error getOperationMode. AK9750(EEPROM).\n");
+            }
+            out->setArgument(0,(char)mode);
+            out->setArgument(1,(char)filter);
+            break;
+        }
+        case Message::CMD_IR_SET_OPERATION_MODE_EEPROM:
+        {
+            char mode = in->getArgument(0);
+            char filter = in->getArgument(1);
+            if(ak9750->setOperationModeToEEPROM((AK9750::OperationMode)mode, (AK9750::DigitalFilter)filter) != AK9750::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Error setOperationMode. AK9750(EEPROM).\n");
+            }
+            out->setArgument(0,(char)status);
+            break;
+        }
+        default:
+        {
+            MSG("#Error no command.\n");
+            status =  AkmSensor::ERROR;
+            break;
+        }
+    }
     return status;
 }