File content as of revision 49:c8f8946129b6:

#include "ak7401ctrl.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;
}

AkmSensor::Status Ak7401Ctrl::startSensor(){
    AK7401::Status status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
    if( status != AK7401::SUCCESS ){
        MSG("#AK7401 set NORMAL mode failed.\r\n"); 
        return AkmSensor::ERROR;
    }
    ticker.attach(callback(this, &base::setEvent), interval);
    return AkmSensor::SUCCESS;
}

AkmSensor::Status Ak7401Ctrl::startSensor(const float sec){
    AK7401::Status status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
    if( status != AK7401::SUCCESS ){
        MSG("#AK7401 set NORMAL mode failed.\r\n"); 
        return AkmSensor::ERROR;
    }
    interval = sec;
    ticker.attach(callback(this, &base::setEvent), interval);
    MSG("#Start sensor %s.\r\n",sensorName);
    return AkmSensor::SUCCESS;
}

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

AkmSensor::Status Ak7401Ctrl::readSensorData(Message* msg){
    AkmSensor::clearEvent();

    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 != AK7401::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_READ:
         {
            // read angle
            char angle[2] = {0x00,0x00};
            char density = 0x00;
            char abnormal = 0x00;
            
            if( ak7401->readAngleMeasureCommand(angle, &density, &abnormal) != AK7401::SUCCESS ){
                MSG("#Error: Read angle\r\n");
                status =  AkmSensor::ERROR;
            }
            out->setCommand(Message::CMD_ANGLE_READ);
            out->setArgument( 0, ((abnormal&0x01)==0x00)|(status==AkmSensor::ERROR) ? 0x01 : 0x00 );
            out->setArgument( 1, angle[0] );
            out->setArgument( 2, angle[1] );
            out->setArgument( 3, density );
            break;
         }
         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;
        }
        case Message::CMD_REG_WRITE:
        case Message::CMD_REG_WRITEN:
        {
            char address = in->getArgument(0);
            const int len = in->getArgument(1);
            if(len != 2){
                MSG("#Error: length=%d. Only support 2byte length\r\n",len);
                status = AkmSensor::ERROR;
                return status;                
            }
            if(in->getArgNum() != len+2){
                MSG("#Error: Argument num. Args=%d\r\n",in->getArgNum());
                status = AkmSensor::ERROR;
                out->setArgument(0,(char)status);
                return status;
            }

            char data[len];
            for(int i=0; i<len; i++){
                data[i] = in->getArgument(i+2);    
            }
            if( ak7401->writeRegister(address, data) != AK7401::SUCCESS) {
                status =  AkmSensor::ERROR;
                MSG("#Error: Register write.\r\n");
            }
            if( ak7401->writeEEPROM(address, data) != AK7401::SUCCESS) {
                status =  AkmSensor::ERROR;
                MSG("#Error: EEPROM write.\r\n");
            }
            out->setArgument(0,(char)status);
            break;
        }
        case Message::CMD_REG_READ:
        case Message::CMD_REG_READN:
        {
            if(in->getArgNum() != 2){
                MSG("#Error: argument num. Args=%d\r\n",in->getArgNum());
                status = AkmSensor::ERROR;
                return status;
            }
            
            char address = in->getArgument(0);
            const int len = in->getArgument(1);
            if(len != 2){
                MSG("#Error: length=%d. Only support 2byte length\r\n",len);
                status = AkmSensor::ERROR;
                return status;                
            }
            char data[len];
            if( ak7401->readRegister(address, data) != AK7401::SUCCESS) {
                status =  AkmSensor::ERROR;
                MSG("#Error: register read.\r\n");
            }
            for(int i=0; i<len; i++){
                out->setArgument(i, data[i]);
            }
            break;
        }
        default:
        {
            MSG("#Error: no command.\r\n");
            status =  AkmSensor::ERROR;
            break;
        }
    }
    
    return status;
}