File content as of revision 49:c8f8946129b6:

#include "ak09970ctrl.h"

#ifndef PI
#define PI           3.14159265358979323846
#endif

#define CONV16I(high,low)  ((int16_t)(((high) << 8) | (low)))
/**
 * Constructor.
 *
 */
Ak09970Ctrl::Ak09970Ctrl() : AkmSensor(){
    ak09970 = NULL;
}

/**
 * Destructor.
 *
 */
Ak09970Ctrl::~Ak09970Ctrl(){
    if (ak09970) delete ak09970;
}

AkmSensor::Status Ak09970Ctrl::init(const uint8_t id, const uint8_t subid){
    primaryId = id;
    subId = subid;
    
    // initialize readConfig
    readConfig.enabledReadX = false;
    readConfig.enabledReadY = false;
    readConfig.enabledReadZ = false;
    readConfig.enabledUpperOnly = false;
    
    if(primaryId == AKM_PRIMARY_ID_AKD_I2C){

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

        switch(subid){
            case Ak09970Ctrl::SUB_ID_AK09970:
                ak09970 = new AK09970();
                Ak09970Ctrl::sensorName = "AK09970";
                break;
            default:
                return AkmSensor::ERROR;
        }
        
        bool foundSensor = false;
        
        AK09970::SlaveAddress slaveAddr[] 
            = { AK09970::SLAVE_ADDR_1,
                AK09970::SLAVE_ADDR_2};

        for(unsigned int i=0; i<sizeof(slaveAddr); i++)
        {
            ak09970->init(i2c, slaveAddr[i]);
            // Checks connectivity
            if(ak09970->checkConnection() == AK09970::SUCCESS) {
                // found
                foundSensor = true;
                break;
            }
        }
        if(foundSensor != true){
            return AkmSensor::ERROR;
        }
        
        // software reset
        ak09970->reset();

    }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(1000000);
        
        DigitalOut* cs = new DigitalOut(SPI_CS);
        
        switch(subid){
            case Ak09970Ctrl::SUB_ID_AK09970:
                ak09970 = new AK09970();
                Ak09970Ctrl::sensorName = "AK09970";
                break;
            default:
                return AkmSensor::ERROR;
        }
        
        bool foundSensor = false;
        ak09970->init(spi, cs);
        if(ak09970->checkConnection() == AK09970::SUCCESS) {
            foundSensor = true;
        }
        if(foundSensor != true){
            MSG("#Error: Failed checkConnetion(SPI). %s\r\n",Ak09970Ctrl::sensorName);
            return AkmSensor::ERROR;
        }
        
        // software reset
        ak09970->reset();
    }
    else{
        return AkmSensor::ERROR;
    }
    
    MSG("#%s detected.\r\n", Ak09970Ctrl::sensorName);
    return AkmSensor::SUCCESS;
}

void Ak09970Ctrl::setEvent(){
//    MSG("#setEvent() in %s.\r\n",sensorName);
    AK09970::Status  status = ak09970->isDataReady();
    if( status == AK09970::DATA_READY ) base::setEvent();
}

AkmSensor::Status Ak09970Ctrl::startSensor(){
    // read one data to clear switch
    AK09970::SwitchStatus sw_status;
    ak09970->getSwitchStatus(&sw_status, readConfig);

    // set operation mode
    if(ak09970->setOperationMode(mode,sensorDriveMode,sensorMeasurementRange) != AK09970::SUCCESS) {
        MSG("#Error: Start sensor %s.\r\n", sensorName);
        return AkmSensor::ERROR;
    }
    
    return AkmSensor::SUCCESS;
}

AkmSensor::Status Ak09970Ctrl::startSensor(const float sec){
    MSG("#Error: Start sensor %s.\r\n", sensorName);
    return AkmSensor::ERROR;
}

AkmSensor::Status Ak09970Ctrl::stopSensor(){
    AkmSensor::clearEvent();
    
    if(ak09970->setOperationMode(AK09970::MODE_POWER_DOWN, sensorDriveMode, sensorMeasurementRange) != AK09970::SUCCESS) {
        MSG("#Error: Stop sensor %s.\r\n", sensorName);
        return AkmSensor::ERROR;
    }

    // read one data to clear switch
    AK09970::SwitchStatus sw_status;
    ak09970->getSwitchStatus(&sw_status, readConfig);
    
    return AkmSensor::SUCCESS;
}

AkmSensor::Status Ak09970Ctrl::readSensorData(Message* msg){
    AkmSensor::clearEvent();
        
    AK09970::SwitchStatus sw_status;
    if( ak09970->getSwitchStatus(&sw_status, readConfig) != AK09970::SUCCESS) {
        return AkmSensor::ERROR;
    }
    
    msg->setCommand(Message::CMD_START_MEASUREMENT);    
    msg->setArgument(0, sensorMeasurementRange);
    msg->setArgument(1, sw_status.st_hi);
    msg->setArgument(2, sw_status.st_lo);
    msg->setArgument(3, (char)(sw_status.mag.mx>>8));
    msg->setArgument(4, (char)(sw_status.mag.mx & 0x00FF));
    msg->setArgument(5, (char)(sw_status.mag.my>>8));
    msg->setArgument(6, (char)(sw_status.mag.my & 0x00FF));
    msg->setArgument(7, (char)(sw_status.mag.mz>>8));
    msg->setArgument(8, (char)(sw_status.mag.mz & 0x00FF));
    
    return AkmSensor::SUCCESS;
}

AkmSensor::Status Ak09970Ctrl::requestCommand(Message* in, Message* out){    
    AkmSensor::Status status = AkmSensor::SUCCESS;
    Message::Command cmd = in->getCommand();    
    out->setCommand(cmd);

    switch(cmd){
        case Message::CMD_PROGSW_GET_THRESHOLD:
        {
            if(in->getArgNum() != 1){
                MSG("#Error: Argument num. AK09970. Args=%d\r\n",in->getArgNum());
                return AkmSensor::ERROR;
            }    
            
            AK09970::SensorAxis axis = (AK09970::SensorAxis)in->getArgument(0);
            AK09970::Threshold th;
            if( ak09970->getThreshold(axis, &th) != AK09970::SUCCESS ){
                return AkmSensor::ERROR;
            }
            out->setArgument(0, axis);
            out->setArgument(1,(char)((int32_t)(th.BOP1) >> 8));
            out->setArgument(2,(char)((int32_t)(th.BOP1) & 0x00FF));
            out->setArgument(3,(char)((int32_t)(th.BRP1) >> 8));
            out->setArgument(4,(char)((int32_t)(th.BRP1) & 0x00FF));
            out->setArgument(5,(char)((int32_t)(th.BOP2) >> 8));
            out->setArgument(6,(char)((int32_t)(th.BOP2) & 0x00FF));
            out->setArgument(7,(char)((int32_t)(th.BRP2) >> 8));
            out->setArgument(8,(char)((int32_t)(th.BRP2) & 0x00FF));
            break;
        }
        case Message::CMD_PROGSW_SET_THRESHOLD:
        {
            if(in->getArgNum() != 9){
                MSG("#Error: Argument num. AK09970. Args=%d\r\n",in->getArgNum());
                status = AkmSensor::ERROR;
                out->setArgument(0,(char)status);
                return status;
            }    

            AK09970::SensorAxis axis = (AK09970::SensorAxis)in->getArgument(0);
            AK09970::Threshold th;
            
            th.BOP1 = CONV16I(in->getArgument(1),in->getArgument(2));
            th.BRP1 = CONV16I(in->getArgument(3),in->getArgument(4));
            th.BOP2 = CONV16I(in->getArgument(5),in->getArgument(6));
            th.BRP2 = CONV16I(in->getArgument(7),in->getArgument(8));
            if( ak09970->setThreshold(axis, th) != AK09970::SUCCESS ){
                status = AkmSensor::ERROR;
            }
            out->setArgument(0,(char)status);
            break;
        }
        case Message::CMD_PROGSW_GET_READ_COFIGURATION:
        {
            if(in->getArgNum() != 0){
                MSG("#Error: Argument num. AK09970. Args=%d\r\n",in->getArgNum());
                return AkmSensor::ERROR;
            }    

            out->setArgument(0, readConfig.enabledReadX);
            out->setArgument(1, readConfig.enabledReadY);
            out->setArgument(2, readConfig.enabledReadZ);
            out->setArgument(3, readConfig.enabledUpperOnly);
            break;
        }
        case Message::CMD_PROGSW_SET_READ_COFIGURATION:
        {
            if(in->getArgNum() != 4){
                MSG("#Error: Argument num. AK09970. Args=%d\r\n",in->getArgNum());
                status = AkmSensor::ERROR;
                out->setArgument(0,(char)status);
                return status;
            }    

            readConfig.enabledReadX = in->getArgument(0);
            readConfig.enabledReadY = in->getArgument(1);
            readConfig.enabledReadZ = in->getArgument(2);
            readConfig.enabledUpperOnly = in->getArgument(3);
            out->setArgument(0,(char)status);            
            break;
        }
        case Message::CMD_PROGSW_GET_SWITCH_COFIGURATION:
        {
            if(in->getArgNum() != 0){
                MSG("#Error: Argument num. AK09970. Args=%d\r\n",in->getArgNum());
                return AkmSensor::ERROR;
            }    

            if(ak09970->getSwitchConfig(&switchConfig) != AK09970::SUCCESS) {
                MSG("#Error: getSwitchConfig. AK09970.\r\n");
                return AkmSensor::ERROR;
            }
            out->setArgument(0,switchConfig.enabledODINTEN);
            out->setArgument(1,switchConfig.enabledINTEN);
            out->setArgument(2,switchConfig.enabledERRADCEN);
            out->setArgument(3,switchConfig.enabledERRXYEN);
            out->setArgument(4,switchConfig.enabledSWZ2EN);
            out->setArgument(5,switchConfig.enabledSWZ1EN);
            out->setArgument(6,switchConfig.enabledSWY2EN);
            out->setArgument(7,switchConfig.enabledSWY1EN);
            out->setArgument(8,switchConfig.enabledSWX2EN);
            out->setArgument(9,switchConfig.enabledSWX1EN);
            out->setArgument(10,switchConfig.enabledDRDYEN);
            break;
        }
        case Message::CMD_PROGSW_SET_SWITCH_COFIGURATION:
        {
            if(in->getArgNum() != 11){
                MSG("#Error: Argument num. AK09970. Args=%d\r\n",in->getArgNum());
                for(int i=0; i<in->getArgNum(); i++){
                    MSG("#%d = %02X\r\n",i,in->getArgument(i));
                }
                status = AkmSensor::ERROR;
                out->setArgument(0,(char)status);
                return status;
            }    

            switchConfig.enabledODINTEN = in->getArgument(0);
            switchConfig.enabledINTEN = in->getArgument(1);
            switchConfig.enabledERRADCEN = in->getArgument(2);
            switchConfig.enabledERRXYEN = in->getArgument(3);
            switchConfig.enabledSWZ2EN = in->getArgument(4);
            switchConfig.enabledSWZ1EN = in->getArgument(5);
            switchConfig.enabledSWY2EN = in->getArgument(6);
            switchConfig.enabledSWY1EN = in->getArgument(7);
            switchConfig.enabledSWX2EN = in->getArgument(8);
            switchConfig.enabledSWX1EN = in->getArgument(9);
            switchConfig.enabledDRDYEN = in->getArgument(10);
            
            if(ak09970->setSwitchConfig(switchConfig) != AK09970::SUCCESS) {
                status =  AkmSensor::ERROR;
                MSG("#Error: setSwitchConfig. AK09970.\r\n");
            }
            out->setArgument(0,(char)status);
            break;
        }
        case Message::CMD_PROGSW_GET_OPERATION_MODE:
        {
            if(in->getArgNum() != 0){
                MSG("#Error: Argument num. AK09970. Args=%d\r\n",in->getArgNum());
                return AkmSensor::ERROR;
            }

            if(ak09970->getOperationMode(&mode, &sensorDriveMode, &sensorMeasurementRange) != AK09970::SUCCESS) {
                MSG("#Error: getOperationMode. AK09970.\r\n");
                return AkmSensor::ERROR;
            }
            out->setArgument(0,(char)sensorMeasurementRange);
            out->setArgument(1,(char)sensorDriveMode);
            out->setArgument(2,(char)mode);
            break;
        }
        case Message::CMD_PROGSW_SET_OPERATION_MODE:
        {
            if(in->getArgNum() != 3){
                MSG("#Error: Argument num. AK09970. Args=%d\r\n",in->getArgNum());
                status = AkmSensor::ERROR;
                out->setArgument(0,(char)status);
                return status;
            }    

            sensorMeasurementRange = (AK09970::SensorMeasurementRange)(in->getArgument(0));
            sensorDriveMode = (AK09970::SensorDriveMode)(in->getArgument(1));
            mode = (AK09970::OperationMode)(in->getArgument(2));
            if(ak09970->setOperationMode(mode, sensorDriveMode, sensorMeasurementRange) != AK09970::SUCCESS) {
                status =  AkmSensor::ERROR;
                MSG("#Error: setOperationMode. AK09970. %d,%d,%d\r\n",mode,sensorDriveMode,sensorMeasurementRange);
            }
            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( ak09970->write(address, data, len) != AK09970::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());
                return AkmSensor::ERROR;
            }

            char address = in->getArgument(0);
            const int len = (int)in->getArgument(1);
            char data[len];
            if( ak09970->read(address, data, len) != AK09970::SUCCESS) {
                MSG("#Error: Register read.\r\n");
                return AkmSensor::ERROR;
            }
            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;
}