vrpn.cpp

	vrpn_Tracker_Remote *tracker#;

	void vrpn_init(std::string connectionName, void (*callback)(void*, const vrpn_TRACKERCB), void (*callback2)(void*, const vrpn_TRACKERCB), void (*callback#)(void*, const vrpn_TRACKERCB), char *key) {

	tracker# = new vrpn_Tracker_Remote("Crazyflie2#", connection);

	tracker#->register_change_handler(0, callback#);

	tracker#->mainloop();


vrpn.h

	void vrpn_init(std::string connectionName, void (*callback)(void*, const vrpn_TRACKERCB), void (*callback2)(void*, const vrpn_TRACKERCB), void (*callback#)(void*, const vrpn_TRACKERCB), char *key);



eris_vrpn.cpp

	void VRPN_CALLBACK handle_Crazyflie#(void*, const vrpn_TRACKERCB t);
	
	CCrazyRadio *crRadio# = 0; //***NOT NECESSARY IF LESS THAN 3 QUADS PER RADIO***

	CCrazyflie *cflieCopter# = 0;

	ControllerObject pidCtrl# = { 0 }; // init all fields to zero

	float rollControlOutput#;
	float pitchControlOutput#;
	float yawControlOutput#;
	float thrustControlOutput#;

	double initTime# = 0;

	int vrpnPacketBackup# = 0;
	double vrpnBackupTime# = 0;
	double vrpnBackupTime#Prev = 0;
	double vrpnBackupTime#Delta = 0;

	char takeOff# = 0;

	float xPositionDesired# = 0.681;
	float yPositionDesired# = 0.129;
	float zPositionDesired# = 0.6;
	float yawDesired# = -90.0;

	float yawDifference# = 0;
	float quadYaw# = 0;
	float prevQuadYaw# = 0;
	float camYawDeg# = 0;
	float camYawDeg#Off = 0;
	float quadYaw#Off = 0;
	float correctedYaw# = 0;

	double loopTime#Start = 0;
	double loopTime#End = 0;
	double loopTime# = 0;
	double loopTime#Prev = 0;
	double loopTime#Delta = 0;

	double vrpnPacketTime# = 0;
	double vrpnPacketTime#Prev = 0;
	double vrpnPacketDelta# = 0;

	FILE * out# = NULL;

	controllerResetAllPID( &pidCtrl# );

	controllerInit( &pidCtrl# );

	vrpn_init("192.168.0.120:3883", handle_pos, handle_Crazyflie2, handle_Crazyflie#, handle_Crazyflie(#+1), ..., &keystroke_now);

	cflieCopter# = new CCrazyflie(crRadio(radio dongle number), (Radio Channel));

	const char * logHeader# = "#Crazyflie# Log File\n\
%Time\t\tPitch\t\tRoll\t\tYaw\t\tX\t\tY\t\tZ\t\tCamYaw\t\tCorrectedYaw\t\tLoopTimeTotal\t\tLoopTime\t\tLoopTimeDelta\t\tvrpnPacketDelta\t\tvrpnPacketTime\t\tvrpnPacketBackup\n\
&sec\t\tdegrees\t\tdegrees\t\tdegrees\t\tmeters\t\tmeters\t\tmeters\t\tdegrees\t\tdegrees\t\tseconds\t\tseconds\t\tseconds\t\tseconds\t\tseconds\t\tpackets\n";

	if(cflieCopter#)
	{
		out# = fopen("cflie#.txt", "w");
		
		if(out# == NULL)
		{
			printf("Could not open cflie#.log:errno %d\n", errno);
			exit(-1);
		}
		
		fprintf(out#, "%s\n", logHeader#);
	}

	initTime# = cflieCopter#->currentTime();

	cflieCopter#->setSendSetpoints( true );

	fclose(out#);

	delete cflieCopter#;

//===========MAKE NEW CALLBACK===================

void VRPN_CALLBACK handle_Crazyflie#(void*, const vrpn_TRACKERCB t) {

if(vrpnPacketBackup# < 2)	//If VRPN Buffer does NOT have old packets stored up, run main code
{
	loopTime#Start = cflieCopter#->currentTime() - initTime#;
	loopTime#Delta = loopTime#Start - loopTime#Prev;		//Calculates time difference between each loop start
	loopTime#Prev = loopTime#Start;

	vrpnPacketTime# = t.msg_time.tv_sec + (t.msg_time.tv_usec / 1000000.0);
	vrpnPacketDelta# = vrpnPacketTime# - vrpnPacketTime#Prev;
	vrpnPacketTime#Prev = vrpnPacketTime#;

	vrpnPacketBackup# = ceil(loopTime#Delta/vrpnPacketDelta#);		//Calculates estimated # of packets built up in VRPN buffer.

	controllerSetAllDt(&pidCtrl#, vrpnPacketDelta#);	//Variable dt for controller based on time between packets | vrpnPacketDelta2

   cflieCopter1->cheat_process_packets();		//Flush out other Copter Packets
   crRadio(#RADIO NUM)->clearPackets();
	crRadio(#RADIO NUM)->setChannel( cflieCopter#->radioChannel() );

	// Get the euler angles
	q_vec_type euler;
	q_to_euler(euler, t.quat);
#if USE_PRINTOUT
	cout << "PID Radio #" << endl << endl;
#endif
#if 0
	//cout << "\033[2J\033[1;1H";  //Clears Terminal Window to display clean output

	/*
	 * Position
	 * x      t.pos[0]
	 * y      t.pos[1]
	 * z      t.pos[2]
	 *
	 * Orientation
	 * yaw    euler[0]  Rotation
	 * pitch  euler[1]  East-West movement
	 * roll   euler[2]  North-South movement
	 * Euler's range goes from -pi : +pi
	 */
	QUAD quad;
	quad.vrpnNow = 0;
	vrpn_data_t *vrpnData;

	vrpnData = (vrpn_data_t*) malloc(sizeof(vrpn_data_t));
	vrpnData->usec = t.msg_time.tv_sec + (t.msg_time.tv_usec / 1000000.0);
	vrpnData->x = t.pos[0];
	vrpnData->y = t.pos[1];
	vrpnData->z = -t.pos[2] + 0.05; //Higher Altitude = Negative z  //Added Offset for floor below camera system origin
	vrpnData->yaw = euler[0];
	vrpnData->pitch = euler[1];
	vrpnData->roll = euler[2];

	xPosition# = t.pos[0];
	yPosition# = t.pos[1];
	zPosition# = -t.pos[2] + 0.05;

	cout << "xPosition of Crazyflie#: " << xPosition# << endl;

	quad.vrpnPrev = quad.vrpnNow;
	if (quad.vrpnNow == 0) {
		quad.vrpnPrev = 0;
		quad.vrpnTime0 = vrpnData->usec;
		quad.vrpnNow = vrpnData->usec;
	} else {
		quad.vrpnNow = vrpnData->usec - quad.vrpnTime0;
	}
#endif
//==============YAW CORRECTION========================= (IMPORTANT!!!: YAW CONTROLLER INPUT IS INVERTED IN simple.cpp FILE***)
	camYawDeg# = -(euler[0] * 57.2958); //Converts From Radians TO Degrees
	quadYaw# = cflieCopter#->yaw();

//	if((prevQuadYaw# - quadYaw# < -50) || (prevQuadYaw#-quadYaw# > 50))		//TODO HACK TO ELIMINATE PACKET CROSSING****
//	{
		//cout << "Yaw# Prev: " << prevQuadYaw# << endl;
		//cout << "Yaw# Current: " << quadYaw# << endl;
//      printf( "Yaw# prev %8.2f  curr %8.2f\n", prevQuadYaw#, quadYaw# );
//		quadYaw# = prevQuadYaw#;
//	}

	if (frameCount == 50) {
		if (camYawDeg# < 0.0)
			camYawDeg#Off = camYawDeg# + 360.0;					//Converts from +-180 to full 360 degree form
		else
			camYawDeg#Off = camYawDeg#;

		

		if(quadYaw# < 0.0)
			quadYaw#Off = quadYaw# + 360.0;
		else
			quadYaw#Off = quadYaw#;

		yawDifference# = quadYaw#Off - camYawDeg#Off;
		cout << "Yaw# Offset Taken: " << yawDifference# << endl;
	}

	correctedYaw# = camYawDeg# + yawDifference#;

	if(correctedYaw# > 180.0)
		correctedYaw# -= 360.0;
	else if(correctedYaw# < -180.0)
		correctedYaw# += 360.0;

//	yawDesired# = yawDesired# + yawDifference#; //Fixes difference in camera and quad yaw measurements

//	if (yawDesired# > 180.0)
//		yawDesired# -= 360.0;
//	else if (yawDesired# < -180.0)
//		yawDesired# += 360.0;

//	prevQuadYaw# = quadYaw#;				//TODO HACK TO ELIMINATE PACKET CROSSING****

//===============END YAW CORRECTION=========================

#if USE_PRINTOUT
		cout << "X: " << t.pos[0] << endl;
		cout << "Y: " << t.pos[1] << endl;
		cout << "Z: " << -t.pos[2] + 0.05 << endl;
#endif

//	free(vrpnData);

	//****INSERT RUNTIME CODE HERE****

	//========Reads Keystrokes from Keyboard==========
	//	nonblock(1);
#if USE_KEYBOARD
	/*
	if (keystroke_now != keystroke_prev) {

		//keystroke = fgetc( stdin );
		if (keystroke_now == 'w') {
			yPositionDesired2 -= 0.2;
			cout << "New Y Setpoint: " << yPositionDesired2 << endl;
		} else if (keystroke_now == 'd') {
			yPositionDesired2 += 0.2;
			cout << "New Y Setpoint: " << yPositionDesired2 << endl;
		} else if (keystroke_now == 'a') {
			xPositionDesired2 -= 0.2;
			cout << "New X Setpoint: " << xPositionDesired2 << endl;
		} else if (keystroke_now == 'd') {
			xPositionDesired2 += 0.2;
			cout << "New X Setpoint: " << xPositionDesired2 << endl;
		} else if (keystroke_now == 'r') {
			zPositionDesired2 += 0.2;
			cout << "New Z Setpoint: " << zPositionDesired2 << endl;
		} else if (keystroke_now == 'f') {
			zPositionDesired2 -= 0.2;
			cout << "New Z Setpoint: " << zPositionDesired2 << endl;
		} else if (keystroke_now == 'e') {
			landingMode ^= 1;
			cout << "Landing Mode Enabled" << endl;
		} else if (keystroke_now == 'm') {
			mirrorMode ^= 1;
			cout << "Mirror Mode Enabled" << endl;
		} else if (keystroke_now == 't') {
			takeOff = 1;
			takeOffTime = vrpnData->usec;
		} else if (keystroke_now == '`') {
			tcp_server_ON ^= 1;
		} else if (keystroke_now == '1') {
			tcp_client_ON ^= 1;
		}

		keystroke_prev = keystroke_now;

	} */
	//nonblock(0);
#endif

#if USE_KEYBOARD
	if (landingMode) //Landing Mode (TODO Change to be a keystroke when implemented)
	{
		controllerCorrectAttitudePID(&pidCtrl#, t.pos[0], t.pos[1], -t.pos[2], correctedYaw#,
				xPositionDesired#, yPositionDesired#, -1.0, yawDesired#,
				&rollControlOutput#, &pitchControlOutput#, &yawControlOutput#,
				&thrustControlOutput#);
		takeOff# = 0;
	} 
//	else if (mirrorMode) {									//***REMOVED (NEED SEPARATE ENABLE VARIABLE)****
//		xPositionDesired# = (xPosition2 + 1.0);
//		yPositionDesired# = (yPosition2);
//		zPositionDesired# = (zPosition2);

//		cout << "New xPositionDesired: " << xPositionDesired# << endl;

//		controllerCorrectAttitudePID(&pidCtrl#, t.pos[0], t.pos[1], -t.pos[2], correctedYaw#,
//				xPositionDesired#, yPositionDesired#, zPositionDesired#,
//				yawDesired#, &rollControlOutput#, &pitchControlOutput#,
//				&yawControlOutput#, &thrustControlOutput#);

//	} 
	else if (takeOff#) {			//correctedYaw#
//		zPositionDesired# = 1.0;

		controllerCorrectAttitudePID(&pidCtrl#, t.pos[0], t.pos[1], -t.pos[2], correctedYaw#,
				xPositionDesired#, yPositionDesired#, zPositionDesired#,
				yawDesired#, &rollControlOutput#, &pitchControlOutput#,
				&yawControlOutput#, &thrustControlOutput#);

//		if ((vrpnData->usec - takeOffTime) > 500000) //To reduce Ground Effect - Boost Thrust for first 0.5 seconds of takeoff
//				{
//			zPositionDesired# = 0.4;
//		}
	}

	else {
		rollControlOutput# = 0;
		pitchControlOutput# = 0;
		yawControlOutput# = 0;
		thrustControlOutput# = 0;
	}
#endif

#if !USE_KEYBOARD
	controllerCorrectAttitudePID(&pidCtrl#, t.pos[0], t.pos[1], -t.pos[2], correctedYaw#,
											xPositionDesired#, yPositionDesired#, zPositionDesired#, yawDesired#,
												&rollControlOutput#, &pitchControlOutput#, &yawControlOutput#,
													&thrustControlOutput#);
#endif
	RunCrazyflie(crRadio(#RADIO NUM), cflieCopter#,
							rollControlOutput#, pitchControlOutput#, yawControlOutput#, thrustControlOutput#);

#if USE_LOGGING							
	fprintf(out#, "%.6f\t\t", cflieCopter#->currentTime() - initTime# );
	fprintf(out#, "%.3f\t\t%.3f\t\t%.3f\t\t%.3f\t\t%.3f\t\t%.3f\t\t%.3f\t\t%.3f\t\t%.6f\t\t%.6f\t\t%.6f\t\t%.6f\t\t%.6f\t\t%d",
				cflieCopter#->pitch(), 
				cflieCopter#->roll(), 
				quadYaw#, 
				t.pos[0], 
				t.pos[1], 
				-t.pos[2],
				camYawDeg#,
				correctedYaw#,
				loopTimeTotal,
				loopTime#,
				loopTime#Delta,
				vrpnPacketDelta#,
				vrpnPacketTime#,
				vrpnPacketBackup#);
	fprintf(out#, "\n");
#endif

	loopTime#End = cflieCopter#->currentTime() - initTime#;
	loopTime# = loopTime#End - loopTime#Start;
//	loopTimeTotal = loopTime1Start - loopTime#End;	

}	//End Packet Backup If check	
else if(vrpnPacketBackup# < 0){	//Failsafe so can't go below 0 (should never happen) |   || (vrpnPacketBackup# > 20)?
	vrpnPacketBackup# = 0;
//	RunCrazyflie(crRadio(#RADIO NUM), cflieCopter#,
//							rollControlOutput#, pitchControlOutput#, yawControlOutput#, thrustControlOutput#);
}
else if(vrpnPacketBackup# >= 2){
//	cout << "VRPN Packet Backup # Detected: " << vrpnPacketBackup# << endl;
	vrpnPacketBackup# --;
//	RunCrazyflie(crRadio(#RADIO NUM), cflieCopter#,
//							rollControlOutput#, pitchControlOutput#, yawControlOutput#, thrustControlOutput#);
}//End Packet Backup Purge

}	//END CALLBACK #

==================END MAKE NEW CALLBACK=================

======In CTRL+C Handler=======

	cflieCopter#->setThrust(10000);

	cout << "Battery#: " << cflieCopter#->batteryLevel() << endl;

	cflieCopter#->cycle();

=====End CTRL+C Handler=======