File content as of revision 13:d008249f0359:

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

/**
 * Constructor.
 *
 */
Ak7401Ctrl::Ak7401Ctrl(){
    ak7401 = NULL;
    event = false;
    sensorName = "";
}

/**
 * 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 user mode failed.\r\n"); 
            return AkmSensor::ERROR;
        }
        MSG("#AK7401 user mode succeed.\r\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.\r\n"); 
            return AkmSensor::ERROR;
        }
        MSG("#AK7401 write EEPROM succeed.\r\n"); 
        
        status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
        if( status != AK7401::SUCCESS ){
            MSG("#AK7401 normal mode failed.\r\n"); 
            return AkmSensor::ERROR;
        }
        MSG("#AK7401 normal mode succeed.\r\n");
        
        interval = SENSOR_SAMPLING_RATE;  // 10Hz
    }
    else{
        return AkmSensor::ERROR;
    }

    return AkmSensor::SUCCESS;
}

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

bool Ak7401Ctrl::isEvent(){
    return event;
}

AkmSensor::Status Ak7401Ctrl::startSensor(){
    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\r\n");
                status = AkmSensor::ERROR;
            }
            
            st = ak7401->setOperationMode(AK7401::AK7401_USER_MODE);
            if( st != AK7401::SUCCESS ){
                MSG("#Error when set user mode\r\n");
                status = AkmSensor::ERROR;
            }
            char temp[2] = {0x00,0x00};
            st = ak7401->writeRegister(0x06,temp);
            if( st != AK7401::SUCCESS ){
                MSG("#Error temp read: code=%d\r\n",st);
                status =  AkmSensor::ERROR;
            }
            st = ak7401->setAngleZero();    // reset ZP data
            if( st != AK7401::SUCCESS ){
                MSG("#Error setAngleZero: code=%d\r\n",st);
                status =  AkmSensor::ERROR;
            }
            st = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
            if( st != AK7401::SUCCESS ){
                MSG("#Error when set normal mode\r\n");
                status =  AkmSensor::ERROR;
            }
            if( Ak7401Ctrl::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;
}
char* Ak7401Ctrl::getSensorName(){
    return sensorName;
}