2540 用osal 进行串口通信,很多例子为什么延迟比较大,如传感器 MPU6050 实 现 DMP 输出,延迟超过5秒?
This thread has been locked.
If you have a related question, please click the "Ask a related question" button in the top right corner. The newly created question will be automatically linked to this question.
/**************************************************************************************************
Filename: simpleBLEPeripheral.c
Revised: $Date: 2010-08-06 08:56:11 -0700 (Fri, 06 Aug 2010) $
Revision: $Revision: 23333 $
Description: This file contains the Simple BLE Peripheral sample application
for use with the CC2540 Bluetooth Low Energy Protocol Stack.
Copyright 2010 - 2013 Texas Instruments Incorporated. All rights reserved.
IMPORTANT: Your use of this Software is limited to those specific rights
granted under the terms of a software license agreement between the user
who downloaded the software, his/her employer (which must be your employer)
and Texas Instruments Incorporated (the "License"). You may not use this
Software unless you agree to abide by the terms of the License. The License
limits your use, and you acknowledge, that the Software may not be modified,
copied or distributed unless embedded on a Texas Instruments microcontroller
or used solely and exclusively in conjunction with a Texas Instruments radio
frequency transceiver, which is integrated into your product. Other than for
the foregoing purpose, you may not use, reproduce, copy, prepare derivative
works of, modify, distribute, perform, display or sell this Software and/or
its documentation for any purpose.
YOU FURTHER ACKNOWLEDGE AND AGREE THAT THE SOFTWARE AND DOCUMENTATION ARE
PROVIDED 揂S IS?WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED,
INCLUDING WITHOUT LIMITATION, ANY WARRANTY OF MERCHANTABILITY, TITLE,
NON-INFRINGEMENT AND FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT SHALL
TEXAS INSTRUMENTS OR ITS LICENSORS BE LIABLE OR OBLIGATED UNDER CONTRACT,
NEGLIGENCE, STRICT LIABILITY, CONTRIBUTION, BREACH OF WARRANTY, OR OTHER
LEGAL EQUITABLE THEORY ANY DIRECT OR INDIRECT DAMAGES OR EXPENSES
INCLUDING BUT NOT LIMITED TO ANY INCIDENTAL, SPECIAL, INDIRECT, PUNITIVE
OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF PROCUREMENT
OF SUBSTITUTE GOODS, TECHNOLOGY, SERVICES, OR ANY CLAIMS BY THIRD PARTIES
(INCLUDING BUT NOT LIMITED TO ANY DEFENSE THEREOF), OR OTHER SIMILAR COSTS.
Should you have any questions regarding your right to use this Software,
contact Texas Instruments Incorporated at www.TI.com.
**************************************************************************************************/
/*********************************************************************
* INCLUDES
*/
#include "bcomdef.h"
#include "OSAL.h"
#include "OSAL_PwrMgr.h"
#include "OnBoard.h"
#include "hal_adc.h"
#include "hal_led.h"
#include "hal_key.h"
#include "hal_lcd.h"
#include "gatt.h"
#include "hci.h"
#include "gapgattserver.h"
#include "gattservapp.h"
#include "devinfoservice.h"
#include "simpleGATTprofile.h"
#if defined( CC2540_MINIDK )
#include "simplekeys.h"
#endif
#if defined ( PLUS_BROADCASTER )
#include "peripheralBroadcaster.h"
#else
#include "peripheral.h"
#endif
#include "gapbondmgr.h"
#include "simpleBLEPeripheral.h"
#if defined FEATURE_OAD
#include "oad.h"
#include "oad_target.h"
#endif
#include "Npi.h"
#include "stdio.h"
//AmoMcu add for SENSOR_MPU6050
#if defined( SENSOR_MPU6050 )
#include "Mpu6050.h"
#endif
#if defined( SENSOR_MPU6050_DMP )
#include "anbt_dmp_fun.h"
#include "anbt_i2c.h"
#include "anbt_dmp_mpu6050.h"
#include "anbt_dmp_driver.h"
#endif
/*********************************************************************
* MACROS
*/
/*********************************************************************
* CONSTANTS
*/
// How often to perform periodic event
#define SBP_PERIODIC_EVT_PERIOD 50//5000
// What is the advertising interval when device is discoverable (units of 625us, 160=100ms)
#define DEFAULT_ADVERTISING_INTERVAL 160
// Limited discoverable mode advertises for 30.72s, and then stops
// General discoverable mode advertises indefinitely
#if defined ( CC2540_MINIDK )
#define DEFAULT_DISCOVERABLE_MODE GAP_ADTYPE_FLAGS_LIMITED
#else
#define DEFAULT_DISCOVERABLE_MODE GAP_ADTYPE_FLAGS_GENERAL
#endif // defined ( CC2540_MINIDK )
// Minimum connection interval (units of 1.25ms, 80=100ms) if automatic parameter update request is enabled
#define DEFAULT_DESIRED_MIN_CONN_INTERVAL 20
// Maximum connection interval (units of 1.25ms, 800=1000ms) if automatic parameter update request is enabled
#define DEFAULT_DESIRED_MAX_CONN_INTERVAL 20
// Slave latency to use if automatic parameter update request is enabled
#define DEFAULT_DESIRED_SLAVE_LATENCY 0
// Supervision timeout value (units of 10ms, 1000=10s) if automatic parameter update request is enabled
#define DEFAULT_DESIRED_CONN_TIMEOUT 500
// Whether to enable automatic parameter update request when a connection is formed
#define DEFAULT_ENABLE_UPDATE_REQUEST TRUE
// Connection Pause Peripheral time value (in seconds)
#define DEFAULT_CONN_PAUSE_PERIPHERAL 6
// Company Identifier: Texas Instruments Inc. (13)
#define TI_COMPANY_ID 0x000D
#define INVALID_CONNHANDLE 0xFFFF
// Length of bd addr as a string
#define B_ADDR_STR_LEN 15
#if defined ( PLUS_BROADCASTER )
#define ADV_IN_CONN_WAIT 500 // delay 500 ms
#endif
/*********************************************************************
* TYPEDEFS
*/
/*********************************************************************
* GLOBAL VARIABLES
*/
/*********************************************************************
* EXTERNAL VARIABLES
*/
/*********************************************************************
* EXTERNAL FUNCTIONS
*/
/*********************************************************************
* LOCAL VARIABLES
*/
static uint8 simpleBLEPeripheral_TaskID; // Task ID for internal task/event processing
static gaprole_States_t gapProfileState = GAPROLE_INIT;
// GAP - SCAN RSP data (max size = 31 bytes)
static uint8 scanRspData[] =
{
// complete name
0x14, // length of this data
GAP_ADTYPE_LOCAL_NAME_COMPLETE,
0x53, // 'S'
0x69, // 'i'
0x6d, // 'm'
0x70, // 'p'
0x6c, // 'l'
0x65, // 'e'
0x42, // 'B'
0x4c, // 'L'
0x45, // 'E'
0x50, // 'P'
0x65, // 'e'
0x72, // 'r'
0x69, // 'i'
0x70, // 'p'
0x68, // 'h'
0x65, // 'e'
0x72, // 'r'
0x61, // 'a'
0x6c, // 'l'
// connection interval range
0x05, // length of this data
GAP_ADTYPE_SLAVE_CONN_INTERVAL_RANGE,
LO_UINT16( DEFAULT_DESIRED_MIN_CONN_INTERVAL ), // 100ms
HI_UINT16( DEFAULT_DESIRED_MIN_CONN_INTERVAL ),
LO_UINT16( DEFAULT_DESIRED_MAX_CONN_INTERVAL ), // 1s
HI_UINT16( DEFAULT_DESIRED_MAX_CONN_INTERVAL ),
// Tx power level
0x02, // length of this data
GAP_ADTYPE_POWER_LEVEL,
0 // 0dBm
};
// GAP - Advertisement data (max size = 31 bytes, though this is
// best kept short to conserve power while advertisting)
static uint8 advertData[] =
{
// Flags; this sets the device to use limited discoverable
// mode (advertises for 30 seconds at a time) instead of general
// discoverable mode (advertises indefinitely)
0x02, // length of this data
GAP_ADTYPE_FLAGS,
DEFAULT_DISCOVERABLE_MODE | GAP_ADTYPE_FLAGS_BREDR_NOT_SUPPORTED,
// service UUID, to notify central devices what services are included
// in this peripheral
0x03, // length of this data
GAP_ADTYPE_16BIT_MORE, // some of the UUID's, but not all
LO_UINT16( SIMPLEPROFILE_SERV_UUID ),
HI_UINT16( SIMPLEPROFILE_SERV_UUID ),
};
// GAP GATT Attributes
static uint8 attDeviceName[GAP_DEVICE_NAME_LEN] = "AmoMcu-Mpu6050-Peri";
/*********************************************************************
* LOCAL FUNCTIONS
*/
static void simpleBLEPeripheral_ProcessOSALMsg( osal_event_hdr_t *pMsg );
static void peripheralStateNotificationCB( gaprole_States_t newState );
static void performPeriodicTask( void );
static void simpleProfileChangeCB( uint8 paramID );
#if defined( CC2540_MINIDK )
static void simpleBLEPeripheral_HandleKeys( uint8 shift, uint8 keys );
#endif
#if (defined HAL_LCD) && (HAL_LCD == TRUE)
static char *bdAddr2Str ( uint8 *pAddr );
#endif // (defined HAL_LCD) && (HAL_LCD == TRUE)
// 发送数据到主机
void simpleBLE_SendData(uint8* buffer, uint8 sendBytes);
// 串口回调函数
static void simpleBLE_NpiSerialCallback( uint8 port, uint8 events );
/*********************************************************************
* PROFILE CALLBACKS
*/
// GAP Role Callbacks
static gapRolesCBs_t simpleBLEPeripheral_PeripheralCBs =
{
peripheralStateNotificationCB, // Profile State Change Callbacks
NULL // When a valid RSSI is read from controller (not used by application)
};
// GAP Bond Manager Callbacks
static gapBondCBs_t simpleBLEPeripheral_BondMgrCBs =
{
NULL, // Passcode callback (not used by application)
NULL // Pairing / Bonding state Callback (not used by application)
};
// Simple GATT Profile Callbacks
static simpleProfileCBs_t simpleBLEPeripheral_SimpleProfileCBs =
{
simpleProfileChangeCB // Charactersitic value change callback
};
/*********************************************************************
* PUBLIC FUNCTIONS
*/
/*********************************************************************
* @fn SimpleBLEPeripheral_Init
*
* @brief Initialization function for the Simple BLE Peripheral App Task.
* This is called during initialization and should contain
* any application specific initialization (ie. hardware
* initialization/setup, table initialization, power up
* notificaiton ... ).
*
* @param task_id - the ID assigned by OSAL. This ID should be
* used to send messages and set timers.
*
* @return none
*/
void SimpleBLEPeripheral_Init( uint8 task_id )
{
simpleBLEPeripheral_TaskID = task_id;
// 串口初始化
NPI_InitTransport(simpleBLE_NpiSerialCallback);
NPI_WriteTransport("SimpleBLEPeripheral_Init\r\n", 26);
// Setup the GAP
VOID GAP_SetParamValue( TGAP_CONN_PAUSE_PERIPHERAL, DEFAULT_CONN_PAUSE_PERIPHERAL );
// Setup the GAP Peripheral Role Profile
{
#if defined( CC2540_MINIDK )
// For the CC2540DK-MINI keyfob, device doesn't start advertising until button is pressed
uint8 initial_advertising_enable = FALSE;
#else
// For other hardware platforms, device starts advertising upon initialization
uint8 initial_advertising_enable = TRUE;
#endif
// By setting this to zero, the device will go into the waiting state after
// being discoverable for 30.72 second, and will not being advertising again
// until the enabler is set back to TRUE
uint16 gapRole_AdvertOffTime = 0;
uint8 enable_update_request = DEFAULT_ENABLE_UPDATE_REQUEST;
uint16 desired_min_interval = DEFAULT_DESIRED_MIN_CONN_INTERVAL;
uint16 desired_max_interval = DEFAULT_DESIRED_MAX_CONN_INTERVAL;
uint16 desired_slave_latency = DEFAULT_DESIRED_SLAVE_LATENCY;
uint16 desired_conn_timeout = DEFAULT_DESIRED_CONN_TIMEOUT;
// Set the GAP Role Parameters
GAPRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &initial_advertising_enable );
GAPRole_SetParameter( GAPROLE_ADVERT_OFF_TIME, sizeof( uint16 ), &gapRole_AdvertOffTime );
GAPRole_SetParameter( GAPROLE_SCAN_RSP_DATA, sizeof ( scanRspData ), scanRspData );
GAPRole_SetParameter( GAPROLE_ADVERT_DATA, sizeof( advertData ), advertData );
GAPRole_SetParameter( GAPROLE_PARAM_UPDATE_ENABLE, sizeof( uint8 ), &enable_update_request );
GAPRole_SetParameter( GAPROLE_MIN_CONN_INTERVAL, sizeof( uint16 ), &desired_min_interval );
GAPRole_SetParameter( GAPROLE_MAX_CONN_INTERVAL, sizeof( uint16 ), &desired_max_interval );
GAPRole_SetParameter( GAPROLE_SLAVE_LATENCY, sizeof( uint16 ), &desired_slave_latency );
GAPRole_SetParameter( GAPROLE_TIMEOUT_MULTIPLIER, sizeof( uint16 ), &desired_conn_timeout );
}
// Set the GAP Characteristics
GGS_SetParameter( GGS_DEVICE_NAME_ATT, GAP_DEVICE_NAME_LEN, attDeviceName );
// Set advertising interval
{
uint16 advInt = DEFAULT_ADVERTISING_INTERVAL;
GAP_SetParamValue( TGAP_LIM_DISC_ADV_INT_MIN, advInt );
GAP_SetParamValue( TGAP_LIM_DISC_ADV_INT_MAX, advInt );
GAP_SetParamValue( TGAP_GEN_DISC_ADV_INT_MIN, advInt );
GAP_SetParamValue( TGAP_GEN_DISC_ADV_INT_MAX, advInt );
}
// Setup the GAP Bond Manager
{
uint32 passkey = 0; // passkey "000000"
uint8 pairMode = GAPBOND_PAIRING_MODE_WAIT_FOR_REQ;
uint8 mitm = TRUE;
uint8 ioCap = GAPBOND_IO_CAP_DISPLAY_ONLY;
uint8 bonding = TRUE;
GAPBondMgr_SetParameter( GAPBOND_DEFAULT_PASSCODE, sizeof ( uint32 ), &passkey );
GAPBondMgr_SetParameter( GAPBOND_PAIRING_MODE, sizeof ( uint8 ), &pairMode );
GAPBondMgr_SetParameter( GAPBOND_MITM_PROTECTION, sizeof ( uint8 ), &mitm );
GAPBondMgr_SetParameter( GAPBOND_IO_CAPABILITIES, sizeof ( uint8 ), &ioCap );
GAPBondMgr_SetParameter( GAPBOND_BONDING_ENABLED, sizeof ( uint8 ), &bonding );
}
// Initialize GATT attributes
GGS_AddService( GATT_ALL_SERVICES ); // GAP
GATTServApp_AddService( GATT_ALL_SERVICES ); // GATT attributes
DevInfo_AddService(); // Device Information Service
SimpleProfile_AddService( GATT_ALL_SERVICES ); // Simple GATT Profile
#if defined FEATURE_OAD
VOID OADTarget_AddService(); // OAD Profile
#endif
// Setup the SimpleProfile Characteristic Values
{
uint8 charValue1 = 1;
uint8 charValue2 = 2;
uint8 charValue3 = 3;
uint8 charValue4 = 4;
uint8 charValue5[SIMPLEPROFILE_CHAR5_LEN] = { 1, 2, 3, 4, 5 };
SimpleProfile_SetParameter( SIMPLEPROFILE_CHAR1, sizeof ( uint8 ), &charValue1 );
SimpleProfile_SetParameter( SIMPLEPROFILE_CHAR2, sizeof ( uint8 ), &charValue2 );
SimpleProfile_SetParameter( SIMPLEPROFILE_CHAR3, sizeof ( uint8 ), &charValue3 );
SimpleProfile_SetParameter( SIMPLEPROFILE_CHAR4, sizeof ( uint8 ), &charValue4 );
SimpleProfile_SetParameter( SIMPLEPROFILE_CHAR5, SIMPLEPROFILE_CHAR5_LEN, charValue5 );
}
#if defined( CC2540_MINIDK )
SK_AddService( GATT_ALL_SERVICES ); // Simple Keys Profile
// Register for all key events - This app will handle all key events
RegisterForKeys( simpleBLEPeripheral_TaskID );
// makes sure LEDs are off
HalLedSet( (HAL_LED_1 | HAL_LED_2), HAL_LED_MODE_OFF );
// For keyfob board set GPIO pins into a power-optimized state
// Note that there is still some leakage current from the buzzer,
// accelerometer, LEDs, and buttons on the PCB.
P0SEL = 0; // Configure Port 0 as GPIO
P1SEL = 0; // Configure Port 1 as GPIO
P2SEL = 0; // Configure Port 2 as GPIO
P0DIR = 0xFC; // Port 0 pins P0.0 and P0.1 as input (buttons),
// all others (P0.2-P0.7) as output
P1DIR = 0xFF; // All port 1 pins (P1.0-P1.7) as output
P2DIR = 0x1F; // All port 1 pins (P2.0-P2.4) as output
P0 = 0x03; // All pins on port 0 to low except for P0.0 and P0.1 (buttons)
P1 = 0; // All pins on port 1 to low
P2 = 0; // All pins on port 2 to low
#endif // #if defined( CC2540_MINIDK )
#if (defined HAL_LCD) && (HAL_LCD == TRUE)
#if defined FEATURE_OAD
#if defined (HAL_IMAGE_A)
HalLcdWriteStringValue( "BLE Peri-A", OAD_VER_NUM( _imgHdr.ver ), 16, HAL_LCD_LINE_1 );
#else
HalLcdWriteStringValue( "BLE Peri-B", OAD_VER_NUM( _imgHdr.ver ), 16, HAL_LCD_LINE_1 );
#endif // HAL_IMAGE_A
#else
HalLcdWriteString( "BLE Peripheral", HAL_LCD_LINE_1 );
#endif // FEATURE_OAD
#endif // (defined HAL_LCD) && (HAL_LCD == TRUE)
// Register callback with SimpleGATTprofile
VOID SimpleProfile_RegisterAppCBs( &simpleBLEPeripheral_SimpleProfileCBs );
// Enable clock divide on halt
// This reduces active current while radio is active and CC254x MCU
// is halted
HCI_EXT_ClkDivOnHaltCmd( HCI_EXT_ENABLE_CLK_DIVIDE_ON_HALT );
#if defined ( DC_DC_P0_7 )
// Enable stack to toggle bypass control on TPS62730 (DC/DC converter)
HCI_EXT_MapPmIoPortCmd( HCI_EXT_PM_IO_PORT_P0, HCI_EXT_PM_IO_PORT_PIN7 );
#endif // defined ( DC_DC_P0_7 )
#if defined( SENSOR_MPU6050 )
accInit();
#endif
#if defined( SENSOR_MPU6050_DMP )
ANBT_I2C_Configuration(); //IIC初始化
AnBT_DMP_Delay_ms(30);
AnBT_DMP_MPU6050_Init(); //6050DMP初始化
#endif
//10 标志SBP_START_DEVICE_EVT启动该event
// Setup a delayed profile startup
osal_set_event( simpleBLEPeripheral_TaskID, SBP_START_DEVICE_EVT );
}
/*********************************************************************
* @fn SimpleBLEPeripheral_ProcessEvent
*
* @brief Simple BLE Peripheral Application Task event processor. This function
* is called to process all events for the task. Events
* include timers, messages and any other user defined events.
*
* @param task_id - The OSAL assigned task ID.
* @param events - events to process. This is a bit map and can
* contain more than one event.
*
* @return events not processed
*/
uint16 SimpleBLEPeripheral_ProcessEvent( uint8 task_id, uint16 events )
{
VOID task_id; // OSAL required parameter that isn't used in this function
if ( events & SYS_EVENT_MSG )
{
uint8 *pMsg;
if ( (pMsg = osal_msg_receive( simpleBLEPeripheral_TaskID )) != NULL )
{
simpleBLEPeripheral_ProcessOSALMsg( (osal_event_hdr_t *)pMsg );
// Release the OSAL message
VOID osal_msg_deallocate( pMsg );
}
// return unprocessed events
return (events ^ SYS_EVENT_MSG);
}
if ( events & SBP_START_DEVICE_EVT )
{
// Start the Device
VOID GAPRole_StartDevice( &simpleBLEPeripheral_PeripheralCBs );
// Start Bond Manager
VOID GAPBondMgr_Register( &simpleBLEPeripheral_BondMgrCBs );
// Set timer for first periodic event
osal_start_timerEx( simpleBLEPeripheral_TaskID, SBP_PERIODIC_EVT, SBP_PERIODIC_EVT_PERIOD );
return ( events ^ SBP_START_DEVICE_EVT );
}
if ( events & SBP_PERIODIC_EVT )
{
// Restart timer
if ( SBP_PERIODIC_EVT_PERIOD )
{
osal_start_timerEx( simpleBLEPeripheral_TaskID, SBP_PERIODIC_EVT, SBP_PERIODIC_EVT_PERIOD );
}
// Perform periodic application task
performPeriodicTask();
return (events ^ SBP_PERIODIC_EVT);
}
#if defined ( PLUS_BROADCASTER )
if ( events & SBP_ADV_IN_CONNECTION_EVT )
{
uint8 turnOnAdv = TRUE;
// Turn on advertising while in a connection
GAPRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &turnOnAdv );
return (events ^ SBP_ADV_IN_CONNECTION_EVT);
}
#endif // PLUS_BROADCASTER
// Discard unknown events
return 0;
}
/*********************************************************************
* @fn simpleBLEPeripheral_ProcessOSALMsg
*
* @brief Process an incoming task message.
*
* @param pMsg - message to process
*
* @return none
*/
static void simpleBLEPeripheral_ProcessOSALMsg( osal_event_hdr_t *pMsg )
{
switch ( pMsg->event )
{
#if defined( CC2540_MINIDK )
case KEY_CHANGE:
simpleBLEPeripheral_HandleKeys( ((keyChange_t *)pMsg)->state, ((keyChange_t *)pMsg)->keys );
break;
#endif // #if defined( CC2540_MINIDK )
default:
// do nothing
break;
}
}
#if defined( CC2540_MINIDK )
/*********************************************************************
* @fn simpleBLEPeripheral_HandleKeys
*
* @brief Handles all key events for this device.
*
* @param shift - true if in shift/alt.
* @param keys - bit field for key events. Valid entries:
* HAL_KEY_SW_2
* HAL_KEY_SW_1
*
* @return none
*/
static void simpleBLEPeripheral_HandleKeys( uint8 shift, uint8 keys )
{
uint8 SK_Keys = 0;
VOID shift; // Intentionally unreferenced parameter
if ( keys & HAL_KEY_SW_1 )
{
SK_Keys |= SK_KEY_LEFT;
}
if ( keys & HAL_KEY_SW_2 )
{
SK_Keys |= SK_KEY_RIGHT;
// if device is not in a connection, pressing the right key should toggle
// advertising on and off
if( gapProfileState != GAPROLE_CONNECTED )
{
uint8 current_adv_enabled_status;
uint8 new_adv_enabled_status;
//Find the current GAP advertisement status
GAPRole_GetParameter( GAPROLE_ADVERT_ENABLED, ¤t_adv_enabled_status );
if( current_adv_enabled_status == FALSE )
{
new_adv_enabled_status = TRUE;
}
else
{
new_adv_enabled_status = FALSE;
}
//change the GAP advertisement status to opposite of current status
GAPRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &new_adv_enabled_status );
}
}
// Set the value of the keys state to the Simple Keys Profile;
// This will send out a notification of the keys state if enabled
SK_SetParameter( SK_KEY_ATTR, sizeof ( uint8 ), &SK_Keys );
}
#endif // #if defined( CC2540_MINIDK )
/*********************************************************************
* @fn peripheralStateNotificationCB
*
* @brief Notification from the profile of a state change.
*
* @param newState - new state
*
* @return none
*/
static void peripheralStateNotificationCB( gaprole_States_t newState )
{
switch ( newState )
{
case GAPROLE_STARTED:
{
uint8 ownAddress[B_ADDR_LEN];
uint8 systemId[DEVINFO_SYSTEM_ID_LEN];
GAPRole_GetParameter(GAPROLE_BD_ADDR, ownAddress);
// use 6 bytes of device address for 8 bytes of system ID value
systemId[0] = ownAddress[0];
systemId[1] = ownAddress[1];
systemId[2] = ownAddress[2];
// set middle bytes to zero
systemId[4] = 0x00;
systemId[3] = 0x00;
// shift three bytes up
systemId[7] = ownAddress[5];
systemId[6] = ownAddress[4];
systemId[5] = ownAddress[3];
DevInfo_SetParameter(DEVINFO_SYSTEM_ID, DEVINFO_SYSTEM_ID_LEN, systemId);
#if (defined HAL_LCD) && (HAL_LCD == TRUE)
// Display device address
HalLcdWriteString( bdAddr2Str( ownAddress ), HAL_LCD_LINE_2 );
HalLcdWriteString( "Initialized", HAL_LCD_LINE_3 );
#endif // (defined HAL_LCD) && (HAL_LCD == TRUE)
}
break;
case GAPROLE_ADVERTISING:
{
#if (defined HAL_LCD) && (HAL_LCD == TRUE)
HalLcdWriteString( "Advertising", HAL_LCD_LINE_3 );
#endif // (defined HAL_LCD) && (HAL_LCD == TRUE)
}
break;
case GAPROLE_CONNECTED:
{
#if (defined HAL_LCD) && (HAL_LCD == TRUE)
HalLcdWriteString( "Connected", HAL_LCD_LINE_3 );
#endif // (defined HAL_LCD) && (HAL_LCD == TRUE)
}
break;
case GAPROLE_WAITING:
{
#if (defined HAL_LCD) && (HAL_LCD == TRUE)
HalLcdWriteString( "Disconnected", HAL_LCD_LINE_3 );
#endif // (defined HAL_LCD) && (HAL_LCD == TRUE)
}
break;
case GAPROLE_WAITING_AFTER_TIMEOUT:
{
#if (defined HAL_LCD) && (HAL_LCD == TRUE)
HalLcdWriteString( "Timed Out", HAL_LCD_LINE_3 );
#endif // (defined HAL_LCD) && (HAL_LCD == TRUE)
}
break;
case GAPROLE_ERROR:
{
#if (defined HAL_LCD) && (HAL_LCD == TRUE)
HalLcdWriteString( "Error", HAL_LCD_LINE_3 );
#endif // (defined HAL_LCD) && (HAL_LCD == TRUE)
}
break;
default:
{
#if (defined HAL_LCD) && (HAL_LCD == TRUE)
HalLcdWriteString( "", HAL_LCD_LINE_3 );
#endif // (defined HAL_LCD) && (HAL_LCD == TRUE)
}
break;
}
gapProfileState = newState;
#if !defined( CC2540_MINIDK )
VOID gapProfileState; // added to prevent compiler warning with
// "CC2540 Slave" configurations
#endif
}
#if defined( SENSOR_MPU6050_DMP )
#define BYTE0(x) (x&0xff)
#define BYTE1(x) (x>>8)
/*函数功能:根据匿名最新上位机协议写的显示姿态的程序
*具体原理看匿名的讲解视频
*/
void Data_Send_Status(float Pitch,float Roll,float Yaw,short *gyro,short *accel)
{
unsigned char i=0;
unsigned char _cnt=0,sum = 0;
unsigned int _temp;
u8 data_to_send[50];
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;
_temp = (int)(Roll*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = 0-(int)(Pitch*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(Yaw*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
//和校验
for(i=0;i<_cnt;i++)
sum+= data_to_send[i];
data_to_send[_cnt++]=sum;
//串口发送数据
//for(i=0;i<_cnt;i++)
// AnBT_Uart1_Send_Char(data_to_send[i]);
//NPI_WriteTransport(data_to_send, _cnt);
sprintf(data_to_send, "%.2f,%.2f,%.2f\r\n", Pitch,Roll,Yaw);
NPI_PrintString(data_to_send);
}
#endif
/*********************************************************************
* @fn performPeriodicTask
*
* @brief Perform a periodic application task. This function gets
* called every five seconds as a result of the SBP_PERIODIC_EVT
* OSAL event. In this example, the value of the third
* characteristic in the SimpleGATTProfile service is retrieved
* from the profile, and then copied into the value of the
* the fourth characteristic.
*
* @param none
*
* @return none
*/
static void performPeriodicTask( void )
{
//AmoMcu add for SENSOR_MPU6050
#if defined( SENSOR_MPU6050 )
//uint8 returnBytes;
{
uint16 numBytes;
char pktBuffer[64];
int8 XAcc, YAcc, ZAcc;
int8 XGro, YGro, ZGro;
HAL_DISABLE_INTERRUPTS(); //关中断
accReadAccGro(&XAcc, &YAcc, &ZAcc, &XGro, &YGro, &ZGro);
HAL_ENABLE_INTERRUPTS(); //开中断
sprintf((char *)pktBuffer, "%d,%d,%d,%d,%d,%d", XAcc, YAcc, ZAcc, XGro, YGro, ZGro);
#if (HAL_UART==TRUE) // 串口打印
NPI_PrintString((uint8*)pktBuffer);
NPI_PrintString("\r\n");
#endif
// 发送数据到主机
if(gapProfileState == GAPROLE_CONNECTED)
{
numBytes = osal_strlen((char *)pktBuffer);
if(numBytes > SIMPLEPROFILE_CHAR6_LEN)
numBytes = SIMPLEPROFILE_CHAR6_LEN;
simpleBLE_SendData((uint8*)pktBuffer, numBytes);
}
}
#else
// uint8 valueToCopy;
// uint8 stat;
// Call to retrieve the value of the third characteristic in the profile
//stat = SimpleProfile_GetParameter( SIMPLEPROFILE_CHAR3, &valueToCopy, &returnBytes);
//if( stat == SUCCESS )
{
/*
* Call to set that value of the fourth characteristic in the profile. Note
* that if notifications of the fourth characteristic have been enabled by
* a GATT client device, then a notification will be sent every time this
* function is called.
*/
//SimpleProfile_SetParameter( SIMPLEPROFILE_CHAR4, sizeof(uint8), &valueToCopy);
}
#endif
#if defined( SENSOR_MPU6050_DMP )
if(1)
{
#define q30 1073741824.0f
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors = INV_WXYZ_QUAT;//陀螺仪存放数组,加速度存放数组,返回状态量
unsigned char more;
long quat[4];//四元数存放数组
float Yaw=0.00,Roll,Pitch;//欧拉角
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;//计算姿态过程用到的变量
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);
#if 1
if(sensors&INV_WXYZ_QUAT)
{
q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //感觉没有价值,注掉
//printf("pitch: %.2f roll:%.2f yaw:%.2f\r\n",Pitch,Roll,Yaw); //普通串口输出
//Data_Send_Status(Pitch,Roll,Yaw,gyro,accel);
//delay_init(72);
{
u8 data_to_send[50];
sprintf(data_to_send, "++%.2f,%.2f,%.2f\r\n", Pitch,Roll,Yaw);
// 打印到串口
NPI_PrintString(data_to_send);
// 发送数据到主机
if(gapProfileState == GAPROLE_CONNECTED)
{
int numBytes = osal_strlen((char *)data_to_send);
if(numBytes > SIMPLEPROFILE_CHAR6_LEN)
numBytes = SIMPLEPROFILE_CHAR6_LEN;
simpleBLE_SendData((uint8*)data_to_send, numBytes);
}
}
}
#else
{
// below calculations unnecessary for orientation only using toxilibs
//long quaternion[4]; //我传给你的四元数 整型
float gravity[3];
float euler[3];
float ypr[3];
float q[3];
q[0]=quat[0] / 16384.0f;
q[1]=quat[1] / 16384.0f;
q[2]=quat[2] / 16384.0f;
q[3]=quat[3] / 16384.0f;
// calculate gravity vector
gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]);
gravity[1] = 2 * (q[0]*q[1] + q[2]*q[3]);
gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
// calculate Euler angles
euler[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1);
euler[1] = -asin(2*q[1]*q[3] + 2*q[0]*q[2]);
euler[2] = atan2(2*q[2]*q[3] - 2*q[0]*q[1], 2*q[0]*q[0] + 2*q[3]*q[3] - 1);
// calculate yaw/pitch/roll angles
ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1);
ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2]));
ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2]));
{
uint8 str[128];
sprintf(str, "ypr[0]: %.2f, ypr[1]:%.2f, ypr[2]:%.2f\r\n",ypr[0], ypr[1],ypr[2]); //普通串口输出
NPI_PrintString((uint8*)str);
}
}
#endif
}
#endif
}
/*********************************************************************
* @fn simpleProfileChangeCB
*
* @brief Callback from SimpleBLEProfile indicating a value change
*
* @param paramID - parameter ID of the value that was changed.
*
* @return none
*/
static void simpleProfileChangeCB( uint8 paramID )
{
uint8 newValue;
uint8 newValueBuf[20]={0};
uint8 returnBytes;
switch( paramID )
{
case SIMPLEPROFILE_CHAR1:
//SimpleProfile_GetParameter( SIMPLEPROFILE_CHAR1, &newValue );
SimpleProfile_GetParameter( SIMPLEPROFILE_CHAR1, newValueBuf, &returnBytes );
#if (defined HAL_LCD) && (HAL_LCD == TRUE)
//HalLcdWriteStringValue( "Char 1:", (uint16)(newValue), 10, HAL_LCD_LINE_3 );
HalLcdWriteString((char*)newValueBuf, HAL_LCD_LINE_4 );
#endif // (defined HAL_LCD) && (HAL_LCD == TRUE)
break;
case SIMPLEPROFILE_CHAR3:
SimpleProfile_GetParameter( SIMPLEPROFILE_CHAR3, &newValue, &returnBytes );
#if (defined HAL_LCD) && (HAL_LCD == TRUE)
HalLcdWriteStringValue( "Char 3:", (uint16)(newValue), 10, HAL_LCD_LINE_3 );
#endif // (defined HAL_LCD) && (HAL_LCD == TRUE)
break;
// AmoMcu 添加的代码, 表示收到主机对 CHAR6 特征值发送过来的数据了
case SIMPLEPROFILE_CHAR6:
{
uint8 newChar6Value[SIMPLEPROFILE_CHAR6_LEN];
SimpleProfile_GetParameter( SIMPLEPROFILE_CHAR6, newChar6Value, &returnBytes );
if(returnBytes > 0)
{
NPI_WriteTransport(newChar6Value,returnBytes);
}
}
break;
default:
// should not reach here!
break;
}
}
#if (defined HAL_LCD) && (HAL_LCD == TRUE)
/*********************************************************************
* @fn bdAddr2Str
*
* @brief Convert Bluetooth address to string. Only needed when
* LCD display is used.
*
* @return none
*/
char *bdAddr2Str( uint8 *pAddr )
{
uint8 i;
char hex[] = "0123456789ABCDEF";
static char str[B_ADDR_STR_LEN];
char *pStr = str;
*pStr++ = '0';
*pStr++ = 'x';
// Start from end of addr
pAddr += B_ADDR_LEN;
for ( i = B_ADDR_LEN; i > 0; i-- )
{
*pStr++ = hex[*--pAddr >> 4];
*pStr++ = hex[*pAddr & 0x0F];
}
*pStr = 0;
return str;
}
#endif // (defined HAL_LCD) && (HAL_LCD == TRUE)
// 发送数据到主机----使用自定义的CHAR6
void simpleBLE_SendData(uint8* buffer, uint8 sendBytes)
{
if( GAPROLE_CONNECTED == gapProfileState) // 已连接上
{
#if 0// 直接通过串口返回
NPI_WriteTransport(buffer, sendBytes);
#else// 通过CHAR6 特征值发送出去, 这里有两种方法
#if 0 // 这种速度慢 SimpleProfile_SetParameter
//simpleBLEChar6DoWrite2 = FALSE;
SimpleProfile_SetParameter( SIMPLEPROFILE_CHAR6,numBytes, buf );
#else // 这种速度快 GATT_Notification
static attHandleValueNoti_t pReport;
pReport.len = sendBytes;
/*
// 也许您会纳闷这个值 0x0035 怎么来的,实际上,Amo我也比较郁闷, 那实际上,
我是通过 主机连接这个从机后获得的, 然后再写到这里来, 作为通知的handle值,
-------------AmoMcu 2014.08.17 ---------------------------------------------
*/
pReport.handle = 0x0035;
osal_memcpy(pReport.value, buffer, sendBytes);
GATT_Notification( 0, &pReport, FALSE );
#endif
#endif
}
else
{
NPI_WriteTransport("Not Connected\r\n", 15);
}
}
static void simpleBLE_NpiSerialCallback( uint8 port, uint8 events )
{
(void)port;
if (events & (HAL_UART_RX_TIMEOUT | HAL_UART_RX_FULL)) //串口有数据
{
uint8 numBytes = 0;
numBytes = NPI_RxBufLen(); //读出串口缓冲区有多少字节
if(numBytes > 0)
{
uint8 *buffer = osal_mem_alloc(numBytes);
if(buffer)
{
NPI_ReadTransport(buffer,numBytes);
NPI_WriteTransport(buffer,numBytes);
osal_mem_free(buffer);
}
}
}
}
/*********************************************************************
*********************************************************************/