File content as of revision 9:6fa3e7b17c27:

#include "akmakd.h"
#include "ak8963.h"
#include "ak099xx.h"
#include "debug.h"

/**
 * Constructor.
 *
 */
AkmAkd::AkmAkd(){
    event = false;
    compass = NULL;
    drdy = NULL;
    sensorName = "";
}

/**
 * Destructor.
 *
 */
AkmAkd::~AkmAkd(){
    drdy->rise(0);
    if (compass) delete compass;
    if (drdy) delete drdy;
}

AkmSensor::Status AkmAkd::init(const uint8_t id, const uint8_t subid){
    primaryId = id;
    subId = subid;
    
    AkmECompass::SensorType type;
    AK099XX* ak099xx;
    AK8963* ak8963;
    
    mode = AkmECompass::MODE_POWER_DOWN;
    nsf = AkmECompass::NSF_DISABLE;
    sdr = AkmECompass::SDR_LOW_POWER_DRIVE;
    
    if(primaryId == AKM_PRIMARY_ID_AKD_I2C){

        drdy = new InterruptIn(I2C_DRDY);    
        drdy->rise(0);

        I2C* i2c = new I2C(I2C_SDA,I2C_SCL);
        i2c->frequency(I2C_SPEED_100KHZ);

        switch(subid){
            case AkmAkd::SUB_ID_AK8963N:
            case AkmAkd::SUB_ID_AK8963C:
                type = AkmECompass::AK8963;
                ak8963 = new AK8963();
                compass = ak8963;
                AkmAkd::sensorName = "AK8963";
                break;          
            case AkmAkd::SUB_ID_AK09911:
                type = AkmECompass::AK09911;
                ak099xx = new AK099XX();
                compass = ak099xx;
                AkmAkd::sensorName = "AK09911";
                break;
            case AkmAkd::SUB_ID_AK09912:
                type = AkmECompass::AK09912;
                ak099xx = new AK099XX();
                compass = ak099xx;
                AkmAkd::sensorName = "AK09912";
                break;
            case AkmAkd::SUB_ID_AK09915:
                type = AkmECompass::AK09915;
                ak099xx = new AK099XX();
                compass = ak099xx;
                AkmAkd::sensorName = "AK09915";
                break;
            case AkmAkd::SUB_ID_AK09916C:
                type = AkmECompass::AK09916C;
                ak099xx = new AK099XX();
                compass = ak099xx;
                AkmAkd::sensorName = "AK09916C";
                break;
            default:
                return AkmSensor::ERROR;
//                break;
        }
        
        bool foundSensor = false;
        
        AkmECompass::SlaveAddress slaveAddr[] 
            = { AkmECompass::SLAVE_ADDR_1,
                AkmECompass::SLAVE_ADDR_2,
                AkmECompass::SLAVE_ADDR_3,
                AkmECompass::SLAVE_ADDR_4};

        for(int i=0; i<sizeof(slaveAddr); i++)
        {
            compass->init(i2c, slaveAddr[i], type);
            // Checks connectivity
            if(compass->checkConnection() == AkmECompass::SUCCESS) {
                // found
                foundSensor = true;
                break;
            }
        }
        if(foundSensor != true){
            return AkmSensor::ERROR;
        }
        
        // read a data to reset DRDY 
        AkmECompass::MagneticVector mag;
        compass->getMagneticVector(&mag);
        
    }else if(primaryId == AKM_PRIMARY_ID_AKD_SPI){
        
        drdy = new InterruptIn(SPI_DRDY);    
        drdy->rise(0);

        SPI* spi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK);
        spi->format(8,3);    // 8bit, Mode=3
        spi->frequency(1000000);
        
        DigitalOut* cs = new DigitalOut(SPI_CS);
        
        switch(subid){
            case AkmAkd::SUB_ID_AK8963N:
            case AkmAkd::SUB_ID_AK8963C:
                type = AkmECompass::AK8963;
                ak8963 = new AK8963();
                compass = ak8963;
                AkmAkd::sensorName = "AK8963";
                break;          
            case AkmAkd::SUB_ID_AK09911:
                type = AkmECompass::AK09911;
                AkmAkd::sensorName = "AK09911";
                return AkmSensor::ERROR;
            case AkmAkd::SUB_ID_AK09912:
                type = AkmECompass::AK09912;
                ak099xx = new AK099XX();
                compass = ak099xx;
                AkmAkd::sensorName = "AK09912";
                break;
            case AkmAkd::SUB_ID_AK09915:
                type = AkmECompass::AK09915;
                ak099xx = new AK099XX();
                compass = ak099xx;
                AkmAkd::sensorName = "AK09915";
                break;
            case AkmAkd::SUB_ID_AK09916C:
                type = AkmECompass::AK09916C;
                AkmAkd::sensorName = "AK09916C";
                // doesn't support SPI
                return AkmSensor::ERROR;
            default:
                return AkmSensor::ERROR;
//                break;
        }
        
        bool foundSensor = false;
        compass->init(spi, cs, type);
        if(compass->checkConnection() == AkmECompass::SUCCESS) {
            foundSensor = true;
        }
        if(foundSensor != true){
            MSG("#failed checkConnetion(SPI). %s\n",AkmAkd::sensorName);
            return AkmSensor::ERROR;
        }
        // read a data to reset DRDY 
        AkmECompass::MagneticVector mag;
        compass->getMagneticVector(&mag);
    }
    
    MSG("#%s detected.\n", AkmAkd::sensorName);
    return AkmSensor::SUCCESS;
}


void AkmAkd::checkDRDY(){
    AkmECompass::Status  status = compass->isDataReady();
    if( status == AkmECompass::DATA_READY || 
        status == AkmECompass::DATA_OVER_RUN ) event = true;
}

void AkmAkd::detectDRDY(){
    event = true;
}

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

AkmSensor::Status AkmAkd::startSensor(){
    // read once to clear DRDY pin
    AkmECompass::MagneticVector mag;
    AkmECompass::Status status = compass->getMagneticVector(&mag);

    // Puts the device into continuous measurement mode.
    if(compass->getSensorType() == AkmECompass::AK09915){
        if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
            return AkmSensor::ERROR;
        }
    }
    else if(compass->getSensorType() == AkmECompass::AK09912){
        if(compass->setOperationMode(mode,nsf) != AkmECompass::SUCCESS) {
            return AkmSensor::ERROR;
        }
    }
    else{
        if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
            return AkmSensor::ERROR;
        }
    }
    
    // DRDY interrupt enable for AK8963/AK09912/AK09915 with I2C interface
    if( primaryId == AKM_PRIMARY_ID_AKD_I2C && 
        (compass->getSensorType() == AkmECompass::AK8963 ||
         compass->getSensorType() == AkmECompass::AK09912 ||
         compass->getSensorType() == AkmECompass::AK09915) ){       
        drdy->rise(this, &AkmAkd::detectDRDY);
    }
    else{
        ticker.attach(this, &AkmAkd::checkDRDY, 0.005);  // 200Hz
    }
    return AkmSensor::SUCCESS;
}

AkmSensor::Status AkmAkd::startSensor(const float sec){
    return AkmSensor::ERROR;    // doesn't support
}

AkmSensor::Status AkmAkd::stopSensor(){
    ticker.detach();
 
    // Puts the device into power down mode.
    if(compass->setOperationMode(AkmECompass::MODE_POWER_DOWN) != AkmECompass::SUCCESS) {
        return AkmSensor::ERROR;
    }
    return AkmSensor::SUCCESS;
}

AkmSensor::Status AkmAkd::readSensorData(Message* msg){
    event = false;

    AkmECompass::MagneticVector mag;
    AkmECompass::Status status = compass->getMagneticVector(&mag);
    if( status != AkmECompass::SUCCESS){
        return AkmSensor::ERROR;
    }
    msg->setCommand(Message::CMD_START_MEASUREMENT);
    msg->setArgument( 0, (mag.isOverflow ? 1 : 0) );
    msg->setArgument( 1,(char)((int32_t)(mag.mx/0.15) >> 8));
    msg->setArgument( 2, (char)((int32_t)(mag.mx/0.15) & 0x00FF) );
    msg->setArgument( 3, (char)((int32_t)(mag.my/0.15) >> 8) );
    msg->setArgument( 4, (char)((int32_t)(mag.my/0.15) & 0x00FF) );
    msg->setArgument( 5, (char)((int32_t)(mag.mz/0.15) >> 8) );
    msg->setArgument( 6, (char)((int32_t)(mag.mz/0.15) & 0x00FF) );
    return AkmSensor::SUCCESS;
}

AkmSensor::Status AkmAkd::requestCommand(Message* in, Message* out){    
    AkmSensor::Status status = AkmSensor::SUCCESS;
    Message::Command cmd = in->getCommand();
    out->setCommand(cmd);
    
    switch(cmd){
        case Message::CMD_COMPASS_GET_OPERATION_MODE:
        {
            if(compass->getOperationMode(&mode) != AkmECompass::SUCCESS) {
                status =  AkmSensor::ERROR;
                MSG("#Error set operation mode.\n");
            }
            out->setArgument(0,mode);
            break;
        }
        case Message::CMD_COMPASS_SET_OPERATION_MODE:
        {
            mode = (AkmECompass::OperationMode)in->getArgument(0);
            if(compass->getSensorType() == AkmECompass::AK09915){
                nsf = (AkmECompass::Nsf)in->getArgument(1);
                sdr = (AkmECompass::Sdr)in->getArgument(2);
                if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
                    status =  AkmSensor::ERROR;
                    MSG("#Error set operation mode.\n");
                }
            }
            else if(compass->getSensorType() == AkmECompass::AK09912){
                nsf = (AkmECompass::Nsf)in->getArgument(1);
                if(compass->setOperationMode(mode,(AkmECompass::Nsf)nsf) != AkmECompass::SUCCESS) {
                    status =  AkmSensor::ERROR;
                    MSG("#Error set operation mode.\n");
                }
            }
            else{
                if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
                    status =  AkmSensor::ERROR;
                    MSG("#Error set operation mode.\n");
                }
            }
            out->setArgument(0,(char)status);                
            break;
        }        
        case Message::CMD_REG_WRITEN:
        {
            char address = in->getArgument(0);
            int len = (int)in->getArgument(1);
            char data[len];
            for(int i=0; i<len; i++){
                data[i] = in->getArgument(i);    
            }
            if( compass->write(address, data, len) != AkmECompass::SUCCESS) {
                status =  AkmSensor::ERROR;
                MSG("#Error register write.\n");
            }
            out->setArgument(0,(char)status);
            break;
        }        
        case Message::CMD_REG_READ:
        {
            char address = in->getArgument(0);
            int len = (int)in->getArgument(1);
            char data;
            if( compass->read(address, &data, len) != AkmECompass::SUCCESS) {
                status =  AkmSensor::ERROR;
                MSG("#Error register read.\n");
            }
            out->setArgument(0,data);
            break;
        }
        case Message::CMD_REG_READN:
        {
            char address = in->getArgument(0);
            int len = (int)in->getArgument(1);
            char data[len];
            if( compass->read(address, data, len) != AkmECompass::SUCCESS) {
                status =  AkmSensor::ERROR;
                MSG("#Error register read.\n");
            }
            for(int i=0; i<len; i++){
                out->setArgument(i, data[i]);
            }
            break;
        }
        default:
        {
            MSG("#Error no command.\n");
            status =  AkmSensor::ERROR;
            break;
        }
    }

    return AkmSensor::SUCCESS;
}

char* AkmAkd::getSensorName(){
    return sensorName;
}