File content as of revision 40:24065d634473:

#include <stdio.h>
#include <stdlib.h>
#include "mbed.h"
#include "ble/BLE.h"
#include "ble/services/UARTService.h"
#include "SerialNano.h"
#include "akdphwinfo.h"
#include "akmsensor.h"
#include "akmsensormanager.h"
#include "debug.h"
#include "tca9554a.h"
#include "mcp342x.h"
#include "akmakd.h"

#define BLE_UUID_TXRX_SERVICE            0x0000 /**< The UUID of the Nordic UART Service. */
#define BLE_UUID_TX_CHARACTERISTIC       0x0002 /**< The UUID of the TX Characteristic. */
#define BLE_UUIDS_RX_CHARACTERISTIC      0x0003 /**< The UUID of the RX Characteristic. */
#define BLE_BUF_LEN                     UARTService::BLE_UART_SERVICE_MAX_DATA_LEN+1
#define TXRX_LEN                        50

BLE                 ble;
UARTService*        uartService;
SerialNano          serial(P0_4, P0_5);  // Rev.D pin configuration
AkmSensorManager*   manager;

uint8_t id;
uint8_t subId;


enum {
    UNIT_0_625_MS = 625,
    UNIT_1_25_MS  = 1250,
    UNIT_10_MS    = 10000
};


// default setting of Nexus 5X, Motorola Droid Turbo:
//#define MIN_CONN_INTERVAL               MSEC_TO_UNITS(45, UNIT_1_25_MS)              /**< Minimum connection interval (45 ms) */
//#define MAX_CONN_INTERVAL               MSEC_TO_UNITS(45, UNIT_1_25_MS)               /**< Maximum connection interval (45 ms). */
//#define SLAVE_LATENCY                   0                                             /**< Slave latency. */
//#define CONN_SUP_TIMEOUT                MSEC_TO_UNITS(20000, UNIT_10_MS)               /**< Connection supervisory timeout (20 seconds). */

/*
iOS requirement:
Interval Max * (Slave Latency + 1) ≤ 2 seconds
Interval Min ≥ 20 ms
Interval Min + 20 ms ≤ Interval Max Slave Latency ≤ 4
connSupervisionTimeout ≤ 6 seconds
Interval Max * (Slave Latency + 1) * 3 < connSupervisionTimeout
*/

#define MSEC_TO_UNITS(TIME, RESOLUTION) (((TIME) * 1000) / (RESOLUTION))
#define MIN_CONN_INTERVAL               MSEC_TO_UNITS(7.5, UNIT_1_25_MS)              /**< Minimum connection interval (7.5 ms) */
#define MAX_CONN_INTERVAL               MSEC_TO_UNITS(30, UNIT_1_25_MS)               /**< Maximum connection interval (30 ms). */
#define SLAVE_LATENCY                   0                                             /**< Slave latency. */
#define CONN_SUP_TIMEOUT                MSEC_TO_UNITS(4000, UNIT_10_MS)               /**< Connection supervisory timeout (4 seconds). */

// Command received from BLE
void WrittenHandler(const GattWriteCallbackParams *Handler)
{   
    static char command[TXRX_LEN]="";
    static uint16_t len=0;

    uint8_t buf[BLE_BUF_LEN];
    uint16_t bytesRead;
    if (Handler->handle == uartService->getTXCharacteristicHandle()) 
    {
        ble.gattServer().read(uartService->getTXCharacteristicHandle(), buf, &bytesRead);

        for(uint16_t i=0; i<bytesRead; i++){
            if(buf[i] == CR)
            {
                ;   // ignore CR
            }
            else if(buf[i] == LF || len > TXRX_LEN)
            {
                manager->commandReceived(command);
                for(int j=0; j<TXRX_LEN; j++){
                    command[j] = 0;
                }
                len = 0;
            }
            else
            {
                command[len++] = (char)buf[i];
            }
        }
    }
}


// Command received from USB
static void usbUartCallback(void)
{   
    static char command[TXRX_LEN] = "";
    static uint16_t len=0;

    while(serial.readable())    
    {
        uint8_t c = serial.getc();
        
        // ignore CR
        if(c != CR)
        {
            command[len++] = c;
            if(len>=TXRX_LEN || c == LF)
            {
                manager->commandReceived(command);
                for(int j=0; j<TXRX_LEN; j++){
                    command[j] = 0;
                }
                len = 0;
            }
        }        
    }
}

static void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
{
    MSG("#From central: minConnectionInterval = %d\r\n", params->connectionParams->minConnectionInterval);
    MSG("#From central: maxConnectionInterval = %d\r\n", params->connectionParams->maxConnectionInterval);
    MSG("#From central: slaveLatency = %d\r\n", params->connectionParams->slaveLatency);
    MSG("#From central: connectionSupervisionTimeout = %d\r\n", params->connectionParams->connectionSupervisionTimeout);
    
    Gap::Handle_t gap_handle = params->handle;
    Gap::ConnectionParams_t gap_conn_params;
    gap_conn_params.minConnectionInterval = params->connectionParams->minConnectionInterval;
    gap_conn_params.maxConnectionInterval = params->connectionParams->maxConnectionInterval;
    gap_conn_params.slaveLatency = params->connectionParams->slaveLatency;
    gap_conn_params.connectionSupervisionTimeout = CONN_SUP_TIMEOUT;
    ble.updateConnectionParams(gap_handle, &gap_conn_params);

    MSG("#From peripheral: minConnectionInterval = %d\r\n", gap_conn_params.minConnectionInterval);
    MSG("#From peripheral: maxConnectionInterval = %d\r\n", gap_conn_params.maxConnectionInterval);
    MSG("#From peripheral: slaveLatency = %d\r\n", gap_conn_params.slaveLatency);
    MSG("#From peripheral: connectionSupervisionTimeout = %d\r\n", gap_conn_params.connectionSupervisionTimeout);

    manager->setEventConnected();
    MSG("#Connected\r\n");
}

static void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
    manager->setEventDisconnected();
    MSG("#Disconnected\r\n");
    ble.gap().startAdvertising();
} 

void bleSetup(char* device_name){
    ble.init();

    // setup advertising 
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
                                    (const uint8_t *)device_name, strlen(device_name));
//                                    (const uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME) - 1);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
                                    (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));

    
    // Set desired connection parameters
    Gap::ConnectionParams_t gap_conn_params;
    gap_conn_params.minConnectionInterval = MIN_CONN_INTERVAL;
    gap_conn_params.maxConnectionInterval = MAX_CONN_INTERVAL;
    gap_conn_params.slaveLatency = SLAVE_LATENCY;
    gap_conn_params.connectionSupervisionTimeout = CONN_SUP_TIMEOUT;
    ble.setPreferredConnectionParams(&gap_conn_params);
    
    ble.gap().onDisconnection(disconnectionCallback);
    ble.gap().onConnection(connectionCallback);
    ble.gattServer().onDataWritten(WrittenHandler);  
    
    // 100ms; in multiples of 0.625ms. 
    ble.gap().setAdvertisingInterval(160);
    ble.gap().startAdvertising(); 
}


int16_t 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 getId(PinName pin, uint8_t bits)
{
    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.\r\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.\r\n", value);

    return value;
}


void 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;
}

bool initAkdpBoard(){
    MSG("#Init AKDP board.\r\n");
    const int TIME_FOR_OE_MS = 100;
    
    // CSN High to activate I2C_GATE
    DigitalOut _cs = DigitalOut(SPI_CS);
    _cs.write(1);

    // I2C communication ports to HIGH(just in case).
    DigitalOut _scl = DigitalOut(I2C_SCL);
    _scl.write(1);
    DigitalOut _sda = DigitalOut(I2C_SDA);
    _sda.write(1);
    MSG("#SCL,SDA port high.\r\n");
    wait_ms(TIME_FOR_OE_MS);
    
    const TCA9554A::Port PORT_OE_LVS1   = TCA9554A::PORT_7;
    const TCA9554A::Port PORT_OE_LVS2   = TCA9554A::PORT_6;
    const TCA9554A::Port PORT_SPIN      = TCA9554A::PORT_5;
    const TCA9554A::Port PORT_RSV_RSTN  = TCA9554A::PORT_0;
    
    I2C i2c(I2C_SDA, I2C_SCL);
    
    // call I2C general reset only once
    char cmd[] = {0x06};    // general reset code
    i2c.write(0x00, cmd, 1);
    MSG("#General Reset.\r\n");
    wait_ms(TIME_FOR_OE_MS);
    
    TCA9554A tca9554a(&i2c, TCA9554A::SLAVE_ADDRESS_38H);
    
    // Initializes TCA9554A (I2C GPIO Expander)
    tca9554a.configurePort(PORT_OE_LVS1, TCA9554A::DIR_OUTPUT);
    tca9554a.configurePort(PORT_OE_LVS2, TCA9554A::DIR_OUTPUT);
    tca9554a.configurePort(PORT_SPIN, TCA9554A::DIR_OUTPUT);
    tca9554a.configurePort(PORT_RSV_RSTN, TCA9554A::DIR_OUTPUT);
    
    //  enable LVS1 and LVS2 level shifter
    tca9554a.setPortLevel(PORT_OE_LVS1, TCA9554A::HIGH);
    tca9554a.setPortLevel(PORT_OE_LVS2, TCA9554A::HIGH);
    tca9554a.setPortLevel(PORT_RSV_RSTN, TCA9554A::HIGH);
    tca9554a.setPortLevel(PORT_SPIN, TCA9554A::HIGH);
    wait_ms(TIME_FOR_OE_MS);

    //  reset LVS2
    tca9554a.setPortLevel(PORT_OE_LVS2, TCA9554A::LOW);
    wait_ms(TIME_FOR_OE_MS);
    tca9554a.setPortLevel(PORT_OE_LVS2, TCA9554A::HIGH);
    wait_ms(TIME_FOR_OE_MS);

    //  reset LVS1
    tca9554a.setPortLevel(PORT_OE_LVS1, TCA9554A::LOW);
    wait_ms(TIME_FOR_OE_MS);
    tca9554a.setPortLevel(PORT_OE_LVS1, TCA9554A::HIGH);
    wait_ms(TIME_FOR_OE_MS);

    //  disable LVS1 level shifter to read ID
    tca9554a.setPortLevel(PORT_OE_LVS1, TCA9554A::LOW);
    wait_ms(TIME_FOR_OE_MS);
    
    //  read ID and subId from ADC
    id = getId(ANALOG_SENSOR_ID,4);
    uint8_t subid_bitlen = 4;
    if(id == AkmSensor::AKM_PRIMARY_ID_AKD_SPI || id == AkmSensor::AKM_PRIMARY_ID_AKD_I2C){
//        MSG("#5 bit sub ID.\r\n");
        subid_bitlen = 5;
    }
    subId = getId(ANALOG_SENSOR_ID_SUB,subid_bitlen);

    if( (id == 11 && subId == 11) || (id == 55 && subId == 55) ){
        return true;
    }

    //  enable 1.8V level shifter
    tca9554a.setPortLevel(PORT_OE_LVS1, TCA9554A::HIGH);
    MSG("#LVS1 High.\r\n");
    wait_ms(TIME_FOR_OE_MS);

    // RSTN control
    if(id == AkmSensor::AKM_PRIMARY_ID_AKD_SPI || id == AkmSensor::AKM_PRIMARY_ID_AKD_I2C){
        tca9554a.setPortLevel(PORT_RSV_RSTN, TCA9554A::LOW);
        wait_ms(TIME_FOR_OE_MS);
        tca9554a.setPortLevel(PORT_RSV_RSTN, TCA9554A::HIGH);                
//        MSG("#Detect AKD, RSTN control.\r\n");
    }

    // SPI disable/enable
    if( id == AkmSensor::AKM_PRIMARY_ID_AKD_SPI || id == AkmSensor::AKM_PRIMARY_ID_ANGLE_SENSOR ){
        tca9554a.setPortLevel(PORT_SPIN, TCA9554A::LOW);
        // Disable 5.0V level shifter in order to ADC doesn't respond.
        tca9554a.setPortLevel(PORT_OE_LVS2, TCA9554A::LOW);
//        MSG("#Detect SPI, set SPIN low.\r\n");
    }
    else{
        tca9554a.setPortLevel(PORT_SPIN, TCA9554A::HIGH);
        tca9554a.setPortLevel(PORT_OE_LVS2, TCA9554A::HIGH);
    }

    wait_ms(TIME_FOR_OE_MS);
    
    releaseTWI();
    
    return false;
}

char* my_strcat(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;
}

void error(const char* format, ...) {
}

int main(void)
{
    // USB serial
    serial.baud(115200);
    
    // serial port RX event
    serial.attach(&usbUartCallback);
    
#ifdef DEBUG
    Debug::setSerial(&serial);
    MSG("#Debug Mode.\r\n");
#endif
    
    // initialize AKDP board
    if( initAkdpBoard() ){
        MSG("#Error: AKDP boot failed.\r\n");
    }
        
    // create sensor manager
    manager = new AkmSensorManager(&serial);
    
    if( manager->init(id, subId) == AkmSensorManager::ERROR){
        MSG("#Error: sensor is NULL\r\n");
    }
    
    // create device name
    // "AKDP D7.0XX SENSOR_NAME"
    char fw_version[5];
    sprintf(fw_version, "%03d", FIRMWARE_VERSION);
    char* name = my_strcat(MOTHER_BOARD_NAME, " ");
    name = my_strcat(name, MOTHER_BOARD_VERSION);
    name = my_strcat(name, ".");
    name = my_strcat(name, fw_version);
    name = my_strcat(name, " ");
    name = my_strcat(name, manager->getSensorName());
    
    // BLE initialize
    bleSetup(name);

    // BLE UART service
    uartService = new UARTService(ble);
    
    // set BLE UART service
    manager->setBleUartService(uartService);
    
    MSG("#Connecting...\r\n");

    // main loop
    while(1)
    {
        if(manager->isEvent()){
            manager->processEvent();
        }else{
            ble.waitForEvent();
        }
    }
}