File content as of revision 1:b46b8653331f:

#include "ble/services/UARTService.h"
#include "akmsensormanager.h"
#include "akmanglesensor.h"
#include "akmhallswitch.h"
#include "akmlinearsensor.h"
#include "akmirsensor.h"
#include "akmakd.h"
#include "debug.h"
#include "Message.h"

#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)",  // Temporary
                                    "Switch, Unipolar",
                                    "Switch, Onmipolar",
                                    "Latch, Bipolar",
                                    "Switch, Dual Output",
                                    "Onechip Encoder",
                                    "TBD1",
                                    "TBD2",
                                    "Linear Sensor Legacy",
                                    "Current Sensor",
                                    "MISC(Analog)",
                                    "Linear Sensor",
                                    "IR Sensor",
                                    "TBD3",
                                    "Angle Sensor(SPI)",        // Temporary
                                    "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_I2C:
            subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,5);    // 5bit sub id
            AkmAkd* akd;
            akd = new AkmAkd();
            sensor = akd;
            break;
            
        case AkmSensor::AKM_PRIMARY_ID_AKD_SPI:                         // Temporary
        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:
            subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4);    // 4bit sub id
            AkmLinearSensor* linearsensor;
            linearsensor = new AkmLinearSensor();
            sensor = linearsensor;
            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
    }
    
    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;
}


uint8_t AkmSensorManager::getId(PinName pin, uint8_t bits)
{
    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));
    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 out is set.\n");
            break;

        case Message::CMD_GET_ID:                       // return Primary ID and Sub ID
            if(id == 0) id = 0xE;                       // Temporary for Angle Sensor
            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_ANGLE_ZERO_RESET:
        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:
            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::dataOut(char* str){
    if(isBleMode) uartService->writeString(str);            
    if(isSerialMode) serial->printf(str);
}
*/