File content as of revision 38:e865dadfe54d:

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

/**
 * Constructor.
 *
 */
AkmAkd::AkmAkd() : AkmSensor(){
    compass = NULL;
//    drdy = NULL;
}

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

AkmSensor::Status AkmAkd::checkSensor( const uint8_t primaryid, const uint8_t subid, AkmECompass::DeviceId* devid){
    AK099XX* ak099xx;
    AK8963* ak8963;
    
    switch(subid){
        case AkmAkd::SUB_ID_AK8963N:
        case AkmAkd::SUB_ID_AK8963C:
            *devid = AkmECompass::AK8963;
            AkmAkd::sensorName = "AK8963";
            ak8963 = new AK8963();
            compass = ak8963;
            break;
        case AkmAkd::SUB_ID_AK09911C:
            *devid = AkmECompass::AK09911;
            AkmAkd::sensorName = "AK09911C";
            if(primaryId == AKM_PRIMARY_ID_AKD_SPI) 
                return AkmSensor::ERROR;    // No SPI support
            ak099xx = new AK099XX();
            compass = ak099xx;
            break;
        case AkmAkd::SUB_ID_AK09912C:
            *devid = AkmECompass::AK09912;
            AkmAkd::sensorName = "AK09912C";
            ak099xx = new AK099XX();
            compass = ak099xx;
            break;
        case AkmAkd::SUB_ID_AK09915C:
            *devid = AkmECompass::AK09915;
            AkmAkd::sensorName = "AK09915C";
            ak099xx = new AK099XX();
            compass = ak099xx;
            break;
        case AkmAkd::SUB_ID_AK09915D:
            *devid = AkmECompass::AK09915;
            AkmAkd::sensorName = "AK09915D";
            ak099xx = new AK099XX();
            compass = ak099xx;
            break;
        case AkmAkd::SUB_ID_AK09916C:
            *devid = AkmECompass::AK09916C;
            AkmAkd::sensorName = "AK09916C";
            if(primaryId == AKM_PRIMARY_ID_AKD_SPI) 
                return AkmSensor::ERROR;    // No SPI support
            ak099xx = new AK099XX();
            compass = ak099xx;
            break;
        case AkmAkd::SUB_ID_AK09916D:
            *devid = AkmECompass::AK09916D;
            AkmAkd::sensorName = "AK09916D";
            if(primaryId == AKM_PRIMARY_ID_AKD_SPI) 
                return AkmSensor::ERROR;    // No SPI support
            ak099xx = new AK099XX();
            compass = ak099xx;
            break;
        case AkmAkd::SUB_ID_AK09918:
            *devid = AkmECompass::AK09918;
            AkmAkd::sensorName = "AK09918";
            if(primaryId == AKM_PRIMARY_ID_AKD_SPI) 
                return AkmSensor::ERROR;    // No SPI support
            ak099xx = new AK099XX();
            compass = ak099xx;
            break;
        case AkmAkd::SUB_ID_AK09917:
            *devid = AkmECompass::AK09917;
            AkmAkd::sensorName = "AK09917";
            if(primaryId == AKM_PRIMARY_ID_AKD_SPI) 
                return AkmSensor::ERROR;    // No SPI support
            ak099xx = new AK099XX();
            compass = ak099xx;
            break;
        default:
            return AkmSensor::ERROR;
    }
    return AkmSensor::SUCCESS;    
}

AkmSensor::Status AkmAkd::init(const uint8_t primaryid, const uint8_t subid){
    primaryId = primaryid;
    subId = subid;
    
    AkmECompass::DeviceId devid;

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

        if( checkSensor(primaryId,subId,&devid) != AkmSensor::SUCCESS ){
            return AkmSensor::ERROR;
        }
        
        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], devid);
            // Checks connectivity
            if(compass->checkConnection() == AkmECompass::SUCCESS) {
                MSG("#%s detected.\r\n", AkmAkd::sensorName);
                return AkmSensor::SUCCESS;
            }
        }
    }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);

        if( checkSensor(primaryId,subId,&devid) != AkmSensor::SUCCESS ){
            return AkmSensor::ERROR;
        }
        
        compass->init(spi, cs, devid);
        if(compass->checkConnection() == AkmECompass::SUCCESS) {
            MSG("#%s detected.\r\n", AkmAkd::sensorName);
            return AkmSensor::SUCCESS;
        }
    }
    return AkmSensor::ERROR;
}

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

AkmAkd::InterruptMode AkmAkd::getInterrupt(uint8_t primaryId, uint8_t subId){
    AkmAkd::InterruptMode ret = AkmAkd::INTERRUPT_DISABLED;

    if( primaryId == AKM_PRIMARY_ID_AKD_I2C && 
        (subId == AkmAkd::SUB_ID_AK8963N ||
         subId == AkmAkd::SUB_ID_AK8963C ||
         subId == AkmAkd::SUB_ID_AK09912C ||
         subId == AkmAkd::SUB_ID_AK09915C ) ){
        ret = AkmAkd::INTERRUPT_ENABLED_PP;
    }
    else if( primaryId == AKM_PRIMARY_ID_AKD_I2C && 
        (subId == AkmAkd::SUB_ID_AK09915D ||
         subId == AkmAkd::SUB_ID_AK09916D ||
         subId == AkmAkd::SUB_ID_AK09917 ) ){       
        ret = AkmAkd::INTERRUPT_ENABLED_OD;
    }
    else{
        // No DRDY
        // No DRDY use for SPI interface
    }
    return ret;
}

AkmSensor::Status AkmAkd::startSensor(){
    // read one data to clear DRDY 
    AkmECompass::MagneticVector mag;
    compass->getMagneticVector(&mag);

    AkmAkd::InterruptMode int_mode = getInterrupt(primaryId,subId);
    // enable interrupt
    if( int_mode == AkmAkd::INTERRUPT_ENABLED_PP ){       
        // Push-Pull DRDY
//        drdy->rise(callback(this, &AkmAkd::detectDRDY));
    }
    else if( int_mode == AkmAkd::INTERRUPT_ENABLED_OD ){       
        // Open Drain DRDY
//        drdy->fall(callback(this, &AkmAkd::detectDRDY));
    }
    else{
        // No DRDY
        ticker.attach(callback(this, &AkmAkd::setEvent), 1.0/AKDP_POLLING_FREQUENCY);
    }
    
    // set operation mode
    if( subId == AkmAkd::SUB_ID_AK09915C || 
        subId == AkmAkd::SUB_ID_AK09915D || 
        subId == AkmAkd::SUB_ID_AK09917 ){
        if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
            MSG("#Start sensor failed %s\r\n", sensorName);
            return AkmSensor::ERROR;
        }
    }
    else if( subId == AkmAkd::SUB_ID_AK09912C ){
        if(compass->setOperationMode(mode,nsf) != AkmECompass::SUCCESS) {
            MSG("#Start sensor failed %s\r\n", sensorName);
            return AkmSensor::ERROR;
        }
    }
    else{
        if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
            MSG("#Start sensor failed %s\r\n", sensorName);
            return AkmSensor::ERROR;
        }
    }
        
    MSG("#Start sensor %s.\r\n",sensorName);
    return AkmSensor::SUCCESS;
}

AkmSensor::Status AkmAkd::startSensor(const float sec){
    return AkmSensor::ERROR;
}

AkmSensor::Status AkmAkd::stopSensor(){
    ticker.detach();
    AkmSensor::clearEvent();

    AkmAkd::InterruptMode int_mode = getInterrupt(primaryId,subId);
    if( int_mode == AkmAkd::INTERRUPT_ENABLED_PP ){       
        // Push-Pull DRDY
//         drdy->rise(NULL);
    }
    else if( int_mode == AkmAkd::INTERRUPT_ENABLED_OD ){       
        // Open Drain DRDY
//        drdy->fall(NULL);
    }
    else{
        // No DRDY
    }

    // Puts the device into power down mode.
    if( subId == AkmAkd::SUB_ID_AK09915C || 
        subId == AkmAkd::SUB_ID_AK09915D || 
        subId == AkmAkd::SUB_ID_AK09917 ){
        if(compass->setOperationMode(AkmECompass::MODE_POWER_DOWN,nsf,sdr) != AkmECompass::SUCCESS) {
            return AkmSensor::ERROR;
        }
    }else{
        if(compass->setOperationMode(AkmECompass::MODE_POWER_DOWN) != AkmECompass::SUCCESS) {
            return AkmSensor::ERROR;
        }        
    }
    
    // read one data to clear DRDY 
    AkmECompass::MagneticVector mag;
    compass->getMagneticVector(&mag);
    
    return AkmSensor::SUCCESS;
}

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

    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/AKDP_MAG_SENSITIVITY) >> 8));
    msg->setArgument( 2, (char)((int32_t)(mag.mx/AKDP_MAG_SENSITIVITY) & 0x00FF) );
    msg->setArgument( 3, (char)((int32_t)(mag.my/AKDP_MAG_SENSITIVITY) >> 8) );
    msg->setArgument( 4, (char)((int32_t)(mag.my/AKDP_MAG_SENSITIVITY) & 0x00FF) );
    msg->setArgument( 5, (char)((int32_t)(mag.mz/AKDP_MAG_SENSITIVITY) >> 8) );
    msg->setArgument( 6, (char)((int32_t)(mag.mz/AKDP_MAG_SENSITIVITY) & 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( subId == AkmAkd::SUB_ID_AK09915C || 
                subId == AkmAkd::SUB_ID_AK09915D || 
                subId == AkmAkd::SUB_ID_AK09917 ){
                if(compass->getOperationMode(&mode, &nsf, &sdr) != AkmECompass::SUCCESS) {
                    status = AkmSensor::ERROR;
                    MSG("#Error set operation mode.\r\n");
                }else{
                    out->setArgument(0,mode);
                    out->setArgument(1,nsf);
                    out->setArgument(2,sdr);
                }                
                
            }else if(subId == AkmAkd::SUB_ID_AK09912C){
                if(compass->getOperationMode(&mode, &nsf) != AkmECompass::SUCCESS) {
                    status = AkmSensor::ERROR;
                    MSG("#Error set operation mode.\r\n");
                }else{
                    out->setArgument(0,mode);
                    out->setArgument(1,nsf);
                }                
            }else{
                if(compass->getOperationMode(&mode) != AkmECompass::SUCCESS) {
                    status = AkmSensor::ERROR;
                    MSG("#Error set operation mode.\r\n");
                }else{
                    out->setArgument(0,mode);
                }                
            }
            break;
        }
        case Message::CMD_COMPASS_SET_OPERATION_MODE:
        {
            mode = (AkmECompass::OperationMode)in->getArgument(0);
            if( subId == AkmAkd::SUB_ID_AK09915C || 
                subId == AkmAkd::SUB_ID_AK09915D || 
                subId == AkmAkd::SUB_ID_AK09917 ){

                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.\r\n");
                }
            }
            else if(subId == AkmAkd::SUB_ID_AK09912C){
                nsf = (AkmECompass::Nsf)in->getArgument(1);
                if(compass->setOperationMode(mode,(AkmECompass::Nsf)nsf) != AkmECompass::SUCCESS) {
                    status =  AkmSensor::ERROR;
                    MSG("#Error set operation mode.\r\n");
                }
            }
            else{
                if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
                    status =  AkmSensor::ERROR;
                    MSG("#Error set operation mode.\r\n");
                }
            }
            out->setArgument(0,(char)status);                
            break;
        }
        case Message::CMD_REG_WRITE:
        case Message::CMD_REG_WRITEN:
        {
            char address = in->getArgument(0);
            int len = (int)in->getArgument(1);
            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( compass->write(address, data, len) != AkmECompass::SUCCESS) {
                status =  AkmSensor::ERROR;
                MSG("#Error register 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);
            int len = (int)in->getArgument(1);
            char data[len];
            if( compass->read(address, data, len) != AkmECompass::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;
}