File content as of revision 12:dd866bd5eaf9:

/* mbed Microcontroller Library
 * Copyright (c) 2006-2013 ARM Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "mbed.h"
#include "BLE.h"

//Debug用
#define USE_KONASHI_PWM 0
#define USE_KONASHI_I2C 1
#define USE_KONASHI_ANALOG 0    //未実装
#define USE_KONASHI_UART 1

const static char  DEVICE_NAME[] = "mbed HRM1017";

static const uint16_t KONASHI_SERVICE_UUID              = 0xFF00;

static const uint16_t KONASHI_PIO_SETTING_UUID          = 0x3000;  
static const uint16_t KONASHI_PIO_PULLUP_UUID           = 0x3001;  
static const uint16_t KONASHI_PIO_OUTPUT_UUID           = 0x3002;  
static const uint16_t KONASHI_PIO_INPUT_NOTIFICATION_UUID = 0x3003;  

static const uint16_t KONASHI_PWM_CONFIG_UUID           = 0x3004;  
static const uint16_t KONASHI_PWM_PARAM_UUID            = 0x3005;  
static const uint16_t KONASHI_PWM_DUTY_UUID             = 0x3006;  

static const uint16_t KONASHI_ANALOG_DRIVE_UUID         = 0x3007;  
static const uint16_t KONASHI_ANALOG_READ0_UUID         = 0x3008;  
static const uint16_t KONASHI_ANALOG_READ1_UUID         = 0x3009;  
static const uint16_t KONASHI_ANALOG_READ2_UUID         = 0x300A;  

static const uint16_t KONASHI_I2C_CONFIG_UUID           = 0x300B;  
static const uint16_t KONASHI_I2C_START_STOP_UUID       = 0x300C;  
static const uint16_t KONASHI_I2C_WRITE_UUID            = 0x300D;  
static const uint16_t KONASHI_I2C_READ_PARAM_UIUD       = 0x300E;  
static const uint16_t KONASHI_I2C_READ_UUID             = 0x300F;  

static const uint16_t KONASHI_UART_CONFIG_UUID          = 0x3010;  
static const uint16_t KONASHI_UART_BAUDRATE_UUID        = 0x3011;  
static const uint16_t KONASHI_UART_TX_UUID              = 0x3012;  
static const uint16_t KONASHI_UART_RX_NOTIFICATION_UUID = 0x3013;  

static const uint16_t KONASHI_HARDWARE_RESET_UUID       = 0x3014;  
static const uint16_t KONASHI_HARDWARE_LOW_BAT_NOTIFICATION_UUID = 0x3015; 

uint16_t    uuid16_list[] = { KONASHI_SERVICE_UUID };

#define NEED_CONSOLE_OUTPUT 0 /* Set this if you need debug messages on the console;
                               * it will have an impact on code-size and power consumption. */

#if NEED_CONSOLE_OUTPUT
//Serial  pc(USBTX, USBRX);
#define DEBUG(...) { pc.printf(__VA_ARGS__); }
#else
#define DEBUG(...) /* nothing */
#endif /* #if NEED_CONSOLE_OUTPUT */

Serial  pc(USBTX, USBRX);
BLEDevice  ble;
DigitalOut led1(LED1);
DigitalOut led2(LED2);

//BusOut ioOut(P0_0,P0_1,P0_2,P0_3,P0_4,P0_5,P0_6,P0_7);
BusOut ioOut(P0_5, P0_6, P0_7, P0_1, P0_0, P0_30 ,P0_29 ,P0_28);
//BusIn ioIn(P0_10,P0_12,P0_13,P0_14,P0_15,P0_17, P0_16, P0_21);
BusIn ioIn(P0_10,P0_12,P0_13,P0_14,P0_15,P0_17, P0_16, P0_21);
uint8_t pioSetting[20]      = {0,};
uint8_t pioPullup[20]       = {0,};
uint8_t pioOutput[20]       = {0,};
uint8_t pioInput[20]        = {0,};

uint8_t pwmConfig[20]       = {0,};
uint8_t pwmPeriod[20]       = {0,};
uint8_t pwmDuty[20]         = {0,};
static uint32_t pwm_period[3] = {20000,20000,20000};
static uint32_t pwm_duty[3] = {0,0,0};
static uint8_t  pwm_config = 0;
//PwmOut pwm[3] = {P0_29, P0_28, P0_25,};
PwmOut pwm[3] = {P0_23, P0_24, P0_25,};

uint8_t analogDrive[20]     = {0,};
uint8_t analogRead0[20]     = {0,};
uint8_t analogRead1[20]     = {0,};
uint8_t analogRead2[20]     = {0,};
AnalogIn analog[3] = {P0_2, P0_3,P0_4};

I2C i2c(P0_22,P0_20);
static uint8_t  i2c_config = 0;
static uint8_t  i2c_recvbuf[20];
static uint8_t  i2c_recvlen = 0;
static bool i2c_repeated = false;
uint8_t i2cConfig[20]       = {0,};
uint8_t i2cStartStop[20]    = {0,};
uint8_t i2cWrite[20]        = {0,};
uint8_t i2cReadParam[20]    = {0,};
uint8_t i2cRead[20]         = {0,};

#define UART_DEFAULT_BAUDRATE 115200
static uint16_t uart_baudrate=UART_DEFAULT_BAUDRATE/240;
uint8_t uartConfig[20]      = {0,};
uint8_t uartBaudrate[20]    = {0,};
uint8_t uartTx[20]          = {0,};
uint8_t uartRx[20]          = {0,};

uint8_t hardwareRest[20]    = {0,};
uint8_t hardwareLowBat[20]  = {0,};

GattCharacteristic  pioSettingCharacteristic (KONASHI_PIO_SETTING_UUID, pioSetting, 1, 1,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | 
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic  pioPullupCharacteristic (KONASHI_PIO_PULLUP_UUID, pioPullup, 1, 1,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | 
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic  pioOutputCharacteristic (KONASHI_PIO_OUTPUT_UUID, pioOutput, 1, 1,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic  pioInputCharacteristic (KONASHI_PIO_INPUT_NOTIFICATION_UUID, pioInput, 1, 1,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | 
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);

GattCharacteristic  pwmConfigCharacteristic (KONASHI_PWM_CONFIG_UUID, pwmConfig, 1, 1,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | 
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic  pwmPeriodCharacteristic (KONASHI_PWM_PARAM_UUID, pwmPeriod, 5, 5,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | 
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic  pwmDutyCharacteristic (KONASHI_PWM_DUTY_UUID, pwmDuty, 5, 5,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | 
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);

GattCharacteristic  analogDriveCharacteristic (KONASHI_ANALOG_DRIVE_UUID, analogDrive, 1, 1,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | 
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic  analogRead0Characteristic (KONASHI_ANALOG_READ0_UUID, analogRead0, 2, 2,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);
GattCharacteristic  analogRead1Characteristic (KONASHI_ANALOG_READ1_UUID, analogRead1, 2, 2,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);
GattCharacteristic  analogRead2Characteristic (KONASHI_ANALOG_READ2_UUID, analogRead2, 2, 2,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);

GattCharacteristic  i2cConfigCharacteristic (KONASHI_I2C_CONFIG_UUID, i2cConfig, 1, 1,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | 
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic  i2cStartStopCharacteristic (KONASHI_I2C_START_STOP_UUID, i2cStartStop, 1, 1,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | 
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic  i2cWriteCharacteristic (KONASHI_I2C_WRITE_UUID, i2cWrite, 3, 20,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic  i2cReadParamCharacteristic (KONASHI_I2C_READ_PARAM_UIUD, i2cReadParam, 2, 2,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic  i2cReadCharacteristic (KONASHI_I2C_READ_UUID, i2cRead, 0, 20,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);

GattCharacteristic  uartConfigCharacteristic (KONASHI_UART_CONFIG_UUID, uartConfig, 1, 1,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | 
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic  uartBaudrateCharacteristic (KONASHI_UART_BAUDRATE_UUID, uartBaudrate, 1, 1,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | 
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic  uartTxCharacteristic (KONASHI_UART_TX_UUID, uartTx, 1, 1,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | 
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic  uartRxCharacteristic (KONASHI_UART_RX_NOTIFICATION_UUID, uartRx, 0, 20,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | 
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);

GattCharacteristic  hardwareRestCharacteristic (KONASHI_HARDWARE_RESET_UUID, hardwareRest, 1, 1,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic  hardwareLowBatCharacteristic (KONASHI_HARDWARE_LOW_BAT_NOTIFICATION_UUID, hardwareLowBat, 1, 1,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | 
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);

GattCharacteristic *konashiChars[] = {//  &pioSettingCharacteristic,
                                      //  &pioPullupCharacteristic,
                                        &pioOutputCharacteristic,
                                        &pioInputCharacteristic,
#if USE_KONASHI_PWM
                                        &pwmConfigCharacteristic,
                                        &pwmPeriodCharacteristic,
                                        &pwmDutyCharacteristic,
#endif
#if USE_KONASHI_ANALOG
                                        &analogDriveCharacteristic,
                                        &analogRead0Characteristic,
                                        &analogRead1Characteristic,
                                        &analogRead2Characteristic,
#endif
#if USE_KONASHI_I2C
                                        &i2cConfigCharacteristic,
                                        &i2cStartStopCharacteristic,
                                        &i2cWriteCharacteristic,
                                        &i2cReadParamCharacteristic,
                                        &i2cReadCharacteristic,
#endif
#if USE_KONASHI_UART
                                        &uartConfigCharacteristic,
                                        &uartBaudrateCharacteristic,
                                        &uartTxCharacteristic,
                                        &uartRxCharacteristic,
#endif
/*                                        &hardwareRestCharacteristic,
                                        &hardwareLowBatCharacteristic,*/  };
GattService         konashiService(KONASHI_SERVICE_UUID, konashiChars, sizeof(konashiChars) / sizeof(GattCharacteristic *));

// SYSTEM
static char         systemId = 'A';
GattCharacteristic  systemID(GattCharacteristic::UUID_SYSTEM_ID_CHAR, (uint8_t *)systemId, sizeof(systemId), sizeof(systemId),
                           GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);
// MODEL
static char         model[31] = "mbed HRM1017";
GattCharacteristic  modelID(GattCharacteristic::UUID_MODEL_NUMBER_STRING_CHAR, (uint8_t *)model, strlen(model), strlen(model),
                           GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);
// Firmware
static char         fwversion[31] = "1.0";
GattCharacteristic  fwChars(GattCharacteristic::UUID_FIRMWARE_REVISION_STRING_CHAR, (uint8_t *)fwversion, strlen(fwversion), strlen(fwversion),
                           GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);
// Software
static char         swversion[31] = "1.0";
GattCharacteristic  swChars(GattCharacteristic::UUID_SOFTWARE_REVISION_STRING_CHAR, (uint8_t *)swversion, strlen(swversion), strlen(swversion),
                           GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);
// Hardware
static char         hwversion[31] = "1.0";
GattCharacteristic  hwChars(GattCharacteristic::UUID_HARDWARE_REVISION_STRING_CHAR, (uint8_t *)hwversion, strlen(hwversion), strlen(hwversion),
                           GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);
// Manufacturer
static char         vendor[31] = "Test Company Inc.";
GattCharacteristic  vendorChars(GattCharacteristic::UUID_MANUFACTURER_NAME_STRING_CHAR, (uint8_t *)vendor, strlen(vendor), strlen(vendor),
                           GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);
// Serial number
static char         serial[31] = "1234567890";
GattCharacteristic  serialChars(GattCharacteristic::UUID_SERIAL_NUMBER_STRING_CHAR, (uint8_t *)serial, strlen(serial), strlen(serial),
                           GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);

//GattCharacteristic *informationChars[] = {&systemID, &modelID, &serialChars, &fwChars, &hwChars, &swChars, &vendorChars, };
GattCharacteristic *informationChars[] = {&modelID, &fwChars, &hwChars, &swChars, &vendorChars, };
GattService         informationService(GattService::UUID_DEVICE_INFORMATION_SERVICE, informationChars, sizeof(informationChars) / sizeof(GattCharacteristic *));


static uint8_t      batteryLevel = 100;
GattCharacteristic  batteryPercentage(GattCharacteristic::UUID_BATTERY_LEVEL_CHAR, (uint8_t *)batteryLevel, sizeof(batteryLevel), sizeof(batteryLevel),
                           GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
GattCharacteristic *batteryChars[] = {&batteryPercentage };
GattService         batteryService(GattService::UUID_BATTERY_SERVICE, batteryChars, sizeof(batteryChars) / sizeof(GattCharacteristic *));

static Gap::ConnectionParams_t connectionParams;

void onConnectionCallback(const Gap::ConnectionCallbackParams_t *params)
{
    //DEBUG("connected. Got handle %u\r\n", handle);
    led2 = 0;
/* 
    connectionParams.slaveLatency = 1;
    if (ble.updateConnectionParams(handle, &connectionParams) != BLE_ERROR_NONE) {
        DEBUG("failed to update connection paramter\r\n");
    }*/
}
void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *reason)
{
    DEBUG("Disconnected handle %u!\n\r", handle);
    DEBUG("Restarting the advertising process\n\r");
    led2 = 1;
    ble.startAdvertising();
}
static void set_pwm_duty(int ch)
{
//    uint32_t    period;
    uint32_t    duty;
    unsigned    bitmask = 1;

    if(ch>=3){
        return;
    }
    duty=pwm_duty[ch];

    if(pwm_config & (bitmask << ch))
    {
        pwm[ch].pulsewidth_us(duty);
    }
    else
    {
        pwm[ch].pulsewidth_us(0);
     }
 }
static void set_pwm_period(int ch)
{
    uint32_t    period,duty;

    if(ch>=3){
        return;
    }
    period=pwm_period[ch];
    duty=pwm_duty[ch];
    pwm[ch].period_us(period);
    pwm[ch].pulsewidth_us(duty);
}
static void set_pwm_config(uint8_t config)
{
    pwm_config = config;
    for(int i = 0; i < 3 ; i++)
    {
        set_pwm_duty(i);
    }

}
void onDataWritten(const GattWriteCallbackParams *params)
{
    DEBUG("onDataWritten\n\r");
    DEBUG("params->len = %d\n\r",params->len);
    uint16_t bytesRead = params->len;
    /*if (charHandle == pioSettingCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("pioSettingCharacteristic!\n\r");
    } else if (charHandle == pioPullupCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("pioPullupCharacteristic!\n\r");
    } else */ if (params->handle == pioOutputCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("pioOutputCharacteristic!\n\r");
//        uint8_t getPioOut[10];
//        uint16_t bytesRead;
//        ble.readCharacteristicValue(pioOutputCharacteristic.getValueAttribute().getHandle(), getPioOut, &bytesRead);
//        DEBUG("DATA: %d %d\n\r",getPioOut[0],pioOutput[0]);
        DEBUG("DATA: %d %d\n\r",params->data[0],pioOutput[0]);
        if(params->data[0]!=pioOutput[0]) {
            pioOutput[0]=params->data[0];
            ioOut = pioOutput[0];
        }
#if USE_KONASHI_PWM
    } else if (charHandle == pwmConfigCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("pwmConfigCharacteristic!\n\r");
//        uint16_t bytesRead;
//        ble.readCharacteristicValue(pwmConfigCharacteristic.getValueAttribute().getHandle(), pwmConfig, &bytesRead);
        pwmConfig[0]=params->data[0];
        set_pwm_config(pwmConfig[0]);     
    } else if (charHandle == pwmPeriodCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("pwmPeriodCharacteristic!\n\r");
        uint32_t    l;
//        uint16_t bytesRead;
//        ble.readCharacteristicValue(pwmPeriodCharacteristic.getValueAttribute().getHandle(), pwmPeriod, &bytesRead);
        if (bytesRead < sizeof(pwmPeriod)) {
            memcpy(pwmPeriod, params->data, bytesRead);
        }
        int i=pwmPeriod[0];
        l =((uint32_t)pwmPeriod[1])<<24;
        l|=((uint32_t)pwmPeriod[2])<<16;
        l|=((uint32_t)pwmPeriod[3])<< 8;
        l|=((uint32_t)pwmPeriod[4])<< 0;
        if(i<3){
            pwmPeriod[i]=l;
            set_pwm_period(i);
        }
    } else if (charHandle == pwmDutyCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("pwmDutyCharacteristic!\n\r");
        uint32_t    l;
//        uint16_t bytesRead;
//        ble.readCharacteristicValue(pwmDutyCharacteristic.getValueAttribute().getHandle(), pwmDuty, &bytesRead);
        if (bytesRead < sizeof(pwmDuty)) {
            memcpy(pwmDuty, params->data, bytesRead);
        }
        int i=pwmDuty[0];
        l =((uint32_t)pwmDuty[1])<<24;
        l|=((uint32_t)pwmDuty[2])<<16;
        l|=((uint32_t)pwmDuty[3])<< 8;
        l|=((uint32_t)pwmDuty[4])<< 0;
        if(i<3){
            pwm_duty[i]=l;
            set_pwm_duty(i);
        }
#endif  //USE_KONASHI_PWM
/*    } else if (charHandle == analogDriveCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("analogDriveCharacteristic!\n\r");*/
#if USE_KONASHI_I2C
    } else if (params->handle == i2cConfigCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("i2cConfigCharacteristic!\n\r");
//        uint16_t bytesRead;
//        ble.readCharacteristicValue(i2cConfigCharacteristic.getValueAttribute().getHandle(), i2cConfig, &bytesRead);
         //write 0:OFF 1:100kHz 2:400kHz
        if(bytesRead){
            i2cConfig[0] = params->data[0];
            i2c_config=i2cConfig[0];
            if(i2cConfig[0] == 1){
                i2c.frequency(100000);
            } else if(i2cConfig[0] == 2){
                i2c.frequency(400000);
            }
        }       
    } else if (params->handle == i2cStartStopCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("i2cStartStopCharacteristic!\n\r");
//        uint16_t bytesRead;
//        ble.readCharacteristicValue(i2cStartStopCharacteristic.getValueAttribute().getHandle(), i2cStartStop, &bytesRead);
        //write 0:stop 1:start condition 2:repeated start condition
        if(bytesRead){
            i2cStartStop[0] = params->data[0];
            if(i2cStartStop[0] == 0) {
                i2c_repeated = false;
//                i2c.stop();
            } else if(i2cStartStop[0] == 1) {
                i2c_repeated = false;
//                i2c.start();
            } else if(i2cStartStop[0] == 2) {
                i2c_repeated = true;
            } else {
                i2c_repeated = false;
            }
        }
    } else if (params->handle == i2cWriteCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("i2cWriteCharacteristic!\n\r");
//        uint16_t bytesRead;
//        ble.readCharacteristicValue(i2cWriteCharacteristic.getValueAttribute().getHandle(), i2cWrite, &bytesRead);
        if (bytesRead < sizeof(i2cWrite)) {
            memcpy(i2cWrite, params->data, bytesRead);
        }
        //write +0:write length(from next byte) +1:(addr<<1)|(R:1/W:0) +2～:Write bytes
        DEBUG("DATA: %d %d %d\n\r",i2cWrite[0],i2cWrite[1],i2cWrite[2]);
        if(bytesRead>=3){
            i2c.write((int)(i2cWrite[1]),(char *)&i2cWrite[2],(int)i2cWrite[0],i2c_repeated);
        }        
    } else if (params->handle == i2cReadParamCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("i2cReadParamCharacteristic!\n\r");
//        uint16_t bytesRead;
//        ble.readCharacteristicValue(i2cReadParamCharacteristic.getValueAttribute().getHandle(), i2cReadParam, &bytesRead);
        if (bytesRead < sizeof(i2cReadParam)) {
            memcpy(i2cReadParam, params->data, bytesRead);
        }
        //write +0:read length(from next byte) +1:(addr<<1)|(R:1/W:0)
        if(bytesRead>=2){
            i2c_recvlen=i2cReadParam[0];
            if(20>=i2c_recvlen){
                i2c.read((int)(i2cReadParam[1]),(char *)&i2c_recvbuf[0],(int)i2cReadParam[0],i2c_repeated);
                for(int i=0;i<i2c_recvlen;i++){
                    i2cRead[i]=i2c_recvbuf[i];
                }
                ble.updateCharacteristicValue(i2cReadCharacteristic.getValueAttribute().getHandle(), (uint8_t*)&i2cRead, i2c_recvlen);        
            }
        }
//        ble.updateCharacteristicValue(i2cReadCharacteristic.getValueAttribute().getHandle(), (uint8_t*)&i2cRead, i2c_recvlen);        
#endif  //USE_KONASHI_I2C
#if USE_KONASHI_UART
    } else if (params->handle == uartConfigCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("uartConfigCharacteristic!\n\r");
//        uint16_t bytesRead;
//        ble.readCharacteristicValue(uartConfigCharacteristic.getValueAttribute().getHandle(), uartConfig, &bytesRead);
        if (bytesRead < sizeof(uartConfig)) {
            memcpy(uartConfig, params->data, bytesRead);
        }
        if(bytesRead>0){
            if(uartConfig[0]) {
                pc.baud(uart_baudrate*240);
            }
        }        
    } else if (params->handle == uartBaudrateCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("uartBaudrateCharacteristic!\n\r");
//        uint16_t bytesRead;
//        ble.readCharacteristicValue(uartBaudrateCharacteristic.getValueAttribute().getHandle(), uartBaudrate, &bytesRead);
        if (bytesRead < sizeof(uartBaudrate)) {
            memcpy(uartBaudrate, params->data, bytesRead);
        }
        if(bytesRead>=2){
            uart_baudrate=((uint16_t)(uartBaudrate[0]))<<8;
            uart_baudrate+=uartBaudrate[1];
        }        
    } else if (params->handle == uartTxCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("uartTxCharacteristic!\n\r");
//        uint16_t bytesRead;
//        ble.readCharacteristicValue(uartTxCharacteristic.getValueAttribute().getHandle(), uartTx, &bytesRead);
        if (bytesRead < sizeof(uartTx)) {
            memcpy(uartTx, params->data, bytesRead);
        }
        DEBUG("ECHO: %s\n\r", (char *)uartTx);
#if NEED_CONSOLE_OUTPUT
        ble.updateCharacteristicValue(uartRxCharacteristic.getValueAttribute().getHandle(), uartTx, bytesRead);
#else
        pc.putc(uartTx[0]);
#endif  //NEED_CONSOLE_OUTPUT
#endif  //USE_KONASHI_UART
/*    } else if (charHandle == hardwareRestCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("hardwareRestCharacteristic!\n\r");*/
    }     
}
static volatile bool pioInputUpdatesEnable = false;
static volatile bool uartRxUpdatesEnable = false;
void onUpdatesEnabled(uint16_t charHandle)
{
    DEBUG("onUpdatesEnabled handle %u!\n\r", charHandle);
//    led2 = 1;
    if (charHandle == pioInputCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("pioInputCharacteristic!\n\r");
        pioInputUpdatesEnable = true;
#if USE_KONASHI_UART
    } else if (charHandle == uartRxCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("uartRxCharacteristic!\n\r");
        uartRxUpdatesEnable = true;
#endif
    }              
}
void onUpdatesDisabled(uint16_t charHandle)
{
    DEBUG("onUpdatesDisabled handle %u!\n\r", charHandle);
//    led2 = 1;
    if (charHandle == pioInputCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("pioInputCharacteristic!\n\r");
        pioInputUpdatesEnable = false;
#if USE_KONASHI_UART
    } else if (charHandle == uartRxCharacteristic.getValueAttribute().getHandle()) {
        DEBUG("uartRxCharacteristic!\n\r");
        uartRxUpdatesEnable = false;
#endif
    }                     
}
static volatile bool triggerIoPolling = false;
uint8_t ledUpdateCounter = 0;
uint8_t batteryLevelUpdateCounter = 0;
void periodicCallback(void)
{
    /* Note that the periodicCallback() executes in interrupt context, so it is safer to do
     * heavy-weight sensor polling from the main thread. */
    triggerIoPolling = true;
}

int main(void)
{
    led1 = 1;
    led2 = 1;
    set_pwm_config(0);
    ioIn.mode(PullUp);
//    ioIn.mode(PullNone);
//    ioIn.mode(PullDown);
    Ticker ticker;
    ticker.attach(periodicCallback, 0.1);

    DEBUG("Initialising the nRF51822\n\r");
    ble.init();
    ble.onConnection(onConnectionCallback);
    ble.onDisconnection(disconnectionCallback);
    ble.onDataWritten(onDataWritten);
    ble.onUpdatesEnabled(onUpdatesEnabled);
    ble.onUpdatesDisabled(onUpdatesDisabled);
    
//    ble.getPreferredConnectionParams(&connectionParams);

    /* setup advertising */
    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t*)uuid16_list, sizeof(uuid16_list));
    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);

    ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
    ble.startAdvertising();

//    ble.addService(batteryService);
//    ble.addService(informationService);
    ble.addService(konashiService);

    while (true) {
        if (triggerIoPolling) {
            triggerIoPolling = false;
            ledUpdateCounter++;
            if(ledUpdateCounter >= 10) {    //1sec
                ledUpdateCounter = 0;
                led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
//              /* Update the battery measurement */
//              batteryLevel--;
//              if (batteryLevel == 1) {
//                  batteryLevel = 100;
//              }
//              ble.updateCharacteristicValue(batteryPercentage.getHandle(), &batteryLevel, sizeof(batteryLevel));
//              ble.updateCharacteristicValue(hardwareLowBatCharacteristic.getHandle(), &batteryLevel, sizeof(batteryLevel));
            }
            if(ble.getGapState().connected) { 
                /* Do blocking calls or whatever is necessary for sensor polling. */
                if(pioInputUpdatesEnable) {
                    uint8_t io = ioIn;
                    if(pioInput[0] != io) {
                        pioInput[0] = io;
                        ble.updateCharacteristicValue(pioInputCharacteristic.getValueAttribute().getHandle(), pioInput, 1);
                    }
                }
#if USE_KONASHI_UART
#if !NEED_CONSOLE_OUTPUT
                if(uartRxUpdatesEnable) {
                    if(pc.readable()) {
                        char data;
                        data = pc.getc();
                        ble.updateCharacteristicValue(uartRxCharacteristic.getValueAttribute().getHandle(), (uint8_t*)&data, 1);
                    }
                }
#endif
#endif
            }
        }
//        else {
            ble.waitForEvent();
//        }
    }
}