File content as of revision 27:41aa9fb23a2f:

#include "ak7451ctrl.h"
#include "debug.h"

/**
 * Constructor.
 *
 */
Ak7451Ctrl::Ak7451Ctrl() : AkmSensor(){
    ak7451 = NULL;
}

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

        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;
}

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

AkmSensor::Status Ak7451Ctrl::startSensor(const float sec){
    interval = sec;
    ticker.attach(callback(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;
            
            st = ak7451->setOperationMode(AK7451::AK7451_USER_MODE);
            if( st != AK7451::SUCCESS ){
                MSG("#Error when set user mode\r\n");
                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( 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;
}