shoes/chenjunchao/ble_app_uart_03_link_PC/main.c

/**
 * Copyright (c) 2014 - 2020, 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 "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 "user_Sleep.h"

#include "twi_master.h"
#include "mpu6050.h"
#include "footPDR.h"
#include "MSE5611.h"

#include "nrf_drv_timer.h"
#include "nrf_drv_gpiote.h"

#include "nrf_drv_saadc.h"
#include "nrf_saadc.h"
#include "nrf_drv_timer.h"

#include "app_timer.h"
#include "main.h"
#include "app.h"
//#include "mpu6050.h"
//#include "inv_mpu.h"
//#include "inv_mpu_dmp_motion_driver.h" 
//#include "imu.h"

//#include "mpu9250.h"

#include "nrf_delay.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 PIN_OUT 16

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

#define DEVICE_NAME                     "SH_chen"                               /**< 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                64                                         /**< The advertising interval (in units of 0.625 ms. This value corresponds to 40 ms). */

#define APP_ADV_DURATION                18000                                        /**< 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(4000, UNIT_10_MS)             /**< Connection supervisory timeout (4 seconds), Supervision Timeout uses 10 ms units. */
#define FIRST_CONN_PARAMS_UPDATE_DELAY  APP_TIMER_TICKS(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   APP_TIMER_TICKS(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 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. */


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. */

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


/**@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 initializing the timer module.
 */
static void timers_init(void)
{
    ret_code_t err_code = app_timer_init();
    APP_ERROR_CHECK(err_code);
}

/**@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_addr_t          m_my_addr;   
    ble_gap_conn_params_t   gap_conn_params;
    ble_gap_conn_sec_mode_t sec_mode;
    char BleReallyName[30]={0};

    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&sec_mode);
    
    err_code = sd_ble_gap_addr_get(&m_my_addr);
    APP_ERROR_CHECK(err_code);

    //sprintf(BleReallyName,"%s_%02x%02x%02x",DEVICE_NAME,m_my_addr.addr[2],m_my_addr.addr[1],m_my_addr.addr[0]);

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


/**@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)
    {
        uint32_t err_code;

//        printf("Received data from BLE NUS. Writing data on UART.");
        NRF_LOG_HEXDUMP_DEBUG(p_evt->params.rx_data.p_data, p_evt->params.rx_data.length);
				ble_phone_command_callback(p_evt->params.rx_data.p_data,p_evt->params.rx_data.length);
        for (uint32_t i = 0; i < p_evt->params.rx_data.length; i++)
        {
            do
            {
                err_code = app_uart_put(p_evt->params.rx_data.p_data[i]);
                if ((err_code != NRF_SUCCESS) && (err_code != NRF_ERROR_BUSY))
                {
//                    printf("Failed receiving NUS message. Error 0x%x. ", err_code);
                    APP_ERROR_CHECK(err_code);
                }
            } while (err_code == NRF_ERROR_BUSY);
        }
//        if (p_evt->params.rx_data.p_data[p_evt->params.rx_data.length - 1] == '\r')
//        {
//            while (app_uart_put('\n') == NRF_ERROR_BUSY);
//        }
    }

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

    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)
{
    uint32_t 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 starting advertising.
 */
void advertising_start(void)
{
    uint32_t err_code = ble_advertising_start(&m_advertising, BLE_ADV_MODE_FAST);
    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:
            err_code = bsp_indication_set(BSP_INDICATE_ADVERTISING);
            APP_ERROR_CHECK(err_code);
            break;
        case BLE_ADV_EVT_IDLE:
					   //advertising_start();
           //low_power_in();
            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:
            printf("Connected\r\n");
			nrf_drv_gpiote_out_clear(PIN_OUT);
            err_code = bsp_indication_set(BSP_INDICATE_CONNECTED);
            APP_ERROR_CHECK(err_code);
            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:
            printf("Disconnected\r\n");
			nrf_drv_gpiote_out_set(PIN_OUT);
            // LED indication will be changed when advertising starts.
            m_conn_handle = BLE_CONN_HANDLE_INVALID;
            break;

        case BLE_GAP_EVT_PHY_UPDATE_REQUEST:
        {
            printf("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_PAIRING_NOT_SUPP, NULL, 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;
        printf("Data len is set to 0x%X(%d)", m_ble_nus_max_data_len, m_ble_nus_max_data_len);
    }
    printf("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:
			printf("BSP_EVENT_SLEEP \r\n");
//            sleep_mode_enter();
		advertising_start();
            break;

        case BSP_EVENT_DISCONNECT:
			printf("BSP_EVENT_DISCONNECT \r\n");
            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:
			printf("BSP_EVENT_WHITELIST_OFF \r\n");
            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;
		case BSP_EVENT_KEY_0:
			printf("1");
            break;

        default:
            break;
    }
}


/**@brief   Function for handling app_uart events.
 *
 * @details This function will receive a single character from the app_uart module and append it to
 *          a string. The string will be be sent over BLE when the last character received was a
 *          'new line' '\n' (hex 0x0A) or if the string has reached the maximum data length.
 */
/**@snippet [Handling the data received over UART] */
    
void uart_event_handle(app_uart_evt_t * p_event)
{
	static uint8_t state = 0;
  static uint8_t data_array[BLE_NUS_MAX_DATA_LEN];
	static uint8_t index = 0;  
	static uint8_t len = 0;
	uint8_t i;
	
	uint32_t       err_code;
	int rev=-1;

    switch (p_event->evt_type)
    {
        case APP_UART_DATA_READY:
            UNUSED_VARIABLE(app_uart_get(&data_array[index]));
            index++;

			switch(state){
				case 0:{
					if(index>=3){
//						for(i=0;i<index;i++){printf("%02X ",data_array[i]);}printf("\r\n");
 						if(data_array[0]==0xAA && data_array[1]==(uint8_t)(~data_array[2])){
							len = data_array[1];
							state = 1;
						}else{
							index--;
							for(i=0;i<index;i++){
								data_array[i] = data_array[i+1];
							}
						}
					}
					}break;
				case 1:{
					if(index>=len){
						uint8_t ver = 0;
						for(i=0;i<len-1;i++){
							ver += data_array[i];
						}						
							if(ver==data_array[len-1])
							{ //校验成功
								rev=uart_command_callback(data_array,len);
//								if(rev==CMD_MOTION)
//								{
//									do{
//										uint16_t len_send = len;
//										err_code = ble_nus_data_send(&m_nus, data_array, &len_send, m_conn_handle);
//										if ((err_code != NRF_ERROR_INVALID_STATE) && (err_code != NRF_ERROR_RESOURCES) && (err_code != NRF_ERROR_NOT_FOUND)){
//											APP_ERROR_CHECK(err_code);
//										}
//									} while (err_code == NRF_ERROR_RESOURCES);									
//								}
								index = index-len;
							}
							else
							{
								index--;
								for(i=0;i<index;i++){
									data_array[i] = data_array[i+1];
							}
						}
						state = 0;
					}
					}break;
				default:state = 0;index=0;break;
			}

        case APP_UART_COMMUNICATION_ERROR:
//            APP_ERROR_HANDLER(p_event->data.error_communication);
            break;

        case APP_UART_FIFO_ERROR:
			printf("APP_UART_FIFO_ERROR\r\n");
            APP_ERROR_HANDLER(p_event->data.error_code);
            break;

        default:
            break;
    }
}

int Send_bytes_to_Ble(unsigned char *bytes,int len)
{
	uint32_t                     err_code;
	do{
			uint16_t len_send = len;
			err_code = ble_nus_data_send(&m_nus, bytes, &len_send, m_conn_handle);
			if ((err_code != NRF_ERROR_INVALID_STATE) && (err_code != NRF_ERROR_RESOURCES) && (err_code != NRF_ERROR_NOT_FOUND)){
				APP_ERROR_CHECK(err_code);
			}
	} while (err_code == NRF_ERROR_RESOURCES);
}
/**@snippet [Handling the data received over UART] */


/**@brief  Function for initializing the UART module.
 */
/**@snippet [UART Initialization] */
void uart_init(void)
{
    uint32_t                     err_code;
    app_uart_comm_params_t const comm_params =
    {
        .rx_pin_no    = RX_PIN_NUMBER,
        .tx_pin_no    = TX_PIN_NUMBER,
        .rts_pin_no   = RTS_PIN_NUMBER,
        .cts_pin_no   = CTS_PIN_NUMBER,
        .flow_control = APP_UART_FLOW_CONTROL_DISABLED,
        .use_parity   = false,
#if defined (UART_PRESENT)
        .baud_rate    = NRF_UART_BAUDRATE_115200
#else
        .baud_rate    = NRF_UARTE_BAUDRATE_115200
#endif
    };

    APP_UART_FIFO_INIT(&comm_params,
                       UART_RX_BUF_SIZE,
                       UART_TX_BUF_SIZE,
                       uart_event_handle,
                       APP_IRQ_PRIORITY_LOWEST,
                       err_code);
    APP_ERROR_CHECK(err_code);
}
/**@snippet [UART Initialization] */


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

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

    init.advdata.name_type          = BLE_ADVDATA_FULL_NAME;
    init.advdata.include_appearance = false;
    init.advdata.flags              = BLE_GAP_ADV_FLAGS_LE_ONLY_LIMITED_DISC_MODE;

    init.srdata.uuids_complete.uuid_cnt = sizeof(m_adv_uuids) / sizeof(m_adv_uuids[0]);
    init.srdata.uuids_complete.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.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 initializing power management.
 */
static void power_management_init(void)
{
    ret_code_t err_code;
    err_code = nrf_pwr_mgmt_init();
    APP_ERROR_CHECK(err_code);
}


/**@brief Function for handling the idle state (main loop).
 *
 * @details If there is no pending log operation, then sleep until next the next event occurs.
 */static void idle_state_handle(void)
{
    if (NRF_LOG_PROCESS() == false)
    {
        nrf_pwr_mgmt_run();
    }
}




/***********************************************************************************************/
void send_to_ble_nus(uint8_t index,uint8_t cmd,uint8_t* dat,uint8_t datLen)
{
//	uint32_t err_code;
	uint8_t buf[255];
	uint16_t Len = datLen+8;
	uint16_t L=0;
	uint8_t i;
	uint8_t ver = 0;
	if(Len>255) return;
	buf[L++] = 0xAA;  ver += 0xAA;	//帧头
	buf[L++] = Len;   ver += Len;	//长度
	buf[L++] = ~Len;  ver += (~Len);//长度反码
	buf[L++] = cmd;   ver += cmd;	//命令
	for(i=0;i<datLen;i++){ buf[L++] = dat[i]; ver += dat[i];} //数据
	buf[L++] = ver;   				//校验
	
//	for(i=0;i<L;i++)		app_uart_put(buf[i]);	  //串口输出数据
	
//	buf[0] = '1';
//	L = 1;
	ble_nus_data_send(&m_nus, buf, &L, m_conn_handle);
	
//	do{
//		err_code = ble_nus_data_send(&m_nus, buf, &L, m_conn_handle);
//		if ((err_code != NRF_ERROR_INVALID_STATE) &&
//			(err_code != NRF_ERROR_RESOURCES) &&
//			(err_code != NRF_ERROR_NOT_FOUND))
//		{
//			APP_ERROR_CHECK(err_code);
//		}
//	} while (err_code == NRF_ERROR_RESOURCES);
}


void send_ble_data_f(float* acc_val,int32_t press_val,int32_t* signal_out)
{
	uint8_t buf[255];
	uint8_t L=0;
	
	int32_t temp;
	
	temp = (int32_t)(acc_val[0]*1000);
	buf[L++] = (uint8_t)(temp>>24);
	buf[L++] = (uint8_t)(temp>>16);
	buf[L++] = (uint8_t)(temp>>8);
	buf[L++] = (uint8_t)(temp>>0);
	
	temp = (int32_t)(acc_val[1]*1000);
	buf[L++] = (uint8_t)(temp>>24);
	buf[L++] = (uint8_t)(temp>>16);
	buf[L++] = (uint8_t)(temp>>8);
	buf[L++] = (uint8_t)(temp>>0);
	
	temp = (int32_t)(acc_val[2]*1000);
	buf[L++] = (uint8_t)(temp>>24);
	buf[L++] = (uint8_t)(temp>>16);
	buf[L++] = (uint8_t)(temp>>8);
	buf[L++] = (uint8_t)(temp>>0);
	
	buf[L++] = (uint8_t)(press_val>>24);
	buf[L++] = (uint8_t)(press_val>>16);
	buf[L++] = (uint8_t)(press_val>>8);
	buf[L++] = (uint8_t)(press_val>>0);
	
	buf[L++] = (uint8_t)(signal_out[0]>>24);
	buf[L++] = (uint8_t)(signal_out[0]>>16);
	buf[L++] = (uint8_t)(signal_out[0]>>8);
	buf[L++] = (uint8_t)(signal_out[0]>>0);
	
	buf[L++] = (uint8_t)(signal_out[1]>>24);
	buf[L++] = (uint8_t)(signal_out[1]>>16);
	buf[L++] = (uint8_t)(signal_out[1]>>8);
	buf[L++] = (uint8_t)(signal_out[1]>>0);
	
	buf[L++] = (uint8_t)(signal_out[2]>>24);
	buf[L++] = (uint8_t)(signal_out[2]>>16);
	buf[L++] = (uint8_t)(signal_out[2]>>8);
	buf[L++] = (uint8_t)(signal_out[2]>>0);
	
	send_to_ble_nus(DEX_NUM,CMD_HEART,buf,L);
}

void send_ble_data_int(short* acc_val,short* gry_val,int32_t press_val,int32_t* signal_out,float* signal_float)
{
	uint8_t buf[255];
	uint8_t L=0;
	int32_t temp;
	
	
	buf[L++] = (uint8_t)(acc_val[0]>>8);
	buf[L++] = (uint8_t)(acc_val[0]>>0);
	buf[L++] = (uint8_t)(acc_val[1]>>8);
	buf[L++] = (uint8_t)(acc_val[1]>>0);
	buf[L++] = (uint8_t)(acc_val[2]>>8);
	buf[L++] = (uint8_t)(acc_val[2]>>0);
	
	buf[L++] = (uint8_t)(gry_val[0]>>8);
	buf[L++] = (uint8_t)(gry_val[0]>>0);
	buf[L++] = (uint8_t)(gry_val[1]>>8);
	buf[L++] = (uint8_t)(gry_val[1]>>0);
	buf[L++] = (uint8_t)(gry_val[2]>>8);
	buf[L++] = (uint8_t)(gry_val[2]>>0);
	
	buf[L++] = (uint8_t)(press_val>>24);
	buf[L++] = (uint8_t)(press_val>>16);
	buf[L++] = (uint8_t)(press_val>>8);
	buf[L++] = (uint8_t)(press_val>>0);
	
	
	buf[L++] = (uint8_t)(signal_out[0]>>24);
	buf[L++] = (uint8_t)(signal_out[0]>>16);
	buf[L++] = (uint8_t)(signal_out[0]>>8);
	buf[L++] = (uint8_t)(signal_out[0]>>0);
	
	buf[L++] = (uint8_t)(signal_out[1]>>24);
	buf[L++] = (uint8_t)(signal_out[1]>>16);
	buf[L++] = (uint8_t)(signal_out[1]>>8);
	buf[L++] = (uint8_t)(signal_out[1]>>0);
	
	buf[L++] = (uint8_t)(signal_out[2]>>24);    
	buf[L++] = (uint8_t)(signal_out[2]>>8);
	buf[L++] = (uint8_t)(signal_out[2]>>0);
	
	buf[L++] = (uint8_t)(signal_out[3]>>24);
	buf[L++] = (uint8_t)(signal_out[3]>>16);
	buf[L++] = (uint8_t)(signal_out[3]>>8);
	buf[L++] = (uint8_t)(signal_out[3]>>0);
	
	temp = (int32_t)signal_float[0];
	buf[L++] = (uint8_t)(temp>>24);
	buf[L++] = (uint8_t)(temp>>16);
	buf[L++] = (uint8_t)(temp>>8);
	buf[L++] = (uint8_t)(temp>>0);
	
	temp = (int32_t)signal_float[1];
	buf[L++] = (uint8_t)(temp>>24);
	buf[L++] = (uint8_t)(temp>>16);
	buf[L++] = (uint8_t)(temp>>8);
	buf[L++] = (uint8_t)(temp>>0);
	
	send_to_ble_nus(DEX_NUM,CMD_HEART,buf,L);
}

void send_ble_data_press(int32_t press_val,short ts)
{
	uint8_t buf[32];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(press_val>>24);
	buf[L++] = (uint8_t)(press_val>>16);
	buf[L++] = (uint8_t)(press_val>>8);
	buf[L++] = (uint8_t)(press_val>>0);
	
	buf[L++] = (uint8_t)(ts>>8);
	buf[L++] = (uint8_t)(ts>>0);
	
	
	send_to_ble_nus(DEX_NUM,CMD_HEART,buf,L);
}

static uint16_t cmd_ntimes[NUMBERS_OF_MOTION];
static uint16_t cmd_timestamp[NUMBERS_OF_MOTION];
void send_ble_motion_process(void)
{
	uint8_t buf[255];
	uint8_t L=0;
	int i;
	for(i=0;i<NUMBERS_OF_MOTION;i++){
		if(cmd_ntimes[i]>0){
			cmd_ntimes[i]--;
			buf[L++] = (uint8_t)(i>>0);
			buf[L++] = (uint8_t)(cmd_timestamp[i]>>8);
			buf[L++] = (uint8_t)(cmd_timestamp[i]>>0);
			send_to_ble_nus(DEX_NUM,CMD_MOTION,buf,L);
		}
	}
}


void send_ble_motion(uint8_t motion,uint16_t ts)
{
	if(motion>=NUMBERS_OF_MOTION) return;
	cmd_ntimes[motion] = 10;
	cmd_timestamp[motion] = ts;
}


/**
  * @brief  卡尔曼滤波函数
  * @param  None
  * @retval None
  */
float kalmanFilter_x(float new_zk)
{
 static float preBestResult=0;
 static float p=10.0,q=0.000001,r=0.0000101,kg=0;
 p=p+q;
 kg=p/(p+r);
 new_zk=preBestResult+(kg*(new_zk-preBestResult));
 p=(1-kg)*p;
 preBestResult=new_zk;
 return new_zk;
}

/**
  * @brief  卡尔曼滤波函数
  * @param  None
  * @retval None
  */
float kalmanFilter_y(float new_zk)
{
 static float preBestResult=0;
 static float p=10.0,q=0.000001,r=0.0000101,kg=0;
 p=p+q;
 kg=p/(p+r);
 new_zk=preBestResult+(kg*(new_zk-preBestResult));
 p=(1-kg)*p;
 preBestResult=new_zk;
 return new_zk;
}

/**
  * @brief  卡尔曼滤波函数
  * @param  None
  * @retval None
  */
float kalmanFilter_z(float new_zk)
{
 static float preBestResult=0;
 static float p=10.0,q=0.000001,r=0.0000101,kg=0;
 p=p+q;
 kg=p/(p+r);
 new_zk=preBestResult+(kg*(new_zk-preBestResult));
 p=(1-kg)*p;
 preBestResult=new_zk;
 return new_zk;
}

float kalmanFilter_f(float new_zk)
{
 static float preBestResult=0;
 static float p=10.0,q=0.000001,r=0.0000101,kg=0;
 p=p+q;
 kg=p/(p+r);
 new_zk=preBestResult+(kg*(new_zk-preBestResult));
 p=(1-kg)*p;
 preBestResult=new_zk;
 return new_zk;
}


/*************************************************************************/
#ifdef BSP_BUTTON_0
    #define PIN_IN BSP_BUTTON_0
#endif
#ifndef PIN_IN
    #error "Please indicate input pin"
#endif


nrf_saadc_value_t saadc_val[2];
short gyro[3], accel[3];
float quat[4];
uint16_t timestamp_ble = 0;

int32_t out[7];
short adc_send[4];

//#define  filter_mid_averange_number 10

void bubble_sort(short* a, short n)
{
    short i,j,temp;
    for(i=0;i<n-1;i++){                           
        for(j=n-1;j>i;j--){
            if(a[j]<a[j-1]){
                temp=a[j];
                a[j]=a[j-1];
                a[j-1]=temp;
            }
        }
    }    
}
#define  filter_mid_averange_number 5
short filter_mid_averange(short* p)
{
	
	int i;
	short buf[filter_mid_averange_number];
	for(i=0;i<filter_mid_averange_number;i++){
		buf[i] = p[i];
	}
	bubble_sort(buf,filter_mid_averange_number);
	return buf[filter_mid_averange_number>>1];
}

short filter_mid(short val)
{
	
	static short buf[filter_mid_averange_number];
	static int dex = 0;
	buf[dex] = val;
	if(++dex>=filter_mid_averange_number) dex = 0;
	return filter_mid_averange(buf);
}

short filter_mid_slide(short val)
{
	static short buf[filter_mid_averange_number] = {0};
	static int dex = 0;
	static int32_t sum = 0;
	sum = sum - buf[dex];
	buf[dex] = val;
	if(++dex>=filter_mid_averange_number) dex = 0;
	sum += val;
	return sum/filter_mid_averange_number;
}

short filter_adc1(short val)
{
	static short buf[filter_mid_averange_number];
	static int dex = 0;
	buf[dex] = val;
	if(++dex>=filter_mid_averange_number) dex = 0;
	return filter_mid_slide(filter_mid_averange(buf));
}

short filter_adc1_slide(short val)
{
	#define filter_adc1_slide_num 5
	static short buf[filter_adc1_slide_num] = {0};
	static int dex = 0;
	static int32_t sum = 0;
	sum = sum - buf[dex];
	buf[dex] = val;
	if(++dex>=filter_adc1_slide_num) dex = 0;
	sum += val;
	return sum/filter_adc1_slide_num;
}



short filter_adc1_squat_slide(short val)
{
	#define filter_adc1_d_slide_num 10
	static short buf[filter_adc1_d_slide_num] = {0};
	static int dex = 0;
	static int32_t sum = 0;
	sum = sum - buf[dex];
	buf[dex] = val;
	if(++dex>=filter_adc1_d_slide_num) dex = 0;
	sum += val;
	return sum/filter_adc1_d_slide_num;
}

short filter_accy_slide(short val)
{
	#define  filter_accy_slide_number 5
	static short buf[filter_accy_slide_number] = {0};
	static int dex = 0;
	static int32_t sum = 0;
	sum = sum - buf[dex];
	buf[dex] = val;
	if(++dex>=filter_accy_slide_number) dex = 0;
	sum += val;
	return sum/filter_accy_slide_number;
}

short filter_accz_slide(short val)
{
	#define  filter_accz_slide_number 5
	static short buf[filter_accz_slide_number] = {0};
	static int dex = 0;
	static int32_t sum = 0;
	sum = sum - buf[dex];
	buf[dex] = val;
	if(++dex>=filter_accz_slide_number) dex = 0;
	sum += val;
	return sum/filter_accz_slide_number;
}


float quatdianc(const float* Q,const float* P)
{
 float result;
 result=P[0]*Q[0] + P[1]*Q[1] + P[2]*Q[2] + P[3]*Q[3];
 return result;
}

void quatconj2(const float *Quat,float *out)
{
 out[0]= Quat[0];
 out[1]=-Quat[1];
 out[2]=-Quat[2];
 out[3]=-Quat[3];
}

//????
void quatinv(const float* Q,float *quatinvQ)
{ 
 float mod;
 float temp[4];
 quatconj2(Q,temp);
 mod=quatdianc(temp,temp); 
 quatinvQ[0]=temp[0]/mod;
 quatinvQ[1]=temp[1]/mod;
 quatinvQ[2]=temp[2]/mod;
 quatinvQ[3]=temp[3]/mod;
}

void quatmultiply(const float * Q,const float * P,float *QP)
{
	QP[0]=P[0]*Q[0] - P[1]*Q[1] - P[2]*Q[2] - P[3]*Q[3];
 QP[1]=P[0]*Q[1] + P[1]*Q[0] + P[2]*Q[3] - P[3]*Q[2]; 
 QP[2]=P[0]*Q[2] + P[2]*Q[0] + P[3]*Q[1] - P[1]*Q[3];
 QP[3]=P[0]*Q[3] + P[3]*Q[0] + P[1]*Q[2] - P[2]*Q[1]; 
}

void quatrotate(float* sour_pion,const float* Q,float *out_poin)
{
	float Quaternion_p[4];
	float temp[4];
	float temp1[4];
	float temp2[4];
	Quaternion_p[0]=0;
	Quaternion_p[1]=sour_pion[0];
 Quaternion_p[2]=sour_pion[1];
 Quaternion_p[3]=sour_pion[2];
 quatmultiply(Q,Quaternion_p,temp); 
 quatinv(Q,temp2);
 quatmultiply(temp,temp2,temp1);
 out_poin[0]=temp1[1];
 out_poin[1]=temp1[2];
 out_poin[2]=temp1[3];
} 



void Separate_G(float* soure_acc,const float* Q,float *NogAcc)//???????
{
 float temp[4];
 float temp1[3];
 quatconj2(Q,temp);
 quatrotate(soure_acc,temp,temp1);//?????????????
// temp1[2]-=1;//??????? 
	NogAcc[0] = temp1[0];
	NogAcc[1] = temp1[1];
	NogAcc[2] = temp1[2];
// quatrotate(temp1,Q,NogAcc);//?????????????
}


short acc_0[3];
short gyr_0[3];
long quat_0[4];
int32_t press_0;

#define press_zero_offer 50000
static int32_t press_zero_0 = 0;
static int32_t press_sub = 0;
static int32_t press_sub_dt = 0;
static uint32_t ble_timestamp = 0;

static int32_t is_runnig = 0;

float out_f[8];

static int32_t press_window_dt_val = 0;


void press_filter_Increasing(int32_t val)
{
	static int32_t press_last = 0;
	if(press_last<val)
		out[1] = 1;
	else
		out[1] = 0;
	press_last = val;
}

int32_t press_filter_slide(int32_t val)
{
	#define press_filter_slide_num 10
	static int32_t buf[press_filter_slide_num] = {0};
	static int dex = 0;
	static long sum = 0;
	sum = sum - buf[dex];
	buf[dex] = val;
	if(++dex>=press_filter_slide_num) dex = 0;
	sum += val;
	return sum/press_filter_slide_num;
}

int32_t press_filter_dt(int32_t val)
{
	static int32_t press_last = 0;
	int32_t sub = val - press_last;
	press_last = val;
	return sub;
}

int32_t press_dt(int32_t val)
{
	static int32_t press_last = 0;
	int32_t sub = val - press_last;
	press_last = val;
	return sub;
}

int32_t press_window_dt(int32_t val)
{
	#define press_window_len 15
	int i;
	int32_t temp_max = 0;
	int32_t temp_min = 0;
	int temp_max_i = 0;
	int temp_min_i = 0;
	
	static int32_t press_window[press_window_len];
	static int dex = 0;
	
	press_window[dex] = val;
	
	temp_max = press_window[0];temp_max_i = press_window_len-1-dex;
	temp_min = press_window[0];temp_min_i = press_window_len-1-dex;
	for(i=1;i<press_window_len;i++){
		if(temp_max<press_window[i]){ 
			temp_max = press_window[i]; 
			if(i>dex){
				temp_max_i = i-1-dex;
			}else{
				temp_max_i = press_window_len-1-(dex-i);
			}
		}
		if(temp_min>press_window[i]){ 
			temp_min = press_window[i]; 
			if(i>dex){
				temp_min_i = i-1-dex;
			}else{
				temp_min_i = press_window_len-1-(dex-i);
			}
		}
	}
	
	if(++dex>=press_window_len) dex = 0;
	
	if(temp_max_i>temp_min_i)
		return temp_max-temp_min;
	else
		return temp_min-temp_max;
}

int32_t accz_dt(short val)
{
	static int32_t accz_last = 0;
	int32_t sub = abs(val - accz_last);
	accz_last = val;
	return sub;
}

int check_accy_stop(void)
{
	#define accy_stop_num 20
	static short buf[accy_stop_num];
	static short dex = 0;
	
	int i;
	buf[dex] = acc_0[2];
	if(++dex>=accy_stop_num) dex = 0;
	for(i=0;i<accy_stop_num;i++){
		if(buf[i]>500 || buf[i]<-500) return 1; 
	}
	return 0;
}

int check_accz_stop(void)
{
	#define accz_stop_num 20
	static short buf[accz_stop_num];
	static short dex = 0;
	
	int i;
	buf[dex] = acc_0[2];
	if(++dex>=accz_stop_num) dex = 0;
	for(i=0;i<accz_stop_num;i++){
		if(buf[i]>500 || buf[i]<-500) return 1; 
	}
	return 0;
}



void check_press_zero(int32_t val)
{
	static int32_t press_zero_arr[300];
	static int dex = 0;
	static int32_t press_zero_max = 0;
	static int32_t press_zero_min = 0;
	
	press_zero_arr[dex] = val;
	if(dex==0){
		press_zero_max = press_zero_arr[dex];
		press_zero_min = press_zero_arr[dex];
	}else{
		if(press_zero_max<press_zero_arr[dex]) press_zero_max = press_zero_arr[dex];
		if(press_zero_min>press_zero_arr[dex]) press_zero_min = press_zero_arr[dex];
	}
	if(++dex>=300){dex = 0;
//		if(press_zero_max - press_zero_min<5000){
//			if(press_zero_0==0){
//				press_zero_0 = press_zero_max;
//				out[0] = press_zero_0;
//			}else if(press_zero_max<press_zero_0){
//				press_zero_0 = press_zero_max;
//				out[0] = press_zero_0;
//			}
//		}
		if(press_zero_max - press_zero_min<3000){
			press_zero_0 = press_zero_max;
			out[0] = press_zero_0;
			send_ble_motion(MOTION_STOP,ble_timestamp);
			is_runnig = 0;
		}
	}
	
	
}



void check_motion(void)
{
	static int32_t vy_n = 0;
	static int32_t sy_n = 0;
	static int32_t cnt = 0;
	
	
	
	static int32_t gy_n = 0;
	static uint32_t cnt_down = 0;
	
	static short acc_yy[3];
	static short acc_yh[10];

	
	acc_yy[0] = acc_yy[1];
	acc_yy[1] = acc_yy[2];
	acc_yy[2] = acc_0[2];
	acc_0[2] = acc_yy[0];
	
	if(cnt_down<100) cnt_down++;
	if(press_sub_dt<=0){
		vy_n += acc_0[1];
		sy_n += vy_n;
		gy_n = 0;
	}else{
		vy_n = 0;
		sy_n = 0;

		gy_n += gyr_0[1];
		
		if(gy_n<-8000){
			if(cnt_down==100){
				send_ble_motion(MOTION_DOWN,ble_timestamp);
			}
			cnt_down = 0;
		}
	}
	
	if(press_sub==0){ //离地
		if(cnt<150){//1.5秒之内 判断
			if(cnt<5){
				acc_yh[cnt] = acc_0[2];
				if(acc_yh[cnt] - acc_yh[0]>2500){
					send_ble_motion(MOTION_JUMP,ble_timestamp);
				}
			}
			if(sy_n>150000){
				send_ble_motion(MOTION_RIGHT,ble_timestamp);
				cnt = 200;
			}else if(sy_n<-150000){
				send_ble_motion(MOTION_LEFT,ble_timestamp);
				cnt = 200;
			}else{
				cnt++;
			}
		}else{
			vy_n = 0;
			sy_n = 0;
		}
	}else{ //在地上  
		if(is_runnig==0){
			is_runnig = 1;
			send_ble_motion(MOTION_RUN,ble_timestamp);
		}
		cnt = 0;
	}
}

int32_t mean_value(int32_t* p,int len)
{
	int32_t sum = 0;
	int i;
	for(i=0;i<len;i++){
		sum += p[i];
	}
	return sum/len;
}

int32_t variance_value(int32_t* p,int len)
{
	int32_t sum = 0;
	int i;
	int32_t mean = mean_value(p,len);
	for(i=0;i<len;i++){
		sum += (p[i]-mean)*(p[i]-mean);
	}
	return sum/len; 
}

void linear_regression(int32_t* y,int len,float* k,float* b)
{
	int i;
	int n = len;
	float kk,bb,  sum_x, sum_y, lxy, xiSubSqr;
	kk = bb = sum_x = sum_y = lxy = xiSubSqr = 0.0;
	
	for(i=0;i<n;i++){
		sum_x += i; 
		sum_y += y[i];
	}
 
	float x_ave = sum_x/n;
	float y_ave = sum_y/n;
	
	for(i=0;i<n;i++){
		lxy += (i - x_ave) * (y[i] - y_ave);
		xiSubSqr += (i - x_ave) * (i - x_ave);
	}
	
	kk = lxy / xiSubSqr;
	bb = y_ave - kk * x_ave;
	
	*k = kk;
	*b = bb;
}

void check_down(int32_t val)
{
	static int32_t window_press_arr[500];
	static int32_t window_val_arr[500];
	static int dex = 0;

	out[2] = 0;
	out[3] = 0;
	out_f[0] = 0;
	out_f[1] = 0;
	
	if(val>0){
		window_val_arr[dex] = val/10000;
		window_press_arr[dex] = press_sub/10000;
		if(++dex>=500) dex = 0;
		out[2] = mean_value(window_val_arr,dex);
		out[3] = variance_value(window_val_arr,dex);
		linear_regression(window_press_arr,dex,&out_f[0],&out_f[1]);
	}else{
		dex = 0;
	}
}

int32_t press_window_dt_dt(int32_t val)
{
	static int32_t press_last = 0;
	int32_t sub = val - press_last;
	press_last = val;
	return sub;
}

void process_motion(void)
{
	check_press_zero(press_0);
	if(press_zero_0==0) return;
	
	
	press_sub = press_0-press_zero_0;
	if(press_sub<press_zero_offer) press_sub = 0;
	press_sub_dt = press_dt(press_sub);
	out[0] = press_sub_dt;
	press_window_dt_val = press_window_dt(press_sub);
	out[1] = press_window_dt_val;

	
	check_motion();
}	




void in_pin_handler(nrf_drv_gpiote_pin_t pin, nrf_gpiote_polarity_t action)
{
#ifdef USE_DMP
//	nrf_drv_gpiote_out_toggle(PIN_OUT);
//	nrf_drv_gpiote_out_set(PIN_OUT);
	
	ReadPressure_Pre();
	if(mpu6050_get_linear_data_int(gyr_0,acc_0,quat_0)==0){
		press_0 = ReadPressure();
//		printf("press=%d\n",press_0);
		process_motion();
		
//		send_ble_data_int(acc_0,gyr_0,press_sub,out,out_f);
		send_ble_data_int(acc_0,gyr_0,ble_timestamp,out,out_f);
//		send_ble_motion_process();
	}
	ble_timestamp++;
	
//	nrf_drv_gpiote_out_clear(PIN_OUT);
#endif
}

#ifndef USE_DMP
APP_TIMER_DEF(s_testTimer); 
#define TEST_PERIOD    APP_TIMER_TICKS(10)
static void timer_testCallback(void *arg)
{
  UNUSED_PARAMETER(arg);
	timer_callback_Chen();
	ble_timestamp++;
}
#endif
/**
 * @brief Function for configuring: PIN_IN pin for input, PIN_OUT pin for output,
 * and configures GPIOTE to give an interrupt on pin change.
 */
static void gpio_init(void)
{
    ret_code_t err_code;

    err_code = nrf_drv_gpiote_init();
    APP_ERROR_CHECK(err_code);

    nrf_drv_gpiote_out_config_t out_config = GPIOTE_CONFIG_OUT_SIMPLE(false);

    err_code = nrf_drv_gpiote_out_init(PIN_OUT, &out_config);
    APP_ERROR_CHECK(err_code);

    nrf_drv_gpiote_in_config_t in_config = GPIOTE_CONFIG_IN_SENSE_HITOLO(true);
    in_config.pull = NRF_GPIO_PIN_PULLUP;

    err_code = nrf_drv_gpiote_in_init(PIN_IN, &in_config, in_pin_handler);
    APP_ERROR_CHECK(err_code);

    nrf_drv_gpiote_in_event_enable(PIN_IN, true);
	
	nrf_drv_gpiote_out_set(PIN_OUT);

}

//saadc回调函数
void saadc_callback(nrf_drv_saadc_evt_t const *p_event){}
//saadc初始化函数
void saadc_init(void)
{
	ret_code_t err_code;
	//定义SAADC初始化结构体
	//使用默认的宏初始化时需要指定该通道的模拟输入引脚
	//具体的引脚分布参考数据手册
	nrf_saadc_channel_config_t saadc_0 = NRF_DRV_SAADC_DEFAULT_CHANNEL_CONFIG_SE(NRF_SAADC_INPUT_AIN2);
	nrf_saadc_channel_config_t saadc_1 = NRF_DRV_SAADC_DEFAULT_CHANNEL_CONFIG_SE(NRF_SAADC_INPUT_AIN3);
	//初始化SAADC，注册事件回调函数，注意因为使用了阻塞模式
	//所以可以不用回调函数，但是nrf_drv_saadc_init()要求必须提供回调函数
	//所以这里要注册回调函数，无论有没有用到
	err_code=nrf_drv_saadc_init(NULL,saadc_callback);
	APP_ERROR_CHECK(err_code);
	//初始化SAADC通道0
	err_code = nrf_drv_saadc_channel_init(0, &saadc_0);
	err_code = nrf_drv_saadc_channel_init(1, &saadc_1);
    APP_ERROR_CHECK(err_code);
}


int main(void)
 {
    uart_init();
    log_init();
	app_timer_create(&s_testTimer, APP_TIMER_MODE_REPEATED, timer_testCallback);
	config_file_init();
    timers_init();
	gpio_init();
    power_management_init();
    ble_stack_init();
    gap_params_init();
    gatt_init();
    services_init();
    advertising_init();
    conn_params_init();
    advertising_start();
	app_timer_start(s_testTimer, TEST_PERIOD, NULL);
	//rtc_config();
	printf("starting... \r\n");	
    for (;;)
    {
			idle_state_handle();
    }
}


/**
 * @}
 */