SD GATT procedure failed on Central device.

[sne_333]

Hi All,

         I am using NRF52840 in my project and working on SDK 17.0.2. I have one central device and 2 Peripherals. The peripherals are connected to the central properly after power ON. Now let's say I Power OFF the central device for sometime. The peripherals are still kept ON and they keep advertising continuously. Let's say after after sometime I powered ON the central device, it should be able to connect to both the peripherals. What I am observing is sometimes it goes for connection but it gets "SD GATT procedure failed on Central device." and disconnects from that peripheral. The log I observed on central device is as follows:-

00> <error> nrf_ble_gq: SD GATT procedure (2) failed on connection handle 1 with error: 0x0000000D.
00>
00> <info> app: BLE_GATTC_EVT_TIMEOUT
00>
00> <info> app: CENTRAL: Disconnected, handle: 1, reason: 0x16

Also when a peripheral is disconnected it gets hanged and I have to reboot it to connect again.

Is this central device firmware issue or the peripheral device firmware issue? How to solve this issue?

Thanks & Regards,

Snehal Bundele.

[Hieu]

Hi Snehal,

How are you setting up the GATT Writes on your central?

Does this error happen a while after the connection is established, or during connection establishment?

00> <error> nrf_ble_gq: SD GATT procedure (2) failed on connection handle 1 with error: 0x0000000D.

This line says that a GATT Write procedure timed out. Do you have any idea what write procedure is happening here?

Could it be that you initiate the write too early/before the connection is fully established, such as along with the function to connect?

Hieu

[sne_333]

Hi Hieu,

           This error happens during connection establishment. My Gatt initialization is as follows:-

void gatt_evt_handler(nrf_ble_gatt_t * p_gatt, nrf_ble_gatt_evt_t const * p_evt)
{
    if ((m_conn_handle == p_evt->conn_handle) && (p_evt->evt_id == NRF_BLE_GATT_EVT_ATT_MTU_UPDATED))
    {        
        sys_set_peripheral_mtu(p_evt->params.att_mtu_effective - OPCODE_LENGTH - HANDLE_LENGTH);

        NRF_LOG_INFO("Data len is set to 0x%X(%d)", sys_get_peripheral_mtu(), sys_get_peripheral_mtu());
        NRF_LOG_PROCESS();
    }
    NRF_LOG_INFO("ATT MTU exchange completed. central 0x%x peripheral 0x%x",
                  p_gatt->att_mtu_desired_central,
                  p_gatt->att_mtu_desired_periph);
    NRF_LOG_PROCESS();
}



/**@brief Function for initializing the GATT module. */
static void gatt_init(void)
{
    ret_code_t err_code = nrf_ble_gatt_init(&m_gatt, gatt_evt_handler);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_ble_gatt_att_mtu_central_set(&m_gatt, NRF_SDH_BLE_GATT_MAX_MTU_SIZE);
    APP_ERROR_CHECK(err_code);
}

After successful connection the sensors send the device status to the central.

Could it be that you initiate the write too early/before the connection is fully established, such as along with the function to connect?

>> But then why does one sensor connect and the other fails?? If writes are performed too early then the central should get disconnected for both I guess.

Also I have a scan filter in central firmware. It scans from the stored list of names to get connected to the devices nearby. 

Thanks & Regards,

Snehal

[Hieu]

Hi Snehal,

I see, I didn't know the first connection work, and only the second one failed. Does this happen consistently?

I think the issue likely lies at the other part of your code.

I wonder if perhaps your write attempts would always try to write to both peripherals. Meanwhile, right after connection of the second peripheral, there are still setup activities, such as MTU Exchange and Connection Parameters negotiation, and that causes a write attempt to time out.

However, without details, that is just speculation. Of course, full source file would help, but let's look at some key points first before we eventually get there. Could you please provide these pieces of information?

• What the nrf_ble_gq** function call, which results in the error, is, and how is it called?
• As much details as possible about the timing of those functions in relation to the connected event
• Full log

By the way, not relevant to the issue, but the implementation of gatt_evt_handler() that you took from the SDK is flawed because it would log "ATT MTU exchange completed..." even for non-MTU events. Perhaps you could update it after we are done with this issue.

Regards,

Hieu

[sne_333]

Hi Hieu,

          I have attached the main and central file over here.

Fullscreen Gmain.c Download

/**
 
 *
 */

#include <stdint.h>
#include <string.h>
#include "nordic_common.h"
#include "nrf.h"
#include "app_error.h"
#include "ble.h"
#include "ble_hci.h"
#include "ble_srv_common.h"
#include "ble_advdata.h"
#include "ble_advertising.h"
#include "ble_dis.h"
#include "ble_conn_params.h"
#include "nrf_sdh.h"
#include "nrf_sdh_soc.h"
#include "nrf_sdh_ble.h"
#include "nrf_sdh_freertos.h"
#include "app_timer.h"
#include "peer_manager.h"
#include "peer_manager_handler.h"
#include "bsp_btn_ble.h"
#include "FreeRTOS.h"
#include "task.h"
#include "timers.h"
#include "semphr.h"
#include "fds.h"
#include "ble_conn_state.h"
#include "ble_dfu.h"
#include "nrf_pwr_mgmt.h"
#include "nrf_drv_clock.h"
#include "nrf_ble_gatt.h"
#include "nrf_ble_qwr.h"
#include "nrf_power.h"
#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"
#include "rp_900_config.h"
#include "ble_aqrs.h"
#include "task_led_button.h"
#include "task_system.h"
#include "nrf_drv_gpiote.h"
#include "version.h"
#include "device_configuration.h"
#include "ble_aqss_c.h"
#include "ble_aqts_c.h"
#include "le_central.h"

#include "task_wifi_comm.h"
#include "task_repeater.h"
#include "repeater.h"
#include "main.h"
#include "nrf_bootloader_info.h"
#include "device_storage.h"
#include "ble_aqrs_c.h"

#define DEVICE_NAME                         LE_DEVICE_NAME                            /**< Name of device. Will be included in the advertising data. */
#define MANUFACTURER_NAME                   "Test"                   /**< Manufacturer. Will be passed to Device Information Service. */

#define APP_BLE_OBSERVER_PRIO               3                                       /**< Application's BLE observer priority. You shouldn't need to modify this value. */
#define APP_BLE_CONN_CFG_TAG                1                                       /**< A tag identifying the SoftDevice BLE configuration. */

#define APP_ADV_INTERVAL                    300                                     /**< The advertising interval (in units of 0.625 ms. This value corresponds to 187.5 ms). */
#define APP_ADV_DURATION                    0                                       /**< The advertising duration (180 seconds) in units of 10 milliseconds. */

#define RTC_INTERVAL                        1000                                    /**< RTC interval (ms). */

#define MIN_CONN_INTERVAL                   MSEC_TO_UNITS(200, UNIT_1_25_MS)        /**< Minimum acceptable connection interval (0.4 seconds). */
#define MAX_CONN_INTERVAL                   MSEC_TO_UNITS(300, UNIT_1_25_MS)        /**< Maximum acceptable connection interval (0.65 second). */



#define SLAVE_LATENCY                       4                                       /**< Slave latency. */
#define CONN_SUP_TIMEOUT                    MSEC_TO_UNITS(5000, UNIT_10_MS)         /**< Connection supervisory time-out (4 seconds). */

#define FIRST_CONN_PARAMS_UPDATE_DELAY      5000                                    /**< Time from initiating event (connect or start of notification) to first time sd_ble_gap_conn_param_update is called (5 seconds). */
#define NEXT_CONN_PARAMS_UPDATE_DELAY       30000                                   /**< Time between each call to sd_ble_gap_conn_param_update after the first call (30 seconds). */
#define MAX_CONN_PARAMS_UPDATE_COUNT        3                                       /**< Number of attempts before giving up the connection parameter negotiation. */

#define SEC_PARAM_BOND                      0                                       /**< Perform bonding. */
#define SEC_PARAM_MITM                      0                                       /**< Man In The Middle protection not required. */
#define SEC_PARAM_LESC                      0                                       /**< LE Secure Connections not enabled. */
#define SEC_PARAM_KEYPRESS                  0                                       /**< Keypress notifications not enabled. */
#define SEC_PARAM_IO_CAPABILITIES           BLE_GAP_IO_CAPS_NONE                    /**< No I/O capabilities. */
#define SEC_PARAM_OOB                       0                                       /**< Out Of Band data not available. */
#define SEC_PARAM_MIN_KEY_SIZE              16                                      /**< Minimum encryption key size. */
#define SEC_PARAM_MAX_KEY_SIZE              16                                      /**< Maximum encryption key size. */

#define DEAD_BEEF                           0xDEADBEEF                              /**< Value used as error code on stack dump, can be used to identify stack location on stack unwind. */

#define OSTIMER_WAIT_FOR_QUEUE              2                                       /**< Number of ticks to wait for the timer queue to be ready */

static char app_fw_version[32];
static char hw_version[32];

/* Device name is composed of base, spacer, Device ID and a null terminator */
static char device_le_name[MAX_SYS_NAME_LEN];
extern char *array_device_list[5][40];

static char * roles_str[] =
{
    "INVALID_ROLE",
    "PERIPHERAL",
    "CENTRAL",
};

device_config_t device_setting;


char * system_get_device_id(void)
{
    char * val = (char *)&device_setting.device_id;
    return val;
}

int device_exist = 0;
void repeater_load_settings(void)
{
    err_rc_t rc = RESULT_ERROR_FAILURE;
    NRF_LOG_INFO("Load Device Setting.");
    
    rc = device_storage_load(&device_setting);
    APP_ERROR_CHECK(rc);

    snprintf(array_device_list[0],23,"test-%s",device_setting.dev1);
    snprintf(array_device_list[1],23,"test-%s",device_setting.dev2);
    snprintf(array_device_list[2],23,"test-%s",device_setting.dev3);
    snprintf(array_device_list[3],23,"test-%s",device_setting.dev4);
    snprintf(array_device_list[4],23,"test-%s",device_setting.dev5);

    NRF_LOG_INFO("Device Id: %s", device_setting.device_id);
    NRF_LOG_INFO("Dev1: %s, %d", array_device_list[0],strlen(device_setting.dev1));
    NRF_LOG_INFO("Dev2: %s", array_device_list[1]);

    if(strlen(device_setting.dev1) != 0)
    {
        device_exist=1;
    }

  
    NRF_LOG_PROCESS();
}

/**@brief Handler for shutdown preparation.
 *
 * @details During shutdown procedures, this function will be called at a 1 second interval
 *          untill the function returns true. When the function returns true, it means that the
 *          app is ready to reset to DFU mode.
 *
 * @param[in]   event   Power manager event.
 *
 * @retval  True if shutdown is allowed by this power manager handler, otherwise false.
 */
static bool app_shutdown_handler(nrf_pwr_mgmt_evt_t event)
{
    switch (event)
    {
        case NRF_PWR_MGMT_EVT_PREPARE_DFU:
            NRF_LOG_INFO("Power management wants to reset to DFU mode.");
            // YOUR_JOB: Get ready to reset into DFU mode
            //
            // If you aren't finished with any ongoing tasks, return "false" to
            // signal to the system that reset is impossible at this stage.
            //
            // Here is an example using a variable to delay resetting the device.
            //
            // if (!m_ready_for_reset)
            // {
            //      return false;
            // }
            // else
            //{
            //
            //    // Device ready to enter
            //    uint32_t err_code;
            //    err_code = sd_softdevice_disable();
            //    APP_ERROR_CHECK(err_code);
            //    err_code = app_timer_stop_all();
            //    APP_ERROR_CHECK(err_code);
            //}
            break;

        default:
            // YOUR_JOB: Implement any of the other events available from the power management module:
            //      -NRF_PWR_MGMT_EVT_PREPARE_SYSOFF
            //      -NRF_PWR_MGMT_EVT_PREPARE_WAKEUP
            //      -NRF_PWR_MGMT_EVT_PREPARE_RESET
            return true;
    }

    NRF_LOG_INFO("Power management allowed to reset to DFU mode.");
    return true;
}

//lint -esym(528, m_app_shutdown_handler)
/**@brief Register application shutdown handler with priority 0.
 */
NRF_PWR_MGMT_HANDLER_REGISTER(app_shutdown_handler, 0);


static void buttonless_dfu_sdh_state_observer(nrf_sdh_state_evt_t state, void * p_context)
{
    if (state == NRF_SDH_EVT_STATE_DISABLED)
    {
        // Softdevice was disabled before going into reset. Inform bootloader to skip CRC on next boot.
        nrf_power_gpregret2_set(BOOTLOADER_DFU_SKIP_CRC);

        //Go to system off.
        nrf_pwr_mgmt_shutdown(NRF_PWR_MGMT_SHUTDOWN_GOTO_SYSOFF);
    }
}

/* nrf_sdh state observer. */
NRF_SDH_STATE_OBSERVER(m_buttonless_dfu_state_obs, 0) =
{
    .handler = buttonless_dfu_sdh_state_observer,
};


static void advertising_config_get(ble_adv_modes_config_t * p_config)
{
    memset(p_config, 0, sizeof(ble_adv_modes_config_t));

    p_config->ble_adv_fast_enabled  = true;
    p_config->ble_adv_fast_interval = APP_ADV_INTERVAL;
    p_config->ble_adv_fast_timeout  = APP_ADV_DURATION;
}


static void disconnect(uint16_t conn_handle, void * p_context)
{
    UNUSED_PARAMETER(p_context);

    ret_code_t err_code = sd_ble_gap_disconnect(conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
    if (err_code != NRF_SUCCESS)
    {
        NRF_LOG_WARNING("Failed to disconnect connection. Connection handle: %d Error: %d", conn_handle, err_code);
    }
    else
    {
        NRF_LOG_DEBUG("Disconnected connection handle %d", conn_handle);
    }
}

/** @brief The maximum number of peripheral and central links combined. */
#define NRF_BLE_LINK_COUNT              (NRF_SDH_BLE_PERIPHERAL_LINK_COUNT + NRF_SDH_BLE_CENTRAL_LINK_COUNT)

BLE_AQRS_DEF(m_aqrs, NRF_SDH_BLE_TOTAL_LINK_COUNT);                  /**< BLE Aquai Repeater service instance. */
 
NRF_BLE_GATT_DEF(m_gatt);                                           /**< GATT module instance. */
BLE_ADVERTISING_DEF(m_advertising);                                 /**< Advertising module instance. */
NRF_BLE_QWRS_DEF(m_qwr, NRF_SDH_BLE_TOTAL_LINK_COUNT);                      /**< Context for the Queued Write module.*/

typedef struct
{
    bool           is_connected;
    ble_gap_addr_t address;
} conn_peer_t;

static conn_peer_t        m_connected_peers[NRF_BLE_LINK_COUNT];                         /**< Array of connected peers. */

static uint16_t m_conn_handle         = BLE_CONN_HANDLE_INVALID;    /**< Handle of the current connection. */

static ble_uuid_t m_adv_uuids[] =                                   /**< Universally unique service identifiers. */
{
    {BLE_UUID_AQRS_SERVICE, BLE_UUID_TYPE_VENDOR_BEGIN}
};

static TimerHandle_t m_rtc_timer;                               /**< Definition of battery timer. */

#if NRF_LOG_ENABLED
static TaskHandle_t m_logger_thread;                                /**< Definition of Logger thread. */
#endif

/********** new change *****************/

ble_aqrs_t * get_aqrs_service(void)
{
    return &m_aqrs;
}


//rp900_device_user_t * get_device_user_settings(void)
//{
//    return &device_user_settings;
//}



/********** new change *****************/


uint16_t get_conn_handle(void)
{
    return m_conn_handle;
}


/**@brief Callback function for asserts in the SoftDevice.
 *
 * @details This function will be called in case of an assert in the SoftDevice.
 *
 * @warning This handler is an example only and does not fit a final product. You need to analyze
 *          how your product is supposed to react in case of Assert.
 * @warning On assert from the SoftDevice, the system can only recover on reset.
 *
 * @param[in]   line_num   Line number of the failing ASSERT call.
 * @param[in]   file_name  File name of the failing ASSERT call.
 */
void assert_nrf_callback(uint16_t line_num, const uint8_t * p_file_name)
{
    app_error_handler(DEAD_BEEF, line_num, p_file_name);
}


/**@brief Function for handling Peer Manager events.
 *
 * @param[in] p_evt  Peer Manager event.
 */
static void pm_evt_handler(pm_evt_t const * p_evt)
{
    bool delete_bonds = false;

    pm_handler_on_pm_evt(p_evt);
    pm_handler_flash_clean(p_evt);

    switch (p_evt->evt_id) {
    case PM_EVT_PEERS_DELETE_SUCCEEDED:
        advertising_start(&delete_bonds);
        break;

    default:
        break;
    }
}

uint32_t rtc_val = 0;

uint32_t get_system_rtc(void)
{
    return rtc_val;
}

/**@brief Function for handling the RTC event
 *
 * @details This function will be called each time the RTC timer expires.
 *
 * @param[in] xTimer Handler to the timer that called this function.
 *                   You may get identifier given to the function xTimerCreate using pvTimerGetTimerID.
 */
static void rtc_timeout_handler(TimerHandle_t xTimer)
{
    UNUSED_PARAMETER(xTimer);
    rtc_val++;
}


static StaticTimer_t xRTCTimer;

/**@brief Function for the Timer initialization.
 *
 * @details Initializes the timer module. This creates and starts application timers.
 */
static void timers_init(void)
{
    // Initialize timer module.
    ret_code_t err_code = app_timer_init();
    APP_ERROR_CHECK(err_code);

    // Create timers.
    m_rtc_timer = xTimerCreateStatic("RTC",
                                     RTC_INTERVAL,
                                     pdTRUE,
                                     NULL,
                                     rtc_timeout_handler,
                                     &xRTCTimer);


    /* Error checking */
    if (NULL == m_rtc_timer) {
        APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
    }
}


/**@brief Function for the GAP initialization.
 *
 * @details This function sets up all the necessary GAP (Generic Access Profile) parameters of the
 *          device including the device name, appearance, and the preferred connection parameters.
 */
static void gap_params_init(void)
{
    ret_code_t              err_code;
    ble_gap_conn_params_t   gap_conn_params;
    ble_gap_conn_sec_mode_t sec_mode;

    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&sec_mode);

    err_code = sd_ble_gap_device_name_set(&sec_mode,
                                          (const uint8_t *)device_le_name,
                                          strlen(device_le_name));
    APP_ERROR_CHECK(err_code);

    err_code = sd_ble_gap_appearance_set(BLE_APPEARANCE_HEART_RATE_SENSOR_HEART_RATE_BELT);
    APP_ERROR_CHECK(err_code);

    memset(&gap_conn_params, 0, sizeof(gap_conn_params));

    gap_conn_params.min_conn_interval = MIN_CONN_INTERVAL;
    gap_conn_params.max_conn_interval = MAX_CONN_INTERVAL;
    gap_conn_params.slave_latency     = SLAVE_LATENCY;
    gap_conn_params.conn_sup_timeout  = CONN_SUP_TIMEOUT;

    err_code = sd_ble_gap_ppcp_set(&gap_conn_params);
    APP_ERROR_CHECK(err_code);
}

/**@brief Function for handling events from the GATT library. */
void gatt_evt_handler(nrf_ble_gatt_t * p_gatt, nrf_ble_gatt_evt_t const * p_evt)
{
    if ((m_conn_handle == p_evt->conn_handle) && (p_evt->evt_id == NRF_BLE_GATT_EVT_ATT_MTU_UPDATED))
    {        
        sys_set_peripheral_mtu(p_evt->params.att_mtu_effective - OPCODE_LENGTH - HANDLE_LENGTH);

        NRF_LOG_INFO("Data len is set to 0x%X(%d)", sys_get_peripheral_mtu(), sys_get_peripheral_mtu());
        NRF_LOG_PROCESS();
    }
    NRF_LOG_INFO("ATT MTU exchange completed. central 0x%x peripheral 0x%x",
                  p_gatt->att_mtu_desired_central,
                  p_gatt->att_mtu_desired_periph);
    NRF_LOG_PROCESS();
}

/**@brief Function for initializing the GATT module. */
static void gatt_init(void)
{
    ret_code_t err_code = nrf_ble_gatt_init(&m_gatt, gatt_evt_handler);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_ble_gatt_att_mtu_central_set(&m_gatt, NRF_SDH_BLE_GATT_MAX_MTU_SIZE);
    APP_ERROR_CHECK(err_code);
}


/**@brief Function for handling Queued Write Module errors.
 *
 * @details A pointer to this function will be passed to each service which may need to inform the
 *          application about an error.
 *
 * @param[in]   nrf_error   Error code containing information about what went wrong.
 */
static void nrf_qwr_error_handler(uint32_t nrf_error)
{
    APP_ERROR_HANDLER(nrf_error);
}



static void aqrs_data_handler(ble_aqrs_evt_t * p_evt)
{
    wifi_comm_task_evt_t evt;

    if (p_evt->type == BLE_AQRS_EVT_RX_DATA) {
        uint32_t err_code;

        if(p_evt->params.rx_data.length < MAX_FRAME_PAYLOAD_SIZE) {
          
            evt.evt_id = WIFI_COMM_TASK_EVT_AQRS_DATA;
            evt.length = p_evt->params.rx_data.length;
            memcpy(evt.payload, p_evt->params.rx_data.p_data, p_evt->params.rx_data.length);            

            wifi_comm_task_queue_event(&evt);
        }
    } else if(p_evt->type == BLE_AQRS_EVT_NOTIF_ENABLED) {
        /* Inform Toilet Task notifications have begun */
        // toilet_task_queue_event(TOILET_TASK_EVT_NOTIF_EN);
    }
}

/**@brief Function for handling dfu events from the Buttonless Secure DFU service
 * @note Update this code if you want to do anything given a DFU event (optional).
 * 
 * @param[in]   event   Event from the Buttonless Secure DFU service.
 */
static void ble_dfu_evt_handler(ble_dfu_buttonless_evt_type_t event)
{
    switch (event)
    {
        case BLE_DFU_EVT_BOOTLOADER_ENTER_PREPARE:
        {
            NRF_LOG_INFO("Device is preparing to enter bootloader mode.");

            // Prevent device from advertising on disconnect.
            ble_adv_modes_config_t config;
            advertising_config_get(&config);
            config.ble_adv_on_disconnect_disabled = true;
            ble_advertising_modes_config_set(&m_advertising, &config);

            // Disconnect all other bonded devices that currently are connected.
            // This is required to receive a service changed indication
            // on bootup after a successful (or aborted) Device Firmware Update.
            uint32_t conn_count = ble_conn_state_for_each_connected(disconnect, NULL);
            NRF_LOG_INFO("Disconnected %d links.", conn_count);
            break;
        }

        case BLE_DFU_EVT_BOOTLOADER_ENTER:
            // YOUR_JOB: Write app-specific unwritten data to FLASH, control finalization of this
            //           by delaying reset by reporting false in app_shutdown_handler
            NRF_LOG_INFO("Device will enter bootloader mode.");
            break;

        case BLE_DFU_EVT_BOOTLOADER_ENTER_FAILED:
            NRF_LOG_ERROR("Request to enter bootloader mode failed asynchroneously.");
            // YOUR_JOB: Take corrective measures to resolve the issue
            //           like calling APP_ERROR_CHECK to reset the device.
            break;

        case BLE_DFU_EVT_RESPONSE_SEND_ERROR:
            NRF_LOG_ERROR("Request to send a response to client failed.");
            // YOUR_JOB: Take corrective measures to resolve the issue
            //           like calling APP_ERROR_CHECK to reset the device.
            APP_ERROR_CHECK(false);
            break;

        default:
            NRF_LOG_ERROR("Unknown event from ble_dfu_buttonless.");
            break;
    }
}


/**@brief Function for initializing services that will be used by the application.
 *
 * @details Initialize the Heart Rate, Battery and Device Information services.
 */
static void services_init(void)
{
    ret_code_t         err_code;
    ble_aqrs_init_t    aqrs_init;
    ble_dis_init_t     dis_init;
    nrf_ble_qwr_init_t qwr_init = {0};
    ble_dfu_buttonless_init_t dfus_init = {0};

    // Initialize Queued Write Module.
    qwr_init.error_handler = nrf_qwr_error_handler;

    err_code = nrf_ble_qwr_init(&m_qwr, &qwr_init);
    APP_ERROR_CHECK(err_code);

    // Initialize Device Information Service.
    memset(&dis_init, 0, sizeof(dis_init));

    ble_srv_ascii_to_utf8(&dis_init.manufact_name_str, (char *)MANUFACTURER_NAME);
    ble_srv_ascii_to_utf8(&dis_init.model_num_str, (char *)DEVICE_MODEL_NUM);
    ble_srv_ascii_to_utf8(&dis_init.serial_num_str, (char *)device_setting.device_id);
    ble_srv_ascii_to_utf8(&dis_init.hw_rev_str, (char*)hw_version);
    ble_srv_ascii_to_utf8(&dis_init.fw_rev_str, (char *)app_fw_version);


    dis_init.dis_char_rd_sec = SEC_OPEN;

    err_code = ble_dis_init(&dis_init);
    APP_ERROR_CHECK(err_code);


    // Initialize AQRS Service
    memset(&aqrs_init, 0, sizeof(aqrs_init));

    aqrs_init.data_handler = aqrs_data_handler;

    err_code = ble_aqrs_init(&m_aqrs, &aqrs_init);
    APP_ERROR_CHECK(err_code);

    #if 0
    // Initialize the Nordic DFU Buttonless Service
    dfus_init.evt_handler = ble_dfu_evt_handler;

    err_code = ble_dfu_buttonless_init(&dfus_init);
    APP_ERROR_CHECK(err_code);
    #endif
}



/**@brief   Function for starting application timers.
 * @details Timers are run after the scheduler has started.
 */
static void application_timers_start(void)
{
    // Start application timers.
    if (pdPASS != xTimerStart(m_rtc_timer, OSTIMER_WAIT_FOR_QUEUE)) {
        APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
    }
}


/**@brief Function for handling the Connection Parameters Module.
 *
 * @details This function will be called for all events in the Connection Parameters Module which
 *          are passed to the application.
 *          @note All this function does is to disconnect. This could have been done by simply
 *                setting the disconnect_on_fail config parameter, but instead we use the event
 *                handler mechanism to demonstrate its use.
 *
 * @param[in]   p_evt   Event received from the Connection Parameters Module.
 */
static void on_conn_params_evt(ble_conn_params_evt_t * p_evt)
{
    ret_code_t err_code;

    if (p_evt->evt_type == BLE_CONN_PARAMS_EVT_FAILED) {
        NRF_LOG_INFO("BLE_CONN_PARAMS_EVT_FAILED");
        err_code = sd_ble_gap_disconnect(m_conn_handle, BLE_HCI_CONN_INTERVAL_UNACCEPTABLE);
        APP_ERROR_CHECK(err_code);
    }
}


/**@brief Function for handling a Connection Parameters error.
 *
 * @param[in]   nrf_error   Error code containing information about what went wrong.
 */
static void conn_params_error_handler(uint32_t nrf_error)
{
    APP_ERROR_HANDLER(nrf_error);
}


/**@brief Function for initializing the Connection Parameters module. */
static void conn_params_init(void)
{
    ret_code_t             err_code;
    ble_conn_params_init_t cp_init;

    memset(&cp_init, 0, sizeof(cp_init));

    cp_init.p_conn_params                  = NULL;
    cp_init.first_conn_params_update_delay = FIRST_CONN_PARAMS_UPDATE_DELAY;
    cp_init.next_conn_params_update_delay  = NEXT_CONN_PARAMS_UPDATE_DELAY;
    cp_init.max_conn_params_update_count   = MAX_CONN_PARAMS_UPDATE_COUNT;
    cp_init.start_on_notify_cccd_handle    = BLE_GATT_HANDLE_INVALID;
    cp_init.disconnect_on_fail             = false;
    cp_init.evt_handler                    = on_conn_params_evt;
    cp_init.error_handler                  = conn_params_error_handler;

    err_code = ble_conn_params_init(&cp_init);
    APP_ERROR_CHECK(err_code);
}


/**@brief Function for putting the chip into sleep mode.
 *
 * @note This function will not return.
 */
static void sleep_mode_enter(void)
{
    ret_code_t err_code;

    err_code = bsp_indication_set(BSP_INDICATE_IDLE);
    APP_ERROR_CHECK(err_code);

    // Prepare wakeup buttons.
    err_code = bsp_btn_ble_sleep_mode_prepare();
    APP_ERROR_CHECK(err_code);

    // Go to system-off mode (this function will not return; wakeup will cause a reset).
    err_code = sd_power_system_off();
    APP_ERROR_CHECK(err_code);
}


/**@brief Function for handling advertising events.
 *
 * @details This function will be called for advertising events which are passed to the application.
 *
 * @param[in] ble_adv_evt  Advertising event.
 */
static void on_adv_evt(ble_adv_evt_t ble_adv_evt)
{
    uint32_t err_code;

    switch (ble_adv_evt) {
    case BLE_ADV_EVT_FAST:
        NRF_LOG_INFO("Fast advertising.");
        err_code = bsp_indication_set(BSP_INDICATE_ADVERTISING);
        APP_ERROR_CHECK(err_code);
        break;

    case BLE_ADV_EVT_IDLE:
        sleep_mode_enter();
        break;

    default:
        break;
    }
}

/**@brief Function for handling BLE Stack events that involves peripheral applications. Manages the
 * LEDs used to report the status of the peripheral applications.
 *
 * @param[in] p_ble_evt  Bluetooth stack event.
 */
static void on_ble_peripheral_evt(ble_evt_t const * p_ble_evt)
{
    ret_code_t err_code;
    switch (p_ble_evt->header.evt_id)
    {
        case BLE_GAP_EVT_CONNECTED:
            NRF_LOG_INFO("PERIPHERAL: Connected, handle %d.", p_ble_evt->evt.gap_evt.conn_handle);
            m_conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
            break;

        case BLE_GAP_EVT_DISCONNECTED:
            NRF_LOG_INFO("PERIPHERAL: Disconnected, handle %d, reason 0x%x.",
                         p_ble_evt->evt.gap_evt.conn_handle,
                         p_ble_evt->evt.gap_evt.params.disconnected.reason);
            // LED indication will be changed when advertising starts.
        break;

        case BLE_GATTC_EVT_TIMEOUT:
            // Disconnect on GATT Client timeout event.
            NRF_LOG_DEBUG("PERIPHERAL: GATT Client Timeout.");
            err_code = sd_ble_gap_disconnect(p_ble_evt->evt.gattc_evt.conn_handle,
                                             BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
            APP_ERROR_CHECK(err_code);
            break;

        case BLE_GATTS_EVT_TIMEOUT:
            // Disconnect on GATT Server timeout event.
            NRF_LOG_DEBUG("PERIPHERAL: GATT Server Timeout.");
            err_code = sd_ble_gap_disconnect(p_ble_evt->evt.gatts_evt.conn_handle,
                                             BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
            APP_ERROR_CHECK(err_code);
            break;
      case BLE_GAP_EVT_ADV_REPORT:
	{
          char name[32];
	  uint16_t offset = 0;
	  uint16_t length = ble_advdata_search(p_ble_evt->evt.gap_evt.params.adv_report.data.p_data, p_ble_evt->evt.gap_evt.params.adv_report.data.len, &offset, BLE_GAP_AD_TYPE_COMPLETE_LOCAL_NAME);
	  if(length == 0){
		length = ble_advdata_search(p_ble_evt->evt.gap_evt.params.adv_report.data.p_data, p_ble_evt->evt.gap_evt.params.adv_report.data.len, &offset, BLE_GAP_AD_TYPE_SHORT_LOCAL_NAME);
		}

       	if(length != 0){
		memcpy(name, &p_ble_evt->evt.gap_evt.params.adv_report.data.p_data[offset], length);
                name[length] = '\0';
        	NRF_LOG_INFO("222 name: %s\n", name);
		}
         break;
	}
        default:
            // No implementation needed.
            break;
    }
}

/**@brief Function for assigning new connection handle to the available instance of QWR module.
 *
 * @param[in] conn_handle New connection handle.
 */
static void multi_qwr_conn_handle_assign(uint16_t conn_handle)
{
    for (uint32_t i = 0; i < NRF_BLE_LINK_COUNT; i++)
    {
        if (m_qwr[i].conn_handle == BLE_CONN_HANDLE_INVALID)
        {
            ret_code_t err_code = nrf_ble_qwr_conn_handle_assign(&m_qwr[i], conn_handle);
            APP_ERROR_CHECK(err_code);
            break;
        }
    }
}

/**@brief Function for handling BLE Stack events that are common to both the central and peripheral roles.
 * @param[in] conn_handle Connection Handle.
 * @param[in] p_ble_evt  Bluetooth stack event.
 */
static void on_ble_evt(uint16_t conn_handle, ble_evt_t const * p_ble_evt)
{
    char        passkey[BLE_GAP_PASSKEY_LEN + 1];
    uint16_t    role = ble_conn_state_role(conn_handle);
    uint32_t err_code;

    //pm_handler_secure_on_connection(p_ble_evt);

    switch (p_ble_evt->header.evt_id)
    {
        case BLE_GAP_EVT_CONNECTED:
            m_connected_peers[conn_handle].is_connected = true;
            m_connected_peers[conn_handle].address = p_ble_evt->evt.gap_evt.params.connected.peer_addr;
            multi_qwr_conn_handle_assign(conn_handle);
            break;

        case BLE_GAP_EVT_DISCONNECTED:
            memset(&m_connected_peers[conn_handle], 0x00, sizeof(m_connected_peers[0]));
            break;

        case BLE_GAP_EVT_SEC_PARAMS_REQUEST:
            break;

            #if 0
        case BLE_GAP_EVT_PASSKEY_DISPLAY:
            memcpy(passkey, p_ble_evt->evt.gap_evt.params.passkey_display.passkey, BLE_GAP_PASSKEY_LEN);
            passkey[BLE_GAP_PASSKEY_LEN] = 0x00;
            NRF_LOG_INFO("%s: BLE_GAP_EVT_PASSKEY_DISPLAY: passkey=%s match_req=%d",
                         nrf_log_push(roles_str[role]),
                         nrf_log_push(passkey),
                         p_ble_evt->evt.gap_evt.params.passkey_display.match_request);

            if (p_ble_evt->evt.gap_evt.params.passkey_display.match_request)
            {
                on_match_request(conn_handle, role);
            }
            break;
            #endif

        case BLE_GAP_EVT_AUTH_KEY_REQUEST:
            NRF_LOG_INFO("%s: BLE_GAP_EVT_AUTH_KEY_REQUEST", nrf_log_push(roles_str[role]));
            break;

        case BLE_GAP_EVT_LESC_DHKEY_REQUEST:
            NRF_LOG_INFO("%s: BLE_GAP_EVT_LESC_DHKEY_REQUEST", nrf_log_push(roles_str[role]));
            break;

         case BLE_GAP_EVT_AUTH_STATUS:
             NRF_LOG_INFO("%s: BLE_GAP_EVT_AUTH_STATUS: status=0x%x bond=0x%x lv4: %d kdist_own:0x%x kdist_peer:0x%x",
                          nrf_log_push(roles_str[role]),
                          p_ble_evt->evt.gap_evt.params.auth_status.auth_status,
                          p_ble_evt->evt.gap_evt.params.auth_status.bonded,
                          p_ble_evt->evt.gap_evt.params.auth_status.sm1_levels.lv4,
                          *((uint8_t *)&p_ble_evt->evt.gap_evt.params.auth_status.kdist_own),
                          *((uint8_t *)&p_ble_evt->evt.gap_evt.params.auth_status.kdist_peer));
            break;
        case BLE_GATTS_EVT_SYS_ATTR_MISSING:
            // No system attributes have been stored.
            err_code = sd_ble_gatts_sys_attr_set(conn_handle, NULL, 0, 0);
            APP_ERROR_CHECK(err_code);
            break; // BLE_GATTS_EVT_SYS_ATTR_MISSING

        case BLE_GAP_EVT_PHY_UPDATE_REQUEST:
        {
            NRF_LOG_DEBUG("PHY update request.");
            ble_gap_phys_t const phys =
            {
                .rx_phys = BLE_GAP_PHY_AUTO,
                .tx_phys = BLE_GAP_PHY_AUTO,
            };
            ret_code_t err_code = sd_ble_gap_phy_update(p_ble_evt->evt.gap_evt.conn_handle, &phys);
            APP_ERROR_CHECK(err_code);
        } break;

        default:
            // No implementation needed.
            break;
    }
}

/**@brief Function for checking whether a link already exists with a newly connected peer.
 *
 * @details This function checks whether the newly connected device is already connected.
 *
 * @param[in]   p_connected_evt Bluetooth connected event.
 * @return                      True if the peer's address is found in the list of connected peers,
 *                              false otherwise.
 */
static bool is_already_connected(ble_gap_addr_t const * p_connected_adr)
{
    for (uint32_t i = 0; i < NRF_BLE_LINK_COUNT; i++)
    {
        if (m_connected_peers[i].is_connected)
        {
            if (m_connected_peers[i].address.addr_type == p_connected_adr->addr_type)
            {
                if (memcmp(m_connected_peers[i].address.addr,
                           p_connected_adr->addr,
                           sizeof(m_connected_peers[i].address.addr)) == 0)
                {
                    return true;
                }
            }
        }
    }
    return false;
}

/**@brief Function for handling BLE events.
 *
 * @param[in]   p_ble_evt   Bluetooth stack event.
 * @param[in]   p_context   Unused.
 */
static void ble_evt_handler(ble_evt_t const * p_ble_evt, void * p_context)
{
    uint16_t conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
    uint16_t role        = ble_conn_state_role(conn_handle);

    if (    (p_ble_evt->header.evt_id == BLE_GAP_EVT_CONNECTED)
        &&  (is_already_connected(&p_ble_evt->evt.gap_evt.params.connected.peer_addr)))
    {
        NRF_LOG_INFO("%s: Already connected to this device as %s (handle: %d), disconnecting.",
                     (role == BLE_GAP_ROLE_PERIPH) ? "PERIPHERAL" : "CENTRAL",
                     (role == BLE_GAP_ROLE_PERIPH) ? "CENTRAL"    : "PERIPHERAL",
                     conn_handle);

        (void)sd_ble_gap_disconnect(conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);

        // Do not process the event further.
        return;
    }

    on_ble_evt(conn_handle, p_ble_evt);

    if (role == BLE_GAP_ROLE_PERIPH)
    {
        // Manages peripheral LEDs.
        on_ble_peripheral_evt(p_ble_evt);
    }
    else if ((role == BLE_GAP_ROLE_CENTRAL) || (p_ble_evt->header.evt_id == BLE_GAP_EVT_ADV_REPORT))
    {
        on_ble_central_evt(p_ble_evt);
    }
}


/**@brief Function for initializing the BLE stack.
 *
 * @details Initializes the SoftDevice and the BLE event interrupt.
 */
static void ble_stack_init(void)
{
    ret_code_t err_code;

    err_code = nrf_sdh_enable_request();
    APP_ERROR_CHECK(err_code);

    // Configure the BLE stack using the default settings.
    // Fetch the start address of the application RAM.
    uint32_t ram_start = 0;
    err_code = nrf_sdh_ble_default_cfg_set(APP_BLE_CONN_CFG_TAG, &ram_start);
    APP_ERROR_CHECK(err_code);

    // Enable BLE stack.
    err_code = nrf_sdh_ble_enable(&ram_start);
    APP_ERROR_CHECK(err_code);

    // Register a handler for BLE events.
    NRF_SDH_BLE_OBSERVER(m_ble_observer, APP_BLE_OBSERVER_PRIO, ble_evt_handler, NULL);
}


/**@brief Function for handling events from the BSP module.
 *
 * @param[in]   event   Event generated by button press.
 */
static void bsp_event_handler(bsp_event_t event)
{
    ret_code_t err_code;

    switch (event) {
    case BSP_EVENT_SLEEP:
        sleep_mode_enter();
        break;

    case BSP_EVENT_DISCONNECT:
        NRF_LOG_INFO("BSP_EVENT_DISCONNECT");
        err_code = sd_ble_gap_disconnect(m_conn_handle,
                                         BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
        if (err_code != NRF_ERROR_INVALID_STATE) {
            APP_ERROR_CHECK(err_code);
        }
        break;

    case BSP_EVENT_WHITELIST_OFF:
        if (m_conn_handle == BLE_CONN_HANDLE_INVALID) {
            err_code = ble_advertising_restart_without_whitelist(&m_advertising);
            if (err_code != NRF_ERROR_INVALID_STATE) {
                APP_ERROR_CHECK(err_code);
            }
        }
        break;

    default:
        break;
    }
}


/**@brief Function for the Peer Manager initialization. */
static void peer_manager_init(void)
{
#if 0
    ble_gap_sec_params_t sec_param;
    ret_code_t           err_code;

    err_code = pm_init();
    APP_ERROR_CHECK(err_code);

    memset(&sec_param, 0, sizeof(ble_gap_sec_params_t));

    // Security parameters to be used for all security procedures.
    sec_param.bond           = SEC_PARAM_BOND;
    sec_param.mitm           = SEC_PARAM_MITM;
    sec_param.lesc           = SEC_PARAM_LESC;
    sec_param.keypress       = SEC_PARAM_KEYPRESS;
    sec_param.io_caps        = SEC_PARAM_IO_CAPABILITIES;
    sec_param.oob            = SEC_PARAM_OOB;
    sec_param.min_key_size   = SEC_PARAM_MIN_KEY_SIZE;
    sec_param.max_key_size   = SEC_PARAM_MAX_KEY_SIZE;
    sec_param.kdist_own.enc  = 0;
    sec_param.kdist_own.id   = 0;
    sec_param.kdist_peer.enc = 0;
    sec_param.kdist_peer.id  = 0;

    err_code = pm_sec_params_set(NULL);
    APP_ERROR_CHECK(err_code);

    err_code = pm_register(pm_evt_handler);
    APP_ERROR_CHECK(err_code);
#endif
}


/**@brief Clear bond information from persistent storage. */
static void delete_bonds(void)
{
    ret_code_t err_code;

    NRF_LOG_INFO("Erase bonds!");

    //err_code = pm_peers_delete();
    //APP_ERROR_CHECK(err_code);
}





/**@brief Function for initializing the Advertising functionality. */
static void advertising_init(void)
{
    ret_code_t             err_code;
    ble_advertising_init_t init;

    memset(&init, 0, sizeof(init));

    init.advdata.name_type               = BLE_ADVDATA_NO_NAME;
    init.advdata.include_appearance      = false;
    init.advdata.flags                   = BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE;
    init.advdata.uuids_more_available.uuid_cnt = sizeof(m_adv_uuids) / sizeof(m_adv_uuids[0]);
    init.advdata.uuids_more_available.p_uuids  = m_adv_uuids;

    init.config.ble_adv_fast_enabled  = true;
    init.config.ble_adv_fast_interval = APP_ADV_INTERVAL;
    init.config.ble_adv_fast_timeout  = APP_ADV_DURATION;


    init.srdata.name_type = BLE_ADVDATA_FULL_NAME;

    init.evt_handler = on_adv_evt;

    err_code = ble_advertising_init(&m_advertising, &init);
    APP_ERROR_CHECK(err_code);

    ble_advertising_conn_cfg_tag_set(&m_advertising, APP_BLE_CONN_CFG_TAG);
}


/**@brief Function for initializing the nrf log module.
 */
static void log_init(void)
{
    ret_code_t err_code = NRF_LOG_INIT(NULL);
    APP_ERROR_CHECK(err_code);

    NRF_LOG_DEFAULT_BACKENDS_INIT();
}

/**@brief Function for starting advertising. */
void advertising_start(void * p_erase_bonds)
{
    bool erase_bonds = *(bool*)p_erase_bonds;

    if (erase_bonds) {
        delete_bonds();
        // Advertising is started by PM_EVT_PEERS_DELETE_SUCCEEDED event.
    } else {
        ret_code_t err_code = ble_advertising_start(&m_advertising, BLE_ADV_MODE_FAST);
        //APP_ERROR_CHECK(err_code); // TODO: Likely removed since asks advertising again when already advertising
    }
}

/* configSUPPORT_STATIC_ALLOCATION is set to 1, so the application must provide an
implementation of vApplicationGetIdleTaskMemory() to provide the memory that is
used by the Idle task. */
void vApplicationGetIdleTaskMemory( StaticTask_t **ppxIdleTaskTCBBuffer,
                                    StackType_t **ppxIdleTaskStackBuffer,
                                    uint32_t *pulIdleTaskStackSize )
{
    /* If the buffers to be provided to the Idle task are declared inside this
    function then they must be declared static - otherwise they will be allocated on
    the stack and so not exists after this function exits. */
    static StaticTask_t xIdleTaskTCB;
    static StackType_t uxIdleTaskStack[ configMINIMAL_STACK_SIZE ];

    /* Pass out a pointer to the StaticTask_t structure in which the Idle task's
    state will be stored. */
    *ppxIdleTaskTCBBuffer = &xIdleTaskTCB;

    /* Pass out the array that will be used as the Idle task's stack. */
    *ppxIdleTaskStackBuffer = uxIdleTaskStack;

    /* Pass out the size of the array pointed to by *ppxIdleTaskStackBuffer.
    Note that, as the array is necessarily of type StackType_t,
    configMINIMAL_STACK_SIZE is specified in words, not bytes. */
    *pulIdleTaskStackSize = configMINIMAL_STACK_SIZE;
}

/* configSUPPORT_STATIC_ALLOCATION and configUSE_TIMERS are both set to 1, so the
application must provide an implementation of vApplicationGetTimerTaskMemory()
to provide the memory that is used by the Timer service task. */
void vApplicationGetTimerTaskMemory( StaticTask_t **ppxTimerTaskTCBBuffer,
                                     StackType_t **ppxTimerTaskStackBuffer,
                                     uint32_t *pulTimerTaskStackSize )
{
    /* If the buffers to be provided to the Timer task are declared inside this
    function then they must be declared static - otherwise they will be allocated on
    the stack and so not exists after this function exits. */
    static StaticTask_t xTimerTaskTCB;
    static StackType_t uxTimerTaskStack[ configTIMER_TASK_STACK_DEPTH ];

    /* Pass out a pointer to the StaticTask_t structure in which the Timer
    task's state will be stored. */
    *ppxTimerTaskTCBBuffer = &xTimerTaskTCB;

    /* Pass out the array that will be used as the Timer task's stack. */
    *ppxTimerTaskStackBuffer = uxTimerTaskStack;

    /* Pass out the size of the array pointed to by *ppxTimerTaskStackBuffer.
    Note that, as the array is necessarily of type StackType_t,
    configTIMER_TASK_STACK_DEPTH is specified in words, not bytes. */
    *pulTimerTaskStackSize = configTIMER_TASK_STACK_DEPTH;
}

void vApplicationMallocFailedHook( void )
{
    NRF_LOG_INFO("Malloc Failed");
}

void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed char *pcTaskName )
{

    for( ;; );
}


#if NRF_LOG_ENABLED
/**@brief Thread for handling the logger.
 *
 * @details This thread is responsible for processing log entries if logs are deferred.
 *          Thread flushes all log entries and suspends. It is resumed by idle task hook.
 *
 * @param[in]   arg   Pointer used for passing some arbitrary information (context) from the
 *                    osThreadCreate() call to the thread.
 */
static void logger_thread(void * arg)
{
    UNUSED_PARAMETER(arg);

    while (1) {
        NRF_LOG_FLUSH();

        vTaskSuspend(NULL); // Suspend myself
    }
}
#endif //NRF_LOG_ENABLED

/**@brief A function which is hooked to idle task.
 * @note Idle hook must be enabled in FreeRTOS configuration (configUSE_IDLE_HOOK).
 */
void vApplicationIdleHook( void )
{
#if NRF_LOG_ENABLED
    vTaskResume(m_logger_thread);
#endif
}


/**@brief Function for initializing the clock.
 */
static void clock_init(void)
{
    ret_code_t err_code = nrf_drv_clock_init();
    APP_ERROR_CHECK(err_code);
}

#define LOGGER_TASK_STACK_SIZE 512
#define LOGGER_TASK_PRIORITY   1
/* Structure that will hold the TCB of the task being created. */
StaticTask_t xLoggerTaskBuffer;

/* Buffer that the task being created will use as its stack.  Note this is
an array of StackType_t variables.  The size of StackType_t is dependent on
the RTOS port. */
StackType_t xLoggerStack[ LOGGER_TASK_STACK_SIZE ];

#define FPU_EXCEPTION_MASK 0x0000009F

void FPU_IRQHandler(void)
{
    uint32_t *fpscr = (uint32_t *)(FPU->FPCAR+0x40);
    (void)__get_FPSCR();

    *fpscr = *fpscr & ~(FPU_EXCEPTION_MASK);
}

/**@brief Function for application main entry.
 */
int main(void)
{
    bool erase_bonds;
    ret_code_t err_code;


    // Initialize modules.
    log_init();

    // Initialize the async SVCI interface to bootloader before any interrupts are enabled.
    // We don't check for error in case bootloader not in place
    //ble_dfu_buttonless_async_svci_init();

    clock_init();

    // Do not start any interrupt that uses system functions before system initialisation.
    // The best solution is to start the OS before any other initalisation.

#if NRF_LOG_ENABLED
    // Start execution.
    m_logger_thread = xTaskCreateStatic(logger_thread, "LOGGER", sizeof(xLoggerStack) / sizeof(xLoggerStack[0]), NULL, LOGGER_TASK_PRIORITY, xLoggerStack, &xLoggerTaskBuffer);
    if (m_logger_thread == NULL) {
        APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
    }
#endif

    // Activate deep sleep mode.
    SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk;

        memset(app_fw_version, 0, sizeof(app_fw_version));
    app_version_str(app_fw_version, sizeof(app_fw_version));

    /* Initialize device storage so we can load system settings */
    device_storage_init();
    repeater_load_settings();
    system_create_device_name(device_le_name, sizeof(device_le_name));

    /* Enable interrupt for FPU so we can clear it */
    NVIC_SetPriority(FPU_IRQn, APP_IRQ_PRIORITY_LOW);
    NVIC_EnableIRQ(FPU_IRQn);

    // Configure and initialize the BLE stack.
    ble_stack_init();

    // Initialize modules.
    timers_init();
    gap_params_init();
    gatt_init();
    services_init();
    advertising_init();
    conn_params_init();
    peer_manager_init();
    application_timers_start();

    /* initialize the Client Services */
    db_discovery_init();
    init_client_services();

    /* Set TX Output Power to +4dBm */
    err_code = sd_ble_gap_tx_power_set(BLE_GAP_TX_POWER_ROLE_ADV, m_advertising.adv_handle, 8); 
    APP_ERROR_CHECK(err_code);


    // Create a FreeRTOS task for the BLE stack.
    // The task will run advertising_start() before entering its loop.
    nrf_sdh_freertos_init(advertising_start, &erase_bonds);

    err_code = nrf_drv_gpiote_init();
    APP_ERROR_CHECK(err_code);

    init_led_button_task();    
    init_system_task();
    init_device_storage_task();
    init_wifi_comm_task();
    init_repeater_task();

    //scan_init();
    scan_init_new();
    

    NRF_LOG_INFO("Central Starting");
    NRF_LOG_INFO("FW Version: %s", app_fw_version);
#if defined(BOARD_PCA10056)
    NRF_LOG_INFO("HW Platform: Nordic PCA10056");
#elif defined(BOARD_AQUAI_RP900)
   
#endif
    // Start FreeRTOS scheduler.
    vTaskStartScheduler();

    for (;;) {
        APP_ERROR_HANDLER(NRF_ERROR_FORBIDDEN);
    }
}

Fullscreen 2335.central.c Download

/*****************************************************************************

*
* File:    le_central.c
* Summary: LE Central
*
*/

#ifdef __cplusplus
extern "C"
{
#endif

/* Standard Includes */
#include <stdarg.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

/* Includes */
#include "le_central.h"

#include "app_error.h"
#include "app_timer.h"
#include "app_util.h"
#include "ble.h"
#include "ble_db_discovery.h"
#include "ble_gap.h"
#include "ble_hci.h"
#include "ble_nus_c.h"
#include "le_central.h"
#include "main.h"
#include "nordic_common.h"
#include "nrf_ble_gatt.h"
#include "nrf_ble_scan.h"
#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"
#include "nrf_pwr_mgmt.h"
#include "nrf_sdh.h"
#include "nrf_sdh_ble.h"
#include "nrf_sdh_soc.h"
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>

#include "ble_aqss.h"
#include "ble_aqss_c.h"
#include "ble_aqts.h"
#include "ble_aqts_c.h"
#include "ble_dis_c.h"
#include "task_repeater.h"
#include "repeater.h"

#define SCAN_DURATION_WITELIST    3000                             /**< Duration of the scanning in units of 10 milliseconds. */

BLE_DB_DISCOVERY_DEF(m_db_disc);                                    /**< DB discovery module instance. */

NRF_BLE_SCAN_DEF(m_scan);                                               /**< Scanning Module instance. */
BLE_AQSS_C_ARRAY_DEF(
        m_ble_aqss_c, NRF_SDH_BLE_CENTRAL_LINK_COUNT
); /**< Aquai Shower Service client instance. */
BLE_AQTS_C_ARRAY_DEF(
        m_ble_aqts_c, NRF_SDH_BLE_CENTRAL_LINK_COUNT
); /**< Aquai Toilet Service client instance. */
BLE_DIS_C_ARRAY_DEF(m_ble_dis_c, NRF_SDH_BLE_CENTRAL_LINK_COUNT
);


NRF_BLE_GQ_DEF(m_ble_gatt_queue,                                        /**< BLE GATT Queue instance. */
NRF_SDH_BLE_CENTRAL_LINK_COUNT, NRF_BLE_GQ_QUEUE_SIZE
);

extern char *array_device_list[5][40];

/**@brief AQSS UUID. */
static ble_uuid_t const m_aqss_uuid = {
        .uuid = BLE_UUID_AQSS_SERVICE, .type = AQSS_SERVICE_UUID_TYPE};

/**@brief AQTS UUID. */
static ble_uuid_t const m_aqts_uuid = {
        .uuid = BLE_UUID_AQTS_SERVICE, .type = AQTS_SERVICE_UUID_TYPE};

static ble_uuid_t const m_dis_uuid = {
        .uuid = BLE_UUID_DEVICE_INFORMATION_SERVICE, .type = BLE_UUID_TYPE_BLE};

static ble_gap_scan_params_t
        m_scan_param = /**< Scan parameters requested for scanning and connection. */
        {
    .active        = 0x01,
    .interval      = NRF_BLE_SCAN_SCAN_INTERVAL,
    .window        = NRF_BLE_SCAN_SCAN_WINDOW,
    .filter_policy = BLE_GAP_SCAN_FP_WHITELIST,
    .timeout       = SCAN_DURATION_WITELIST,
    .scan_phys     = BLE_GAP_PHY_1MBPS,
        };

test_device_t test_toilet_device_list[NRF_SDH_BLE_CENTRAL_LINK_COUNT];
test_device_t test_shower_device_list[NRF_SDH_BLE_CENTRAL_LINK_COUNT];
test_device_t m_aq_device[NRF_SDH_BLE_CENTRAL_LINK_COUNT];

ble_aqss_c_t *get_shower_le_dev_instance(uint16_t conn_handle) {
    return &m_ble_aqss_c[conn_handle];
}

ble_aqts_c_t *get_toilet_le_dev_instance(uint16_t conn_handle) {
    return &m_ble_aqts_c[conn_handle];
}



test_device_t *get_aq_dev_instance(uint16_t conn_handle) { return &m_aq_device[conn_handle]; }

/**@brief Function for handling database discovery events.
 *
 * @details This function is a callback function to handle events from the database discovery
 * module. Depending on the UUIDs that are discovered, this function forwards the events to
 * their respective services.
 *
 * @param[in] p_event  Pointer to the database discovery event.
 */
static void db_disc_handler(ble_db_discovery_evt_t *p_evt) {
    NRF_LOG_DEBUG("Disc Handler: evt type: %d", p_evt->evt_type);
    ble_dis_c_on_db_disc_evt(&m_ble_dis_c[p_evt->conn_handle], p_evt);
    ble_aqss_c_on_db_disc_evt(&m_ble_aqss_c[p_evt->conn_handle], p_evt);
    ble_aqts_c_on_db_disc_evt(&m_ble_aqts_c[p_evt->conn_handle], p_evt);
}


/**@brief Function for handling BLE Stack events that are related to central application.
 *
 * @details This function keeps the connection handles of central application up-to-date. It
 * parses scanning reports, initiates a connection attempt to peripherals when a target UUID
 * is found, and manages connection parameter update requests. Additionally, it updates the
 * status of LEDs used to report the central application's activity.
 *
 * @note        Since this function updates connection handles, @ref BLE_GAP_EVT_DISCONNECTED
 * events must be dispatched to the target application before invoking this function.
 *
 * @param[in]   p_ble_evt   Bluetooth stack event.
 */
void on_ble_central_evt(ble_evt_t const *p_ble_evt) {
    ble_gap_evt_t const *p_gap_evt = &p_ble_evt->evt.gap_evt;
    ret_code_t            err_code;


    switch (p_ble_evt->header.evt_id) {
        // Upon connection, check which peripheral is connected (HR or RSC), initiate DB
        //  discovery, update LEDs status, and resume scanning, if necessary.
        case BLE_GAP_EVT_CONNECTED: {
            NRF_LOG_INFO("CENTRAL: Connected, handle: %d.", p_gap_evt->conn_handle);
		//memcpy(send_device_list[0], &p_ble_evt->evt.gap_evt.params.adv_report.data.p_data[2], 22);
            // Discover peer's services.
            err_code = ble_db_discovery_start(&m_db_disc, p_ble_evt->evt.gap_evt.conn_handle);
            if(err_code != NRF_ERROR_BUSY) {
                APP_ERROR_CHECK(err_code);
            }

            if (ble_conn_state_central_conn_count() < NRF_SDH_BLE_CENTRAL_LINK_COUNT) {
                scan_start();
            }
        }
            break; // BLE_GAP_EVT_CONNECTED

        // Upon disconnection, reset the connection handle of the peer that disconnected, update
        // the status of LEDs, and start scanning again.
        case BLE_GAP_EVT_DISCONNECTED: {
            NRF_LOG_INFO("CENTRAL: Disconnected, handle: %d, reason: 0x%x",
                         p_gap_evt->conn_handle, p_gap_evt->params.disconnected.reason);
            //connect_count--;
            /* Send  RSSI = -100 to indicate disconnected for conn_handle */
            repeater_rssi_update(p_gap_evt->conn_handle, -100);

            /* Clear test device context */
            NRF_LOG_INFO("CENTRAL: Clear test device context");
            m_aq_device[p_gap_evt->conn_handle].device_model = NO_MODEL;
            m_aq_device[p_gap_evt->conn_handle].conn_handle = BLE_CONN_HANDLE_INVALID;
            
            memset(m_aq_device[p_gap_evt->conn_handle].device_id, 0x00, sizeof(m_aq_device[p_gap_evt->conn_handle].device_id));

            memset(m_aq_device[p_gap_evt->conn_handle].device_model_name, 0x00, 7);

            if (ble_conn_state_central_conn_count() < NRF_SDH_BLE_CENTRAL_LINK_COUNT) {
                scan_start();
            }
        }
            break; // BLE_GAP_EVT_DISCONNECTED

        case BLE_GAP_EVT_TIMEOUT: {
            // Timeout for scanning is not specified, so only connection attemps can time out.
            if (p_gap_evt->params.timeout.src == BLE_GAP_TIMEOUT_SRC_CONN) {
                NRF_LOG_DEBUG("CENTRAL: Connection Request timed out.");
            }
        }
            break;

        case BLE_GAP_EVT_CONN_PARAM_UPDATE_REQUEST: {
            // Accept parameters requested by peer.
            err_code = sd_ble_gap_conn_param_update(p_gap_evt->conn_handle,
                                        &p_gap_evt->params.conn_param_update_request.conn_params);
            APP_ERROR_CHECK(err_code);
        }
            break;

        case BLE_GATTC_EVT_TIMEOUT:
            // Disconnect on GATT Client timeout event.
            NRF_LOG_DEBUG("CENTRAL: GATT Client Timeout.");
            NRF_LOG_INFO("BLE_GATTC_EVT_TIMEOUT");
            err_code = sd_ble_gap_disconnect(p_ble_evt->evt.gattc_evt.conn_handle,
                                             BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
            APP_ERROR_CHECK(err_code);
            break;

        case BLE_GATTS_EVT_TIMEOUT:
            // Disconnect on GATT Server timeout event.
            NRF_LOG_DEBUG("CENTRAL: GATT Server Timeout.");
            NRF_LOG_INFO("BLE_GATTS_EVT_TIMEOUT");
            err_code = sd_ble_gap_disconnect(p_ble_evt->evt.gatts_evt.conn_handle,
                                             BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
            APP_ERROR_CHECK(err_code);
            break;
        case BLE_GAP_EVT_RSSI_CHANGED: {
            int8_t rssi;
            uint8_t chn_index;
            sd_ble_gap_rssi_get(p_gap_evt->conn_handle, &rssi, &chn_index);
            repeater_rssi_update(p_gap_evt->conn_handle, rssi);
            break;
        }
       case BLE_GAP_EVT_ADV_REPORT:
	{
          char name[32];
	  uint16_t offset = 0;
	  uint16_t length = ble_advdata_search(p_ble_evt->evt.gap_evt.params.adv_report.data.p_data, p_ble_evt->evt.gap_evt.params.adv_report.data.len, &offset, BLE_GAP_AD_TYPE_COMPLETE_LOCAL_NAME);
	 // if(length == 0){
		//length = ble_advdata_search(p_ble_evt->evt.gap_evt.params.adv_report.data.p_data, p_ble_evt->evt.gap_evt.params.adv_report.data.len, &offset, BLE_GAP_AD_TYPE_SHORT_LOCAL_NAME);
		//}

       	if(length != 0){
		memcpy(name, &p_ble_evt->evt.gap_evt.params.adv_report.data.p_data[offset], length);
               // name[length] = '\0';
        	//scan_debug NRF_LOG_INFO("333 name: %s - %d, offset - %d\n", name,length,offset);
		}
         break;
	}
        default:
            // No implementation needed.
            break;
    }
}

/**@brief Function for handling Scanning Module events.
 */
static void scan_evt_handler(scan_evt_t const *p_scan_evt) {
    ret_code_t err_code;

    switch (p_scan_evt->scan_evt_id) {
    case NRF_BLE_SCAN_EVT_CONNECTING_ERROR: {
        err_code = p_scan_evt->params.connecting_err.err_code;
        APP_ERROR_CHECK(err_code);
    }
    break;

    case NRF_BLE_SCAN_EVT_CONNECTED: {
            ble_gap_evt_connected_t const *p_connected =
            p_scan_evt->params.connected.p_connected;
        // Scan is automatically stopped by the connection.
            NRF_LOG_INFO("Connecting to target %02x%02x%02x%02x%02x%02x",
                         p_connected->peer_addr.addr[0], p_connected->peer_addr.addr[1],
                         p_connected->peer_addr.addr[2], p_connected->peer_addr.addr[3],
                         p_connected->peer_addr.addr[4], p_connected->peer_addr.addr[5]);
    }
    break;

    case NRF_BLE_SCAN_EVT_SCAN_TIMEOUT: {
        NRF_LOG_INFO("Scan timed out.");
        scan_start();
    }
    break;

    default:
        break;
    }
}


/**@brief Function to start scanning. */
void scan_start(void) {
    ret_code_t ret;

    ret = nrf_ble_scan_start(&m_scan);
    APP_ERROR_CHECK(ret);
}



void scan_init_new(void) {
    ret_code_t ret;
    nrf_ble_scan_init_t init_scan;
    ret_code_t          err_code;

        for (int i = 0; i < 5; ++i) 
        {
            NRF_LOG_INFO("list = %s ", array_device_list[i]);
        }


  //if(received_list == 1)
  //{
    err_code = nrf_ble_scan_filters_disable(&m_scan);
    APP_ERROR_CHECK(err_code);


    memset(&init_scan, 0, sizeof(init_scan));

    init_scan.connect_if_match = true;
    init_scan.conn_cfg_tag     = APP_BLE_CONN_CFG_TAG;

    err_code = nrf_ble_scan_init(&m_scan, &init_scan, scan_evt_handler);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_ble_scan_filter_set(&m_scan, SCAN_UUID_FILTER, &m_aqss_uuid);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_ble_scan_filter_set(&m_scan, SCAN_UUID_FILTER, &m_aqts_uuid);
    APP_ERROR_CHECK(err_code);



   for (int i=0; i< NRF_BLE_SCAN_NAME_CNT; i++){
     err_code = nrf_ble_scan_filter_set(&m_scan, SCAN_NAME_FILTER, array_device_list[i]);
    APP_ERROR_CHECK(err_code);
    }

    //err_code = nrf_ble_scan_filter_set(&m_scan, SCAN_NAME_FILTER, arr1);
    //APP_ERROR_CHECK(err_code);

    err_code = nrf_ble_scan_filters_enable(&m_scan, NRF_BLE_SCAN_NAME_FILTER && NRF_BLE_SCAN_UUID_FILTER, false);
    APP_ERROR_CHECK(err_code);

   
  //}
}

/**@brief Function to stop scanning. */
void scan_stop(void) {
  nrf_ble_scan_stop();
}
#define NUMBER_OF_STRING 5
#define MAX_STRING_SIZE 40



/**@brief Function for initializing the scanning and setting the filters.
only for testing purpose
 */
void scan_init(void) {
    ret_code_t          err_code;
    nrf_ble_scan_init_t init_scan;

    char arr[NUMBER_OF_STRING][MAX_STRING_SIZE] =
    { "test-AQ100WCD00000011",
      "test-AQ100WCD00000012",
      "test-AQ100WCD00000013",
      "test-AQ100WCD00000014"
    };

    memset(&init_scan, 0, sizeof(init_scan));

    init_scan.connect_if_match = true;
    init_scan.conn_cfg_tag     = APP_BLE_CONN_CFG_TAG;

    err_code = nrf_ble_scan_init(&m_scan, &init_scan, scan_evt_handler);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_ble_scan_filter_set(&m_scan, SCAN_UUID_FILTER, &m_aqss_uuid);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_ble_scan_filter_set(&m_scan, SCAN_UUID_FILTER, &m_aqts_uuid);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_ble_scan_filter_set(&m_scan, SCAN_NAME_FILTER, arr);
    APP_ERROR_CHECK(err_code);

   // err_code = nrf_ble_scan_filters_enable(&m_scan, NRF_BLE_SCAN_UUID_FILTER, false);
    err_code = nrf_ble_scan_filters_enable(&m_scan, NRF_BLE_SCAN_NAME_FILTER && NRF_BLE_SCAN_UUID_FILTER, false);
   // err_code = nrf_ble_scan_filters_enable(&m_scan, NRF_BLE_SCAN_NAME_FILTER, false);

    //err_code = nrf_ble_scan_filters_enable(&m_scan, NRF_BLE_SCAN_ALL_FILTER, true);
    APP_ERROR_CHECK(err_code);

    err_code = sd_ble_gap_tx_power_set(BLE_GAP_TX_POWER_ROLE_SCAN_INIT, BLE_CONN_HANDLE_INVALID, 8); 
    APP_ERROR_CHECK(err_code);

}

/** @brief Function for initializing the database discovery module. */
void db_discovery_init(void) {
    ble_db_discovery_init_t db_init;

    memset(&db_init, 0, sizeof(ble_db_discovery_init_t));

    db_init.evt_handler  = db_disc_handler;
    db_init.p_gatt_queue = &m_ble_gatt_queue;

    ret_code_t err_code = ble_db_discovery_init(&db_init);
    APP_ERROR_CHECK(err_code);
}

repeater_task_evt_t rtevt;

/**@brief Function for handling notification received by the Aquai Shower Service (AQSS).
 *
 * @details This function takes a list of characters of length data_len and prints the
 * characters out on UART. If @ref ECHOBACK_BLE_UART_DATA is set, the data is sent back to
 * sender.
 */
static void ble_aqss_chars_received_data(
        uint16_t conn_handle, uint8_t *p_data, uint16_t data_len) {
    
    rtevt.evt_id = REPEATER_TASK_EVT_SHOWER_DATA;
    rtevt.conn_handle = conn_handle;
    if (data_len < MAX_FRAME_PAYLOAD_SIZE) {
        memcpy(rtevt.payload, p_data, data_len);
        rtevt.len = data_len;
    } else {
        memcpy(rtevt.payload, p_data, MAX_FRAME_PAYLOAD_SIZE);
        rtevt.len = MAX_FRAME_PAYLOAD_SIZE;
        NRF_LOG_INFO("%d Error: Payload too large: %d", __LINE__, data_len);
    }
    repeater_task_queue_event(&rtevt);
}

/**@brief Function for handling notification received by the Aquai Toilet Service (AQTS).
 *
 * @details This function takes a list of characters of length data_len and prints the
 * characters out on UART. If @ref ECHOBACK_BLE_UART_DATA is set, the data is sent back to
 * sender.
 */
static void ble_aqts_chars_received_data(
        uint16_t conn_handle, uint8_t *p_data, uint16_t data_len) {
    
    rtevt.evt_id = REPEATER_TASK_EVT_TOILET_DATA;
    rtevt.conn_handle = conn_handle;
    if (data_len < MAX_FRAME_PAYLOAD_SIZE) {
        memcpy(rtevt.payload, p_data, data_len);
        rtevt.len = data_len;
    } else {
        memcpy(rtevt.payload, p_data, MAX_FRAME_PAYLOAD_SIZE);
        NRF_LOG_INFO("%d Error: Payload too large: %d", __LINE__, data_len);
        rtevt.len = MAX_FRAME_PAYLOAD_SIZE;
    }
    repeater_task_queue_event(&rtevt);    
}

/**@brief Callback handling Aquai Toilet Service (AQTS) client events.
 *
 * @details This function is called to notify the application of AQTS client events.
 *
 * @param[in]   p_ble_aqts_c   AQTS client handle. This identifies the AQTS client.
 * @param[in]   p_ble_aqts_evt Pointer to the AQTS client event.
 */

/**@snippet [Handling events from the ble_nus_c module] */
static void ble_aqts_c_evt_handler(
        ble_aqts_c_t *p_ble_aqts_c, ble_aqts_c_evt_t const *p_ble_aqts_evt) {
    ret_code_t err_code;

    switch (p_ble_aqts_evt->evt_type) {
    case BLE_AQTS_C_EVT_DISCOVERY_COMPLETE:
        NRF_LOG_INFO("Discovery complete.");
            err_code = ble_aqts_c_handles_assign(
                    p_ble_aqts_c, p_ble_aqts_evt->conn_handle, &p_ble_aqts_evt->handles);
        APP_ERROR_CHECK(err_code);

            if (m_aq_device[p_ble_aqts_evt->conn_handle].device_id[0] != 0x00) {
        err_code = ble_aqts_c_tx_notif_enable(p_ble_aqts_c);
        APP_ERROR_CHECK(err_code);
                NRF_LOG_INFO("Connected to AQ1xxWC device with AQTS Service.");
            }

            m_aq_device[p_ble_aqts_evt->conn_handle].conn_handle = p_ble_aqts_evt->conn_handle;
            m_aq_device[p_ble_aqts_evt->conn_handle].device_model = AQ_1xx_WC;

        break;

    case BLE_AQTS_C_EVT_AQTS_TX_EVT:
            ble_aqts_chars_received_data(
                    p_ble_aqts_c->conn_handle, p_ble_aqts_evt->p_data, p_ble_aqts_evt->data_len);
        break;

    case BLE_AQTS_C_EVT_DISCONNECTED:
        NRF_LOG_INFO("Disconnected AQTS.");
        scan_start();
        break;
    }
}

/**@brief Callback handling Aquai Shower Service (AQSS) client events.
 *
 * @details This function is called to notify the application of AQSS client events.
 *
 * @param[in]   p_ble_aqss_c   AQSS client handle. This identifies the AQSS client.
 * @param[in]   p_ble_aqss_evt Pointer to the AQSS client event.
 */

/**@snippet [Handling events from the ble_aqss_c module] */
static void ble_aqss_c_evt_handler(
        ble_aqss_c_t *p_ble_aqss_c, ble_aqss_c_evt_t const *p_ble_aqss_evt) {
    ret_code_t err_code;

    switch (p_ble_aqss_evt->evt_type) {
    case BLE_AQSS_C_EVT_DISCOVERY_COMPLETE:
        NRF_LOG_INFO("Discovery complete.");
            err_code = ble_aqss_c_handles_assign(
                    p_ble_aqss_c, p_ble_aqss_evt->conn_handle, &p_ble_aqss_evt->handles);
        APP_ERROR_CHECK(err_code);

        err_code = ble_aqss_c_tx_notif_enable(p_ble_aqss_c);
        APP_ERROR_CHECK(err_code);
            NRF_LOG_INFO("Connected to AQ3xxSH device with AQSS service.");

            m_aq_device[p_ble_aqss_evt->conn_handle].conn_handle = p_ble_aqss_evt->conn_handle;
            m_aq_device[p_ble_aqss_evt->conn_handle].device_model = AQ_3xx_SH;


        break;

    case BLE_AQSS_C_EVT_AQSS_TX_EVT:
            ble_aqss_chars_received_data(
                    p_ble_aqss_c->conn_handle, p_ble_aqss_evt->p_data, p_ble_aqss_evt->data_len);
        break;

    case BLE_AQSS_C_EVT_DISCONNECTED:
        NRF_LOG_INFO("Disconnected AQSS.");
        scan_start();
        break;
    }
}


/**@brief Callback handling DIS client events.
 *
 * @details This function is called to notify the application of DIS client events.
 *
 * @param[in]   p_ble_aqts_c   DIS client handle. This identifies the DIS client.
 * @param[in]   p_ble_aqts_evt Pointer to the DIS client event.
 */
static void ble_disc_evt_handler(
        ble_dis_c_t *p_ble_dis_c, ble_dis_c_evt_t const *p_ble_dis_evt) {
    ret_code_t err_code;
    char arr_1[5];

    switch (p_ble_dis_evt->evt_type) {
        case BLE_DIS_C_EVT_DISCOVERY_COMPLETE:
            NRF_LOG_INFO("Discovery complete.");
            err_code = ble_dis_c_handles_assign(
                    p_ble_dis_c, p_ble_dis_evt->conn_handle, &p_ble_dis_evt->params.read_rsp);
            APP_ERROR_CHECK(err_code);

            err_code = ble_dis_c_read(p_ble_dis_c, BLE_DIS_C_SERIAL_NUM);
            APP_ERROR_CHECK(err_code);
            NRF_LOG_INFO("Connected to device with DIS Service.");
            break;
        case BLE_DIS_C_EVT_DIS_C_READ_RSP:
            NRF_LOG_INFO("Disc read");
            // todo:
            if (p_ble_dis_evt->params.read_rsp.char_type == BLE_DIS_C_SERIAL_NUM) {
                memcpy(m_aq_device[p_ble_dis_evt->conn_handle].device_id,
                       p_ble_dis_evt->params.read_rsp.content.string.p_data, 16);

                /* Start RSSI */
                sd_ble_gap_rssi_start(p_ble_dis_evt->conn_handle, 10, 5);

                if (m_aq_device[p_ble_dis_evt->conn_handle].device_model == AQ_1xx_WC) {
                    err_code = ble_aqts_c_tx_notif_enable(
                            get_toilet_le_dev_instance(p_ble_dis_evt->conn_handle));
                    APP_ERROR_CHECK(err_code);
                    NRF_LOG_INFO("Connected to AQ1xxWC device with AQTS Service.");
               }

                if (m_aq_device[p_ble_dis_evt->conn_handle].device_model == AQ_3xx_SH) {
                    err_code = ble_aqss_c_tx_notif_enable(
                            get_shower_le_dev_instance(p_ble_dis_evt->conn_handle));
                    APP_ERROR_CHECK(err_code);
                    NRF_LOG_INFO("Connected to AQ3xxSH device with AQSS service.");              

                }

            }
            if (p_ble_dis_evt->params.read_rsp.char_type == BLE_DIS_C_MODEL_NUM) {
                memcpy(m_aq_device[p_ble_dis_evt->conn_handle].device_model_name,
                       p_ble_dis_evt->params.read_rsp.content.string.p_data, 3);
            }          	

            break;
        case BLE_DIS_C_EVT_DISCONNECTED:
            NRF_LOG_INFO("Disconnected DIS.");
            break;
    }
}
/**@snippet [Handling events from the ble_nus_c module] */


/**@brief Function for handling the Aquai Shower Service Client errors.
 *
 * @param[in]   nrf_error   Error code containing information about what went wrong.
 */
static void aqss_error_handler(uint32_t nrf_error) { APP_ERROR_HANDLER(nrf_error); }


/**@brief Function for handling the Aquai Toilet Service Client errors.
 *
 * @param[in]   nrf_error   Error code containing information about what went wrong.
 */
static void aqts_error_handler(uint32_t nrf_error) { APP_ERROR_HANDLER(nrf_error); }

/**@brief Function for handling the DIS Client errors.
 *
 * @param[in]   nrf_error   Error code containing information about what went wrong.
 */
static void disc_error_handler(uint32_t nrf_error) { APP_ERROR_HANDLER(nrf_error); }

/**@brief Function for initializing the Aquai Shower Service (AQSS) client. */
static void aqss_c_init(void) {
    ret_code_t       err_code;
    ble_aqss_c_init_t init;

    init.evt_handler   = ble_aqss_c_evt_handler;
    init.error_handler = aqss_error_handler;
    init.p_gatt_queue  = &m_ble_gatt_queue;

    uint32_t i = 0;
    for (i = 0; i < NRF_SDH_BLE_CENTRAL_LINK_COUNT; i++) {
        err_code = ble_aqss_c_init(&m_ble_aqss_c[i], &init);
        APP_ERROR_CHECK(err_code);
    }
}

/**@brief Function for initializing the Aquai Toilet Service (AQTS) client. */
static void aqts_c_init(void) {
    ret_code_t       err_code;
    ble_aqts_c_init_t init;

    init.evt_handler   = ble_aqts_c_evt_handler;
    init.error_handler = aqts_error_handler;
    init.p_gatt_queue  = &m_ble_gatt_queue;

    uint32_t i = 0;
    for (i = 0; i < NRF_SDH_BLE_CENTRAL_LINK_COUNT; i++) {
        err_code = ble_aqts_c_init(&m_ble_aqts_c[i], &init);
        APP_ERROR_CHECK(err_code);
    }
}

/**@brief Function for initializing the DIS client. */
static void dis_c_init(void) {
    ret_code_t err_code;
    ble_dis_c_init_t init;
    ble_dis_c_char_group_t char_group;
    ble_dis_c_char_type_t char_type[] = {BLE_DIS_C_MANUF_NAME, BLE_DIS_C_MODEL_NUM,
                                         BLE_DIS_C_SERIAL_NUM, BLE_DIS_C_HW_REV, BLE_DIS_C_FW_REV};

    char_group.size = 5;
    char_group.p_char_type = char_type;

    init.evt_handler = ble_disc_evt_handler;
    init.error_handler = disc_error_handler;
    init.p_gatt_queue = &m_ble_gatt_queue;
    init.char_group = char_group;
    

    uint32_t i = 0;
    for (i = 0; i < NRF_SDH_BLE_CENTRAL_LINK_COUNT; i++) {
        err_code = ble_dis_c_init(&m_ble_dis_c[i], &init);
        APP_ERROR_CHECK(err_code);
    }
}

void init_client_services(void) {
    /* initialize the Client Services */
    aqss_c_init();
    aqts_c_init();
    dis_c_init();

    /* Init test connection context */
    for (uint8_t i = 0; i < NRF_SDH_BLE_CENTRAL_LINK_COUNT; i++) {
        m_aq_device[i].p_ble_aqss_c = &m_ble_aqss_c[i];
        m_aq_device[i].p_ble_aqts_c = &m_ble_aqts_c[i];
        m_aq_device[i].p_ble_dis_c = &m_ble_dis_c[i];
        memset(m_aq_device[i].device_id, 0x00, 16);
        memset(m_aq_device[i].device_model_name, 0x00, 7);
        m_aq_device[i].conn_handle = 0xFFFF;
        // APP_ERROR_CHECK(err_code);
    }
}

#ifdef __cplusplus
}
#endif

The gatt_error_handler handler is called from 2 functions from the service initialization file.

Fullscreen service1.c Download

/**
 
 */

#include "sdk_common.h"

#include <stdlib.h>

#include "ble.h"
#include "ble_aqss_c.h"
#include "ble_gattc.h"
#include "ble_srv_common.h"
#include "app_error.h"

#include "ble_aqss.h"

#define NRF_LOG_MODULE_NAME ble_aqss_c
#include "nrf_log.h"
NRF_LOG_MODULE_REGISTER();


/**@brief Function for intercepting the errors of GATTC and the BLE GATT Queue.
 *
 * @param[in] nrf_error   Error code.
 * @param[in] p_ctx       Parameter from the event handler.
 * @param[in] conn_handle Connection handle.
 */
static void gatt_error_handler(uint32_t   nrf_error,
                               void     * p_ctx,
                               uint16_t   conn_handle)
{
    ble_aqss_c_t * p_ble_aqss_c = (ble_aqss_c_t *)p_ctx;

    NRF_LOG_DEBUG("A GATT Client error has occurred on conn_handle: 0X%X", conn_handle);

    if (p_ble_aqss_c->error_handler != NULL)
    {
        p_ble_aqss_c->error_handler(nrf_error);
    }
}


void ble_aqss_c_on_db_disc_evt(ble_aqss_c_t * p_ble_aqss_c, ble_db_discovery_evt_t * p_evt)
{
    ble_aqss_c_evt_t aqss_c_evt;
    memset(&aqss_c_evt,0,sizeof(ble_aqss_c_evt_t));

    ble_gatt_db_char_t * p_chars = p_evt->params.discovered_db.charateristics;

    // Check if the AQSS was discovered.
    if (    (p_evt->evt_type == BLE_DB_DISCOVERY_COMPLETE)
        &&  (p_evt->params.discovered_db.srv_uuid.uuid == BLE_UUID_AQSS_SERVICE)
        &&  (p_evt->params.discovered_db.srv_uuid.type == p_ble_aqss_c->uuid_type))
    {
        for (uint32_t i = 0; i < p_evt->params.discovered_db.char_count; i++)
        {
            switch (p_chars[i].characteristic.uuid.uuid)
            {
                case BLE_UUID_AQSS_SETTING_CHARACTERISTIC:
                    aqss_c_evt.handles.aqss_rx_handle = p_chars[i].characteristic.handle_value;
                    break;

                case BLE_UUID_AQSS_NOTIF_CHARACTERISTIC:
                    aqss_c_evt.handles.aqss_tx_handle = p_chars[i].characteristic.handle_value;
                    aqss_c_evt.handles.aqss_tx_cccd_handle = p_chars[i].cccd_handle;
                    break;

                default:
                    break;
            }
        }
        if (p_ble_aqss_c->evt_handler != NULL)
        {
            aqss_c_evt.conn_handle = p_evt->conn_handle;
            aqss_c_evt.evt_type    = BLE_AQSS_C_EVT_DISCOVERY_COMPLETE;
            p_ble_aqss_c->evt_handler(p_ble_aqss_c, &aqss_c_evt);
        }
    }
}

/**@brief     Function for handling Handle Value Notification received from the SoftDevice.
 *
 * @details   This function uses the Handle Value Notification received from the SoftDevice
 *            and checks if it is a notification of the AQSS TX characteristic from the peer.
 *            If it is, this function decodes the data and sends it to the application.
 *            
 * @param[in] p_ble_aqss_c Pointer to the AQSS Client structure.
 * @param[in] p_ble_evt   Pointer to the BLE event received.
 */
static void on_hvx(ble_aqss_c_t * p_ble_aqss_c, ble_evt_t const * p_ble_evt)
{
    // HVX can only occur from client sending.
    if (   (p_ble_aqss_c->handles.aqss_tx_handle != BLE_GATT_HANDLE_INVALID)
        && (p_ble_evt->evt.gattc_evt.params.hvx.handle == p_ble_aqss_c->handles.aqss_tx_handle)
        && (p_ble_aqss_c->evt_handler != NULL))
    {
        ble_aqss_c_evt_t ble_aqss_c_evt;

        ble_aqss_c_evt.evt_type = BLE_AQSS_C_EVT_AQSS_TX_EVT;
        ble_aqss_c_evt.p_data   = (uint8_t *)p_ble_evt->evt.gattc_evt.params.hvx.data;
        ble_aqss_c_evt.data_len = p_ble_evt->evt.gattc_evt.params.hvx.len;

        p_ble_aqss_c->evt_handler(p_ble_aqss_c, &ble_aqss_c_evt);
        NRF_LOG_DEBUG("Client sending data.");
    }
}

uint32_t ble_aqss_c_init(ble_aqss_c_t * p_ble_aqss_c, ble_aqss_c_init_t * p_ble_aqss_c_init)
{
    uint32_t      err_code;
    ble_uuid_t    uart_uuid;
    ble_uuid128_t aqss_base_uuid = AQSS_BASE_UUID;

    VERIFY_PARAM_NOT_NULL(p_ble_aqss_c);
    VERIFY_PARAM_NOT_NULL(p_ble_aqss_c_init);
    VERIFY_PARAM_NOT_NULL(p_ble_aqss_c_init->p_gatt_queue);

    err_code = sd_ble_uuid_vs_add(&aqss_base_uuid, &p_ble_aqss_c->uuid_type);
    VERIFY_SUCCESS(err_code);

    uart_uuid.type = p_ble_aqss_c->uuid_type;
    uart_uuid.uuid = BLE_UUID_AQSS_SERVICE;

    p_ble_aqss_c->conn_handle           = BLE_CONN_HANDLE_INVALID;
    p_ble_aqss_c->evt_handler           = p_ble_aqss_c_init->evt_handler;
    p_ble_aqss_c->error_handler         = p_ble_aqss_c_init->error_handler;
    p_ble_aqss_c->handles.aqss_tx_handle = BLE_GATT_HANDLE_INVALID;
    p_ble_aqss_c->handles.aqss_rx_handle = BLE_GATT_HANDLE_INVALID;
    p_ble_aqss_c->p_gatt_queue          = p_ble_aqss_c_init->p_gatt_queue;

    return ble_db_discovery_evt_register(&uart_uuid);
}

void ble_aqss_c_on_ble_evt(ble_evt_t const * p_ble_evt, void * p_context)
{
    ble_aqss_c_t * p_ble_aqss_c = (ble_aqss_c_t *)p_context;

    if ((p_ble_aqss_c == NULL) || (p_ble_evt == NULL))
    {
        return;
    }

    if ( (p_ble_aqss_c->conn_handle == BLE_CONN_HANDLE_INVALID)
       ||(p_ble_aqss_c->conn_handle != p_ble_evt->evt.gap_evt.conn_handle)
       )
    {
        return;
    }

    switch (p_ble_evt->header.evt_id)
    {
        case BLE_GATTC_EVT_HVX:
            on_hvx(p_ble_aqss_c, p_ble_evt);
            break;

        case BLE_GAP_EVT_DISCONNECTED:
            if (p_ble_evt->evt.gap_evt.conn_handle == p_ble_aqss_c->conn_handle
                    && p_ble_aqss_c->evt_handler != NULL)
            {
                ble_aqss_c_evt_t aqss_c_evt;

                aqss_c_evt.evt_type = BLE_AQSS_C_EVT_DISCONNECTED;

                p_ble_aqss_c->conn_handle = BLE_CONN_HANDLE_INVALID;
                p_ble_aqss_c->evt_handler(p_ble_aqss_c, &aqss_c_evt);
            }
            break;

        default:
            // No implementation needed.
            break;
    }
}

/**@brief Function for creating a message for writing to the CCCD. */
static uint32_t cccd_configure(ble_aqss_c_t * p_ble_aqss_c, bool notification_enable)
{
    nrf_ble_gq_req_t cccd_req;
    uint8_t          cccd[BLE_CCCD_VALUE_LEN];
    uint16_t         cccd_val = notification_enable ? BLE_GATT_HVX_NOTIFICATION : 0;

    memset(&cccd_req, 0, sizeof(nrf_ble_gq_req_t));

    cccd[0] = LSB_16(cccd_val);
    cccd[1] = MSB_16(cccd_val);

    cccd_req.type                        = NRF_BLE_GQ_REQ_GATTC_WRITE;
    cccd_req.error_handler.cb            = gatt_error_handler;
    cccd_req.error_handler.p_ctx         = p_ble_aqss_c;
    cccd_req.params.gattc_write.handle   = p_ble_aqss_c->handles.aqss_tx_cccd_handle;
    cccd_req.params.gattc_write.len      = BLE_CCCD_VALUE_LEN;
    cccd_req.params.gattc_write.offset   = 0;
    cccd_req.params.gattc_write.p_value  = cccd;
    cccd_req.params.gattc_write.write_op = BLE_GATT_OP_WRITE_REQ;
    cccd_req.params.gattc_write.flags    = BLE_GATT_EXEC_WRITE_FLAG_PREPARED_WRITE;

    return nrf_ble_gq_item_add(p_ble_aqss_c->p_gatt_queue, &cccd_req, p_ble_aqss_c->conn_handle);
}


uint32_t ble_aqss_c_tx_notif_enable(ble_aqss_c_t * p_ble_aqss_c)
{
    VERIFY_PARAM_NOT_NULL(p_ble_aqss_c);

    if ( (p_ble_aqss_c->conn_handle == BLE_CONN_HANDLE_INVALID)
       ||(p_ble_aqss_c->handles.aqss_tx_cccd_handle == BLE_GATT_HANDLE_INVALID)
       )
    {
        return NRF_ERROR_INVALID_STATE;
    }
    return cccd_configure(p_ble_aqss_c, true);
}


uint32_t ble_aqss_c_setting_send(ble_aqss_c_t * p_ble_aqss_c, uint8_t * p_string, uint16_t length)
{
    VERIFY_PARAM_NOT_NULL(p_ble_aqss_c);

    nrf_ble_gq_req_t write_req;

    memset(&write_req, 0, sizeof(nrf_ble_gq_req_t));

    if (length > BLE_AQSS_MAX_DATA_LEN)
    {
        NRF_LOG_WARNING("Content too long.");
        return NRF_ERROR_INVALID_PARAM;
    }
    if (p_ble_aqss_c->conn_handle == BLE_CONN_HANDLE_INVALID)
    {
        NRF_LOG_WARNING("Connection handle invalid.");
        return NRF_ERROR_INVALID_STATE;
    }

    write_req.type                        = NRF_BLE_GQ_REQ_GATTC_WRITE;
    write_req.error_handler.cb            = gatt_error_handler;
    write_req.error_handler.p_ctx         = p_ble_aqss_c;
    write_req.params.gattc_write.handle   = p_ble_aqss_c->handles.aqss_rx_handle;
    write_req.params.gattc_write.len      = length;
    write_req.params.gattc_write.offset   = 0;
    write_req.params.gattc_write.p_value  = p_string;
    write_req.params.gattc_write.write_op = BLE_GATT_OP_WRITE_CMD;
    write_req.params.gattc_write.flags    = BLE_GATT_EXEC_WRITE_FLAG_PREPARED_WRITE;

    return nrf_ble_gq_item_add(p_ble_aqss_c->p_gatt_queue, &write_req, p_ble_aqss_c->conn_handle);
}


uint32_t ble_aqss_c_handles_assign(ble_aqss_c_t               * p_ble_nus,
                                  uint16_t                    conn_handle,
                                  ble_aqss_c_handles_t const * p_peer_handles)
{
    VERIFY_PARAM_NOT_NULL(p_ble_nus);

    p_ble_nus->conn_handle = conn_handle;
    if (p_peer_handles != NULL)
    {
        p_ble_nus->handles.aqss_tx_cccd_handle = p_peer_handles->aqss_tx_cccd_handle;
        p_ble_nus->handles.aqss_tx_handle      = p_peer_handles->aqss_tx_handle;
        p_ble_nus->handles.aqss_rx_handle      = p_peer_handles->aqss_rx_handle;
    }
    return nrf_ble_gq_conn_handle_register(p_ble_nus->p_gatt_queue, conn_handle);
}

The request_err_code_handle is called in "request_process" & "queue_process" functions from where the error is seen in logs.

Regarding logs I will capture it today and send you. 

Thanks & Regards,

Snehal.

[Susheel Nuguru]

Thanks Snehal, 

Sniffer logs will help understand the problem better and faster.

[sne_333]

Sorry Susheel I dont have sniffer.