File content as of revision 9:6fa3e7b17c27:

#include "ble/services/UARTService.h"
#include "akmsensormanager.h"
#include "akmanglesensor.h"
#include "akmhallswitch.h"
#include "akmlinearsensor.h"
#include "akmcurrentsensor.h"
#include "akmirsensor.h"
#include "akmakd.h"
#include "debug.h"
#include "Message.h"
#ifdef REV_D
#include "mcp342x.h"
#include "I2CNano.h"
#include "tca9554a.h"
#endif

#define FIRMWARE_VERSION   0x02
#define MAGNETOMETER_ID    0x0A

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


static char* AKM_PRIMARY_ID_STR[] = {
                                    "AKD Daughter Cards(SPI)",
                                    "Switch, Unipolar",
                                    "Switch, Onmipolar",
                                    "Latch, Bipolar",
                                    "Switch, Dual Output",
                                    "Onechip Encoder",
                                    "TBD1",
                                    "TBD2",
                                    "Linear Sensor Legacy",
                                    "Current Sensor",
                                    "MISC(Analog)",
                                    "Linear Sensor",
                                    "TBD3",
                                    "IR Sensor",
                                    "Angle Sensor(SPI)",
                                    "AKD Daughter Cards(I2C)",
                                     };
                                     
AkmSensorManager::AkmSensorManager(SerialNano* com, UARTService* service)
{
    serial = com;
    uartService = service;
    isEnabledBle = true;
    isEnabledUsb = true;
    eventCommandReceived = false;
    eventConnected = false;
    eventDisconnected = false;
}

AkmSensorManager::Status AkmSensorManager::init()
{
    sensor = AkmSensorManager::getAkmSensor();
    if(sensor == NULL) return AkmSensorManager::ERROR;  
    return AkmSensorManager::SUCCESS;
}

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

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

AkmSensor* AkmSensorManager::getAkmSensor()
{
    AkmSensor* sensor = NULL;
    
    // primary id check
    id = AkmSensorManager::getId(ANALOG_SENSOR_ID,4);
    
    switch(id){
        case AkmSensor::AKM_PRIMARY_ID_AKD_SPI:
        case AkmSensor::AKM_PRIMARY_ID_AKD_I2C:
            subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,5);    // 5bit sub id
            AkmAkd* akd;
            akd = new AkmAkd();
            sensor = akd;
                        

#ifdef REV_D
    {
        const int TIME_FOR_OE_MS = 100;
        const TCA9554A::Port PORT_OE_LVS2 = TCA9554A::PORT_6;
        I2C i2c(I2C_SDA, I2C_SCL);
        TCA9554A tca9554a(&i2c, TCA9554A::SLAVE_ADDRESS_38H);
        // Makes sure that the OE is low first.
        tca9554a.setPortLevel(PORT_OE_LVS2, TCA9554A::LOW);
        wait_ms(TIME_FOR_OE_MS);
    }
#endif    

            break;
            
        case AkmSensor::AKM_PRIMARY_ID_ANGLE_SENSOR:
            subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4);    // 4bit sub id
            AkmAngleSensor* angleSensor;
            angleSensor = new AkmAngleSensor();
            sensor = angleSensor;
            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:
            subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4);    // 4bit sub id
            AkmHallSwitch* hallswitch;
            hallswitch = new AkmHallSwitch();
            sensor = hallswitch;
            break;
        
        case AkmSensor::AKM_PRIMARY_ID_LINEAR_SENSOR_LEGACY:
        case AkmSensor::AKM_PRIMARY_ID_LINEAR_SENSOR:
            subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4);    // 4bit sub id
            AkmLinearSensor* linearsensor;
            linearsensor = new AkmLinearSensor();
            sensor = linearsensor;
            break;
        
        case AkmSensor::AKM_PRIMARY_ID_CURRENT_SENSOR:
            subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4);    // 4bit sub id
            AkmCurrentSensor* currentsensor;
            currentsensor = new AkmCurrentSensor();
            sensor = currentsensor;
            break;
        
        case AkmSensor::AKM_PRIMARY_ID_IR_SENSOR:
            subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4);    // 4bit sub id
            AkmIrSensor* irsensor;
            irsensor = new AkmIrSensor();
            sensor = irsensor;
            break;
        
        default:
            subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4);    // 4bit sub id
            MSG("#Can't find ID=%d SubID=%d %s\r\n", id, subId, AKM_PRIMARY_ID_STR[id]);    
            return NULL;    // couldn't find
    }

#ifdef REV_D
   // I2C turn off 
    releaseTWI();
#endif  
    
    if(sensor->init(id, subId) != AkmSensor::SUCCESS){
        MSG("#sensor->init failed. ID=%d SubID=%d %s\r\n", id, subId, AKM_PRIMARY_ID_STR[id]);    
        return NULL;    // couldn't find
    }

    MSG("#ID=%d SubID=%d %s\r\n", id, subId, AKM_PRIMARY_ID_STR[id]);    
    return sensor;
}

AkmSensorManager::Status AkmSensorManager::commandReceived(char* buf){
    // Construct message
    Status status = SUCCESS;
    
    Message::Status st;
    if ((st=Message::parse(&msg, buf)) != Message::SUCCESS) {
        MSG("#Failed to parse message. status = %02x. %s\n", st, buf);
        status = ERROR;
        eventCommandReceived = false;
    }
    eventCommandReceived = true;
    return status;
}

#ifdef REV_D
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;
}
#endif

uint8_t AkmSensorManager::getId(PinName pin, uint8_t bits)
{
#ifndef REV_D
    /* Rev.C */
    AnalogIn id(pin);
    MSG("#Voltage=%5.2f[V]\n",id*3.0);
    double s = id + 1.0/(double)(pow(2.0,bits+1));
    uint8_t value = (uint8_t)(s*pow(2.0,bits));
#else
    /* Rev.D */
    MSG("#GetID\n");
    
    I2C i2c(I2C_SDA, I2C_SCL);
    // ADC
    MCP342X mcp342x(&i2c, MCP342X::SLAVE_ADDRESS_6EH);
    mcp342x.setConversionMode(MCP342X::ONE_SHOT);
    MCP342X::AdcChannel ch;
    if (pin == ANALOG_SENSOR_ID) {
        ch = MCP342X::ADC_CH1;
    } else { // pin == ANALOG_SENSOR_ID_SUB
        ch = MCP342X::ADC_CH2;
    }
    int16_t val = getAdcData(&mcp342x, ch, MCP342X::SAMPLE_240HZ_12BIT);
    MSG("#12bit ADC Val = %d.\n", val);
    
    const int16_t VAL_MAX = 3000-2048;   // Corresponds to 3V
    const int16_t VAL_MIN = -2048;       // Corresponds to 0V
    
    uint8_t value = (uint8_t)((val - VAL_MIN)/(float)(VAL_MAX - VAL_MIN) * (1 << bits) + 0.5);
    MSG("#ID = %d.\n", value);

#endif    
    return value;
}

bool AkmSensorManager::isEvent()
{
    return (sensor->isEvent() || 
            eventCommandReceived ||
            eventConnected ||
            eventDisconnected);
}


void AkmSensorManager::processCommand()
{
    AkmSensorManager::Status status = AkmSensorManager::SUCCESS;
//    MSG("#processCommand.\n");
    
    // Gets command in the message
    Message::Command cmd = msg.getCommand();
    
    // Creates an message object to return
    Message resMsg;
    
    // Return message has the same command as input
    resMsg.setCommand(cmd);
    
    switch(cmd)
    {
        case Message::CMD_GET_FW_VERSION:
            resMsg.setArgument(0, FIRMWARE_VERSION);
            throwMessage(&resMsg);
            MSG("#FW version is reported.\n");
            break;
            
        case Message::CMD_GET_MAG_PART:
            resMsg.setArgument(0, MAGNETOMETER_ID);
            throwMessage(&resMsg);
            MSG("#Mag ID is reported.\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;
            resMsg.setArgument(0, 0);
            throwMessage(&resMsg);
            MSG("#Serial target is set.\n");
            break;

        case Message::CMD_GET_ID:                       // return Primary ID and Sub ID
            resMsg.setArgument(0, id);
            resMsg.setArgument(1, subId);
            throwMessage(&resMsg);
            wait(0.4);  // wait for App initialization            
            MSG("#Mag ID is reported.\n");
            break;

        case Message::CMD_STOP_MEASUREMENT:
            if( sensor->stopSensor() != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR;
            resMsg.setArgument(0, status==AkmSensorManager::SUCCESS ? 0 : 1);
            throwMessage(&resMsg);
            MSG("#Stop measurement.\n");
            break;

        case Message::CMD_START_MEASUREMENT:
            MSG("#Start measurement.\n");
            sensor->startSensor();
            // get initial sensor state for switch type sensors
            if( id == AkmSensor::AKM_PRIMARY_ID_UNIPOLAR ||
                id == AkmSensor::AKM_PRIMARY_ID_OMNIPOLAR ||
                id == AkmSensor::AKM_PRIMARY_ID_LATCH ||
                id == AkmSensor::AKM_PRIMARY_ID_DUAL_OUTPUT ||
                id == AkmSensor::AKM_PRIMARY_ID_ONECHIP_ENCODER ){                
                Message temp;
                sensor->readSensorData(&temp);
                throwMessage(&temp);
            }
            break;

        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_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:
            sensor->requestCommand(&msg,&resMsg);
            throwMessage(&resMsg);
            break;
            
        default:
            MSG("#Can't find command:%s\n", (char)cmd);
            break;
    }
}

AkmSensorManager::Status AkmSensorManager::processEvent()
{
    AkmSensorManager::Status status = AkmSensorManager::SUCCESS;
    
    // command received from the host
    if(eventCommandReceived)
    {
        processCommand();
        eventCommandReceived = false;
    }
    if(sensor->isEvent())  // sensor read data event
    {
        Message msg;
        if( sensor->readSensorData(&msg) != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR;
        throwMessage(&msg);            
    }

    if(eventConnected)     // BLE connected. Start sensor.
    {
//        if( sensor->startSensor() != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR;
        eventConnected = false;   
    }
    if(eventDisconnected)  // BLE dis-connected. Stop sensor.
    {
        if( sensor->stopSensor() != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR;
        eventDisconnected = false;   
    }
    return status;
}

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

    // Processes command
    char cmd = (char)msg->getCommand();
    Message::charToAscii(&buf[1], &cmd);
    
    // Processes arguments
    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.\n");
        return ERROR;
    }
    buf[tIdx++] = 0x0D;        // '\r'
    buf[tIdx++] = '\n';
    buf[tIdx] = '\0';
        
    // Send to the BLE buffer
//    Hardware *hw = Hardware::getInstance();
//    hw->uartService->writeString(buf);
    if(isEnabledBle) uartService->writeString(buf);            
    if(isEnabledUsb) serial->printf(buf);
    
//    MSG("#A message thrown: \"%s\"\n", buf);
    
    return SUCCESS;   
}

void AkmSensorManager::releaseTWI(){
    NRF_TWI0->ENABLE = TWI_ENABLE_ENABLE_Disabled << TWI_ENABLE_ENABLE_Pos;
    NRF_TWI0->POWER  = 0;
    NRF_TWI1->ENABLE = TWI_ENABLE_ENABLE_Disabled << TWI_ENABLE_ENABLE_Pos;
    NRF_TWI1->POWER  = 0;
}

/*
void AkmSensorManager::dataOut(char* str){
    if(isBleMode) uartService->writeString(str);            
    if(isSerialMode) serial->printf(str);
}
*/