File content as of revision 49:c8f8946129b6:

#include "akmsensormanager.h"

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

const char* AKM_PRIMARY_ID_STR[] = {
                                    "AKD Daughter Cards(SPI)",
                                    "Switch, Unipolar",
                                    "Switch, Onmipolar",
                                    "Latch, Bipolar",
                                    "Switch, Dual Output",
                                    "Onechip Encoder",
                                    "TBD1",
                                    "Current Sensor 3V",
                                    "Test",
                                    "Current Sensor 5V",
                                    "MISC(Analog)",
                                    "Linear Sensor",
                                    "Motor Drivers",
                                    "IR Sensor",
                                    "Angle Sensor(SPI)",
                                    "AKD Daughter Cards(I2C)"
                                     };
                                     
AkmSensorManager::AkmSensorManager(SerialNano* com)
{
    serial = com;

    isEnabledUsb = true;
    isEnabledBle = false;
    eventCommandReceived = false;
    eventConnected = false;
    eventDisconnected = false;
    interrupt = NULL;
    sensorIndex = 0;
    sensorNum = 0;
    drdyType = AkmAkd::INTERRUPT_DISABLED;
}

AkmSensorManager::Status AkmSensorManager::init(uint8_t id, uint8_t subid)
{
    primaryId = id;
    subId = subid;
    sensorIndex = 0;
    sensorNum = 0;
    if(AkmSensorManager::checkAkmSensor()!= true) return AkmSensorManager::ERROR;  
    return AkmSensorManager::SUCCESS;
}


void AkmSensorManager::setBleUartService(UARTService* service)
{
    uartService = service;
    isEnabledBle = true;
}

void AkmSensorManager::setEventConnected()
{
    eventConnected = true;
}

void AkmSensorManager::setEventDisconnected()
{
    eventDisconnected = true;
}

bool AkmSensorManager::checkAkmSensor()
{   
    sensorNum = 0;

//    int i=0;
//    while(sensor[i] != NULL){
//        delete(sensor[i]);
//        i++;
//    }
    detachInterrupt();
    drdyType = AkmAkd::INTERRUPT_DISABLED;
    
    switch(primaryId){

        case AkmSensor::AKM_PRIMARY_ID_AKD_SPI:
        {
            MSG("#SPI Interface.\r\n");    
        }
        case AkmSensor::AKM_PRIMARY_ID_AKD_I2C:
        {
            if(subId == Ak09970Ctrl::SUB_ID_AK09970){
                drdyType = AkmAkd::INTERRUPT_ENABLED_PP;    // Only support push-pull 
                Ak09970Ctrl* ak09970 = new Ak09970Ctrl();
                sensor[0] = ak09970;
                sensorNum = 1;
                MSG("#AK09970.\r\n");    
            }
            else{
                AkmAkd* akd = new AkmAkd();
                drdyType = akd->getInterrupt(primaryId, subId);
                sensor[0] = akd;
                sensorNum = 1;
                MSG("#e-Compass.\r\n");    
            }
            break;
        }
        case AkmSensor::AKM_PRIMARY_ID_ANGLE_SENSOR:
        {
            if(subId == Ak7451Ctrl::SUB_ID_AK7451){
                Ak7451Ctrl* ak7451ctrl = new Ak7451Ctrl();
                sensor[0] = ak7451ctrl;
                sensorNum = 1;
            }
            else if(subId == Ak7401Ctrl::SUB_ID_AK7401){
                Ak7401Ctrl* ak7401ctrl = new Ak7401Ctrl();
                sensor[0] = ak7401ctrl;
                sensorNum = 1;
            }
            break;
        }

        case AkmSensor::AKM_PRIMARY_ID_UNIPOLAR:
        case AkmSensor::AKM_PRIMARY_ID_OMNIPOLAR:
        case AkmSensor::AKM_PRIMARY_ID_LATCH:
        case AkmSensor::AKM_PRIMARY_ID_DUAL_OUTPUT:
        case AkmSensor::AKM_PRIMARY_ID_ONECHIP_ENCODER:
        {
            AkmHallSwitch* hallswitch = new AkmHallSwitch();
            sensor[0] = hallswitch;
            sensorNum = 1;
            break;
        }
/*
        case AkmSensor::AKM_PRIMARY_ID_DEMO:
        {
            if(subId == 0x08){
                drdyType = AkmAkd::INTERRUPT_ENABLED_OD;    // Only support open drain type DRDY 
    
                Ak09970Ctrl* ak09970 = new Ak09970Ctrl();
                sensor[0] = ak09970;
                if(sensor[0]->init(AkmSensor::AKM_PRIMARY_ID_AKD_I2C, Ak09970Ctrl::SUB_ID_AK09970) != AkmSensor::SUCCESS){
                    MSG("#Error: sensor[0]->init failed.\r\n");    
                    return false;    // couldn't find
                }
    
                Ak9752Ctrl* ak9752ctrl = new Ak9752Ctrl();
                sensor[1] = ak9752ctrl;
                if(sensor[1]->init(AkmSensor::AKM_PRIMARY_ID_IR_SENSOR, Ak9752Ctrl::SUB_ID_AK9752) != AkmSensor::SUCCESS){
                    MSG("#Error: sensor[1]->init failed.\r\n");    
                    return false;    // couldn't find
                }
                sensorNum = 2;
            }
            break;
        }
*/
        case AkmSensor::AKM_PRIMARY_ID_LINEAR_SENSOR:
        case AkmSensor::AKM_PRIMARY_ID_CURRENT_SENSOR_3V:
        case AkmSensor::AKM_PRIMARY_ID_CURRENT_SENSOR_5V:
        case AkmSensor::AKM_PRIMARY_ID_MISC_ANALOG:
        {
            AkmAnalogSensor* analogsensor = new AkmAnalogSensor();
            sensor[0] = analogsensor;
            sensorNum = 1;
            break;
        }

        case AkmSensor::AKM_PRIMARY_ID_IR_SENSOR:
        {
            drdyType = AkmAkd::INTERRUPT_ENABLED_OD;
            
            if(subId == Ak9750Ctrl::SUB_ID_AK9750){
                Ak9750Ctrl* ak9750ctrl = new Ak9750Ctrl();
                sensor[0] = ak9750ctrl;
                sensorNum = 1;
            }else if(subId == Ak9750Ctrl::SUB_ID_AK9753){
                Ak9750Ctrl* ak9753ctrl = new Ak9750Ctrl();
                sensor[0] = ak9753ctrl;
                sensorNum = 1;
            }else if(subId == Ak9752Ctrl::SUB_ID_AK9752){
                Ak9752Ctrl* ak9752ctrl = new Ak9752Ctrl();
                sensor[0] = ak9752ctrl;
                sensorNum = 1;
            }else{
                return false;    // couldn't find
            }
            break;
        }
        case AkmSensor::AKM_PRIMARY_ID_MOTOR_DRIVER:
        {       // TODO: Other motor driver cases
            if(subId == Ap1017Ctrl::SUB_ID_AP1017){
                Ap1017Ctrl* ap1017ctrl = new Ap1017Ctrl();
                sensor[0] = ap1017ctrl;
                sensorNum = 1;
            }else{
                return NULL;
            }
            break;
        }
        default:
        {
            MSG("#Error: Can't find ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]);
            return false;    // couldn't find
        }
    }
    
    if(primaryId != AkmSensor::AKM_PRIMARY_ID_DEMO){       
//        for(int i=0; i<sensorNum; i++){
            if(sensor[0]->init(primaryId, subId) != AkmSensor::SUCCESS){
                MSG("#Error: sensor[i]->init failed. ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]);    
                return false;    // couldn't find
            }
//        }
        MSG("#ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]);    
    }
    
    attachInterrupt();
    
    return true;
}

void AkmSensorManager::detectDRDY(){
    VERBOSE("#detect DRDY.\r\n");
//    for(int i=0; i<sensorNum; i++){
        sensor[0]->setEvent();
//    }
}

void AkmSensorManager::commandReceivedCallback(){
	VERBOSE("#commandReceivedCallback: Command Received\r\n");	
}


// For commands received via BLE
AkmSensorManager::Status AkmSensorManager::commandReceived(char* buf){
    // Construct message
    Status status = SUCCESS;
    
    if ((Message::parse(&msg, buf)) != Message::SUCCESS) {
        MSG("#Error: Failed to parse message. %s\r\n", buf);
        status = ERROR;
        eventCommandReceived = false;           // Reset flag
    }else{       
        eventCommandReceived = true;            // Set flag
            VERBOSE("#Parsed message. %s\r\n", buf);
    }
    t.attach(callback(this, &AkmSensorManager::commandReceivedCallback),0); // wake-up from ble.waitForEvent
    return status;
}

/*
int16_t AkmSensorManager::getAdcData(MCP342X *mcp3428, MCP342X::AdcChannel ch, MCP342X::SampleSetting s) {
    const int WAIT_ADC_MS = 1;

    // Configure channel and trigger.
    mcp3428->setChannel(ch);
    mcp3428->setSampleSetting(s);
    mcp3428->trigger();

    // polling data (!blocking)
    MCP342X::Data data;
    do {
        wait_ms(WAIT_ADC_MS);
        mcp3428->getData(&data);
    } while(data.st == MCP342X::DATA_NOT_UPDATED);
    
    return data.value;
}
*/

uint8_t AkmSensorManager::getId(PinName pin, uint8_t bits)
{
    VERBOSE("#AkmSensorManager: GetID\r\n");
    
    AnalogIn ain(pin);
	double value = 0;
	uint8_t ID = 0;
	
	value = ain.read();
	
	MSG("#ADC ID Raw Value: %.5f\r\n", value);

	// Scale reading
	// Range of reading is 0.0 to 1.0
	value = 2.0 * (3.0 * value);
//	value = (1/0.5555556) * (3.0 * value);		// multiply by 1/(voltage divider) and 3.0V

	MSG("#ADC ID Reading: %.4f\r\n", value);
    
    if(bits == 4){
	    // Convert reading into a 4-bit ID (i.e. 0-15)
   		ID = (uint8_t)((value/3.0)*16 + 0.5);
    	MSG("#4-bit ID = %d.\r\n", ID);
	}
	else if(bits == 5){
		// Convert reading into a 5-bit ID (i.e. 0-15)
   		ID = (uint8_t)((value/3.0)*32 + 0.5);
    	MSG("#5-bit ID = %d.\r\n", ID);
	}
	
    return ID;
}


bool AkmSensorManager::isEvent()
{
    // check sensor related event
//    for(int i=0; i<sensorNum; i++){
        if(sensor[0]->isEvent()){
            return true;
        }
//    }

    // other events
    return (
            eventCommandReceived ||
            eventConnected ||
            eventDisconnected);
}

void AkmSensorManager::processCommand()
{
    // Extracts command contained in the message
    Message::Command cmd = msg.getCommand();
    
    // Creates a message object to return
    Message resMsg;
    
    // Return message contains the extracted command
    resMsg.setCommand(cmd);
    
    switch(cmd)
    {
        case Message::CMD_GET_FW_VERSION:
        {
            resMsg.setArgument(0, FIRMWARE_VERSION);
            throwMessage(&resMsg);
            MSG("#FW version is reported.\r\n");
            break;
        }    
        case Message::CMD_GET_MAG_PART:
        {
            resMsg.setArgument(0, MAGNETOMETER_ID);
            throwMessage(&resMsg);
            MSG("#Mag ID is reported.\r\n");
            break;
        }
        case Message::CMD_SET_SERIAL_TARGET:
        {
            isEnabledBle = msg.getArgument(0)==Message::SW_ON ? true : false;
            isEnabledUsb = msg.getArgument(1)==Message::SW_ON ? true : false;
            break;
        }
        case Message::CMD_GET_ID:                       // return Primary ID and Sub ID
        {
            resMsg.setArgument(0, primaryId);
            resMsg.setArgument(1, subId);
            throwMessage(&resMsg);
            MSG("#ID is reported.\r\n");
            break;
        }
        case Message::CMD_GET_SENSOR_INDEX:
        {
            resMsg.setArgument(0, sensorIndex);
            throwMessage(&resMsg);
            MSG("#Get Sensor Index=%d.\r\n", sensorIndex);
            break;
        }
        case Message::CMD_SET_SENSOR_INDEX:
        {
            uint8_t index = (uint8_t)msg.getArgument(0);
            if(msg.getArgNum() == 1 && index<sensorNum){
                resMsg.setArgument(0, 0);
                sensorIndex = index;        
                MSG("#Set Sensor Index=%d.\r\n", sensorIndex);
            }else{
                resMsg.setArgument(0, 1);
                MSG("#Error: Set Sensor Index=%d.\r\n", sensorIndex);
            }            
            throwMessage(&resMsg);
            break;
        }
        case Message::CMD_GET_TOTAL_SENSOR_NUM:
        {
            resMsg.setArgument(0, sensorNum);
            throwMessage(&resMsg);
            MSG("#Get Sensor Total Num=%d.\r\n", sensorNum);
            break;
        }
        case Message::CMD_STOP_MEASUREMENT:
        {
            if( sensor[sensorIndex]->stopSensor() != AkmSensor::SUCCESS){
                resMsg.setArgument(0, 1);
            }else{
                resMsg.setArgument(0, 0);
            }
            throwMessage(&resMsg);

            detachInterrupt();
            
            MSG("#Stop measurement:%s.\r\n",sensor[sensorIndex]->getSensorName());
            break;
        }
        case Message::CMD_START_MEASUREMENT:
        {
            int error_code = AkmSensor::SUCCESS;
            if(msg.getArgNum() == 0)
            {
                error_code = sensor[sensorIndex]->startSensor();
                if( error_code != AkmSensor::SUCCESS ){
                    MSG("#Error: StartSensor Error. Code=%d\r\n",error_code);            
                }
                else
                {
	                switch(drdyType){
	                    case AkmAkd::INTERRUPT_ENABLED_PP:
	                    {
	                        interrupt->rise(callback(this, &AkmSensorManager::detectDRDY));                            
	                        break;
	                    }
	                    case AkmAkd::INTERRUPT_ENABLED_OD:
	                    {
	                        interrupt->fall(callback(this, &AkmSensorManager::detectDRDY));                            
	                        break;
	                    }
	                    default:
	                    {
	                        // nothing.
	                    }
	                }
				}
            }
            else if(msg.getArgNum() == 1)
            {
                float interval = (float)(1.0 / (double)msg.getArgument(0));
                error_code = sensor[sensorIndex]->startSensor(interval);
                if( error_code != AkmSensor::SUCCESS ){
                    MSG("#Error: StartSensor Error. Code=%d\r\n",error_code);            
                }
                else{
                    detachInterrupt();
                }
            }else{
                MSG("#Error: StartSensor Error: Wrong number of arguments.\r\n");            
            }
            
            if(error_code == AkmSensor::SUCCESS){
                // get initial sensor state for switch type sensors
                if( primaryId == AkmSensor::AKM_PRIMARY_ID_UNIPOLAR ||
                    primaryId == AkmSensor::AKM_PRIMARY_ID_OMNIPOLAR ||
                    primaryId == AkmSensor::AKM_PRIMARY_ID_LATCH ||
                    primaryId == AkmSensor::AKM_PRIMARY_ID_DUAL_OUTPUT ||
                    primaryId == AkmSensor::AKM_PRIMARY_ID_ONECHIP_ENCODER ){                
                    Message temp;
                    sensor[sensorIndex]->readSensorData(&temp);
                    throwMessage(&temp);
                }
	            MSG("#Start measurement:%s index=%d.\r\n",sensor[sensorIndex]->getSensorName(), sensorIndex);
            }
            break;
        }
        case Message::CMD_MOTOR_START_MOTOR:
        case Message::CMD_MOTOR_STOP_MOTOR:
        case Message::CMD_MOTOR_SET_DIRECTION:
        case Message::CMD_MOTOR_SET_DUTY_CYCLE:
        case Message::CMD_PROGSW_GET_THRESHOLD:
        case Message::CMD_PROGSW_SET_THRESHOLD:
        case Message::CMD_PROGSW_GET_READ_COFIGURATION:
        case Message::CMD_PROGSW_SET_READ_COFIGURATION:
        case Message::CMD_PROGSW_GET_SWITCH_COFIGURATION:
        case Message::CMD_PROGSW_SET_SWITCH_COFIGURATION:
        case Message::CMD_PROGSW_GET_OPERATION_MODE:
        case Message::CMD_PROGSW_SET_OPERATION_MODE:
        case Message::CMD_IR_GET_THRESHOLD:
        case Message::CMD_IR_SET_THRESHOLD:
        case Message::CMD_IR_GET_HYSTERESIS:
        case Message::CMD_IR_SET_HYSTERESIS:
        case Message::CMD_IR_GET_INTERRUPT:
        case Message::CMD_IR_SET_INTERRUPT:
        case Message::CMD_IR_GET_OPERATION_MODE:
        case Message::CMD_IR_SET_OPERATION_MODE:
        case Message::CMD_IR_GET_THRESHOLD_EEPROM:
        case Message::CMD_IR_SET_THRESHOLD_EEPROM:
        case Message::CMD_IR_GET_HYSTERESIS_EEPROM:
        case Message::CMD_IR_SET_HYSTERESIS_EEPROM:
        case Message::CMD_IR_GET_INTERRUPT_EEPROM:
        case Message::CMD_IR_SET_INTERRUPT_EEPROM:
        case Message::CMD_IR_GET_OPERATION_MODE_EEPROM:
        case Message::CMD_IR_SET_OPERATION_MODE_EEPROM:
        case Message::CMD_ANGLE_ZERO_RESET:
        case Message::CMD_ANGLE_READ:
        case Message::CMD_REG_WRITE:
        case Message::CMD_REG_WRITEN:
        case Message::CMD_REG_READ:
        case Message::CMD_REG_READN:
        case Message::CMD_COMPASS_GET_OPERATION_MODE:
        case Message::CMD_COMPASS_SET_OPERATION_MODE:
        {
            AkmSensor::Status st = sensor[sensorIndex]->requestCommand(&msg,&resMsg);
            if( (resMsg.getArgNum() == 0) && (st != AkmSensor::SUCCESS) )
            {
                MSG("#Error: Command failed.\r\n");            
            }else{
                throwMessage(&resMsg);
            }
            break;
        }
        default:
        {
            MSG("#Error: Can't find command.\r\n");
            break;
        }
    }
}

AkmSensorManager::Status AkmSensorManager::processEvent()
{
    AkmSensorManager::Status status = AkmSensorManager::SUCCESS;
    
    // If event is a command received from BLE
    if(eventCommandReceived)
    {
        VERBOSE("#Command received.\r\n");
        processCommand();
        eventCommandReceived = false;          // clear the flag
    }

    // check sensor event
//    for(int i=0; i<sensorNum; i++){
        if( sensor[0]->isEvent() ){
            Message msg;
            if( sensor[0]->readSensorData(&msg) != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR;
            throwMessage(&msg);     // Process and output message to USB/BLE
        }
//    }

    if(eventConnected)     // If BLE connected, clear flag and start sensor.
    {
        eventConnected = false;                 // clear the flag
        MSG("#BLE connected.\r\n");
    }
    
    // If event is the BLE being disconnected, stop the sensor
    if(eventDisconnected)
    {
        MSG("#BLE disconnected.\r\n");
//        for(int i=0; i<sensorNum; i++){
            if( sensor[0]->stopSensor() != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR;        
//        }
        eventDisconnected = false;      // clear the flag
    }
    return status;
}

AkmSensorManager::Status AkmSensorManager::throwMessage(const Message *msg) {
    const int len = Message::getMaxMessageLength();
    char buf[len];
    
    buf[0] = '$';           // Output message prefix

    // Processes command in msg to ASCII
    char cmd = (char)msg->getCommand();
    Message::charToAscii(&buf[1], &cmd);
    
    // Processes arguments in msg to ASCII
    for (int i=0; i < msg->getArgNum(); i++) {
        char arg = msg->getArgument(i);
        Message::charToAscii(&buf[3+2*i], &arg);
    }

    // Add termination characters, 0x0D(\r), \n and \0, to the end of string
    int tIdx = 3 + 2 * (msg->getArgNum());
    int bufSize = sizeof(buf)/sizeof(buf[0]);
    if ((tIdx + 3) > (bufSize - 1)) {
        MSG("#Error: Message data exceeds the buffer.\r\n");
        return ERROR;
    }
    buf[tIdx++] = CR;        // '\r'
    buf[tIdx++] = LF;        // '\n' 
    buf[tIdx] = '\0';    
    
    // If BLE is enabled, send to AKDP app
    if(isEnabledBle) uartService->writeString(buf);
    
    // If USB is enabled, send to serial terminal
    if(isEnabledUsb) serial->printf(buf);
    
    return SUCCESS;   
}

char* AkmSensorManager::stringConcat(char* str1, char* str2)
{
    int num1;
    char* str;
    
    num1=strlen(str1) + strlen(str2);
    str = (char *)malloc(num1 + 1);
    sprintf(str,"%s%s",str1,str2);
    return str;
}

char* AkmSensorManager::getSensorName(){
    char* name = "";
//    for(int i=0; i<sensorNum; i++){
        name = stringConcat(name, (char *)sensor[0]->getSensorName());
//        if( sensorNum > (i+1) )name = stringConcat(name, "+");
//    }
    MSG("#Sensor Name='%s'.\r\n",name);
    return name;
}

void AkmSensorManager::attachInterrupt(){
    switch(drdyType){
        case AkmAkd::INTERRUPT_ENABLED_PP:
        {
            if(primaryId == AkmSensor::AKM_PRIMARY_ID_AKD_SPI) interrupt = new InterruptIn(SPI_DRDY);
            else interrupt = new InterruptIn(I2C_DRDY);
            break;
        }
        case AkmAkd::INTERRUPT_ENABLED_OD:
        {
            if(primaryId == AkmSensor::AKM_PRIMARY_ID_AKD_SPI) interrupt = new InterruptIn(SPI_DRDY);
            else interrupt = new InterruptIn(I2C_DRDY);
            break;
        }
        default:
        {
            // nothing.
        }
    }
}

void AkmSensorManager::detachInterrupt(){
    switch(drdyType){
        case AkmAkd::INTERRUPT_ENABLED_PP:
        {
            interrupt->rise(NULL);
            break;
        }
        case AkmAkd::INTERRUPT_ENABLED_OD:
        {
            interrupt->fall(NULL);
            break;
        }
        default:
        {
            // nothing.
        }
    }
}