Diff: ak7401ctrl.cpp

Revision:

10:5c69b067d88a

Child:

11:cef8dc1cf010

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ak7401ctrl.cpp	Fri Jul 08 22:26:26 2016 +0000
@@ -0,0 +1,163 @@
+#include "ak7401ctrl.h"
+#include "debug.h"
+
+/**
+ * Constructor.
+ *
+ */
+Ak7401Ctrl::Ak7401Ctrl(){
+    ak7401 = NULL;
+    event = false;
+}
+
+/**
+ * Destructor.
+ *
+ */
+Ak7401Ctrl::~Ak7401Ctrl(){
+    if (ak7401) delete ak7401;
+}
+
+AkmSensor::Status Ak7401Ctrl::init(const uint8_t id, const uint8_t subid){
+    primaryId = id;
+    subId = subid;
+    
+    if(subId == SUB_ID_AK7401){
+        SPI* mSpi;
+        DigitalOut* mCs;
+        AK7401::Status status = AK7401::ERROR;        
+        mSpi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK);
+        mSpi->format(8,1);    // 8bit, Mode=1
+        mSpi->frequency(1000000);
+        mCs = new DigitalOut(SPI_CS);
+        ak7401 = new AK7401();
+        ak7401->begin(mSpi, mCs);
+
+        status = ak7401->setOperationMode(AK7401::AK7401_USER_MODE);
+        if( status != AK7401::SUCCESS ){
+            MSG("#AK7401 user mode failed.\n"); 
+            return AkmSensor::ERROR;
+        }
+        MSG("#AK7401 user mode succeed.\n"); 
+                
+        char data[2] = {0x00,0x07};                 // set clockwise rotation
+        status = ak7401->writeEEPROM(0x05,data);    // set clockwise
+        if( status != AK7401::SUCCESS ){
+            MSG("#AK7401 write EEPROM failed.\n"); 
+            return AkmSensor::ERROR;
+        }
+        MSG("#AK7401 write EEPROM succeed.\n"); 
+        
+        status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
+        if( status != AK7401::SUCCESS ){
+            MSG("#AK7401 normal mode failed.\n"); 
+            return AkmSensor::ERROR;
+        }
+        MSG("#AK7401 normal mode succeed.\n"); 
+    }
+    else{
+        return AkmSensor::ERROR;
+    }
+
+    return AkmSensor::SUCCESS;
+}
+
+void Ak7401Ctrl::eventCallback(){
+    event = true;
+}
+
+bool Ak7401Ctrl::isEvent(){
+    return event;
+}
+
+AkmSensor::Status Ak7401Ctrl::startSensor(){
+    interval = 0.1;  // 10Hz
+    ticker.attach(this, &Ak7401Ctrl::eventCallback, interval);
+    return AkmSensor::SUCCESS;
+}
+
+AkmSensor::Status Ak7401Ctrl::startSensor(const float sec){
+    interval = sec;
+    ticker.attach(this, &Ak7401Ctrl::eventCallback, interval);
+    return AkmSensor::SUCCESS;
+}
+
+AkmSensor::Status Ak7401Ctrl::stopSensor(){
+    ticker.detach();
+    event = false;
+    return AkmSensor::SUCCESS;
+}
+
+AkmSensor::Status Ak7401Ctrl::readSensorData(Message* msg){
+    event = false;
+    char angle[2] = {0x00,0x00};
+    AK7401::Status status = ak7401->readAngle(angle);
+    
+    msg->setCommand(Message::CMD_START_MEASUREMENT);
+    msg->setArgument( 0, status );
+    msg->setArgument( 1, angle[0] );
+    msg->setArgument( 2, angle[1] );
+
+    if( status != SUCCESS){
+        return AkmSensor::ERROR;
+    }
+    return AkmSensor::SUCCESS;
+}
+
+AkmSensor::Status Ak7401Ctrl::requestCommand(Message* in, Message* out){
+    AkmSensor::Status status = AkmSensor::SUCCESS;
+    
+    Message::Command cmd = in->getCommand();
+
+    switch(cmd){
+         case Message::CMD_ANGLE_ZERO_RESET:
+         {
+             AK7401::Status st;
+            if( Ak7401Ctrl::stopSensor() != AkmSensor::SUCCESS ){
+                MSG("#Error stop sensor\n");
+                status = AkmSensor::ERROR;
+            }
+            
+            st = ak7401->setOperationMode(AK7401::AK7401_USER_MODE);
+            if( st != AK7401::SUCCESS ){
+                MSG("#Error when set user mode\n");
+                status = AkmSensor::ERROR;
+            }
+            char temp[2] = {0x00,0x00};
+            st = ak7401->writeRegister(0x06,temp);
+            if( st != AK7401::SUCCESS ){
+                MSG("#Error temp read: code=%d\n",st);
+                status =  AkmSensor::ERROR;
+            }
+            st = ak7401->setAngleZero();    // reset ZP data
+            if( st != AK7401::SUCCESS ){
+                MSG("#Error setAngleZero: code=%d\n",st);
+                status =  AkmSensor::ERROR;
+            }
+            st = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
+            if( st != AK7401::SUCCESS ){
+                MSG("#Error when set normal mode\n");
+                status =  AkmSensor::ERROR;
+            }
+            if( Ak7401Ctrl::startSensor(interval) != AkmSensor::SUCCESS ){
+                MSG("#Error start sensor\n");
+                status =  AkmSensor::ERROR;
+            }
+            if( status == AkmSensor::ERROR ){
+                out->setArgument(0,1);
+            }else{
+                out->setArgument(0,0);
+            }
+            break;
+        }
+        default:
+        {
+            MSG("#Error no command.\n");
+            status =  AkmSensor::ERROR;
+            break;
+        }
+    }
+    
+    return status;
+}
+