SDK15.2.0 firmware crashing if more than 21 bytes sent from the nrf connect app.

[Manoj Hiwarkar]

Earlier with SDK 13 in custom advertisement I was able to send 30 bytes under unknown characteristics tab in Nrf connect APP but in SDK 15 I am not. Because the firmware or app getting stuck if I sen more than  bytes.

[Simon]

Where exactly does your example get stuck? Could you try to debug the problem and figure it out? Could you also provide some logs? Also, if you could upload a snippet of the relevant code that would be nice.

Best regards,

Simon

[Manoj Hiwarkar]

/**
 * Copyright (c) 2014 - 2018, Nordic Semiconductor ASA
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form, except as embedded into a Nordic
 *    Semiconductor ASA integrated circuit in a product or a software update for
 *    such product, must reproduce the above copyright notice, this list of
 *    conditions and the following disclaimer in the documentation and/or other
 *    materials provided with the distribution.
 *
 * 3. Neither the name of Nordic Semiconductor ASA nor the names of its
 *    contributors may be used to endorse or promote products derived from this
 *    software without specific prior written permission.
 *
 * 4. This software, with or without modification, must only be used with a
 *    Nordic Semiconductor ASA integrated circuit.
 *
 * 5. Any software provided in binary form under this license must not be reverse
 *    engineered, decompiled, modified and/or disassembled.
 *
 * THIS SOFTWARE IS PROVIDED BY NORDIC SEMICONDUCTOR ASA "AS IS" AND ANY EXPRESS
 * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL NORDIC SEMICONDUCTOR ASA OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */
/** @file
 *
 * @defgroup ble_sdk_uart_over_ble_main main.c
 * @{
 * @ingroup  ble_sdk_app_nus_eval
 * @brief    UART over BLE application main file.
 *
 * This file contains the source code for a sample application that uses the Nordic UART service.
 * This application uses the @ref srvlib_conn_params module.
 */

#include <stdint.h>
#include <string.h>

#include "nordic_common.h"
#include "nrf.h"
#include "nrf_drv_saadc.h"
#include "nrf_drv_rtc.h"
#include "nrf_drv_power.h"
#include "nrf_drv_clock.h"
#include "ble_hci.h"
#include "ble_advdata.h"
#include "ble_advertising.h"
#include "ble_conn_params.h"
#include "nrf_sdh.h"
#include "nrf_sdh_soc.h"
#include "nrf_sdh_ble.h"
#include "nrf_ble_gatt.h"
#include "nrf_ble_qwr.h"
#include "app_timer.h"
#include "ble_nus.h"
#include "app_uart.h"
#include "app_util_platform.h"
#include "bsp_btn_ble.h"
#include "nrf_pwr_mgmt.h"
#include "ble_gap.h"
#include "boards.h"
#include "bsp_config.h"
#include "nrf_gpiote.h"
#include "app_scheduler.h"
#include "pca10040.h"
#include "boards.h"
#include "app_uart.h"
//#include "nrf_soc.h"
//#include "nrf_fstorage.h"
#include "nrf_delay.h"
//#include "nrf_fstorage_sd.h"

#if defined (UART_PRESENT)
#include "nrf_uart.h"
#endif
#if defined (UARTE_PRESENT)
#include "nrf_uarte.h"
#endif

#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"

#define APP_BLE_CONN_CFG_TAG            0                                           /**< A tag identifying the SoftDevice BLE configuration. */

#define DEVICE_NAME                     "Nordic_Uart"                               /**< Name of device. Will be included in the advertising data. */
//#define NUS_SERVICE_UUID_TYPE           BLE_UUID_TYPE_VENDOR_BEGIN                  /**< UUID type for the Nordic UART Service (vendor specific). */

#define APP_BLE_OBSERVER_PRIO           3                                           /**< Application's BLE observer priority. You shouldn't need to modify this value. */

#define APP_ADV_INTERVAL                1600                                        /**< The advertising interval (in units of 0.625 ms. This value corresponds to 40 ms). */

#define APP_ADV_DURATION                0                                       /**< The advertising duration (180 seconds) in units of 10 milliseconds. */

#define MIN_CONN_INTERVAL               MSEC_TO_UNITS(20, UNIT_1_25_MS)             /**< Minimum acceptable connection interval (20 ms), Connection interval uses 1.25 ms units. */
#define MAX_CONN_INTERVAL               MSEC_TO_UNITS(75, UNIT_1_25_MS)             /**< Maximum acceptable connection interval (75 ms), Connection interval uses 1.25 ms units. */
#define SLAVE_LATENCY                   0                                           /**< Slave latency. */
#define CONN_SUP_TIMEOUT                MSEC_TO_UNITS(20000, UNIT_10_MS)             /**< Connection supervisory timeout (4 seconds), Supervision Timeout uses 10 ms units. */
#define FIRST_CONN_PARAMS_UPDATE_DELAY  APP_TIMER_TICKS(100)                       /**< 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   APP_TIMER_TICKS(500)                      /**< Time between each call to sd_ble_gap_conn_param_update after the first call (30 seconds). */
#define MAX_CONN_PARAMS_UPDATE_COUNT    7                                          /**< Number of attempts before giving up the connection parameter negotiation. */

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

#define UART_TX_BUF_SIZE                256                                         /**< UART TX buffer size. */
#define UART_RX_BUF_SIZE                256                                         /**< UART RX buffer size. */
#define SEC_PARAM_BOND                   1                                           /**< Perform bonding. */
#define SEC_PARAM_MITM                   0                                           /**< Man In The Middle protection not required. */
#define SEC_PARAM_IO_CAPABILITIES        BLE_GAP_IO_CAPS_NONE                /**< Display Only capabilities. */
#define SEC_PARAM_OOB                    0                                           /**< Out Of Band data not available. */
#define SEC_PARAM_MIN_KEY_SIZE           7                                           /**< Minimum encryption key size. */
#define SEC_PARAM_MAX_KEY_SIZE           16                                          /**< Maximum encryption key size. */
#define U8_CMD_MAX_STRING				 10
#define U8_MAX_CMD_ENTRIES			     5
#define SUCCESS				     		 1
#define FAIL						     0
#define TRUE				             1
#define FALSE			              	 0
#define U8_MOTION_THRESHOLD				 30
#define U8_NO_MOTION_THRESHOLD			5
#define U8_MOTION_DETECT_WT 			5
#define U8_MAX_MOTION_DETECT_CNT		60
#define U8_MOTION_NOT_DETECT_WT			3
#define U8_NO_RPM_DETECTED				10
#define U8_NO_RPM_TRIPEND_CNT		    5
#define U8_NO_RPM_DETECT				2
#define U8_VALID_RPM					60
#define U8_RTC_RESOLUTION				30//(1000000/32768)
#define APP_TIMER_DELAY                  APP_TIMER_TICKS(1000)
#define TX_POWER_LEVEL                  (-12)             /**< (-12) 10 meter - TX Power Level value. This will be set both in the TX Power service, in the advertising data, and also used to set the radio transmit power. */


#define SAADC_CALIBRATION_INTERVAL 		5              //Determines how often the SAADC should be calibrated relative to NRF_DRV_SAADC_EVT_DONE event. E.g. value 5 will make the SAADC calibrate every fifth time the NRF_DRV_SAADC_EVT_DONE is received.
#define SAADC_SAMPLES_IN_BUFFER 	    4                 //Number of SAADC samples in RAM before returning a SAADC event. For low power SAADC set this constant to 1. Otherwise the EasyDMA will be enabled for an extended time which consumes high current.
#define SAADC_OVERSAMPLE 				NRF_SAADC_OVERSAMPLE_4X  //Oversampling setting for the SAADC. Setting oversample to 4x This will make the SAADC output a single averaged value when the SAMPLE task is triggered 4 times. Enable BURST mode to make the SAADC sample 4 times when triggering SAMPLE task once.
#define SAADC_BURST_MODE			    1                        //Set to 1 to enable BURST mode, otherwise set to 0.

#define ADC_REF_VOLTAGE_IN_MILLIVOLTS   600                                     /**< Reference voltage (in milli volts) used by ADC while doing conversion. */
#define ADC_PRE_SCALING_COMPENSATION    6                                       /**< The ADC is configured to use VDD with 1/3 prescaling as input. And hence the result of conversion is to be multiplied by 3 to get the actual value of the battery voltage.*/
#define DIODE_FWD_VOLT_DROP_MILLIVOLTS  270                                     /**< Typical forward voltage drop of the diode . */
#define ADC_RES_10BIT                   1024                                    /**< Maximum digital value for 10-bit ADC conversion. */
#define NON_CONNECTABLE_ADV_INTERVAL    MSEC_TO_UNITS(100, UNIT_0_625_MS)  /**< The advertising interval for non-connectable advertisement (100 ms). This value can vary between 100ms to 10.24s). */

#define APP_BEACON_INFO_LENGTH          0x1A                               /**< Total length of information advertised by the Beacon. */
#define APP_ADV_DATA_LENGTH             0x1A                               /**< Length of manufacturer specific data in the advertisement. */
#define APP_DEVICE_TYPE                 0x02                               /**< 0x02 refers to Beacon. */
#define APP_MEASURED_RSSI               0xC3                               /**< The Beacon's measured RSSI at 1 meter distance in dBm. */
#define APP_COMPANY_IDENTIFIER          0x0059                             /**< Company identifier for Nordic Semiconductor ASA. as per www.bluetooth.org. */
         /**< Proprietary UUID for Beacon. */

#define CONNECTABLE_BEACON  0x1f
#define NON_CONNECTABLE_BEACON  0x00

#define ADC_RESULT_IN_MILLI_VOLTS(ADC_VALUE)\
        ((((ADC_VALUE) * ADC_REF_VOLTAGE_IN_MILLIVOLTS) / ADC_RES_10BIT) * ADC_PRE_SCALING_COMPENSATION)


BLE_NUS_DEF(m_nus, NRF_SDH_BLE_TOTAL_LINK_COUNT);                                   /**< BLE NUS service instance. */
NRF_BLE_GATT_DEF(m_gatt);                                                           /**< GATT module instance. */
NRF_BLE_QWR_DEF(m_qwr);                                                             /**< Context for the Queued Write module.*/
BLE_ADVERTISING_DEF(m_advertising);                                                 /**< Advertising module instance. */
APP_TIMER_DEF(m_timer_id);

static uint16_t   m_conn_handle          = BLE_CONN_HANDLE_INVALID;                 /**< Handle of the current connection. */
static uint16_t   m_ble_nus_max_data_len = BLE_GATT_ATT_MTU_DEFAULT - 3;            /**< Maximum length of data (in bytes) that can be transmitted to the peer by the Nordic UART service module. */
//static ble_uuid_t m_adv_uuids[]          =                                          /**< Universally unique service identifier. */
//{
//    {BLE_UUID_NUS_SERVICE, NUS_SERVICE_UUID_TYPE}
//};

unsigned int gu32_Cumulativetime = 0;
unsigned int gu32_CurrentAddress = 0;
unsigned int gu32_CurRPMVal = 0;
unsigned int gu32_PrevRPMVal = 0;
uint8_t gu8_ActivateStatus = FALSE;
unsigned int gu32_BattVolt = 0;

uint8_t gu8_VibrationCount = 0;
static uint8_t su8_WakeupTimer = 0;
static uint8_t su8_TripStartFlag = FALSE;
static uint8_t su8_RPMDetectFlag = FALSE;
static uint8_t su8_TripDebounceCnt = 0;
static uint8_t su8_RPMTimerIndex = 0;
static uint32_t su32_RPMTimerValue[5] = {0};
static uint32_t su32_RPMPulseWidth;
//static void on_conn_params_evt(ble_conn_params_evt_t * p_evt);
typedef struct
{
	int8_t ms8_Command[U8_CMD_MAX_STRING];
	uint8_t (*function_ptr)(uint8_t *lu8p_RespBuff,uint8_t *lu8p_ResultPtr ,uint8_t *lu8_SavePtr);
}st_btmcfgCommand_t;

//static ble_gap_sec_params_t gst_secparams;
static void apptimer_Init(void);
void rtc2_open(void);
void rtc2_start(void);
void rtc2_stop(void);
uint32_t rtc2_GetRtcCounterValue(void);
void rtc2_close(void);
void rtc2_resetCounter(void);
void gpiote_MagSenseInit(void);
void pwr_VariablesInit(void);
void pwr_MagSensEnable(void);
void pwr_MagSensDisable(void);
void pwr_VibrationDetectEnable(void);
void pwr_VibrationDetectDisable(void);
void gpiote_Vibrationinit(void);
void app_timer_handler(void * p_context);
void gpiote_MagSenseIntEn(void);
void gpiote_MagSenseIntDis(void);
static void power_manage(void);
void GPIOTE_IRQHandler(void);
void apptimer_Start(void);
//static void advertising_start(void);
int16_t saadc_init(void);
int16_t saadc_measure(void);
//void _init(void);
//static void fs_event_handler(nrf_fstorage_evt_t * evt);
void RetriveCumulativetime(void);
//void storeCumulativetime(void);
//static void tx_power_set(void);
static void advertising_start(void);
uint8_t btm_ExecuteSetTimeReq(uint8_t *lu8p_RespBuff,uint8_t *lu8p_Result, uint8_t *lu8p_SavePtr);
uint8_t btm_ExecuteSetGearReq(uint8_t *lu8p_RespBuff,uint8_t *lu8p_Result, uint8_t *lu8p_SavePtr);
uint8_t btm_ExecuteSetRateReq(uint8_t *lu8p_RespBuff,uint8_t *lu8p_Result, uint8_t *lu8p_SavePtr);
uint8_t btm_ExecuteGetRPMReq(uint8_t *lu8p_RespBuff,uint8_t *lu8p_Result, uint8_t *lu8p_SavePtr);
uint8_t btm_ExecuteSetActivateReq(uint8_t *lu8p_RespBuff,uint8_t *lu8p_Result, uint8_t *lu8p_SavePtr);
uint8_t  btm_CmdProcess(uint8_t *lu8p_CmdBuff,uint8_t *lu8p_ResBuf);
static void advertising_init(uint8_t AdvMode);

uint8_t remote_char_data[30];
uint8_t custom_char_data[26];
void remote_To_custom_char(uint8_t *r_data, int r_size);



static ble_gap_adv_params_t m_adv_params;                                  /**< Parameters to be passed to the stack when starting advertising. */
static uint8_t              m_adv_handle = BLE_GAP_ADV_SET_HANDLE_NOT_SET; /**< Advertising handle used to identify an advertising set. */
static uint8_t              m_enc_advdata[BLE_GAP_ADV_SET_DATA_SIZE_MAX];  /**< Buffer for storing an encoded advertising set. */
static ble_gap_adv_data_t m_adv_data =
{
    .adv_data =
    {
        .p_data = m_enc_advdata,
        .len    = BLE_GAP_ADV_SET_DATA_SIZE_MAX
    },
    .scan_rsp_data =
    {
        .p_data = NULL,
        .len    = 0

    }
};
static const st_btmcfgCommand_t cst_btmcfgSetCommand[] = {
	{"TIME",btm_ExecuteSetTimeReq},
	{"GEAR",btm_ExecuteSetGearReq},
	{"RATE",btm_ExecuteSetRateReq},
	{"DEACTIVATE",btm_ExecuteSetActivateReq}
};
static const st_btmcfgCommand_t cst_btmcfgGetCommand[] = {
	{"RPM",btm_ExecuteGetRPMReq},
};

//NRF_FSTORAGE_DEF(nrf_fstorage_t m_fs) =
//{
//    .evt_handler = fs_event_handler,
//    .start_addr = 0x0007E000,
//    .end_addr = 0x0007F000,
//};

/**************************************************************************
* Function Name	:	apptimer_Init
*
* Description   :   initializing app timer for 1 sec
*
* Arguments     :   None
*
* Return values :   None
*
****************************************************************************/
static void apptimer_Init(void)
{
    ret_code_t err_code = app_timer_init();
    APP_ERROR_CHECK(err_code);
	// Create 1sec timer.
	//app_timer_create(&m_timer_id,APP_TIMER_MODE_REPEATED,app_timer_handler);
}
/******************************************************************************
* Function Name	:  apptimer_Start
*
* Description	: start rtc1 in timer mode repeated every 1 sec
*
* Arguments     : void
*
* Return Values	: void
******************************************************************************/
void apptimer_Start(void)
{
	app_timer_start(m_timer_id,APP_TIMER_DELAY, NULL );
}
/**************************************************************************
* Function Name	:	pwr_VariablesInit
*
* Description   :   Initialize of variables
*
* Arguments     :   None
*
* Return values :   None
*
****************************************************************************/
void pwr_VariablesInit(void)
{
	gu32_Cumulativetime = 0;
	gu32_CurRPMVal = 0;
	gu32_PrevRPMVal = 0;
	su8_TripStartFlag = FALSE;
	su8_RPMDetectFlag = FALSE;
	gu8_VibrationCount = 0;
	su8_WakeupTimer = 0;
	su8_TripDebounceCnt = 0;
	su8_RPMTimerIndex = 0;
	gu32_BattVolt = 0;
	rtc2_close();
}
/******************************************************************************
* Function Name	:  gpiote_MagSenseInit
*
* Description	: Initialise the event on magnetic sensor output pin
								on every rising edge
*
* Arguments     : void
*
* Return Values	: void
******************************************************************************/
void gpiote_MagSenseInit(void)
{
	NRF_GPIOTE->CONFIG[0] 	= 	0x01 << 0; 								// MODE: Event
	NRF_GPIOTE->CONFIG[0] 	|= 	BT_MAG_SENSE_OUTPUT << 8;								// Pin number
	NRF_GPIOTE->CONFIG[0] 	|= 	NRF_GPIOTE_POLARITY_HITOLO	<< 16;		// Event rising edge
}
/******************************************************************************
* Function Name	:  gpiote_MagSenseIntEn
*
* Description		: 	Enable the interrupt on magnetic sensor output pin for rising edge
*
* Arguments     : void
*
* Return Values	: void
******************************************************************************/
void gpiote_MagSenseIntEn(void)
{
	NRF_GPIOTE->INTENSET = 0x01 << 0;  // enable interrupt
	NVIC_ClearPendingIRQ(GPIOTE_IRQn);
	NVIC_SetPriority(GPIOTE_IRQn, APP_IRQ_PRIORITY_LOW);
	NVIC_EnableIRQ(GPIOTE_IRQn);
}
/******************************************************************************
* Function Name	:  gpiote_MagSenseIntDis
*
* Description	:	Disable the interrupt on magnetic sensor output pin
*
* Arguments     : void
*
* Return Values	: void
******************************************************************************/
void gpiote_MagSenseIntDis(void)
{
	NRF_GPIOTE->INTENCLR = 0x01 << 0; //clear interrupt
	NVIC_ClearPendingIRQ(GPIOTE_IRQn);
}

/******************************************************************************
* Function Name	:  gpiote_Vibrationinit
*
* Description	:		configure event for vibration sensor on raising edge
*
* Arguments     : void
*
* Return Values	: void
******************************************************************************/
void gpiote_Vibrationinit(void)
{
	NRF_GPIOTE->CONFIG[1] 	= 	0x01 << 0; 								// MODE: Event
	NRF_GPIOTE->CONFIG[1] 	|= 	BT_SW_OUTPUT << 8;								// Pin number
	NRF_GPIOTE->CONFIG[1] 	|= 	NRF_GPIOTE_POLARITY_LOTOHI	<< 16;		// Event rising edge
}
/**************************************************************************
* Function Name	:	pwr_MagSensEnable
*
* Description   :   Enable the magnetic sensor
*
* Arguments     :   None
*
* Return values :   None
*
****************************************************************************/
void pwr_MagSensEnable(void)
{
	nrf_gpio_pin_set(BT_MAG_SENSE_VSS);
}
/**************************************************************************
* Function Name	:	pwr_MagSensDisable
*
* Description   :   Disable the magnetic sensor
*
* Arguments     :   None
*
* Return values :   None
*
****************************************************************************/
void pwr_MagSensDisable(void)
{
	nrf_gpio_pin_clear(BT_MAG_SENSE_VSS);
}
/**************************************************************************
* Function Name	:	pwr_VibrationDetectEnable
*
* Description   :   Enable the Vibration detection
*
* Arguments     :   None
*
* Return values :   None
*
****************************************************************************/
void pwr_VibrationDetectEnable(void)
{
	nrf_gpio_pin_set(BT_SW_VSS);
	gpiote_Vibrationinit();
}
/**************************************************************************
* Function Name	:	pwr_VibrationDetectDisable
*
* Description   :   Disable the vibration detection
*
* Arguments     :   None
*
* Return values :   None
*
****************************************************************************/
void pwr_VibrationDetectDisable(void)
{
	nrf_gpio_pin_clear(BT_SW_VSS);
}
/**************************************************************************
* Function Name    :  btm_CmdProcess(s8 *ls8p_CmdBuff,uint8_t *lu8p_ResBuf)
*
* Description      : Process the command recieved from app
*
* Arguments        :  ls8p_CmdBuff,lu8p_ResBuf
*
* Return values    :  SUCCESS/FAIL
*************************************************************************************/
uint8_t btm_CmdProcess(uint8_t *lu8p_CmdBuff,uint8_t *lu8p_ResBuf)
{
	uint8_t lu8_Status = FAIL;
	uint8_t lu8_size = 0;
	uint8_t *lu8p_word = NULL;
	uint8_t *lu8p_SavePtr = NULL;
	uint8_t lu8_FirstCmdBuf[10];
	uint8_t lu8_SecCmdBuf[10];
	uint8_t lu8_Index = 0;
	uint8_t *lu8p_buf = NULL;

	//check for "#" in command buffer if found
	lu8p_buf =(uint8_t *) strstr((char *)lu8p_CmdBuff,"#");
	if(lu8p_buf == NULL)
	{
		return FAIL;
	}
	//check for max size
	lu8_size = strlen((char *)lu8p_CmdBuff);
	if(lu8_size > 30)
	{
		return FAIL;
	}
	//skip # from the command
	lu8p_CmdBuff += 1;
	//process the command
	lu8p_word = (uint8_t *)strtok_r((char *)lu8p_CmdBuff,(const char *)",",(char **)&lu8p_SavePtr);
	if(NULL == lu8p_word)
	{
		strcpy((char *)lu8p_ResBuf,"Unknown Request");
		lu8_Status = FAIL;
	}
	else
	{
		strcpy((char *)	lu8_FirstCmdBuf,(const char *)lu8p_word);
	}
	lu8p_word = (uint8_t *)strtok_r((char *)NULL,(const char *)",\r\n",(char **)&lu8p_SavePtr);
	if(NULL == lu8p_word)
	{
		strcpy((char *)lu8p_ResBuf,"Unknown Request");
		lu8_Status = FAIL;
	}
	else
	{
		strcpy((char *)	lu8_SecCmdBuf,(const char *)lu8p_word);
	}
	//uartd_write(lu8_SecCmdBuf,strlen(lu8_SecCmdBuf));
	if(0 == strcmp((char*)lu8_FirstCmdBuf,"SET"))
	{
		for(lu8_Index = 0;lu8_Index < U8_MAX_CMD_ENTRIES; lu8_Index++)
		{
			if(0 == (strcmp( (const char*)lu8_SecCmdBuf,(const char *)cst_btmcfgSetCommand[lu8_Index].ms8_Command)))
			{
				lu8_Status = cst_btmcfgSetCommand[lu8_Index].function_ptr(lu8p_ResBuf,lu8p_word,lu8p_SavePtr);
				break;
			}
		}
	}
	if(0 == strcmp((char*)lu8_FirstCmdBuf,"GET"))
	{
		for(lu8_Index = 0;lu8_Index < U8_MAX_CMD_ENTRIES; lu8_Index++)
		{
			if(0 == (strcmp( (const char*)lu8_SecCmdBuf,(const char *)cst_btmcfgGetCommand[lu8_Index].ms8_Command)))
			{
				lu8_Status = cst_btmcfgGetCommand[lu8_Index].function_ptr(lu8p_ResBuf,lu8p_word,lu8p_SavePtr);
				break;
			}
		}
	}
	return lu8_Status;
}
/**************************************************************************
* Function Name    :  btm_ExecuteSetTimeReq(uint8_t *lu8p_RespBuff,uint8_t *lu8p_Result, uint8_t *lu8p_SavePtr)
*
* Description      :  Execute the received command
*
* Arguments        :  lu8p_RespBuff,lu8p_Result,lu8p_SavePtr
*
* Return values    :  SUCCESS/FAIL
***********************************************************************************/
uint8_t btm_ExecuteSetTimeReq(uint8_t *lu8p_RespBuff,uint8_t *lu8p_Result, uint8_t *lu8p_SavePtr)
{
	lu8p_Result = (uint8_t *) strtok_r(NULL,(const char *)"\n",(char **)&lu8p_SavePtr);
	if(NULL == lu8p_Result)
	{
		strcpy((char *)lu8p_RespBuff,"#SET,TIME,FAIL");
		return SUCCESS;
	}
	else
	{
		//TODO: need to set Time
		strcpy((char *)lu8p_RespBuff,"#SET,TIME,SUCCESS");
	}
	return SUCCESS;
}
/**************************************************************************
* Function Name    :  btm_ExecuteSetGearReq(uint8_t *lu8p_RespBuff,uint8_t *lu8p_Result, uint8_t *lu8p_SavePtr)
*
* Description      :  Execute the received command
*
* Arguments        :  lu8p_RespBuff,lu8p_Result,lu8p_SavePtr
*
* Return values    :  SUCCESS/FAIL
************************************************************************************/
uint8_t btm_ExecuteSetGearReq(uint8_t *lu8p_RespBuff,uint8_t *lu8p_Result, uint8_t *lu8p_SavePtr)
{
	lu8p_Result = (uint8_t *) strtok_r(NULL,(const char *)"$",(char **)&lu8p_SavePtr);
	if(NULL == lu8p_Result)
	{
		strcpy((char *)lu8p_RespBuff,"#SET,GEAR,FAIL");
		return SUCCESS;
	}
	else
	{
		//TODO: need to set Gear
		strcpy((char *)lu8p_RespBuff,"#SET,GEAR,SUCCESS");
	}
	return SUCCESS;
}
/**************************************************************************
* Function Name    :  btm_ExecuteSetRateReq(uint8_t *lu8p_RespBuff,uint8_t *lu8p_Result, uint8_t *lu8p_SavePtr)
*
* Description      :  Execute the received command
*
* Arguments        :  lu8p_RespBuff,lu8p_Result,lu8p_SavePtr
*
* Return values    :  SUCCESS/FAIL
************************************************************************************/
uint8_t btm_ExecuteSetRateReq(uint8_t *lu8p_RespBuff,uint8_t *lu8p_Result, uint8_t *lu8p_SavePtr)
{
	lu8p_Result = (uint8_t *) strtok_r(NULL,(const char *)"\n",(char **)&lu8p_SavePtr);
	if(NULL == lu8p_Result)
	{
		strcpy((char *)lu8p_RespBuff,"#SET,RATE,FAIL");
		return SUCCESS;
	}
	else
	{
		//TODO: need to set rate
		strcpy((char *)lu8p_RespBuff,"#SET,RATE,SUCCESS");
	}
	return SUCCESS;
}
/**************************************************************************
* Function Name    :  btm_ExecuteSetActivateReq(uint8_t *lu8p_RespBuff,uint8_t *lu8p_Result, uint8_t *lu8p_SavePtr)
*
* Description      :  Execute the received command
*
* Arguments        :  lu8p_RespBuff,lu8p_Result,lu8p_SavePtr
*
* Return values    :  SUCCESS/FAIL
************************************************************************************/
uint8_t btm_ExecuteSetActivateReq(uint8_t *lu8p_RespBuff,uint8_t *lu8p_Result, uint8_t *lu8p_SavePtr)
{
	lu8p_Result = (uint8_t *) strtok_r(NULL,(const char *)"\n",(char **)&lu8p_SavePtr);
	if(NULL == lu8p_Result)
	{
		strcpy((char *)lu8p_RespBuff,"#SET,DEACTIVATE,FAIL");
		return SUCCESS;
	}
	else
	{
		gu8_ActivateStatus = (*lu8p_Result - '0');
		strcpy((char *)lu8p_RespBuff,"#SET,DEACTIVATE,SUCCESS");
	}
	if(TRUE == gu8_ActivateStatus)
	{
	        app_timer_stop(m_timer_id);
		rtc2_close();
		pwr_MagSensDisable();
		pwr_VibrationDetectDisable();
		gpiote_MagSenseIntDis();
	}
	return SUCCESS;
}
/**************************************************************************
* Function Name    :  btm_ExecuteGetRPMReq(uint8_t *lu8p_RespBuff,uint8_t *lu8p_Result, uint8_t *lu8p_SavePtr)
*
* Description      :  Execute the received command
*
* Arguments        :  lu8p_RespBuff,lu8p_Result,lu8p_SavePtr
*
* Return values    :  SUCCESS/FAIL
************************************************************************************/
uint8_t btm_ExecuteGetRPMReq(uint8_t *lu8p_RespBuff,uint8_t *lu8p_Result, uint8_t *lu8p_SavePtr)
{
	sprintf((char *)lu8p_RespBuff,"#GET,RPM,%u,%u,%u,%u",gu32_CurRPMVal,gu32_Cumulativetime,gu32_BattVolt,gu8_ActivateStatus);
	return SUCCESS;
}
/*****************************************************************************
* Function Name :    rtc2_open
*
* Description   :
*
* Arguments     :
*
* Return values :
*
*****************************************************************************/
void rtc2_open(void)
{
	NRF_RTC2->PRESCALER = 0;
	NVIC_SetPriority(RTC2_IRQn, APP_IRQ_PRIORITY_LOW);
	NVIC_ClearPendingIRQ(RTC2_IRQn);
	NVIC_EnableIRQ(RTC2_IRQn);

	NRF_RTC2->TASKS_CLEAR = 1;
	NRF_RTC2->TASKS_START = 1;
}

/*****************************************************************************
* Function Name :    rtc2_start
*
* Description   :
*
* Arguments     :
*
* Return values :
*
*****************************************************************************/
void rtc2_start(void)
{
	NRF_RTC2->TASKS_CLEAR = 1;
	NRF_RTC2->TASKS_START = 1;
}
/*****************************************************************************
* Function Name :    rtc2_stop
*
* Description   :
*
* Arguments     :
*
* Return values :
*
*****************************************************************************/
void rtc2_stop(void)
{
	NRF_RTC2->TASKS_STOP = 1;
}
/*****************************************************************************
* Function Name    :    rtc2_GetRtcCounterValue
*
* Description   :   This Function returns real time counter value in milli
*                   seconds
*
* Arguments     :   None
*
* Return values :
*****************************************************************************/
uint32_t rtc2_GetRtcCounterValue(void)
{
    return(NRF_RTC2->COUNTER);
}
/*****************************************************************************
* Function Name    :    rtc2_close
*
* Description   :   This Function for de-initializing the RTC2 driver. This
*                   function disables the RTC event interrupt and disables
*                   compare events and stops RTC2 events
*
* Arguments     :   None
*
* Return values :   None
*
*****************************************************************************/
void rtc2_close(void)
{
    volatile uint32_t lu32_Counter = 10000;
    uint32_t mask = (     RTC_INTENCLR_TICK_Pos |
                        RTC_INTENCLR_OVRFLW_Pos |
                      RTC_INTENCLR_COMPARE0_Msk |
                      RTC_INTENCLR_COMPARE1_Msk |
                      RTC_INTENCLR_COMPARE2_Msk |
                      RTC_INTENCLR_COMPARE3_Msk
                    );

    NVIC_DisableIRQ(RTC2_IRQn);

    NRF_RTC2->TASKS_STOP = 1;

    NRF_RTC2->EVTENCLR = mask;
    NRF_RTC2->INTENCLR = mask;

    /* Once RTC is stopped 15 to 46 micro seconds delay is
    * required before we start new selection for the RTC*/
    while (lu32_Counter)
    {
        lu32_Counter --;
    }
}
/*****************************************************************************
* Function Name : rtcd_resetCounter
*
* Description   : Reset the RTC counter value
*
* Arguments     :
*
* Return values :
*
*****************************************************************************/
void rtc2_resetCounter(void)
{
	NRF_RTC2->TASKS_CLEAR = 1;
}
/******************************************************************************
* Function Name	:  GPIOTE_IRQHandler
*
* Description	: Interrupt every rising edge of Sensor. Timer resolution of 30u Seconds
*
* Arguments     :
*
* Return Values	:
******************************************************************************/
void GPIOTE_IRQHandler(void)
{
	uint32_t lu32_PulseWidth[3];

	su32_RPMTimerValue[su8_RPMTimerIndex] = rtc2_GetRtcCounterValue();

	if( 3 == su8_RPMTimerIndex)
	{
		lu32_PulseWidth[0] = (su32_RPMTimerValue[2] - su32_RPMTimerValue[1]);
		lu32_PulseWidth[1] = (su32_RPMTimerValue[3] - su32_RPMTimerValue[2]);

		if(abs(lu32_PulseWidth[1]  - lu32_PulseWidth[0]) < 400)
		{
			su32_RPMPulseWidth = (lu32_PulseWidth[1] + lu32_PulseWidth[0])/2;
			su8_RPMDetectFlag = TRUE;
		}
		gpiote_MagSenseIntDis();
		rtc2_stop();
		pwr_MagSensDisable();
		su8_RPMTimerIndex = 0;

	}
	su8_RPMTimerIndex++;
	//clear GPIOTE events
	NRF_GPIOTE->EVENTS_IN[0] = 0;
}
/******************************************************************************
* Function Name	:  app_timer_handler
*
* Description	: Event every 1 second. Channel 1 is for Vibration Sensor
*
* Arguments     :
*
* Return Values	:
******************************************************************************/
void app_timer_handler(void * p_context)
{
	uint32_t lu32_RpmValue = 0;

	//detect vibration
	if(NRF_GPIOTE->EVENTS_IN[1] == 1)
	{	// Vibration detected this second
		NRF_GPIOTE->EVENTS_IN[1] = 0;
		gu8_VibrationCount += U8_MOTION_DETECT_WT;
		if(U8_MAX_MOTION_DETECT_CNT <= gu8_VibrationCount)
		{
			gu8_VibrationCount = U8_MAX_MOTION_DETECT_CNT;  // Saturate at a maximum value
		}
	}
	else
	{
		// Vibration not detected this second
		if(gu8_VibrationCount > U8_MOTION_NOT_DETECT_WT)
		{
			gu8_VibrationCount -= U8_MOTION_NOT_DETECT_WT;
		}
		else
		{
			gu8_VibrationCount = 0;
		}
	}
	gu8_VibrationCount = 30;     //Vibration disabled
	if((U8_MOTION_THRESHOLD <= gu8_VibrationCount) || (TRUE == su8_TripStartFlag))
	{
		if(0 == su8_WakeupTimer) // Time ut ealpsed-  If nor rpm detected in previous attempts wait for U8_NO_RPM_DETECTED number of seconds
		{
			//Pulses detected
			if(TRUE == su8_RPMDetectFlag)
			{
				lu32_RpmValue = ((su32_RPMPulseWidth * U8_RTC_RESOLUTION));
				lu32_RpmValue = ((100000000 / lu32_RpmValue) * 60);
				lu32_RpmValue /= 400;
			}
			//validate the RPM
			if(U8_VALID_RPM < lu32_RpmValue)
			{
				if(0 == gu32_PrevRPMVal)
					gu32_PrevRPMVal = lu32_RpmValue;
				gu32_CurRPMVal = (((gu32_PrevRPMVal * 7) + lu32_RpmValue) / 8);
				gu32_PrevRPMVal = gu32_CurRPMVal;
				//valid RPM detected
				if(FALSE == su8_TripStartFlag)
				{
					su8_TripStartFlag = TRUE;
				}
				su8_TripDebounceCnt = 0;
			}
			else
			{
				su8_TripDebounceCnt ++;
				if(TRUE == su8_TripStartFlag)
				{
					if(U8_NO_RPM_TRIPEND_CNT <= su8_TripDebounceCnt)
					{
						//Trip end
						gu32_CurRPMVal = 0;
						su8_TripStartFlag = FALSE;
						su8_RPMDetectFlag = FALSE;
						gu32_PrevRPMVal = 0;
						//Store cumulative time to Flash
                      //  storeCumulativetime();
						su8_WakeupTimer = U8_NO_RPM_DETECTED;
						su8_TripDebounceCnt = 0;
						pwr_MagSensDisable();
						rtc2_stop();
						gpiote_MagSenseIntDis();
						return;
					}
				}
				else
				{
					//Trip not detected
					if(U8_NO_RPM_DETECT <= su8_TripDebounceCnt)
					{
						su8_WakeupTimer = U8_NO_RPM_DETECTED;
						su8_TripDebounceCnt = 0;
						pwr_MagSensDisable();
						rtc2_stop();
						gpiote_MagSenseIntDis();
						return;
					}
				}
			}
			// Enable sensor RTC and interrupt
			su8_RPMDetectFlag = FALSE;
			su8_RPMTimerIndex = 0;
			su32_RPMPulseWidth = 0;
			rtc2_resetCounter();
			rtc2_start();
			pwr_MagSensEnable();
			gpiote_MagSenseIntEn();
		}
		else
		{
			// Wait time not elapsed
			if(su8_WakeupTimer)
			{
				su8_WakeupTimer--;
			}
			pwr_MagSensDisable();
			rtc2_stop();
			gpiote_MagSenseIntDis();
			su8_RPMDetectFlag = FALSE;
		}
	}
	else
	{
		// No Vibration detected
		su8_RPMDetectFlag = FALSE;
		su8_RPMTimerIndex = 0;
		su32_RPMPulseWidth = 0;
		su8_TripDebounceCnt = 0;
		pwr_MagSensDisable();
		rtc2_stop();
		gpiote_MagSenseIntDis();
	}
	if(TRUE == su8_TripStartFlag)
	{
		gu32_Cumulativetime++;
	}
}
/******************************************************************************
* Function Name	:  saadc_event_handler
*
* Description	:  Do nothing, as we use blocking mode
*
* Arguments     :
*
* Return Values	: Void
******************************************************************************/
void saadc_event_handler(nrf_drv_saadc_evt_t const * p_event)
{
	// Do nothing, as we use blocking mode
}
/******************************************************************************
* Function Name	:  saadc_init
*
* Description	:  Initialising SAADC for measuring Vdd
*
* Arguments     : Void
*
* Return Values	: Ret
******************************************************************************/
int16_t saadc_init(void)
{
  int ret = nrf_drv_saadc_init(NULL, saadc_event_handler);
  if (ret) return ret;
  nrf_saadc_channel_config_t config =
		NRF_DRV_SAADC_DEFAULT_CHANNEL_CONFIG_SE(SAADC_CH_PSELP_PSELP_VDD);
  ret = nrf_drv_saadc_channel_init(0, &config);
  return ret;
}
/******************************************************************************
* Function Name	:  saadc_measure
*
* Description	  :  Measuring battery voltage
*
* Arguments     : Void
*
* Return Values	: Batt voltage
******************************************************************************/
int16_t saadc_measure(void)
{
  nrf_saadc_value_t value;
  nrf_drv_saadc_sample_convert(0, &value);
  return  ADC_RESULT_IN_MILLI_VOLTS(value);
}

/******************************************************************************
* Function Name	:   nrf_fstorage_init
*
* Description	:   Initialization for FLASH to store cummulative time
*
* Arguments     : void
*
* Return Values	: void
******************************************************************************/
//void fstorage_init(void)
//{
//	uint32_t err_code;
//
//  err_code = nrf_fstorage_init(&m_fs, &nrf_fstorage_sd, NULL);
//  APP_ERROR_CHECK(err_code);
//}
/******************************************************************************
* Function Name	:   fs_event_handler
*
* Description	:
*
* Arguments     : void
*
* Return Values	: void
******************************************************************************/
//static void fs_event_handler(nrf_fstorage_evt_t * evt){
//    if (evt->result != NRF_SUCCESS){
//        NRF_LOG_ERROR("Flash error");
//
//    } else {
//        NRF_LOG_INFO("Flash event success");
//    }
//}
/******************************************************************************
* Function Name	:   storeCumulativetime
*
* Description	  : store Cumulative time
*
* Arguments     : void
*
* Return Values	: void
******************************************************************************/
//void storeCumulativetime(void)
//{
//	ret_code_t rc;
//
//		if(gu32_CurrentAddress == m_fs.end_addr)
//		{
//			rc = nrf_fstorage_erase( &m_fs,m_fs.start_addr,1,NULL);
//
//			if (rc == NRF_SUCCESS)
//			{
//				gu32_CurrentAddress = m_fs.start_addr;
//			}
//			nrf_delay_ms(1);
//		}
//		rc = nrf_fstorage_write(&m_fs, gu32_CurrentAddress, &gu32_Cumulativetime, 4, NULL);
//		if (rc == NRF_SUCCESS)
//		{
//			gu32_CurrentAddress += 4;
//		}
//}
///******************************************************************************
//* Function Name	:   RetriveCumulativetime
//*
//* Description	: Retrive Cumulative time
//*
//* Arguments     : void
//*
//* Return Values	: void
//******************************************************************************/
void RetriveCumulativetime(void)
{
//	uint32_t lu32_currentAddress = 0;
//	uint32_t lu32_currentData = 0;
//	uint32_t lu32_previousData = 0;
//	ret_code_t rc;
//	uint8_t Flag = FALSE;
//
//	for(lu32_currentAddress = m_fs.start_addr;lu32_currentAddress < m_fs.end_addr; lu32_currentAddress += 4)
//	{
//		rc = nrf_fstorage_read(&m_fs, lu32_currentAddress, &lu32_currentData, 4);
//		if (rc == NRF_SUCCESS)
//		{
//			if(lu32_currentData == 0xFFFFFFFF)
//			{
//				gu32_CurrentAddress = lu32_currentAddress;
//				break;
//			}
//			else
//			{
//				 lu32_previousData = lu32_currentData;
//			}
//		}
//	}
//
//
//	gu32_Cumulativetime = lu32_previousData;
//	if( lu32_currentAddress > m_fs.end_addr )
//	{
//		gu32_CurrentAddress = m_fs.start_addr;
//		Flag = TRUE;
//	}
//
//	if ( TRUE == Flag)
//	{
//		rc = nrf_fstorage_erase( &m_fs,m_fs.start_addr,1,NULL);
//	}
}
/**@brief Function for assert macro callback.
 *
 * @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 analyse
 *          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] p_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 the GAP initialization.
 *
 * @details This function will set up all the necessary GAP (Generic Access Profile) parameters of
 *          the device. It also sets the permissions and appearance.
 */
static void gap_params_init(void)
{
    uint32_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_NAME,
                                          strlen(DEVICE_NAME));
    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);

    // Update security parameters for connection reply
//    gst_secparams.bond         = SEC_PARAM_BOND;
//    gst_secparams.mitm         = SEC_PARAM_MITM;
//    gst_secparams.io_caps      = SEC_PARAM_IO_CAPABILITIES;
//    gst_secparams.oob          = SEC_PARAM_OOB;
//    gst_secparams.min_key_size = SEC_PARAM_MIN_KEY_SIZE;
//    gst_secparams.max_key_size = SEC_PARAM_MAX_KEY_SIZE;
}


/**@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);
}


/**@brief Function for handling the data from the Nordic UART Service.
 *
 * @details This function will process the data received from the Nordic UART BLE Service and send
 *          it to the UART module.
 *
 * @param[in] p_evt       Nordic UART Service event.
 */
/**@snippet [Handling the data received over BLE] */
static void nus_data_handler(ble_nus_evt_t * p_evt)
{

//	if (p_evt->type == BLE_NUS_EVT_RX_DATA)
//	{
		memcpy(remote_char_data, p_evt->params.rx_data.p_data, p_evt->params.rx_data.length);

			remote_To_custom_char(remote_char_data, sizeof(remote_char_data));


	//}
}

void remote_To_custom_char(uint8_t *r_data, int r_size)
{
    memcpy(custom_char_data+1, r_data+1, r_size-14);

	custom_char_data[(r_size-14)+1]= 0x64;

	memcpy(custom_char_data+18, r_data+17, r_size-14);
}
/**@snippet [Handling the data received over BLE] */


/**@brief Function for initializing services that will be used by the application.
 */
static void services_init(void)
{
    uint32_t           err_code;
    ble_nus_init_t     nus_init;
    nrf_ble_qwr_init_t qwr_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 NUS.
    memset(&nus_init, 0, sizeof(nus_init));

    nus_init.data_handler = nus_data_handler;

    err_code = ble_nus_init(&m_nus, &nus_init);
    APP_ERROR_CHECK(err_code);
}
//

/**@brief Function for handling an event from 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.
 */

/**@brief Function for handling errors from the Connection Parameters module.
 *
 * @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)
{
    uint32_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;

//	  cp_init.p_conn_params->min_conn_interval = MIN_CONN_INTERVAL;
//    cp_init.p_conn_params->max_conn_interval = MAX_CONN_INTERVAL;
//    cp_init.p_conn_params->slave_latency     = SLAVE_LATENCY;
//    cp_init.p_conn_params->conn_sup_timeout  = CONN_SUP_TIMEOUT;

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

//static void on_conn_params_evt(ble_conn_params_evt_t * p_evt)
//{
//    uint32_t err_code;
//
//    if (p_evt->evt_type == 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 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)
//{
//    switch (ble_adv_evt)
//    {
//        case BLE_ADV_EVT_FAST:
//             break;
//        case BLE_ADV_EVT_IDLE:
//             //advertising_start();   // advertising forever
//            break;
//        default:
//            break;
//    }
//}
/**@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)
{
    uint32_t err_code;

    switch (p_ble_evt->header.evt_id)
    {
        case BLE_GAP_EVT_CONNECTED:
        	m_conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
        	            err_code = nrf_ble_qwr_conn_handle_assign(&m_qwr, m_conn_handle);
        	            APP_ERROR_CHECK(err_code);

            break;

        case BLE_GAP_EVT_DISCONNECTED:
            //m_conn_handle = BLE_CONN_HANDLE_INVALID;
            sd_ble_gap_adv_stop(m_adv_handle);
            advertising_init(BLE_GAP_ADV_TYPE_NONCONNECTABLE_SCANNABLE_UNDIRECTED);
            advertising_start();
			break;

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

        case BLE_GAP_EVT_SEC_PARAMS_REQUEST:
            // Pairing not supported
//            err_code = sd_ble_gap_sec_params_reply(m_conn_handle, BLE_GAP_SEC_STATUS_SUCCESS, &gst_secparams, NULL);
//            APP_ERROR_CHECK(err_code);
            break;

        case BLE_GATTS_EVT_SYS_ATTR_MISSING:
            // No system attributes have been stored.
            err_code = sd_ble_gatts_sys_attr_set(m_conn_handle, NULL, 0, 0);
            APP_ERROR_CHECK(err_code);
            break;

        case BLE_GATTC_EVT_TIMEOUT:
            // Disconnect on GATT Client timeout event.
//            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.
            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;

        default:
            // No implementation needed.
            break;
    }
}
/**@brief Function for the SoftDevice initialization.
 *
 * @details This function 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 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))
    {
        m_ble_nus_max_data_len = p_evt->params.att_mtu_effective - OPCODE_LENGTH - HANDLE_LENGTH;
        NRF_LOG_INFO("Data len is set to 0x%X(%d)", m_ble_nus_max_data_len, m_ble_nus_max_data_len);
    }
    NRF_LOG_DEBUG("ATT MTU exchange completed. central 0x%x peripheral 0x%x",
                  p_gatt->att_mtu_desired_central,
                  p_gatt->att_mtu_desired_periph);
}


/**@brief Function for initializing the GATT library. */
void gatt_init(void)
{
    ret_code_t err_code;

    err_code = nrf_ble_gatt_init(&m_gatt, gatt_evt_handler);
    APP_ERROR_CHECK(err_code);

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


/**@brief Function for handling events from the BSP module.
 *
 * @param[in]   event   Event generated by button press.
 */
void bsp_event_handler(bsp_event_t event)
{
    uint32_t err_code;
    switch (event)
    {
        case BSP_EVENT_SLEEP:
            //advertising_start();   // advertising forever
            break;
//
        case BSP_EVENT_DISCONNECT:
		//    advertising_start();  // advertising forever
            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;
    }
}

static void advertising_init(uint8_t AdvMode)
{
	 uint32_t      err_code;
	    ble_advdata_t advdata;
	    //uint8_t       flags = BLE_GAP_ADV_FLAG_BR_EDR_NOT_SUPPORTED;

	    ble_advdata_manuf_data_t manuf_specific_data;

	    manuf_specific_data.company_identifier = APP_COMPANY_IDENTIFIER;

	    if(AdvMode ==BLE_GAP_ADV_TYPE_CONNECTABLE_SCANNABLE_UNDIRECTED)

	    		{

	    		custom_char_data[0]=CONNECTABLE_BEACON;

	    	    }
	    	else
	    	{

	    		custom_char_data[0]=NON_CONNECTABLE_BEACON;

	    	}

	    manuf_specific_data.data.p_data = (uint8_t *) custom_char_data;
	    manuf_specific_data.data.size   = APP_BEACON_INFO_LENGTH;

	    // Build and set advertising data.
	    memset(&advdata, 0, sizeof(advdata));

	    advdata.name_type             = BLE_ADVDATA_NO_NAME;
	   //advdata.flags                 = flags;
	    advdata.p_manuf_specific_data = &manuf_specific_data;

	    // Initialize advertising parameters (used when starting advertising).
	    memset(&m_adv_params, 0, sizeof(m_adv_params));

	    m_adv_params.properties.type = AdvMode;
	    m_adv_params.p_peer_addr     = NULL;    // Undirected advertisement.
	    m_adv_params.filter_policy   = BLE_GAP_ADV_FP_ANY;
	    m_adv_params.interval        = NON_CONNECTABLE_ADV_INTERVAL;
	    m_adv_params.duration        = 0;       // Never time out.

	    err_code = ble_advdata_encode(&advdata, m_adv_data.adv_data.p_data, &m_adv_data.adv_data.len);
	    APP_ERROR_CHECK(err_code);

	    err_code = sd_ble_gap_adv_set_configure(&m_adv_handle, &m_adv_data, &m_adv_params);
	    APP_ERROR_CHECK(err_code);
}
/**@brief Function for waiting for event
 */
static void power_manage(void)
{
    sd_app_evt_wait();
}

/**@brief Function for starting advertising.
 */
static void advertising_start(void)
{
	ret_code_t err_code;

	    err_code = sd_ble_gap_adv_start(m_adv_handle, APP_BLE_CONN_CFG_TAG);
	    APP_ERROR_CHECK(err_code);
}





/******************************************************************************
* Function Name	:   tx_power_set
*
* Description	  : Setting TX power level
*
* Arguments     : void
*
* Return Values	: void
******************************************************************************/
//static void tx_power_set(void)
//{
//	 uint32_t err_code = sd_ble_gap_tx_power_set(BLE_GAP_TX_POWER_ROLE_ADV, m_advertising.adv_handle, TX_POWER_LEVEL);
//   APP_ERROR_CHECK(err_code);
//}


int main(void)
{
	// uart_init();
	 // Initialize pins.
//	io_pinhandler();
//app timer initialization for 1 sec
	apptimer_Init();
	rtc2_open();
	saadc_init();
	ble_stack_init();
    //sd_power_dcdc_mode_set(NRF_POWER_DCDC_ENABLE);
	gap_params_init();
    gatt_init();
	services_init();
	memset(custom_char_data,0,sizeof(custom_char_data));
	advertising_init(BLE_GAP_ADV_TYPE_CONNECTABLE_SCANNABLE_UNDIRECTED);
	conn_params_init();

	advertising_start();
	//printf("\r\nadvertising_start() is ok.\r\n");
	//tx_power_set();
//
	pwr_VariablesInit();
	pwr_MagSensDisable();
//    RetriveCumulativetime();
//
//    printf(" Beaconm Low power started");
	// Enter main loop.

	for (;;)
	{
		app_sched_execute();
		power_manage();
	}
}
/**
 * @}
 */

[Simon]

Where does your program get stuck? Could you try to figure it out by doing some debugging? Some logs would be nice as well. When you figure out where your program gets stuck, check if that is due to some function failing, and check what the returned error value is equal to.