C++ ARDrone::setFlatTrim方法代码示例

// --------------------------------------------------------------------------
// main(Number of arguments, Argument values)
// Description  : This is the entry point of the program.
// Return value : SUCCESS:0  ERROR:-1
// --------------------------------------------------------------------------
int main(int argc, char* argv[])
{
	const std::string Training = "quad2.png";
	quadrotor_matcher::image_template_gray = cv::imread(Training, 0);
	if( !quadrotor_matcher::image_template_gray.data )
	{
		std::cout << "...Couldn't find template image." << std::endl;
		return -1;
	}

	Draw draw;

	quadrotor_affine::initialize_st();

	quadrotor_job::ctrl_gain = 0.0025;
	quadrotor_job::ctrl_sweetspot = 60000.0;
	quadrotor_job::ctrl_gain2 = 1.55;//1.15;
	
	pipe* stages[] = {new quadrotor_image, new quadrotor_sift, new quadrotor_matcher, new quadrotor_affine};

	parallel_pipeline::pipeline quadrotor_pipeline;
	quadrotor_pipeline.addStages(4, stages);
	//quadrotor_pipeline.startDebugging();

	//Send the template into the pipe
	quadrotor_job* newjob = new quadrotor_job;
	newjob->image_gray = quadrotor_matcher::image_template_gray;
	quadrotor_pipeline.pushJob("Sift",newjob);

	// AR.Drone class
	ARDrone ardrone;

	// Initialize
	if (!ardrone.open(/*"192.168.1.3"*/)) {
		printf("Failed to initialize.\n");
		return -1;
	}

	ardrone.setFlatTrim();
	
	// Instructions
	printf("***************************************\n");
	printf("*      CV Drone Tracker Program       *\n");
	printf("***************************************\n");
	printf("*                                     *\n");
	printf("* - Controls -                        *\n");
	printf("*    'Space' -- Takeoff/Landing       *\n");
	printf("*                                     *\n");
	printf("* - Others -                          *\n");
	printf("*    'C'     -- Change camera         *\n");
	printf("*    'Esc'   -- Exit                  *\n");
	printf("*                                     *\n");
	printf("***************************************\n\n");

	{
		cv::Mat image = ardrone.getImage();
		quadrotor_affine::image_center_query.at<double>(0) = (double)(image.cols)/2.0;
		quadrotor_affine::image_center_query.at<double>(1) = (double)(image.rows)/2.0;
		quadrotor_affine::image_center_query.at<double>(2) = 1.0;
	}

	{		
		quadrotor_affine::image_center_training.at<double>(0) = (double)(quadrotor_matcher::image_template_gray.cols)/2.0;
		quadrotor_affine::image_center_training.at<double>(1) = (double)(quadrotor_matcher::image_template_gray.rows)/2.0;
		quadrotor_affine::image_center_training.at<double>(2) = 1.0;
	}

	unsigned long curTime = timeGetTime();
	unsigned long lastTime = curTime;
	unsigned long lastBatteryTime = curTime;
	unsigned long lastControl = curTime;
	int cameraMode = 0;

	cv::Mat savedControl;
	while (1)
	{
		curTime = timeGetTime();

		// Battery
		if(lastBatteryTime < curTime)
		{
			lastBatteryTime = curTime + 10000;
			printf("Battery = %d%%\n", ardrone.getBatteryPercentage());
		}

		// Sleep and get key
		int key = cv::waitKey(30);
		switch(key)
		{
		case 0x1b:
			quadrotor_pipeline.stopDebugging();
			goto quit;
		case ' ':
			if (ardrone.onGround()) 
				ardrone.takeoff();
//.........这里部分代码省略.........

//.........这里部分代码省略.........
        if (key == 'q')      vz =  1.0;
        if (key == 'a')      vz = -1.0;

        if (key == 'r')
		{
			//Reset
			//ardrone.emergency();
			if(mNonDronDebug == true)
			{
			}else
			{   
				//2014.03.09 add
				vx = 0.0;
				vy = 0.0;
				vz = 0.0;
				vr = 0.0;
				//ardrone.close();
				if ( initdrone(&ardrone) == -1) {
					printf("Failed to initialize.\n");
					return -1;
				}
			}
		}
		
		// 2014.03.02 add
        if((key == 'f')||(key == 'F')){
			mFaceDetectMode = !mFaceDetectMode;
			printf("Face Mode:%02X  Battery = %d%%\n",mFaceDetectMode, ardrone.getBatteryPercentage());
		}
		
		if((key == 'l')||(key == 'L')){
			pLeapData.mLeapMode = !pLeapData.mLeapMode;
			printf("Leap Mode:%02X  Battery = %d%%\n",pLeapData.mLeapMode, ardrone.getBatteryPercentage());
		}

		if((key == 'v')||(key == 'V')){
			printf("Btry:%d%% mSendCommandflag:%02d\n", ardrone.getBatteryPercentage(), mSendCommandflag);
		}

		if((key == '0')||(key == '0')){
			printf("Btry:%d%% reset setFlatTrim():%02d\n", ardrone.getBatteryPercentage(), key);
			ardrone.setFlatTrim();
			msleep(500);
		}


		if(mNonDronDebug == false)
		{
			gotoPlaySound(vx, vy, vr, mSoundCommandcounter, mSoundCommandOKcounter);
			if((!mNonDronRDebug)&&(!ardrone.onGround()))
			{
				if((mArDroneCommandFlag == false)&&(MouseARMode == false))
				{
					ardrone.move3D(vx, vy, vz, vr);
					//ardrone.move3D(vx, vy, vz, vr);
					//ardrone.move3D(vx,  vr, 0.0, vy);
					msleep(150);
					time_t now = time(NULL);
					struct tm *pnow = localtime(&now);
					printf("KLT:%02d:%02d:%02d vx:%02.1f vy:%02.1f vz:%02.1f vr:%02.1f \n",pnow->tm_hour,pnow->tm_min,pnow->tm_sec, vx, vy, vz, vr);
				}
			}
			//ardrone.move3D(0, 0, 0, 0);
		}

        // Change camera
		if(mNonDronDebug == false)
		{
	        static int mode = 0;
		    if((key == 'c')||(key == 'C')) 
				ardrone.setCamera(++mode%2);
				//ardrone.setCamera(++mode%4);

	        // Display the image
			//cvCircle (image,pt1,30,rcolor,2);
			IplImage wGrayImage = grayscaleFrame;
			cvShowImage ("FaceDetectW", &wGrayImage);             
		    cvShowImage("camera", image);

			cvMoveWindow( "FaceDetectW", 600, 0 );
			cvMoveWindow( "camera", 50, 0 );
			//WindowFromPoint(point(100,200));
		
			if ((key == 's') && (ardrone.getCameraMode() == 1))
			{
				imgSave("..\\..\\SaveFileName.jpg", &wGrayImage);
			}
		}
    }//while loop

    // See you
	if(mNonDronDebug == false)
	{
	    ardrone.close();
		cvDestroyWindow ("camera");
		cvDestroyWindow ("FaceDetectW");
	}

    return 0;
}