Diff: ak9752ctrl.cpp

Revision:

10:5c69b067d88a

Child:

11:cef8dc1cf010

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ak9752ctrl.cpp	Fri Jul 08 22:26:26 2016 +0000
@@ -0,0 +1,278 @@
+#include "ak9752ctrl.h"
+#include "debug.h"
+
+#define CONV16I(high,low)  ((int16_t)(((high) << 8) | (low)))
+
+/**
+ * Constructor.
+ *
+ */
+Ak9752Ctrl::Ak9752Ctrl(){
+    ak9752 = NULL;
+    event = false;
+    interrupt = NULL;
+}
+
+/**
+ * Destructor.
+ *
+ */
+Ak9752Ctrl::~Ak9752Ctrl(){
+    if (ak9752) delete ak9752;
+    if (interrupt) delete interrupt;
+}
+
+AkmSensor::Status Ak9752Ctrl::init(const uint8_t id, const uint8_t subid){
+    primaryId = id;
+    subId = subid;
+
+    interrupt = new InterruptIn(I2C_DRDY);    
+    I2C* i2c = new I2C(I2C_SDA,I2C_SCL);
+    i2c->frequency(I2C_SPEED_100KHZ);
+
+    if(subId == SUB_ID_AK9752){
+      ak9752 = new AK9752();
+    }
+    else{
+        return AkmSensor::ERROR;
+    }
+
+    bool foundSensor = false;
+
+    AK9752::SlaveAddress slaveAddr[] 
+    = { AK9752::SLAVE_ADDR_1 };
+
+    for(int i=0; i<sizeof(slaveAddr); i++)
+    {
+        ak9752->init(i2c, slaveAddr[i]);
+        // Checks connectivity
+        if(ak9752->checkConnection() == Ak9752Ctrl::SUCCESS) {
+            // found
+            foundSensor = true;
+            break;
+        }
+    }
+    if(foundSensor != true) return AkmSensor::ERROR;
+
+    // reset
+    if (ak9752->reset() != AK9752::SUCCESS) {
+        MSG("#Failed to reset AK9752.\n");
+    }
+        
+    MSG("#Init success AK9752.\n");
+    return AkmSensor::SUCCESS;
+}
+
+void Ak9752Ctrl::detectINT(){
+    event = true;
+}
+
+bool Ak9752Ctrl::isEvent(){
+    return event;
+}
+
+AkmSensor::Status Ak9752Ctrl::startSensor(){
+    if(ak9752->setOperationMode(mode,fc_tmp, fc_ir) != AK9752::SUCCESS) {
+        MSG("#Error setOperationMode. AK9752.\n");
+        return AkmSensor::ERROR;
+    }
+
+    interrupt->fall(this, &Ak9752Ctrl::detectINT);
+    MSG("#Start Sensor success AK9752.\n");
+    return AkmSensor::SUCCESS;
+}
+
+AkmSensor::Status Ak9752Ctrl::startSensor(const float sec){
+    interrupt->fall(this, &Ak9752Ctrl::detectINT);
+    return AkmSensor::ERROR;
+}
+
+AkmSensor::Status Ak9752Ctrl::stopSensor(){
+    event = false;
+    interrupt->fall(0);
+    if(ak9752->setOperationMode(AK9752::MODE_STANDBY, fc_tmp, fc_ir) != AK9752::SUCCESS) {
+        MSG("#Error setOperationMode. AK9752.\n");
+        return AkmSensor::ERROR;
+    }
+    
+    // read one data to clear INT pin
+    Message msg;
+    Ak9752Ctrl::readSensorData(&msg);
+    
+    return AkmSensor::SUCCESS;
+}
+
+AkmSensor::Status Ak9752Ctrl::readSensorData(Message* msg){
+    event = false;
+
+    AK9752::SensorData data;
+    AK9752::Status status = ak9752->getSensorData(&data);
+    if( status != AK9752::SUCCESS){
+        MSG("#Error getSensorData. AK9752.\n");
+        return AkmSensor::ERROR;
+    }
+    msg->setCommand(Message::CMD_START_MEASUREMENT);
+    msg->setArgument( 0, data.intStatus.irh);
+    msg->setArgument( 1, data.intStatus.irl);
+    msg->setArgument( 2, data.intStatus.tmph);
+    msg->setArgument( 3, data.intStatus.tmpl);
+    msg->setArgument( 4, data.intStatus.dr);
+    msg->setArgument( 5,(char)((int32_t)(data.ir) >> 8));
+    msg->setArgument( 6, (char)((int32_t)(data.ir) & 0x00FF) );
+    msg->setArgument( 7,(char)((int32_t)(data.temperature) >> 8));
+    msg->setArgument( 8, (char)((int32_t)(data.temperature) & 0x00FF) );    
+    msg->setArgument( 9, data.dor);
+
+    return AkmSensor::SUCCESS;
+}
+
+AkmSensor::Status Ak9752Ctrl::requestCommand(Message* in, Message* out){
+    AkmSensor::Status status = AkmSensor::SUCCESS;
+    Message::Command cmd = in->getCommand();
+    AK9752::Threshold th;
+    AK9752::InterruptStatus interrupt;
+    
+    out->setCommand(cmd);
+    
+    switch(cmd){
+        case Message::CMD_IR_GET_THRESHOLD:
+        {
+            if (ak9752->getThreshold(&th) != AK9752::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Failed to set threshold to AK9752.\n");
+            }
+            out->setArgument(0,(char)((int32_t)(th.thirh) >> 8));
+            out->setArgument(1,(char)((int32_t)(th.thirh) & 0x00FF));
+            out->setArgument(2,(char)((int32_t)(th.thirl) >> 8));
+            out->setArgument(3,(char)((int32_t)(th.thirl) & 0x00FF));
+            out->setArgument(4,(char)((int32_t)(th.thtmph) >> 8));
+            out->setArgument(5,(char)((int32_t)(th.thtmph) & 0x00FF));
+            out->setArgument(6,(char)((int32_t)(th.thtmpl) >> 8));
+            out->setArgument(7,(char)((int32_t)(th.thtmpl) & 0x00FF));
+            break;
+        }
+        case Message::CMD_IR_SET_THRESHOLD:
+        {
+            th.thirh = CONV16I(in->getArgument(0), in->getArgument(1));
+            th.thirl = CONV16I(in->getArgument(2), in->getArgument(3));
+            th.thtmph = CONV16I(in->getArgument(4), in->getArgument(5));
+            th.thtmpl = CONV16I(in->getArgument(6), in->getArgument(7));
+            if (ak9752->setThreshold(&th) != AK9752::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Failed to set threshold to AK9752.\n");
+            }
+            out->setArgument(0,(char)status);
+            break;
+        }
+        case Message::CMD_IR_GET_INTERRUPT:
+        {
+            if (ak9752->getInterruptEnable(&interrupt) != AK9752::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Failed to set hysteresis to AK9752.\n");
+            }
+            out->setArgument(0, interrupt.irh);
+            out->setArgument(1, interrupt.irl);
+            out->setArgument(2, interrupt.tmph);
+            out->setArgument(3, interrupt.tmpl);
+            out->setArgument(4, interrupt.dr);
+            break;
+        }
+        case Message::CMD_IR_SET_INTERRUPT:
+        {
+            interrupt.irh = in->getArgument(0);
+            interrupt.irl = in->getArgument(1);
+            interrupt.tmph = in->getArgument(2);
+            interrupt.tmpl = in->getArgument(3);
+            interrupt.dr = in->getArgument(4);
+            if (ak9752->setInterruptEnable(&interrupt) != AK9752::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Failed to set hysteresis to AK9752.\n");
+            }
+            out->setArgument(0,(char)status);
+            break;            
+        }
+        case Message::CMD_IR_GET_OPERATION_MODE:
+        {
+            if(ak9752->getOperationMode(&mode, &fc_tmp, &fc_ir) != AK9752::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Error setOperationMode. AK9752.\n");
+            }
+            out->setArgument(0,(char)mode);
+            out->setArgument(1,(char)fc_tmp);
+            out->setArgument(2,(char)fc_ir);
+            break;
+        }
+        case Message::CMD_IR_SET_OPERATION_MODE:
+        {
+            mode = (AK9752::OperationMode)in->getArgument(0);
+            fc_tmp = (AK9752::FcTmp)in->getArgument(1);
+            fc_ir = (AK9752::FcIr)in->getArgument(2);
+            if(ak9752->setOperationMode(mode, fc_tmp, fc_ir) != AK9752::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Error setOperationMode. AK9752.\n");
+            }
+            out->setArgument(0,(char)status);
+            break;
+        }
+        case Message::CMD_REG_WRITE:
+        {
+            char address = in->getArgument(0);
+            int len = (int)in->getArgument(1);
+            const char data = in->getArgument(2);
+            if( ak9752->write(address, &data, len) != AK9752::SUCCESS) {
+                status =  AkmSensor::ERROR;
+                MSG("#Error register write.\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( ak9752->write(address, data, len) != AK9752::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( ak9752->read(address, &data, len) != AK9752::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( ak9752->read(address, data, len) != AK9752::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 status;
+}