File content as of revision 33:d3e1e9eb2ef9:

#include "ble/services/UARTService.h"
#include "akdphwinfo.h"
#include "akmsensormanager.h"
#include "akmhallswitch.h"
#include "akmanalogsensor.h"
#include "ak09970ctrl.h"
#include "ak9750ctrl.h"
#include "ak9752ctrl.h"
#include "ak7451ctrl.h"
#include "ak7401ctrl.h"
#include "ap1017ctrl.h"
#include "akmakd.h"
#include "debug.h"
#include "Message.h"
#include "mcp342x.h"
#include "I2CNano.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",
                                    "TBD2",
                                    "Linear Sensor Legacy",
                                    "Current Sensor",
                                    "MISC(Analog)",
                                    "Linear Sensor",
                                    "Motor Drivers",
                                    "IR Sensor",
                                    "Angle Sensor(SPI)",
                                    "AKD Daughter Cards(I2C)"
                                     };
                                     
AkmSensorManager::AkmSensorManager(SerialNano* com)
{
    serial = com;
    isEnabledUsb = true;
    eventCommandReceived = false;
    eventConnected = false;
    eventDisconnected = false;
}

AkmSensorManager::Status AkmSensorManager::init(uint8_t id, uint8_t subid)
{
    primaryId = id;
    subId = subid;
    sensor = AkmSensorManager::getAkmSensor();
    if(sensor == NULL) 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;
}

AkmSensor* AkmSensorManager::getAkmSensor()
{
    AkmSensor* sensor = NULL;
    
    switch(primaryId){

        case AkmSensor::AKM_PRIMARY_ID_AKD_SPI:
        case AkmSensor::AKM_PRIMARY_ID_AKD_I2C:
        {
            if(subId != Ak09970Ctrl::SUB_ID_AK09970){
                AkmAkd* akd = new AkmAkd();
                sensor = akd;
            }
            else{
                Ak09970Ctrl* ak09970 = new Ak09970Ctrl();
                sensor = ak09970;
            }
            break;
        }    

        case AkmSensor::AKM_PRIMARY_ID_ANGLE_SENSOR:
        {
            if(subId == Ak7451Ctrl::SUB_ID_AK7451){
                Ak7451Ctrl* ak7451ctrl = new Ak7451Ctrl();
                sensor = ak7451ctrl;
                break;
            }
            else if(subId == Ak7401Ctrl::SUB_ID_AK7401){
                Ak7401Ctrl* ak7401ctrl = new Ak7401Ctrl();
                sensor = ak7401ctrl;
                break;
            }
            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 = hallswitch;
            break;
        }
//        case AkmSensor::AKM_PRIMARY_ID_LINEAR_SENSOR_LEGACY:
        case AkmSensor::AKM_PRIMARY_ID_LINEAR_SENSOR:
        case AkmSensor::AKM_PRIMARY_ID_CURRENT_SENSOR:
        case AkmSensor::AKM_PRIMARY_ID_MISC_ANALOG:
        {
            AkmAnalogSensor* analogsensor = new AkmAnalogSensor();
            sensor = analogsensor;
            break;
        }

        case AkmSensor::AKM_PRIMARY_ID_IR_SENSOR:
        {
            if(subId == Ak9750Ctrl::SUB_ID_AK9750){
                Ak9750Ctrl* ak9750ctrl = new Ak9750Ctrl();
                sensor = ak9750ctrl;
            }else if(subId == Ak9750Ctrl::SUB_ID_AK9753){
                Ak9750Ctrl* ak9753ctrl = new Ak9750Ctrl();
                sensor = ak9753ctrl;
            }else if(subId == Ak9752Ctrl::SUB_ID_AK9752){
                Ak9752Ctrl* ak9752ctrl = new Ak9752Ctrl();
                sensor = ak9752ctrl;
            }else{
                return NULL;    // 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 = ap1017ctrl;
            }else{
                return NULL;
            }
            break;
        }
        default:
        {
            MSG("#Can't find ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]);
            return NULL;    // couldn't find
        }
    }

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

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

        
void AkmSensorManager::dummyCallbackForCommandReceived(){}


// For commands received via BLE
AkmSensorManager::Status AkmSensorManager::commandReceived(char* buf){
    // Construct message
    Status status = SUCCESS;
    
    if ((Message::parse(&msg, buf)) != Message::SUCCESS) {
        MSG("#Failed to parse message. %s\r\n", buf);
        status = ERROR;
        eventCommandReceived = false;           // Reset flag
    }else{       
        eventCommandReceived = true;            // Set flag
//        MSG("#Parsed message. %s\r\n", buf);
    }
    t.attach(callback(this, &AkmSensorManager::dummyCallbackForCommandReceived),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)
{
    MSG("#GetID\r\n");
    
    I2C i2c(I2C_SDA, I2C_SCL);      // establish I2C to read ID
    
    // ADC
    MCP342X mcp342x(&i2c, MCP342X::SLAVE_ADDRESS_6EH);  // ADC to convert voltage
    mcp342x.setConversionMode(MCP342X::ONE_SHOT);       // Set to single sample
    MCP342X::AdcChannel ch;
    
    if (pin == ANALOG_SENSOR_ID) {                      // Primary ID
        ch = MCP342X::ADC_CH1;
    } else { // pin == ANALOG_SENSOR_ID_SUB         
        ch = MCP342X::ADC_CH2;                          // Secondary ID
    }
    
    int16_t val = getAdcData(&mcp342x, ch, MCP342X::SAMPLE_240HZ_12BIT);
    MSG("#12bit ADC Val = %d.\r\n", val);
    
    // Voltage boundaries for ID voltage divider system
    const int16_t VAL_MAX = 3000-2048;   // Corresponds to 3V
    const int16_t VAL_MIN = -2048;       // Corresponds to 0V
    
    // Convert voltage to ID value
    uint8_t value = (uint8_t)((val - VAL_MIN)/(float)(VAL_MAX - VAL_MIN) * (1 << bits) + 0.5);
    MSG("#ID = %d.\r\n", value);

    return value;
}


bool AkmSensorManager::isEvent()
{
    return (sensor->isEvent() || 
            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("#Mag ID is reported.\r\n");
            break;
        }
        case Message::CMD_STOP_MEASUREMENT:
        {
            if( sensor->stopSensor() != AkmSensor::SUCCESS){
                resMsg.setArgument(0, 1);
            }else{
                resMsg.setArgument(0, 0);                 
            }
            throwMessage(&resMsg);
            MSG("#Stop measurement.\r\n");
            break;
        }
        case Message::CMD_START_MEASUREMENT:
        {
            int error_code = AkmSensor::SUCCESS;
            if(msg.getArgNum() == 0){
                error_code = sensor->startSensor();
                if( error_code != AkmSensor::SUCCESS ){
                    MSG("#StartSensor Error. Code=%d\r\n",error_code);            
                }
                else{
                    MSG("#Start measurement.\r\n");
                }
            }else if(msg.getArgNum() == 1){
                float interval = (float)(1.0 / (float)msg.getArgument(0));
                error_code = sensor->startSensor(interval);
                if( error_code != AkmSensor::SUCCESS ){
                    MSG("#StartSensor Error. Code=%d\r\n",error_code);            
                }
                else{
                    MSG("#Start measurement.\r\n");
                }
            }else{
                MSG("#StartSensor Error. Wrong Argument num.\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->readSensorData(&temp);
                    throwMessage(&temp);
                }
            }
            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_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->requestCommand(&msg,&resMsg);
            if( (resMsg.getArgNum() == 0) && (st != AkmSensor::SUCCESS) )
            {
                MSG("#Command failed.\r\n");            
            }else{
                throwMessage(&resMsg);
            }
            break;
        }
        default:
        {
            MSG("#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)
    {
//        MSG("#Command received.\r\n");
        processCommand();
        eventCommandReceived = false;          // clear the flag
    }
    
    // If event is sensor read data event, read the message
    if(sensor->isEvent())
    {
//        MSG("#Sensor event received.\r\n");
        Message msg;
        if( sensor->readSensorData(&msg) != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR;
        throwMessage(&msg);                 // Process and output message to USB/BLE
    }

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

AkmSensorManager::Status AkmSensorManager::throwMessage(const Message *msg) {
    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::getSensorName(){
    return sensor->getSensorName();
}