File content as of revision 5:3ba4c087853b:

//  Include --------------------------------------------------------------------
#include "mbed.h"
#include "BLE.h"
#include "nrf_ble_gap.h"
#include "DiscoveredCharacteristic.h"
#include "DiscoveredService.h"
#include "UARTService.h"
#include "WatchdogTimer.h"
#include "BufferedSerial.h"

//  Definition -----------------------------------------------------------------
#define     NUM_ONCE            20
#define     BFSIZE              (NUM_ONCE+4)

#define SOFT_DEVICE_FATHER_HANDLE   3

#define VER     12   // ソフトバージョン（３桁　1.00 ->  100） // Ver12 20190314

//  Object ---------------------------------------------------------------------
BLE&        ble_uart = BLE::Instance();
//Serial      pc(P0_1, P0_3, 115200);     // DEBUG BOARD
//Serial      pc(P0_9, P0_11, 9600);  // NEW BOARD
BufferedSerial      pc(P0_9, P0_11, 256);  // NEW BOARD
Ticker      main_timer;      // メインタイ216

DigitalOut led(P0_6);

//DigitalOut state(P0_24);    // DEBUG BOARD
DigitalOut state(P0_3);   // NEW BOARD

//  ROM / Constant data --------------------------------------------------------
Gap::Address_t    mac_board_0   = {0x72, 0x00, 0xD4, 0x34, 0x0B, 0xF7};

//  RAM ------------------------------------------------------------------------
Gap::Handle_t   connectionHandle        = 0xFFFF;
DiscoveredCharacteristic uartTXCharacteristic;
DiscoveredCharacteristic uartRXCharacteristic;
bool            foundUartRXCharacteristic = false;
bool            connected2server        = false;
bool            connected2server_d      = false;
bool            connection_tx           = false;
bool            connection_rx           = false;
UARTService *   uartServicePtr          = NULL;
Gap::Address_t  my_mac;
int             my_board_index          = -1;
bool            received_uart_dat       = false;
int8_t          uart_buffer[BFSIZE];
uint8_t         uart_bf_len;
volatile bool   rx_isr_busy             = false;

unsigned long mac_1;
unsigned long mac_2;
unsigned long mac_3;

Mutex           bletx_mutex;

int connect_check = 0;

#define SIO_BUF_SIZE    256     /* SIO ﾘﾝｸﾞﾊﾞｯﾌｧｰｻｲｽﾞ (1K byte) */

typedef struct {
    unsigned short      data_cnt;               /* ﾘﾝｸﾞ･ﾊﾞｯﾌｧ ﾃﾞｰﾀｶｳﾝﾄ */
    unsigned short      rp;                     /* ﾘﾝｸﾞ･ﾊﾞｯﾌｧ Read ﾎﾟｲﾝﾀ */
    unsigned short      wp;                     /* ﾘﾝｸﾞ･ﾊﾞｯﾌｧ Write ﾎﾟｲﾝﾀ */
    unsigned char       buf[SIO_BUF_SIZE];      /* ﾘﾝｸﾞ･ﾊﾞｯﾌｧ */
} ring_cnt_t;                                   /* ﾘﾝｸﾞ･ﾊﾞｯﾌｧ 制御用構造体 */

ring_cnt_t pc_uart_buf;     // 通信用リングバッファ
ring_cnt_t pc_cmd_buf;      // 通信用コマンドバッファ

ring_cnt_t ble_uart_buf;       // 通信用リングバッファ
ring_cnt_t ble_cmd_buf;        // 通信用コマンドバッファ

int timer_cnt;
int timer_cnt_d;
int read_cnt = 0;

bool start_flag = false;

//  Function prototypes --------------------------------------------------------
//      BLE
void advertisementCallback(const Gap::AdvertisementCallbackParams_t *);
void serviceDiscoveryCallback(const DiscoveredService *);
void characteristicDiscoveryCallback(const DiscoveredCharacteristic *);
void discoveryTerminationCallback(Gap::Handle_t );
void onReceivedDataFromDeviceCallback(const GattHVXCallbackParams *);
void connectionCallback(const Gap::ConnectionCallbackParams_t *);
void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *);
//      Pre-check
bool mac_equals(const Gap::Address_t, const Gap::Address_t);
int  get_board_index(const Gap::Address_t);

void uart_init();
void main_timer_proc();
unsigned int atow(char *str,unsigned short leng);
unsigned long atoh(char *str,unsigned short leng);
void ble_rs232c_cmd();
void pc_rx();
void pc_rs232c_cmd();
void ble_write(char* dat);

void led_p( int n )
{
    for( int i=0; i<n; i++ )
    {
        led = 1;
        led = 0;
    }
    
    led = 0;
    led = 0;
}

//------------------------------------------------------------------------------
//  Control Program
//------------------------------------------------------------------------------
int main(void)
{
    uart_init();

    //NVIC_SetPriority(UART0_IRQn,1);
    //NVIC_SetPriority(RADIO_IRQn,1);
    //NVIC_SetPriority(TIMER0_IRQn,1);
    //NVIC_SetPriority(TIMER1_IRQn,1);
    //NVIC_SetPriority(TIMER2_IRQn,1);

    //pc.attach( pc_rx, Serial::RxIrq );
    
    // メインタイマー設定(1ms)
    //main_timer.attach(&main_timer_proc, 0.001);
        
    // opening message
    //pc.printf("UART Communication / Client(Central) side\r\n");
    //pc.printf("  need Server module (run BLE_Uart_Server program)\r\n");
    // Mixed role **************************************************************
    ble_uart.init();
    ble_uart.gap().onConnection(connectionCallback);
    ble_uart.gap().onDisconnection(disconnectionCallback);
    // Client(Central) role ****************************************************
    ble_uart.gattClient().onHVX(onReceivedDataFromDeviceCallback);
    ble_uart.gap().setScanParams(500, 450);
    //ble_uart.gap().startScan(advertisementCallback);
    
//    WatchdogTimer  wdt( 3 );
    
    uint32_t res = sd_ble_gap_tx_power_set( 4 );
    
    //pc.printf( "tx_power_set_res = %d\r", res );
    
    int cnt = 0;  
    
    Timer tt;
    int msec;
    
    connect_check = 0;
    start_flag = false;
     
    int wait_cnt = 0;
        
    while(true) 
    {        
//        wdt.kick();
        
        //led = !led;
       
        // allow notifications from Server(Peripheral)
        if (foundUartRXCharacteristic &&
                !ble_uart.gattClient().isServiceDiscoveryActive()) {
                    
            //pc.printf("--allow notifications-S\r");
            // need to do the following only once
            foundUartRXCharacteristic = false;
            uint16_t value = BLE_HVX_NOTIFICATION;
            ble_uart.gattClient().write(
                GattClient::GATT_OP_WRITE_REQ,
                connectionHandle,
                uartRXCharacteristic.getValueHandle() + 1,
                sizeof(uint16_t),
                reinterpret_cast<const uint8_t *>(&value)
            );
//led_p(6);
//pc.printf( "notifications\r" );            
            //pc.printf("--allow notifications-E\r");
                       
            connected2server = true;
        }

        // タイマーカウント更新時（10ms以上経過している）
        //if( timer_cnt != timer_cnt_d )
        //{
            //timer_cnt_d = timer_cnt;
            //pc.printf("%d\r", cnt);
 #if 0
            //if( connected2server == true )
            //{
                if( ( cnt % 2000 ) == 0 )
                {               
                    int8_t rssi;
                    sd_ble_gap_rssi_get( connectionHandle, &rssi );
                    
                    pc.printf( "rssi = %d\r", rssi );
                }
            //}        
 #endif       
            if( ( connected2server_d == false ) && ( connected2server == true ) )
            {
                connect_check = 1;             
            }
            else if( ( connected2server_d == true ) && ( connected2server == false ) )
            {
                state = connected2server;
                connect_check = 0;
            }
            
            connected2server_d = connected2server;
            
            if( connect_check == 1 )
            {
                tt.stop();
                tt.reset();
                tt.start();

                ble_write( "STOP\r" );
                connect_check = 2;               
            }
            else if( connect_check == 2 )
            {
                msec = tt.read_ms();

                if( msec >= 500 )
                {
                    connect_check = 1;
                }                
            }
            else if( connect_check == 3 )
            {
                msec = tt.read_ms();

                if( msec >= 500 )
                {
                    connect_check = 0;
                    
                    state = connected2server;
                }                        
            }            
           
            ble_rs232c_cmd();
          
            pc_rs232c_cmd();
           
            cnt++;
            
            //led = ( cnt >> 8 ) & 0x0001;         
            
            //led = !led;
            
            //wait(0.002);
        //}

        ble_uart.waitForEvent();
       
    }
}

void onReceivedDataFromDeviceCallback(const GattHVXCallbackParams *params)
{
    //pc.printf("onReceivedDataFromDeviceCallback\r");
    
    if (params->type == BLE_HVX_NOTIFICATION) {
        if ((params->handle
                == uartRXCharacteristic.getValueHandle()) && (params->len > 0)) {
            
            if( params->len <= BFSIZE )
            {
                uart_bf_len = params->len;
     
                //strcpy((char *)uart_buffer, (char *)params->data);
                memcpy((char *)uart_buffer, (char *)params->data, uart_bf_len );
                received_uart_dat = true;         
            
                //ble_rs232c_cmd();
      
                for( int i=0; i<uart_bf_len; i++ )
                {
                    //pc.putc(uart_buffer[i]);
                    ble_uart_buf.buf[ ble_uart_buf.wp ] = uart_buffer[i];
                    ble_uart_buf.data_cnt++;
            
                    if( ble_uart_buf.wp == SIO_BUF_SIZE-1 )
                    {
                        ble_uart_buf.wp = 0;
                    }
                    else
                    {
                        ble_uart_buf.wp++;
                    }
                }
                uart_bf_len = 0;
            }
            else
            {
                
            }   
        }
    }
}

bool mac_equals(const Gap::Address_t mac_1, const Gap::Address_t mac_2)
{
    for (int i = 0; i < 6; i++) {

    }

    for (int i = 0; i < 6; i++) {
        if (mac_1[i] != mac_2[i]) {
            return false;
        } else {
        }
    }
    return true;
}

int get_board_index(const Gap::Address_t mac)
{
    if (mac_equals(mac, mac_board_0)) {
        return 0;
    }
#if 0
    if (mac_equals(mac, mac_board_1)) {
        return 1;
    }
    if (mac_equals(mac, mac_board_2)) {
        return 2;
    }
    if (mac_equals(mac, mac_board_3)) {
        return 3;
    }
    if (mac_equals(mac, mac_board_4)) {
        return 4;
    }
#endif
    return -1;
}

// Client(Central) role ********************************************************
void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params)
{
    // connections
    int peer_board_index = get_board_index(params->peerAddr);
    if (peer_board_index != -1) {
        //pc.printf("");
        //pc.printf(
        //    "adv peerAddr [%02x %02x %02x %02x %02x %02x]\r\n",
        //    params->peerAddr[5], params->peerAddr[4], params->peerAddr[3],
        //    params->peerAddr[2], params->peerAddr[1], params->peerAddr[0]
        //);
        //pc.printf(
        //    "rssi=%+4d, isScanResponse %u, AdvertisementType %u\r\n",
        //    params->rssi, params->isScanResponse, params->type
        //);
        ble_uart.gap().connect(
            params->peerAddr, Gap::ADDR_TYPE_RANDOM_STATIC, NULL, NULL);
    }
}

void serviceDiscoveryCallback(const DiscoveredService *service)
{
    //pc.printf("serviceDiscoveryCallback\r");
    
    if (service->getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
        
    } else {
        const uint8_t *longUUIDBytes = service->getUUID().getBaseUUID();
        for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
            
        }
    }
    
    //pc.printf("--serviceDiscoveryCallback\r");
}

void characteristicDiscoveryCallback(
    const DiscoveredCharacteristic *characteristicP)
{ 
    if (characteristicP->getUUID().getShortUUID()
            == UARTServiceTXCharacteristicShortUUID) {
        uartTXCharacteristic = *characteristicP;
        connection_tx = true;
//led_p(4);
//pc.printf( "connection_tx\r" );      
        //pc.printf("--connection_tx = true\r");
    } else if (characteristicP->getUUID().getShortUUID()
               == UARTServiceRXCharacteristicShortUUID) {
        uartRXCharacteristic = *characteristicP;
        foundUartRXCharacteristic = true;
        connection_rx = true;
//led_p(5);
//pc.printf( "connection_rx\r" );         
        //pc.printf("--connection_rx = true\r");
    }
}

void discoveryTerminationCallback(Gap::Handle_t connectionHandle)
{
    //pc.printf("discoveryTerminationCallback\r");
}

// Mixed role ******************************************************************
void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
{
    if (params->role == Gap::CENTRAL) {

//pc.printf( "connected\r" );
        //pc.printf("connected as Client(Central) (handle = %d)\r\n\r",
        //          params->handle);
        //connected2server = true;
        connectionHandle = params->handle;
        ble_uart.gattClient().onServiceDiscoveryTermination(
            discoveryTerminationCallback);
        ble_uart.gattClient().launchServiceDiscovery(
            params->handle,
            serviceDiscoveryCallback,
            characteristicDiscoveryCallback
        );
    }
    //pc.printf(
    //    "Client(Central/Myself)       %02x:%02x:%02x:%02x:%02x:%02x\r\n",
    //    params->ownAddr[5], params->ownAddr[4], params->ownAddr[3],
    //    params->ownAddr[2], params->ownAddr[1], params->ownAddr[0]
    //);
    //pc.printf(
    //    "Connected Server(Peripheral) %02x:%02x:%02x:%02x:%02x:%02x\r\n",
    //    params->peerAddr[5], params->peerAddr[4], params->peerAddr[3],
    //    params->peerAddr[2], params->peerAddr[1], params->peerAddr[0]
    //);
    
//led_p(3);

    uint32_t res = sd_ble_gap_rssi_start( connectionHandle, 0, 0 );

    //pc.printf( "rssi_start_res = %d\r", res );
}


void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
//pc.printf( "disconnected\r" );
    //pc.printf(" -> disconnected\r\n", params->handle);
    connected2server = false;
//    connection_1st = false;
    connection_tx = false;
    connection_rx = false;
    
#if 0
    if (params->handle == SOFT_DEVICE_FATHER_HANDLE) {
        ble_uart.startAdvertising();                    // restart advertising
    } else {
        ble_uart.gap().startScan(advertisementCallback);// restart scan
    }
#endif
}

/****************************************************************************/
/*  関数名 ： uart_init                                                 */
/*  概要  ： 通信初期化                                                 */
/*  作成者 ： JPMS H.Harada                                             */
/*  作成日 ： 2017.08.30                                                    */
/****************************************************************************/
void uart_init()
{
    int k;

    // 初期化時のごみを出力
//    while( pc.readable() )
//    {
//        pc.getc();
//    }

    // UART バッファ初期化 ///////////////////////////////////////////////
    pc_uart_buf.data_cnt = 0;
    pc_uart_buf.rp = 0;
    pc_uart_buf.wp = 0;

    ble_uart_buf.data_cnt = 0;
    ble_uart_buf.rp = 0;
    ble_uart_buf.wp = 0;

    for(k=0;k<SIO_BUF_SIZE;k++) {
        pc_uart_buf.buf[k] = 0;
        ble_uart_buf.buf[k] = 0;
    }

    // コマンド バッファ初期化 ///////////////////////////////////////////////
    pc_cmd_buf.data_cnt = 0;
    pc_cmd_buf.rp = 0;
    pc_cmd_buf.wp = 0;

    ble_cmd_buf.data_cnt = 0;
    ble_cmd_buf.rp = 0;
    ble_cmd_buf.wp = 0;

    for(k=0;k<SIO_BUF_SIZE;k++) {
        pc_cmd_buf.buf[k] = 0;
        ble_cmd_buf.buf[k] = 0;
    }

    /////////////////////////////////////////////////////////////////
}

/****************************************************************************/
/*  関数名 ： main_timer_proc                                               */
/*  概要   : メインタイマ処理                                                      */
/*  作成者 ： JPMS H.Harada                                                 */
/*  作成日 ： 2018.02.22                                                        */
/****************************************************************************/
void main_timer_proc()
{

     timer_cnt++;       // タイマカウントを加算

}

/****************************************************************************/
/*  関数名 ： atow                                                           */
/*  概要   :　アスキー変換処理                                                 */
/*  作成者 ： JPMS H.Harada                                                  */
/*  作成日 ： 2017.08.30                                                     */
/****************************************************************************/
unsigned int atow(char *str,unsigned short leng)
{
    register unsigned short i;
    unsigned int ret_data = 0;

    /*---------------- データー長エラー -----------------*/
//  if(leng >= 11)
//  {
//      ret_data = ATOW_ERROR;          /* ﾃﾞｰﾀｴﾗｰ */
//  }
    /*---------------- データー長 ＯＫ ------------------*/
//  else
//  {
        i = leng;

        while(i)
        {
            ret_data *= 10;             /* 10倍 */

            /*------ データ '０' ～ '９' 範囲内 -----*/
            if( (*str >= '0') && (*str <= '9') )
            {
                ret_data += (*str) - '0';
                str++;
            }
            /*------ データ '０' ～ '９' 範囲外 -----*/
            else
            {
//              ret_data = ATOW_ERROR;      /* ﾃﾞｰﾀｴﾗｰ */
                break;
            }
            i--;
        }
        /*-------------- データオバーフロー ----------*/
//      if(ret_data & 0x8000)
//      {
//          ret_data = ATOW_ERROR;          /* ﾃﾞｰﾀｴﾗｰ */
//      }
//  }
    return(ret_data);
}

/****************************************************************************/
/*  関数名 ： atoh                                                           */
/*  概要      ： アスキー変換処理                                              */
/*  作成者 ： JPMS H.Harada                                                  */
/*  作成日 ： 2017.08.30                                                     */
/****************************************************************************/
unsigned long atoh(char *str,unsigned short leng)
{
    register unsigned short i;
    unsigned long ret_data = 0;

    /*---------------- データー長エラー -----------------*/
//  if(leng >= 11)
//  {
//      ret_data = ATOW_ERROR;          /* ﾃﾞｰﾀｴﾗｰ */
//  }
    /*---------------- データー長 ＯＫ ------------------*/
//  else
//  {
        i = leng;

        while(i)
        {
            ret_data *= 16;             /* 16倍 */

            /*------ データ '０' ～ '９' 範囲内 -----*/
            if( (*str >= '0') && (*str <= '9') )
            {
                ret_data += (*str) - '0';
                str++;
            }
            /*------ データ 'Ａ' ～ 'Ｆ' 範囲内 -----*/
            else if( (*str >= 'A') && (*str <= 'F') )
            {
                ret_data += (*str) - 'A' + 10;
                str++;
            }
            /*------ データ '０' ～ '９' 範囲外 -----*/
            else
            {
//              ret_data = ATOW_ERROR;      /* ﾃﾞｰﾀｴﾗｰ */
                break;
            }
            i--;
        }
        /*-------------- データオバーフロー ----------*/
//      if(ret_data & 0x8000)
//      {
//          ret_data = ATOW_ERROR;          /* ﾃﾞｰﾀｴﾗｰ */
//      }
//  }
    return(ret_data);
}

#define CMD_NUM_BLE     19

char ble_str[32][10];
char ble_str2[CMD_NUM_BLE][8] = {   "START  ",     // 0
                                    "STOP   ",     // 1
                                    "       ",     // 2
                                    "ISETL  ",     // 3
                                    "ISETR  ",     // 4
                                    "       ",     // 5
                                    "TC     ",     // 6
                                    "TS     ",     // 7
                                    "IDGET  ",     // 8
                                    "P      ",     // 9
                                    "U      ",     // 10
                                    "RSSI   ",     // 11
                                    "M      ",     // 12
                                    "X      ",     // 13
                                    "PAUSE  ",     // 14
                                    "REG09  ",     // 15    Ver12
                                    "REG0A  ",     // 16    Ver12
                                    "REG0B  ",     // 17    Ver12
                                    "ICMR   "};    // 18    Ver12

/****************************************************************************/
/*  関数名 ： rs232c_cmd                                                     */
/*  概要   ： 通信コマンド処理                                                 */
/*  作成者 ： JPMS H.Harada                                                  */
/*  作成日 ： 2017.08.30                                                     */
/****************************************************************************/
void ble_rs232c_cmd()
{
    unsigned int c, i, j;
    int rdat;

#if 0

    for( int i=0; i<uart_bf_len; i++ )
    {
        //pc.putc(uart_buffer[i]);
        ble_uart_buf.buf[ ble_uart_buf.wp ] = uart_buffer[i];
        ble_uart_buf.data_cnt++;

        if( ble_uart_buf.wp == SIO_BUF_SIZE-1 )
        {
            ble_uart_buf.wp = 0;
        }
        else
        {
            ble_uart_buf.wp++;
        }
    }

    uart_bf_len = 0;   

#endif 
    
    if( ble_uart_buf.data_cnt > 0 )
    {                 
        unsigned int len = ble_uart_buf.data_cnt;
        
        for( c=0; c<len; c++ )
        {
            //NVIC_DisableIRQ(RADIO_IRQn);   // 割り込み禁止
                
            rdat = ble_uart_buf.buf[ble_uart_buf.rp];
            ble_uart_buf.data_cnt--;

            if( ble_uart_buf.rp == SIO_BUF_SIZE-1 )
            {
                ble_uart_buf.rp = 0;
            }
            else
            {
                ble_uart_buf.rp++;
            }

            //NVIC_EnableIRQ(RADIO_IRQn);    // 割り込み許可

            if( rdat != '\n' )
            {
                ble_cmd_buf.buf[ble_cmd_buf.data_cnt++] = rdat;
            }

            if (rdat == '\r')
            {
                int k = 0;
                int l = 0;
                int p = 0;
                
                for (i=0;i<ble_cmd_buf.data_cnt;i++)
                {
                    if( ( ( ble_cmd_buf.buf[i] == ' ' ) && ( p > 0 ) ) || 
                          ( ble_cmd_buf.buf[i] == '\r') )
                    {
                        ble_str[k++][l] = '\0';
                        l = 0;
                        p = 0;
                    }
                    else
                    {
                        p++;
                        ble_str[k][l++] = ble_cmd_buf.buf[i];
                    }
                }

                if( ble_cmd_buf.data_cnt == 1 )
                {
                    k = 0;
                }

                ble_cmd_buf.data_cnt = 0;
                ble_cmd_buf.buf[0] = '\0';

                if (k > 0)
                {
                    int ret;
                    int cmd_no = 0xFF;

                    for( j=0; j<CMD_NUM_BLE; j++ )
                    {
                        ret = 1;

                        if( strlen(ble_str[0]) > 0 )
                        {
                            for( i=0; i<strlen(ble_str[0]); i++ )
                            {
                                if( ble_str[0][i] != ble_str2[j][i] )
                                {
                                    ret = 0;
                                }
                            }

                            if( ret )
                            {
                                cmd_no = j;
                                break;
                            }
                        }
                    }

                    // コマンド START
                    if( cmd_no == 0 )
                    {
                        start_flag = true;
                        pc.printf( "START\r" );
                    }
                    // コマンド STOP
                    else if( cmd_no == 1 )
                    {
                        if( connect_check == 0 )
                        {
                            pc.printf( "STOP\r" );
                        }
                        else
                        {
                            connect_check = 3;
                        }
                    }
                    // コマンド
                    else if( cmd_no == 2 )
                    {

                    }
                    // コマンド ISETL
                    else if( cmd_no == 3 )
                    {
                        pc.printf( "ISETL\r" );
                    }
                    // コマンド ISETR
                    else if( cmd_no == 4 )
                    {
                        pc.printf( "ISETR\r" );
                    }
                    // コマンド
                    else if( cmd_no == 5 )
                    {

                    }
                    // コマンド TC
                    else if( cmd_no == 6 )
                    {
                        pc.printf( "TC\r" );
                    }
                    // コマンド TS
                    else if( cmd_no == 7 )
                    {
                        pc.printf( "TS\r" );
                    }
                    // コマンド IDGET
                    else if( cmd_no == 8 )
                    {
                        pc.printf( "IDGET\r" );
                    }
                    // コマンド P
                    else if( cmd_no == 9 )
                    {
                        int data_cnt     = atow(ble_str[1], strlen(ble_str[1]));
                        int left         = atow(ble_str[2], strlen(ble_str[2]));
                        int right        = atow(ble_str[3], strlen(ble_str[3]));
                        
                        led = 1;
                        
                        int8_t rssi;
                        sd_ble_gap_rssi_get( connectionHandle, &rssi );
                        
                        if( rssi < 0 )
                        {
                            if( rssi < -99 )
                            {
                                rssi = -99;
                            }
                            
                            rssi += 100;
                        }
                        else
                        {
                            rssi = 0;
                        }                     
                        
                        pc.printf( "POINT %d %d %d %d\r", data_cnt, left, right, rssi );
                                            
                        led = 0;
                        
                        //wait(0.001);                     
                    }
                    // コマンド U
                    else if( cmd_no == 10 )
                    {
                        unsigned int high = atoh(ble_str[1], strlen(ble_str[1]));
                        unsigned int low  = atoh(ble_str[2], strlen(ble_str[2]));

                        pc.printf( "UID %08X %08X\r\n", high, low );
                    }
                    // コマンド RSSI
                    else if( cmd_no == 11 )
                    {
                        int8_t rssi;
                        sd_ble_gap_rssi_get( connectionHandle, &rssi );
                        
                        if( rssi < 0 )
                        {
                            if( rssi < -99 )
                            {
                                rssi = -99;
                            }
                            
                            rssi += 100;
                        }
                        else
                        {
                            rssi = 0;
                        }
                        
                        pc.printf( "RSSI %d\r\n", rssi );                  
                    }
                    // コマンド MES
                    else if( cmd_no == 12 )
                    {
                        unsigned int level = atow(ble_str[1], strlen(ble_str[1]));
                        unsigned long uid_L  = atoh(ble_str[2], strlen(ble_str[2]));
                        
                        pc.printf( "MES %d %08X\r\n", level, uid_L );
                    }
                    // コマンド
                    else if( cmd_no == 13 )
                    {
                        unsigned int high = atoh(ble_str[1], strlen(ble_str[1]));
                        unsigned int low  = atoh(ble_str[2], strlen(ble_str[2]));

                        pc.printf( "MMR %08X %08X\r\n", high, low );
                    }
                    // コマンド PAUSE
                    else if( cmd_no == 14 )
                    {
                        if( connect_check == 0 )
                        {
                            pc.printf( "PAUSE\r" );
                        }
                        else
                        {
                            connect_check = 3;
                        }
                    }
                    // コマンド REG09  Ver12
                    else if( cmd_no == 15 )
                    {
                        pc.printf( "REG09\r" );
                    }
                    // コマンド REG0A  Ver12
                    else if( cmd_no == 16 )
                    {
                        pc.printf( "REG0A\r" );
                    }
                    // コマンド REG0B  Ver12
                    else if( cmd_no == 17 )
                    {
                        pc.printf( "REG0B\r" );
                    }
                    // コマンド ICMR  Ver12
                    else if( cmd_no == 18 )
                    {
                        unsigned int adr = atow(ble_str[1], strlen(ble_str[1]));
                        unsigned int dat = atoh(ble_str[2], strlen(ble_str[2]));
                        
                        pc.printf( "ICMR %d %08X\r", adr, dat );                                         
                    }
                    else
                    {
                        //pc.printf("CMD_ERR\r\n");
                    }
                }
            }
        }
    }
}

void pc_rx()
{
    //NVIC_DisableIRQ(UART0_IRQn);   // 割り込み禁止

    pc_uart_buf.buf[pc_uart_buf.wp] = pc.getc();
    pc_uart_buf.data_cnt++;
    
    if( pc_uart_buf.wp == SIO_BUF_SIZE-1 )
    {
        pc_uart_buf.wp = 0;
    }
    else
    {
        pc_uart_buf.wp++;
    }

    //NVIC_EnableIRQ(UART0_IRQn);    // 割り込み許可
}

#define CMD_NUM     21

char pc_str[16][10];
char pc_str2[CMD_NUM][8] = { "START  ",     // 0
                             "STOP   ",     // 1
                             "VER    ",     // 2
                             "ISETL  ",     // 3
                             "ISETR  ",     // 4
                             "CONNECT",     // 5
                             "DISC   ",     // 6
                             "MMR    ",     // 7
                             "TC     ",     // 8
                             "MES    ",     // 9
                             "IDGET  ",     // 10
                             "RSSI   ",     // 11
                             "RES    ",     // 12
                             "READ   ",     // 13
                             "PRES   ",     // 14
                             "TS     ",     // 15
                             "PAUSE  ",     // 16
                             "REG09  ",     // 17   Ver12
                             "REG0A  ",     // 18   Ver12
                             "REG0B  ",     // 19   Ver12
                             "ICMR   "};    // 20   Ver12

/****************************************************************************/
/*  関数名 ： rs232c_cmd                                                     */
/*  概要      ： 通信コマンド処理                                              */
/*  作成者 ： JPMS H.Harada                                                  */
/*  作成日 ： 2017.08.30                                                     */
/****************************************************************************/
void pc_rs232c_cmd()
{
    unsigned int c, i, j;
    int rdat;

#if 1
    while( pc.readable() )
    {
        pc_uart_buf.buf[pc_uart_buf.wp] = pc.getc();
        pc_uart_buf.data_cnt++;
 
        //pc.putc(pc_uart_buf.buf[pc_uart_buf.wp]);
        
        if( pc_uart_buf.wp == SIO_BUF_SIZE-1 )
        {
            pc_uart_buf.wp = 0;
        }
        else
        {
            pc_uart_buf.wp++;
        }
    }
#endif

    if( pc_uart_buf.data_cnt > 0 )
    {   
        unsigned int len = pc_uart_buf.data_cnt;
        
        for( c=0; c<len; c++ )
        {
            //NVIC_DisableIRQ(UART0_IRQn);   // 割り込み禁止
            
            rdat = pc_uart_buf.buf[pc_uart_buf.rp];
            pc_uart_buf.data_cnt--;

            if( pc_uart_buf.rp == SIO_BUF_SIZE-1 )
            {
                pc_uart_buf.rp = 0;
            }
            else
            {
                pc_uart_buf.rp++;
            }

            //NVIC_EnableIRQ(UART0_IRQn);    // 割り込み許可
            
            if( rdat != '\n' )
            {
                pc_cmd_buf.buf[pc_cmd_buf.data_cnt++] = rdat;
            }

            if( rdat == '\r' )
            {
                int k = 0;
                int l = 0;
                int p = 0;

                for( i=0;i<pc_cmd_buf.data_cnt;i++ )
                {
                    if( ( ( pc_cmd_buf.buf[i] == ' ' ) && ( p > 0 ) ) || 
                          ( pc_cmd_buf.buf[i] == '\r') )
                    {
                        pc_str[k++][l] = '\0';
                        l = 0;
                        p = 0;
                    }
                    else
                    {
                        p++;
                        pc_str[k][l++] = pc_cmd_buf.buf[i];
                    }
                }

                if( pc_cmd_buf.data_cnt == 1 )
                {
                    k = 0;
                }

                pc_cmd_buf.data_cnt = 0;
                pc_cmd_buf.buf[0] = '\0';

                if (k > 0)
                {
                    int ret;
                    int cmd_no = 0xFF;

                    for( j=0; j<CMD_NUM; j++ )
                    {
                        ret = 1;

                        if( strlen(pc_str[0]) > 0 )
                        {
                            for( i=0; i<strlen(pc_str[0]); i++ )
                            {
                                if( pc_str[0][i] != pc_str2[j][i] )
                                {
                                    ret = 0;
                                }
                            }

                            if( ret )
                            {
                                cmd_no = j;
                                break;
                            }
                        }
                    }

                    // コマンド START
                    if (cmd_no == 0)
                    {
                        ble_write( "START\r" );

                        //pc.printf( "START OK\r\n" );
                    }
                    // コマンド STOP
                    else if( cmd_no == 1 )
                    {
                        ble_write( "STOP\r" );
                    }
                    // コマンド VER
                    else if( cmd_no == 2 )
                    {
                        pc.printf( "VER %03d\r", VER );
                    }
                    // コマンド ISETL
                    else if( cmd_no == 3 )
                    {
                        int no = atow(pc_str[1], strlen(pc_str[1]));
                        int id_LH = atoh(pc_str[2], strlen(pc_str[2]));
                        int id_LL = atoh(pc_str[3], strlen(pc_str[3]));

                        char buf[BFSIZE];
                        
                        if( no == 0 )
                        {
                            sprintf( buf, "A %8X %8X\r", id_LH, id_LL );
                        }
                        else if( no == 1 )
                        {
                            sprintf( buf, "B %8X %8X\r", id_LH, id_LL );
                        }
                        else if( no == 2 )
                        {
                            sprintf( buf, "C %8X %8X\r", id_LH, id_LL );
                        }
                        else if( no == 3 )
                        {
                            sprintf( buf, "D %8X %8X\r", id_LH, id_LL );
                        }
                           
                        //sprintf( buf, "L %8X %8X\r", id_LH, id_LL );
 
                        ble_write( buf );
                        
                        //pc.printf( "ISETL OK\r\n" );
                    }
                    // コマンド ISETR
                    else if( cmd_no == 4 )
                    {
                        int no = atow(pc_str[1], strlen(pc_str[1]));
                        int id_RH = atoh(pc_str[2], strlen(pc_str[2]));
                        int id_RL = atoh(pc_str[3], strlen(pc_str[3]));

                        char buf[BFSIZE];
                        
                        if( no == 0 )
                        {
                            sprintf( buf, "E %8X %8X\r", id_RH, id_RL );
                        }
                        else if( no == 1 )
                        {
                            sprintf( buf, "F %8X %8X\r", id_RH, id_RL );
                        }
                        else if( no == 2 )
                        {
                            sprintf( buf, "G %8X %8X\r", id_RH, id_RL );
                        }
                        else if( no == 3 )
                        {
                            sprintf( buf, "H %8X %8X\r", id_RH, id_RL );
                        } 
                                                      
                        //sprintf( buf, "R %8X %8X\r", id_RH, id_RL );
 
                        ble_write( buf );
                        
                        //pc.printf( "ISETR OK\r\n" );
                    }
                    // コマンド CONNECT
                    else if( cmd_no == 5 )
                    {
//pc.printf( "CONNECT\r" );
//led_p(1);
                        mac_1 = atoh(pc_str[1], strlen(pc_str[1]));
                        mac_2 = atoh(pc_str[2], strlen(pc_str[2]));
                        mac_3 = atoh(pc_str[3], strlen(pc_str[3]));

                        mac_board_0[0] = ( mac_3 >> 0 ) & 0x00FF;
                        mac_board_0[1] = ( mac_3 >> 8 ) & 0x00FF;
                        mac_board_0[2] = ( mac_2 >> 0 ) & 0x00FF;
                        mac_board_0[3] = ( mac_2 >> 8 ) & 0x00FF;
                        mac_board_0[4] = ( mac_1 >> 0 ) & 0x00FF;
                        mac_board_0[5] = ( mac_1 >> 8 ) & 0x00FF;

                        // CONNECT処理 
                        ble_uart.gap().connect( mac_board_0, Gap::ADDR_TYPE_RANDOM_STATIC, NULL, NULL);
//led_p(2);
                        
     
                    }
                    // コマンド DISC
                    else if( cmd_no == 6 )
                    {
                        // DISC処理
                        Gap::DisconnectionReason_t reason;
                        ble_uart.gap().disconnect( reason );
                    }
                    // コマンド MMR
                    else if( cmd_no == 7 )
                    {
                        int no = atow(pc_str[1], strlen(pc_str[1]));
                        
                        char buf[BFSIZE];  
                        sprintf( buf, "MMR %d\r", no);
                        ble_write( buf );
                    }
                    // コマンド  TC
                    else if( cmd_no == 8 )
                    {
                        unsigned int not_touch_cnt = atow(pc_str[1], strlen(pc_str[1]));
                        unsigned int touch_cnt = atow(pc_str[2], strlen(pc_str[2]));
                        
                        char buf[BFSIZE];                               
                        sprintf( buf, "TC %d %d\r", not_touch_cnt, touch_cnt);
                        ble_write( buf );

                        //pc.printf( "NTC OK\r\n" );
                    }
                    // コマンド MES
                    else if( cmd_no == 9 )
                    {
                        ble_write( "MES\r" );
                    }
                    // コマンド IDGET
                    else if( cmd_no == 10 )
                    {
                        ble_write( "IDGET\r" );
                    }
                    // コマンド RSSI
                    else if( cmd_no == 11 )
                    {
                        ble_write( "RSSI\r" );
                    }
                    // コマンド
                    else if( cmd_no == 12 )
                    {
                        pc.printf( "RES\r" );
                    }
                    // コマンド READ
                    else if( cmd_no == 13 )
                    {
                        //led = 1;
                        ble_write( "READ\r" );
                        
                        //pc.printf( "READ_BLE\r" );
                        //char buf[BFSIZE];                               
                        //sprintf( buf, "READ %d\r", read_cnt++ );
                        //ble_write( buf );

                        //ble_write( "~R\r" );
                    }
                    // コマンド PRES
                    else if( cmd_no == 14 )
                    {
                        ble_write( "PRES\r" );
                    }
                    // コマンド TS
                    else if( cmd_no == 15 )
                    {
                        unsigned int touch_start = atow(pc_str[1], strlen(pc_str[1]));
                        
                        char buf[BFSIZE];                               
                        sprintf( buf, "TS %d\r", touch_start );
                        ble_write( buf );                      
                    }
                    // コマンド PAUSE
                    else if( cmd_no == 16 )
                    {
                        ble_write( "PAUSE\r" );
                    }
                    // コマンド REG09  Ver12
                    else if( cmd_no == 17 )
                    {
                        char reg09 = ( char )( atoh(pc_str[1], strlen(pc_str[1])) );
                        char buf[BFSIZE];                               
                        sprintf( buf, "REG09 %02X\r", reg09 );
                        ble_write( buf );
                    }
                    // コマンド REG0A  Ver12
                    else if( cmd_no == 18 )
                    {
                        char reg0A = ( char )( atoh(pc_str[1], strlen(pc_str[1])) );
                        char buf[BFSIZE];                               
                        sprintf( buf, "REG0A %02X\r", reg0A );
                        ble_write( buf );  
                    }
                    // コマンド REG0B  Ver12
                    else if( cmd_no == 19 )
                    {
                        char reg0B = ( char )( atoh(pc_str[1], strlen(pc_str[1])) );
                        char buf[BFSIZE];                               
                        sprintf( buf, "REG0B %02X\r", reg0B );
                        ble_write( buf );  
                    }
                    // コマンド ICMR  Ver12
                    else if( cmd_no == 20 )
                    {
                        unsigned int adr = atow(pc_str[1], strlen(pc_str[1]));
                        
                        char buf[BFSIZE];                               
                        sprintf( buf, "ICMR %d\r", adr );
                        ble_write( buf );                       
                    }
                    else
                    {
                        pc.printf("CMD_ERR_BLE\r");
                    }
                }
            }
        }
    }
}

void ble_write(char* dat)
{
    uint8_t linebf_irq[BFSIZE];
    
    sprintf( (char *)linebf_irq, "%s", dat);
    
    if( connected2server == true )
    {
        bletx_mutex.lock();
                   
        uartTXCharacteristic.write(NUM_ONCE, linebf_irq);
        
        bletx_mutex.unlock();
    }   
}