File content as of revision 27:41aa9fb23a2f:

#include "ak7401ctrl.h"
#include "debug.h"

/**
 * Constructor.
 *
 */
Ak7401Ctrl::Ak7401Ctrl() : AkmSensor(){
    ak7401 = NULL;
}

/**
 * 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);
        sensorName = "AK7401";

        status = ak7401->setOperationMode(AK7401::AK7401_USER_MODE);
        if( status != AK7401::SUCCESS ){
            MSG("#AK7401 set USER mode failed.\r\n"); 
            return AkmSensor::ERROR;
        }

        // check connection
        status = ak7401->checkConnection();
        if( status != AK7401::SUCCESS ){
            MSG("#AK7401 check connection failed.\r\n"); 
            return AkmSensor::ERROR;
        }
        
        // change rotation direction to CW from CCW
        char data[2] = {0x00,0x07};                 // set clockwise rotation
        status = ak7401->writeEEPROM(0x05,data);    // set clockwise
        if( status != AK7401::SUCCESS ){
            MSG("#AK7401 set rotation failed.\r\n"); 
            return AkmSensor::ERROR;
        }
        
        status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
        if( status != AK7401::SUCCESS ){
            MSG("#AK7401 set NORMAL mode failed.\r\n"); 
            return AkmSensor::ERROR;
        }

        MSG("#AK7401 init succeed.\r\n"); 
        
        interval = SENSOR_SAMPLING_RATE;  // 10Hz
    }
    else{
        return AkmSensor::ERROR;
    }

    return AkmSensor::SUCCESS;
}

void Ak7401Ctrl::eventCallback(){
    event = true;
}

AkmSensor::Status Ak7401Ctrl::startSensor(){
    ticker.attach(callback(this, &Ak7401Ctrl::eventCallback), interval);
    return AkmSensor::SUCCESS;
}

AkmSensor::Status Ak7401Ctrl::startSensor(const float sec){
    interval = sec;
    ticker.attach(callback(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:
         {
            if( ak7401->setAngleZero() != AK7401::SUCCESS ){
                MSG("#Error set angle zero\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;
}