Diff: ak7451ctrl.cpp

Revision:

11:cef8dc1cf010

Child:

13:d008249f0359

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ak7451ctrl.cpp	Fri Jul 22 22:54:11 2016 +0000
@@ -0,0 +1,164 @@
+#include "ak7451ctrl.h"
+#include "debug.h"
+
+/**
+ * Constructor.
+ *
+ */
+Ak7451Ctrl::Ak7451Ctrl(){
+    ak7451 = NULL;
+    event = false;
+}
+
+/**
+ * Destructor.
+ *
+ */
+Ak7451Ctrl::~Ak7451Ctrl(){
+    if (ak7451) delete ak7451;
+}
+
+AkmSensor::Status Ak7451Ctrl::init(const uint8_t id, const uint8_t subid){
+    primaryId = id;
+    subId = subid;
+    
+    if(subId == SUB_ID_AK7451){
+        SPI* mSpi;
+        DigitalOut* mCs;
+        AK7451::Status status = AK7451::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);
+        ak7451 = new AK7451();
+        ak7451->begin(mSpi, mCs);
+
+        status = ak7451->setOperationMode(AK7451::AK7451_USER_MODE);
+        if( status != AK7451::SUCCESS ){
+            MSG("#AK7451 user mode failed.\r\n"); 
+            return AkmSensor::ERROR;
+        }
+        MSG("#AK7451 user mode succeed.\r\n"); 
+                
+        char data[2] = {0x02,0x00};                 // set clockwise rotation
+        status = ak7451->writeEEPROM(0x07,data);    // set clockwise
+        if( status != AK7451::SUCCESS ){
+            MSG("#AK7451 write EEPROM failed.\r\n"); 
+            return AkmSensor::ERROR;
+        }
+        MSG("#AK7451 write EEPROM succeed.\r\n"); 
+        
+        status = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE);
+        if( status != AK7451::SUCCESS ){
+            MSG("#AK7451 normal mode failed.\r\n"); 
+            return AkmSensor::ERROR;
+        }
+        MSG("#AK7451 normal mode succeed.\r\n"); 
+        
+        interval = SENSOR_SAMPLING_RATE; // 10Hz
+    }
+    else{
+        return AkmSensor::ERROR;
+    }
+
+    return AkmSensor::SUCCESS;
+}
+
+void Ak7451Ctrl::eventCallback(){
+    event = true;
+}
+
+bool Ak7451Ctrl::isEvent(){
+    return event;
+}
+
+AkmSensor::Status Ak7451Ctrl::startSensor(){
+    ticker.attach(this, &Ak7451Ctrl::eventCallback, interval);
+    return AkmSensor::SUCCESS;
+}
+
+AkmSensor::Status Ak7451Ctrl::startSensor(const float sec){
+    interval = sec;
+    ticker.attach(this, &Ak7451Ctrl::eventCallback, interval);
+    return AkmSensor::SUCCESS;
+}
+
+AkmSensor::Status Ak7451Ctrl::stopSensor(){
+    ticker.detach();
+    event = false;
+    return AkmSensor::SUCCESS;
+}
+
+AkmSensor::Status Ak7451Ctrl::readSensorData(Message* msg){
+    event = false;
+    char angle[2] = {0x00,0x00};
+    AK7451::Status status = ak7451->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 Ak7451Ctrl::requestCommand(Message* in, Message* out){
+    AkmSensor::Status status = AkmSensor::SUCCESS;
+    
+    Message::Command cmd = in->getCommand();
+
+    switch(cmd){
+         case Message::CMD_ANGLE_ZERO_RESET:
+         {
+             AK7451::Status st;
+            if( Ak7451Ctrl::stopSensor() != AkmSensor::SUCCESS ){
+                MSG("#Error stop sensor\r\n");
+                status = AkmSensor::ERROR;
+            }
+            
+            st = ak7451->setOperationMode(AK7451::AK7451_USER_MODE);
+            if( st != AK7451::SUCCESS ){
+                MSG("#Error when set user mode\r\n");
+                status = AkmSensor::ERROR;
+            }
+            char temp[2] = {0x00,0x00};
+            st = ak7451->writeRegister(0x06,temp);
+            if( st != AK7451::SUCCESS ){
+                MSG("#Error temp read: code=%d\r\n",st);
+                status =  AkmSensor::ERROR;
+            }
+            st = ak7451->setAngleZero();    // reset ZP data
+            if( st != AK7451::SUCCESS ){
+                MSG("#Error setAngleZero: code=%d\r\n",st);
+                status =  AkmSensor::ERROR;
+            }
+            st = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE);
+            if( st != AK7451::SUCCESS ){
+                MSG("#Error when set normal mode\r\n");
+                status =  AkmSensor::ERROR;
+            }
+            if( Ak7451Ctrl::startSensor(interval) != AkmSensor::SUCCESS ){
+                MSG("#Error start sensor\r\n");
+                status =  AkmSensor::ERROR;
+            }
+            if( status == AkmSensor::ERROR ){
+                out->setArgument(0,1);
+            }else{
+                out->setArgument(0,0);
+            }
+            break;
+        }
+        default:
+        {
+            MSG("#Error no command.\r\n");
+            status =  AkmSensor::ERROR;
+            break;
+        }
+    }
+    
+    return status;
+}
+