基于ESP32四旋翼无人机同蓝牙遥控器通信处理方式

1.蓝牙初始化

在app_main调用函数初始化蓝牙
inf_ble_gatt_init();

2.判断蓝牙是否链接

在dandelion_task中调用函数inf_ble_gatt_connected()判断蓝牙是否链接，进而调用dandelion_handle_RC() 处理同遥控器蓝牙通信。

static void dandelion_task(void* arg)
{

uint32_t console_notify_at = 0;

while(1)
{
	vTaskDelay(2);

	// Get Semaphore >>>> 
	if(xSemaphoreTake(mDandelion.syn.mut, 10))
	{

		/* Check RC State */
		if(!inf_ble_gatt_connected())
		{
			/* Force Stop When Disconnect */
			if(mDandelion.syn.mode != DANDELION_STOP)
			{
				ESP_LOGI(TAG,"mode any -> stop # GATT disconnected");
				mDandelion.syn.mode = DANDELION_STOP;
				loop_process_stop();
			}
			
		}

		/* Check DMP State */
		if(mDandelion.dmp.state != DANDELION_POSE_ERR_OK)
		{
			if(mDandelion.syn.mode != DANDELION_STOP)
			{
				ESP_LOGI(TAG,"mode any -> stop # DMP warning");
				mDandelion.syn.mode = DANDELION_STOP;
				loop_process_stop();
			}
		}

		/* Handle RC-Message Queue */
		dandelion_handle_RC();   //根据消息，控制飞行状态

		/* Loop State Machine */
		dandelion_loop_state(); //没有消息时调用该函数控制飞行状态

		/* Invoke Bluetooth Notify State */
		notify_current_state(0);//通过蓝牙上传飞行数据

		xSemaphoreGive(mDandelion.syn.mut);
	}


	// Print >>>> 
	if(xTaskGetTickCount()-console_notify_at>2000)
	{
		console_notify_at = xTaskGetTickCount();
		ESP_LOGI(TAG,"State Mode=0x%02X Drive=[%1.2f,%1.2f,%1.2f,%1.2f]",mDandelion.syn.mode,
			mDandelion.pid.output[0],mDandelion.pid.output[1],mDandelion.pid.output[2],mDandelion.pid.output[3]);
	}

}

}

3.接收蓝牙信息并处理函数dandelion_handle_RC

static void dandelion_handle_RC(void)
{

if(mDandelion.control.sysmUpdate!=mDandelion.control.userUpdate)
{

	mDandelion.control.sysmUpdate = mDandelion.control.userUpdate;
	hid_msg* pMsg = &mDandelion.control.msg;

	ESP_LOGI(TAG,"Handle RC Update %02X",pMsg->mode);

	if(pMsg->mode==DANDELION_DPOWER)
	{
		if(mDandelion.syn.mode==DANDELION_STOP)
		{
			ESP_LOGI(TAG,"mode stop -> test.power");
			mDandelion.syn.mode = DANDELION_DPOWER;
			loop_process_debug_power(1,pMsg);
		}
		else if(mDandelion.syn.mode==DANDELION_DPOWER)
		{
			ESP_LOGI(TAG,"test.power %3d %3d %3d %3d",pMsg->output[0],pMsg->output[1],pMsg->output[2],pMsg->output[3]);
			loop_process_debug_power(0,pMsg);
		}
	}
	else if(pMsg->mode==DANDELION_LAND)
	{
		if(mDandelion.syn.mode==DANDELION_STOP)
		{
			if(loop_process_land(1)>0)
			{
				ESP_LOGI(TAG,"mode stop -> land");
				mDandelion.syn.mode = DANDELION_LAND;
			}
			else
			{
				ESP_LOGI(TAG,"mode stop -> stop # Switch failed");
			}
		}
	}
	else if(pMsg->mode==DANDELION_FLIGHT)
	{
		if(mDandelion.syn.mode==DANDELION_LAND)
		{
			ESP_LOGI(TAG,"mode land -> launch");
			mDandelion.syn.mode = DANDELION_LAUNCH;
			loop_process_launch(1,0);
		}
		else if(mDandelion.syn.mode==DANDELION_LAUNCH)
		{
			loop_process_launch(0,1);
		}
		else if(mDandelion.syn.mode==DANDELION_FLIGHT)
		{
			loop_process_flight(0,pMsg);
		}
	}
	else
	{
		ESP_LOGI(TAG,"mode any -> stop");
		mDandelion.syn.mode = DANDELION_STOP;
		loop_process_stop();
	}

	notify_current_state(1);
	
}

}

4.将无人机状态信息回传给蓝牙遥控器

通过调用notify_current_state()函数来实现：
/* Packet GATT Characterstic Value /
static void notify_current_state(int force)
{
/ Period Limit */
if( (force==0) && (xTaskGetTickCount() - mDandelion.control.timeNotify<250) )
return ;

hid_msg msg = {0};

/* State Machine */
msg.mode = mDandelion.syn.mode ;

/* Motor Output */
msg.output[0] = mDandelion.pid.output[0] * 100 ; msg.output[1] = mDandelion.pid.output[1] * 100 ;
msg.output[2] = mDandelion.pid.output[2] * 100 ; msg.output[3] = mDandelion.pid.output[3] * 100 ;

/* Pose */
msg.mpu = mDandelion.dmp.state ;
msg.imu.pitch  = mDandelion.dmp.poseCurrent.pitch   * 180 / 3.14;
msg.imu.roll   = mDandelion.dmp.poseCurrent.roll    * 100 / 3.14;
msg.imu.yaw    = mDandelion.dmp.poseCurrent.yawGyro * 100 / 3.14;
msg.imu.height = 0;

/* Battery */
msg.battery = bsp_battery();
msg.charge  = 0;

/* Invoke Interface */
inf_ble_gatt_notify(&msg, sizeof(hid_msg), chara_uuid_KeyboardInput);

mDandelion.control.timeNotify = xTaskGetTickCount();

}