File content as of revision 49:c8f8946129b6:

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

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

/**
 * Destructor.
 *
 */
AkmAkd::~AkmAkd(){
    if (compass) delete compass;
}

AkmSensor::Status AkmAkd::checkSensor( const uint8_t primaryid, const uint8_t subid, AkmECompass::DeviceId* devid){
    AK099XX* ak099xx;
    AK09940* ak09940;
    AK8963* ak8963;
    lenOptions = 0;

    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;
            lenOptions = 1;
            break;
        case AkmAkd::SUB_ID_AK09915C:
            *devid = AkmECompass::AK09915;
            AkmAkd::sensorName = "AK09915C";
            ak099xx = new AK099XX();
            compass = ak099xx;
            lenOptions = 2;
            break;
        case AkmAkd::SUB_ID_AK09915D:
            *devid = AkmECompass::AK09915;
            AkmAkd::sensorName = "AK09915D";
            ak099xx = new AK099XX();
            compass = ak099xx;
            lenOptions = 2;
            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;
            lenOptions = 2;
            break;
        case AkmAkd::SUB_ID_AK09940:
            *devid = AkmECompass::AK09940;
            AkmAkd::sensorName = "AK09940";
            ak09940 = new AK09940();
            compass = ak09940;
            lenOptions = 5;
            break;
        default:
            return AkmSensor::ERROR;
    }
    
    uint8_t options[lenOptions];
    mode.options = options;
    
    return AkmSensor::SUCCESS;    
}

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

    mode.mode = AkmECompass::MODE_POWER_DOWN;
    
    if(primaryId == AKM_PRIMARY_ID_AKD_I2C){

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

        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(unsigned int i=0; i<sizeof(slaveAddr); i++)
        {
            MSG("#look for e-compass.\r\n");
            compass->init(i2c, slaveAddr[i], devid);
            // Checks connectivity
            MSG("#checkConnection.\r\n");
            if(compass->checkConnection() == AkmECompass::SUCCESS) {
                MSG("#%s detected.\r\n", AkmAkd::sensorName);
                return AkmSensor::SUCCESS;
            }
        }
    }else if(primaryId == AKM_PRIMARY_ID_AKD_SPI){
        
        SPI* spi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK);
        spi->format(8,3);    // 8bit, Mode=3
        spi->frequency(SPI_SPEED);
        
        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;
        }
    }
    MSG("#ERROR: couldn't detected.\r\n");
    return AkmSensor::ERROR;
}

void AkmAkd::setEvent(){
    AkmECompass::Status status = compass->isDataReady();
    if( status == AkmECompass::DATA_READY ){
        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 ||
         subId == AkmAkd::SUB_ID_AK09940 ) ){
        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::MagneticVectorLsb mag;
    if( compass->getMagneticVectorLsb(&mag) != AkmECompass::SUCCESS)
    {
        MSG("#Error: Start sensor failed %s during read LSB.\r\n", sensorName);
        return AkmSensor::ERROR;
	}

    AkmAkd::InterruptMode int_mode = getInterrupt(primaryId,subId);
    if( int_mode == AkmAkd::INTERRUPT_DISABLED ){       
        // No DRDY, attach timer and start
        ticker.attach(callback(this, &AkmAkd::setEvent), 1.0/AKDP_POLLING_FREQUENCY);
    }
    
    // set operation mode
    if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
        MSG("#Error: 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();
    
    AkmECompass::Mode temp;
    temp.mode = AkmECompass::MODE_POWER_DOWN;
    uint8_t options[lenOptions];
    for(int i=0; i<lenOptions; i++){
        options[i] = mode.options[i];
    }
    temp.options = options;
    if(compass->setOperationMode(temp) != AkmECompass::SUCCESS) {
        return AkmSensor::ERROR;
    }
    
    // read one data to clear DRDY 
    AkmECompass::MagneticVectorLsb mag;
    compass->getMagneticVectorLsb(&mag);
    
    return AkmSensor::SUCCESS;
}

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

    AkmECompass::MagneticVectorLsb lsb;
    AkmECompass::Status status = compass->getMagneticVectorLsb(&lsb);
    if( status != AkmECompass::SUCCESS){
        return AkmSensor::ERROR;
    }
    msg->setCommand(Message::CMD_START_MEASUREMENT);
    msg->setArgument( 0, (lsb.isOverflow ? 1 : 0) );
    if(subId == AkmAkd::SUB_ID_AK09940)
    {
        msg->setArgument( 1,(char)( lsb.lsbX>>16  ) );
        msg->setArgument( 2,(char)( lsb.lsbX>>8  ) );
        msg->setArgument( 3,(char)( lsb.lsbX ) );        
        msg->setArgument( 4,(char)( lsb.lsbY>>16 ) );
        msg->setArgument( 5,(char)( lsb.lsbY>>8 ) );
        msg->setArgument( 6,(char)( lsb.lsbY ) );        
        msg->setArgument( 7,(char)( lsb.lsbZ>>16 ) );
        msg->setArgument( 8,(char)( lsb.lsbZ>>8 ) );
        msg->setArgument( 9,(char)( lsb.lsbZ ) );
        msg->setArgument(10,(char)( lsb.lsbTemp ) );
    }
    else
    {
        msg->setArgument( 1,(char)( lsb.lsbX>>8 ) );
        msg->setArgument( 2,(char)( lsb.lsbX ) );
        msg->setArgument( 3,(char)( lsb.lsbY>>8 ) );
        msg->setArgument( 4,(char)( lsb.lsbY ) );
        msg->setArgument( 5,(char)( lsb.lsbZ>>8  ) );
        msg->setArgument( 6,(char)( lsb.lsbZ ) );        
    }
    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.\r\n");
            }else{
                out->setArgument(0,mode.mode);
                for(int i=0; i<lenOptions; i++){
                    out->setArgument(i+1,mode.options[i]);                
                }
            }
            break;
        }
        case Message::CMD_COMPASS_SET_OPERATION_MODE:
        {
            mode.mode = (AkmECompass::OperationMode)in->getArgument(0);
            for(int i=0; i<lenOptions; i++){
                mode.options[i] = in->getArgument(i+1);
            }
            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);
            const 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);
            const 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;
}