m100(2) 云台自动跟踪

大疆 210云台控制 https://developer.dji.com/onboard-sdk/documentation/sample-doc/advanced-sensing-target-tracking.html

0 开启图像端

0.1 图像跟踪 http://www.cnblogs.com/kekeoutlook/p/8353357.html

图像鼠标选取目标，之后不断保存目标信息，存入共享内存

• 中心（x,y）和长宽
• 发送标志位1判断是否

工程

0.2  共享内存获取和存数据

http://www.cnblogs.com/kekeoutlook/p/8330336.html

1原有速度和角度控制模式

最终执行了 函数

修改控制云台发送速率 为20HZ

        cameraSend = new QTimer();
	cameraSend->setInterval(50); // 20Hz
	connect(cameraSend, SIGNAL(timeout()), this, SLOT(on_tmr_Camera_autosend()));

　　

修改共享内存读取速度

connect(timerBroadcast, SIGNAL(timeout()), this, SLOT(on_tmr_Broadcast()));
timerBroadcast->start(50);//  共享内存接收数据的频率  原有300  改为 50  -20HZ

2像素差值控制速度调整云台

10个像素 - 1角度

2.1云台初始化

void DJIonboardSDK::on_tmr_Broadcast()
{
	//! @note this function cost too much time to run.
	//! it is better run outside the broadcastCallback.
	/*读取共享内存数据*/

	memcpy(&recBOX, recBOX_DATA, sizeof(TrackBox));

	qDebug("flag= %d ", recBOX.flag);
	qDebug("old_flag= %d ", old_flag);
	if (old_flag == 0 && recBOX.flag == 1)
	{
		firstflag = 1;
	}
	else
	{
		firstflag = 0;
	}
	//qDebug("firtflag= %d ", firstflag);
	//qDebug("  %f  ", cam->getYaw());
	old_flag = recBOX.flag;

	if (recBOX.flag != 0)
	{
		//qDebug("  %d  ", recBOX.flag);
		itarget_x = recBOX.x;//  中心
		//qDebug("target_x:%d ", recBOX.x);
		itarget_y = recBOX.y;
		//qDebug("target_x:%d  target_y:%d", recBOX.x, recBOX.y);
		itarget_h = recBOX.height;
		itarget_w = recBOX.width;

		// img_w,
		// img_h;

		//	waitKey(1);
	}
	else {

		itarget_x = 1920 / 2;//  中心
		//qDebug("Test:%d", recBOX.x);
		itarget_y = 1080 / 2;
		//qDebug("Test:%d", recBOX.y);
		qDebug("target_x:%d  target_y:%d", itarget_x, itarget_y);
	}

　　

2.2云台速度限制

void DJIonboardSDK::on_btr_camera_speed_clicked()
{
	ui->hs_camera_yaw->setMinimum(-1800);
	ui->hs_camera_yaw->setMaximum(1800);
	ui->hs_camera_roll->setMinimum(-1800);
	ui->hs_camera_roll->setMaximum(1800);
	ui->hs_camera_pitch->setMinimum(-1800);
	ui->hs_camera_pitch->setMaximum(1800);
	resetCameraAngle();
	ui->gb_cameraFlag->setEnabled(false);
}

　　

2.3速度云台控制   10个像素转化每秒一度

void DJIonboardSDK::on_btn_camera_send_clicked()
{
	GimbalSpeedData speedData;
	GimbalAngleData angleData;

	if (ui->btg_cameraMode->checkedButton()->text() == "Speed")
	{

		//int itarget_x, itarget_y, itarget_h, itarget_w;
		//int img_w, img_h;
		int dx = 0;
		int dy = 0;
		int yawRate = 0;
		int pitchRate = 0;

		//dx = (int)(itarget_x + itarget_w / 2 - img_w / 2);
		//dy = (int)(itarget_y + itarget_h / 2 - img_h / 2);

		dx = (int)(itarget_x - 1920 / 2);
		dy = (int)(itarget_y - 1080 / 2);

		yawRate = dx;
		pitchRate = -dy;

		if (itarget_w / 2 < 30)  { itarget_w = 60; };
		if (itarget_h / 2 < 30){ itarget_h = 60; };

		if (abs(dx) < itarget_w/2)
		{
			yawRate = 0;
		}
		if (abs(dy) < itarget_h/2)
		{
			pitchRate = 0;
		}

		speedData.yaw = yawRate;
		speedData.roll = 0;
		speedData.pitch = pitchRate;

		cam->setGimbalSpeed(&speedData);

			}
	else
	{
		angleData.yaw = ui->hs_camera_yaw->value();// 每次+5 --0.5
		angleData.roll = ui->hs_camera_roll->value();
		angleData.pitch = ui->hs_camera_pitch->value();
		angleData.mode = camFlag;
		angleData.duration = ui->lineEdit_cameraTime->text().toInt();
		cam->setGimbalAngle(&angleData);
	}

}

2.4 加入PID调参的速度控制策略

PID初始化

DJIonboardSDK::DJIonboardSDK(QWidget *parent) : QMainWindow(parent), ui(new Ui::DJIonboardSDK)
{
	ui->setupUi(this);

	ui->btn_flightCheckStatus->setVisible(false);
	ui->btn_waypointDownload->setVisible(false);
	ui->btn_waypoint_reset->setVisible(false);
	ui->tabWidget->removeTab(3);

	PID_X.reset();
	PID_Y.reset();
	PID_X.setParam(0.44, 0.06, 0.007, 0);
	PID_Y.setParam(0.6, 0.05, 0.02, 0);
	//不过冲，但速度慢
	//PID_X.setParam(0.4, 0.04, 0.03, 0);
	//PID_Y.setParam(0.5, 0.05, 0.02, 0);
	initSDK();
	initDisplay();
	initFlight();
	initCamera();
	initFollow();
	initWayPoint();
	initVirtualRC();
	initShareMemory();

　　取数据

connect(timerBroadcast, SIGNAL(timeout()), this, SLOT(on_tmr_Broadcast()));
	timerBroadcast->start(20);//  共享内存接收数据的频率  原有300  改为 50HZ

　　

　　

3 像素控制  像素差转化为角度增量

X3相机

视场角度    W 73.9度   H 53度

图像分辨率     1920 *   1080

• 问题 int/float 型 引发 0的问题

选择Increments 会在原来的基础之上angleData.yaw， angleData.roll，angleData.pitch 增加差量大小

• angleData.yaw = yawRate * 793 / 1920;//   横向 像素差---角度
  angleData.roll = 0;
  angleData.pitch = pitchRate * 500 / 1080 ; // 纵向 像素差---角度

选择Absolute 会绝对控制 直接将angleData.yaw， angleData.roll，angleData.pitch设置成差量大小

3.1 设置角度范围

void DJIonboardSDK::on_btr_camera_angle_clicked()
{
	ui->hs_camera_yaw->setMinimum(-3200);
	ui->hs_camera_yaw->setMaximum(3200);
	ui->hs_camera_roll->setMinimum(-350);
	ui->hs_camera_roll->setMaximum(350);
	ui->hs_camera_pitch->setMinimum(-900);
	ui->hs_camera_pitch->setMaximum(300);
	ui->gb_cameraFlag->setEnabled(true);
	resetCameraAngle();
}

　　

3.2 控制策略

void DJIonboardSDK::on_btn_camera_send_clicked()
{
	GimbalSpeedData speedData;
	GimbalAngleData angleData;

	if (ui->btg_cameraMode->checkedButton()->text() == "Speed")
	{

		//int itarget_x, itarget_y, itarget_h, itarget_w;
		//int img_w, img_h;

	}
	else
	{

		int yawRate = 0;
		int pitchRate = 0;
                
                // 坐标为左上角坐标
		//dx = (int)(itarget_x + itarget_w / 2 - img_w / 2);
		//dy = (int)(itarget_y + itarget_h / 2 - img_h / 2);
		//qDebug("x= %d ,y=%d", itarget_x, itarget_y);

                //坐标为中心坐标
		dx = (int)(itarget_x - 1920 / 2);
		dy = (int)(itarget_y - 1080 / 2);

		yawRate = dx;
		pitchRate = -dy;

		angleData.yaw = yawRate * 793 / 1920;//   横向 像素差---角度
		angleData.roll = 0;
		angleData.pitch = pitchRate * 500 / 1080 ; // 纵向 像素差---角度

		angleData.mode = camFlag;
		angleData.duration = ui->lineEdit_cameraTime->text().toInt();
		cam->setGimbalAngle(&angleData);
			}

}