File content as of revision 49:c8f8946129b6:

#include "ak9752ctrl.h"

#define CONV16I(high,low)  ((int16_t)(((high) << 8) | (low)))

/**
 * Constructor.
 *
 */
Ak9752Ctrl::Ak9752Ctrl() : AkmSensor(){
    ak9752 = NULL;
}

/**
 * Destructor.
 *
 */
Ak9752Ctrl::~Ak9752Ctrl(){
    if (ak9752) delete ak9752;
}

AkmSensor::Status Ak9752Ctrl::init(const uint8_t id, const uint8_t subid){
    primaryId = id;
    subId = subid;

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

    if(subId == SUB_ID_AK9752){
      ak9752 = new AK9752();
      sensorName = "AK9752";
    }
    else{
        return AkmSensor::ERROR;
    }

    bool foundSensor = false;

    AK9752::SlaveAddress slaveAddr[] 
    = { AK9752::SLAVE_ADDR_1 };

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

    // reset
    if (ak9752->reset() != AK9752::SUCCESS) {
        MSG("#Error: Failed to reset AK9752.\r\n");
    }
        
    MSG("#Init success AK9752.\r\n");
    return AkmSensor::SUCCESS;
}

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

AkmSensor::Status Ak9752Ctrl::startSensor(){
    // read one data to clear INT pin
    AK9752::SensorData data;
    ak9752->getSensorData(&data);
    
    // set operation mode
    if(ak9752->setOperationMode(mode,fc_tmp, fc_ir) != AK9752::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 Ak9752Ctrl::startSensor(const float sec){
    return AkmSensor::ERROR;
}

AkmSensor::Status Ak9752Ctrl::stopSensor(){
    AkmSensor::clearEvent();

    if(ak9752->setOperationMode(AK9752::MODE_STANDBY, fc_tmp, fc_ir) != AK9752::SUCCESS) {
        MSG("#Error: setOperationMode. AK9752.\r\n");
        return AkmSensor::ERROR;
    }
    
    // read one data to clear INT pin
    AK9752::SensorData data;
    ak9752->getSensorData(&data);
    
    return AkmSensor::SUCCESS;
}

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

    AK9752::SensorData data;
    AK9752::Status status = ak9752->getSensorData(&data);
    if( status != AK9752::SUCCESS){
        MSG("#Error: getSensorData. AK9752.\r\n");
        return AkmSensor::ERROR;
    }
    msg->setCommand(Message::CMD_START_MEASUREMENT);
    msg->setArgument( 0, data.intStatus.irh);
    msg->setArgument( 1, data.intStatus.irl);
    msg->setArgument( 2, data.intStatus.tmph);
    msg->setArgument( 3, data.intStatus.tmpl);
    msg->setArgument( 4, data.intStatus.dr);
    msg->setArgument( 5,(char)((int32_t)(data.ir) >> 8));
    msg->setArgument( 6, (char)((int32_t)(data.ir) & 0x00FF) );
    msg->setArgument( 7,(char)((int32_t)(data.temperature) >> 8));
    msg->setArgument( 8, (char)((int32_t)(data.temperature) & 0x00FF) );    
    msg->setArgument( 9, data.dor);

    return AkmSensor::SUCCESS;
}

AkmSensor::Status Ak9752Ctrl::requestCommand(Message* in, Message* out){
    AkmSensor::Status status = AkmSensor::SUCCESS;
    Message::Command cmd = in->getCommand();
    AK9752::Threshold th;
    AK9752::InterruptStatus interruptStatus;
    
    out->setCommand(cmd);
    
    switch(cmd){
        case Message::CMD_IR_GET_THRESHOLD:
        {
            if (ak9752->getThreshold(&th) != AK9752::SUCCESS) {
                MSG("#Error: Failed to set threshold to AK9752.\r\n");
                return  AkmSensor::ERROR;
            }
            out->setArgument(0,(char)((int32_t)(th.thirh) >> 8));
            out->setArgument(1,(char)((int32_t)(th.thirh) & 0x00FF));
            out->setArgument(2,(char)((int32_t)(th.thirl) >> 8));
            out->setArgument(3,(char)((int32_t)(th.thirl) & 0x00FF));
            out->setArgument(4,(char)((int32_t)(th.thtmph) >> 8));
            out->setArgument(5,(char)((int32_t)(th.thtmph) & 0x00FF));
            out->setArgument(6,(char)((int32_t)(th.thtmpl) >> 8));
            out->setArgument(7,(char)((int32_t)(th.thtmpl) & 0x00FF));
            break;
        }
        case Message::CMD_IR_SET_THRESHOLD:
        {
            th.thirh = CONV16I(in->getArgument(0), in->getArgument(1));
            th.thirl = CONV16I(in->getArgument(2), in->getArgument(3));
            th.thtmph = CONV16I(in->getArgument(4), in->getArgument(5));
            th.thtmpl = CONV16I(in->getArgument(6), in->getArgument(7));
            if (ak9752->setThreshold(&th) != AK9752::SUCCESS) {
                status =  AkmSensor::ERROR;
                MSG("#Error: Failed to set threshold to AK9752.\r\n");
            }
            out->setArgument(0,(char)status);
            break;
        }
        case Message::CMD_IR_GET_INTERRUPT:
        {
            if (ak9752->getInterruptEnable(&interruptStatus) != AK9752::SUCCESS) {
                MSG("#Error: Failed to set hysteresis to AK9752.\r\n");
                return  AkmSensor::ERROR;
            }
            out->setArgument(0, interruptStatus.irh);
            out->setArgument(1, interruptStatus.irl);
            out->setArgument(2, interruptStatus.tmph);
            out->setArgument(3, interruptStatus.tmpl);
            out->setArgument(4, interruptStatus.dr);
            break;
        }
        case Message::CMD_IR_SET_INTERRUPT:
        {
            interruptStatus.irh = in->getArgument(0);
            interruptStatus.irl = in->getArgument(1);
            interruptStatus.tmph = in->getArgument(2);
            interruptStatus.tmpl = in->getArgument(3);
            interruptStatus.dr = in->getArgument(4);
            if (ak9752->setInterruptEnable(&interruptStatus) != AK9752::SUCCESS) {
                status =  AkmSensor::ERROR;
                MSG("#Error: Failed to set hysteresis to AK9752.\r\n");
            }
            out->setArgument(0,(char)status);
            break;            
        }
        case Message::CMD_IR_GET_OPERATION_MODE:
        {
            if(ak9752->getOperationMode(&mode, &fc_tmp, &fc_ir) != AK9752::SUCCESS) {
                MSG("#Error: getOperationMode. AK9752.\r\n");
                return  AkmSensor::ERROR;
            }
            out->setArgument(0,(char)mode);
            out->setArgument(1,(char)fc_tmp);
            out->setArgument(2,(char)fc_ir);
            break;
        }
        case Message::CMD_IR_SET_OPERATION_MODE:
        {
            mode = (AK9752::OperationMode)in->getArgument(0);
            fc_tmp = (AK9752::FcTmp)in->getArgument(1);
            fc_ir = (AK9752::FcIr)in->getArgument(2);
            if(ak9752->setOperationMode(mode, fc_tmp, fc_ir) != AK9752::SUCCESS) {
                status =  AkmSensor::ERROR;
                MSG("#Error: setOperationMode. AK9752.\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( ak9752->write(address, data, len) != AK9752::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( ak9752->read(address, data, len) != AK9752::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;
}