Callbacks: Difference between revisions

From Distributed Autonomous and Networked Control Lab
Jump to navigation Jump to search
← Older edit
Jnoronha (talk | contribs)
203 edits
Added full callback example
Jnoronha (talk | contribs)
203 edits
m →‎Full Callback Example: added timestamp to callback example
 
(4 intermediate revisions by the same user not shown)
Line 1: Line 1:
A callback is a segment of code that will run only when a certain condition is satisfied. For the Swarm Platform, the callbacks we use are triggered by the VRPN communications protocol used by the Camera System. Whenever the Client receives a new packet of data from the Camera System the callback will be triggered.
A callback is a segment of code that will run only when a certain condition is satisfied. For the Swarm Platform, the callbacks we use are triggered by the VRPN communications protocol used by the Camera System. Whenever the Client receives a new packet of data from the Camera System the callback will be triggered.
See [[Camera System and VRPN|VRPN and the Camera System]] if you want more detail on those systems before you start.


==Registering a Callback==
==Registering a Callback==
Line 28: Line 32:
:#Link that constellation to a callback
:#Link that constellation to a callback


:*Flowchart of the Init process
[[Image:VRPN_INIT.png]]
:*Full code
<blockquote>
<blockquote>
<code>
<code>
Line 115: Line 124:
::'''Note:''' If we don't want to track a certain Crazyflie (i.e. we only want to fly 2 Crazyflies) we can just pass <code>NULL</code> into the init function for the callbacks we don't care about.
::'''Note:''' If we don't want to track a certain Crazyflie (i.e. we only want to fly 2 Crazyflies) we can just pass <code>NULL</code> into the init function for the callbacks we don't care about.


===Trigger a Callback===
===Calling a Callback===
:3. Start the mainloop: <code>vrpn_go()</code> (code in ''vrpn.cpp'') (function called in ''eris_vrpn.cpp'')
::*A flowchart of the vrpn_go() function:
[[Image:VRPNGO.png|500px]]
 
::*Full vrpn_go() code (code in ''vrpn.cpp''):
<blockquote>
<code>
<pre>


void vrpn_go() {
int i = 0;  //for ending the while loop


while(!i){ 
readKeyboard(); //Triggers actions based on key value read
nearGround();
#if USE_HAND
updateHand();  //Checks for Valid Hand Gestures
#endif
                //Calculates Total Loop Timing
loopTimeTotalStart = cflieCopter1->currentTime() - initTime1;
loopTimeTotalDelta = loopTimeTotalStart - loopTimeTotalPrev;
loopTimeTotalPrev = loopTimeTotalStart;


connection->mainloop();  //Pings the Server Connection to see if still alive
#if USE_HAND
trackerHand->mainloop(); //Checks if Hand constellation has new data (runs callback if true)
#endif
tracker->mainloop();    //IMPORTANT: Checks if Crazyflie1 constellation has new data (runs callback if true)
tracker2->mainloop();    //Same...
tracker3->mainloop();
tracker4->mainloop();
usleep(200);  //Was 200


nonblock(1);
usleep(1);
i = kbhit(); //keyboard hit (state change)


if (i!=0)
{
keystroke_now = fgetc(stdin); //Stores key value pressed
    if (keystroke_now == 'q')
i=1;


else{
i=0;
}


//fprintf(stderr,"%d ",i);
  }


}//END WHILE


    if(keystroke_now == 'q'){
cout << "Program Forced End" << endl;
    }
loopTimeTotalEnd = cflieCopter1->currentTime() - initTime1;
loopTimeTotal = loopTimeTotalEnd - loopTimeTotalStart;
} //END VRPN GO


</pre>
</code>
</blockquote>


::'''Note:''' The most important part of this is the <code>tracker->mainloop();</code> functions. These functions are what calls the Callback if there is new data waiting!!


::You can see an example of the code that is executed whenever the [[:Callbacks#Full Callback Example|callback is triggered]]


===Full Callback Example===
==Full Callback Example==
Below is a full Callback example for Crazyflie1:
Below is a full Callback example for Crazyflie1 (as of 7/27/16):


<blockquote>
<blockquote>

Latest revision as of 17:16, 28 July 2016

A callback is a segment of code that will run only when a certain condition is satisfied. For the Swarm Platform, the callbacks we use are triggered by the VRPN communications protocol used by the Camera System. Whenever the Client receives a new packet of data from the Camera System the callback will be triggered.


See VRPN and the Camera System if you want more detail on those systems before you start.


Registering a Callback

Before we can do anything we need to set up a callback that executes when new data is present.

1. Use the VRPN Library to create a Connection and Tracker_Remote object pointers (code in vrpn.cpp):


vrpn_Connection *connection;
vrpn_Tracker_Remote *trackerHand;
vrpn_Tracker_Remote *tracker;
vrpn_Tracker_Remote *tracker2;
vrpn_Tracker_Remote *tracker3;
vrpn_Tracker_Remote *tracker4;

Note: We need a Tracker_Remote object for each constellation we want to track (Hand and each Crazyflie in the Swarm)
2. Now we create a function to (code in vrpn.cpp):
  1. Register the connection to the Server computer
  2. Assign each Tracker_Remote object to a valid constellation name on the Server computer
  3. Link that constellation to a callback
  • Flowchart of the Init process

  • Full code


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

	connection = vrpn_get_connection_by_name(connectionName.c_str());
#if USE_HAND
	trackerHand = new vrpn_Tracker_Remote("hand", connection);
#endif

	tracker = new vrpn_Tracker_Remote("Crazyflie21", connection);
	
	tracker2 = new vrpn_Tracker_Remote("Crazyflie2", connection);

	tracker3 = new vrpn_Tracker_Remote("Crazyflie22", connection);

	tracker4 = new vrpn_Tracker_Remote("Crazyflie23", connection);
#if USE_HAND
	trackerHand->register_change_handler(0, callbackHand);
#endif
	tracker->register_change_handler(0, callback);
	tracker2->register_change_handler(0, callback2);
	tracker3->register_change_handler(0, callback3);
	tracker4->register_change_handler(0, callback4);

	usleep(2000);
}

Note 1: We pass in the following as inputs to the function:
  • string connectionName is the "IP address:Port" of the Computer running the Camera System
    • (You can see the VRPN Broadcast Port used in the on the middle-right pane called Streaming Properties)
  • A callback pointer for each constellation we want to track (These will be established in the Using a Callback section)
Note 2: Notice the names of each tracker match the names of the constellations in the Camera System Software (shown below):

Using a Callback in the Swarm Client

The Swarm Client needs to re-establish the connection to the VRPN server for each test.

Initializing the Callbacks

1. Create the Callback Objects (code in eris_vrpn.cpp):


void VRPN_CALLBACK handle_hand(void*, const vrpn_TRACKERCB t);
void VRPN_CALLBACK handle_pos(void*, const vrpn_TRACKERCB t);         //Old name scheme (represents Callback for Crazyflie1)
void VRPN_CALLBACK handle_Crazyflie2(void*, const vrpn_TRACKERCB t);
void VRPN_CALLBACK handle_Crazyflie3(void*, const vrpn_TRACKERCB t);
void VRPN_CALLBACK handle_Crazyflie4(void*, const vrpn_TRACKERCB t);

2. Register the connection and constellations to the Callbacks (code in eris_vrpn.cpp):


#if NUM_QUADS == 1
	vrpn_init("192.168.0.120:3883", handle_hand, handle_pos, NULL, NULL, NULL, NULL);

#elif NUM_QUADS == 2
	vrpn_init("192.168.0.120:3883", handle_hand, handle_pos, handle_Crazyflie2, NULL, NULL, NULL);

#elif NUM_QUADS == 3
	vrpn_init("192.168.0.120:3883", handle_hand, handle_pos, handle_Crazyflie2, handle_Crazyflie3, NULL, NULL);

#elif NUM_QUADS == 4
	vrpn_init("192.168.0.120:3883", handle_hand, handle_pos, handle_Crazyflie2, handle_Crazyflie3, handle_Crazyflie4, NULL);
#endif

Note: If we don't want to track a certain Crazyflie (i.e. we only want to fly 2 Crazyflies) we can just pass NULL into the init function for the callbacks we don't care about.

Calling a Callback

3. Start the mainloop: vrpn_go() (code in vrpn.cpp) (function called in eris_vrpn.cpp)
  • A flowchart of the vrpn_go() function:

  • Full vrpn_go() code (code in vrpn.cpp):


void vrpn_go() {
	int i = 0;  //for ending the while loop

	while(!i){  
		readKeyboard(); //Triggers actions based on key value read
		nearGround();	
#if USE_HAND
		updateHand();   //Checks for Valid Hand Gestures
#endif
                //Calculates Total Loop Timing
		loopTimeTotalStart = cflieCopter1->currentTime() - initTime1;
		loopTimeTotalDelta = loopTimeTotalStart - loopTimeTotalPrev;
		loopTimeTotalPrev = loopTimeTotalStart;

		connection->mainloop();  //Pings the Server Connection to see if still alive
#if USE_HAND
		trackerHand->mainloop(); //Checks if Hand constellation has new data (runs callback if true)
#endif
		tracker->mainloop();     //IMPORTANT: Checks if Crazyflie1 constellation has new data (runs callback if true)
		tracker2->mainloop();    //Same...
		tracker3->mainloop();
		tracker4->mainloop();
		usleep(200);  //Was 200

	nonblock(1);
		usleep(1);
		i = kbhit();		//keyboard hit (state change)

		if (i!=0)
		{
			keystroke_now = fgetc(stdin); //Stores key value pressed
		    if (keystroke_now == 'q')
			i=1;

		else{
			i=0;
		}

		//fprintf(stderr,"%d ",i);
	   }

	}//END WHILE

    if(keystroke_now == 'q'){
	cout << "Program Forced End" << endl;
    }
	loopTimeTotalEnd = cflieCopter1->currentTime() - initTime1;
	loopTimeTotal = loopTimeTotalEnd - loopTimeTotalStart;
}	//END VRPN GO

Note: The most important part of this is the tracker->mainloop(); functions. These functions are what calls the Callback if there is new data waiting!!
You can see an example of the code that is executed whenever the callback is triggered

Full Callback Example

Below is a full Callback example for Crazyflie1 (as of 7/27/16): 


#if NUM_QUADS >= 1
void VRPN_CALLBACK handle_pos(void*, const vrpn_TRACKERCB t) {
//cout << "First Callback" << endl;

if(vrpnPacketBackup1 < 2)	//If VRPN Buffer does NOT have old packets stored up, run main code
{
	loopTime1Start = cflieCopter1->currentTime() - initTime1;	
	loopTime1Delta = loopTime1Start - loopTime1Prev;	//Calculates time difference between each loop start
	loopTime1Prev = loopTime1Start;

	vrpnPacketTime1 = t.msg_time.tv_sec + (t.msg_time.tv_usec / 1000000.0);
	vrpnPacketDelta1 = vrpnPacketTime1 - vrpnPacketTime1Prev;			//Calculates time between VRPN Data Packets (almost always 0.01 sec)
	vrpnPacketTime1Prev = vrpnPacketTime1;

	vrpnPacketBackup1 = ceil(loopTime1Delta/vrpnPacketDelta1);		//Calculates estimated # of packets built up in VRPN buffer.

	controllerSetAllDt(&pidCtrl1, vrpnPacketDelta1);	//Variable dt for controller based on time between packets | vrpnPacketDelta1
	
#if NUM_QUADS == 2
	cflieCopter2->cheat_process_packets();
#elif NUM_QUADS == 3
	cflieCopter3->cheat_process_packets();
#elif NUM_QUADS == 4
   cflieCopter2->cheat_process_packets();
#endif

   crRadio1->clearPackets();      //Clears the USB Radio Buffer before switching Radio channels
   crRadio1->setChannel( cflieCopter1->radioChannel() );

	frameCount++;

	// Get the euler angles
	q_vec_type euler;
	q_to_euler(euler, t.quat);

	xPosition1 = t.pos[0];
	yPosition1 = t.pos[1];			//Stores X, Y, and Z Position for use in other Callbacks (external)
	zPosition1 = -t.pos[2] + 0.05;

#if USE_PRINTOUT
	cout << "PID Radio 1" << endl << endl;
#endif

#if 0	//Old way of storing Camera Data (most of it was unnecessary) UNUSED

	cfliePitch = cflieCopter1->pitch();
	cflieRoll = cflieCopter1->roll();
	cflieYaw = cflieCopter1->yaw();
	cflieThrust = cflieCopter1->thrust();

	/* Legend for Camera Data Packet
	 * 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];

	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		//END UNUSED

//==============YAW CORRECTION========================= (IMPORTANT!!!: YAW CONTROLLER INPUT IS INVERTED IN simple.cpp FILE***)
	camYawDeg1 = -(euler[0] * 57.2958); //Getting Camera Yaw (Converts From Radians TO Degrees)
	quadYaw1 = cflieCopter1->yaw();	    //Getting Crazyflie Yaw

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


		if(quadYaw1 < 0.0)
			quadYaw1Off = quadYaw1 + 360.0;
		else
			quadYaw1Off = quadYaw1;

		yawDifference1 = quadYaw1Off - camYawDeg1Off;
		cout << "Yaw1 Offset Taken: " << yawDifference1 << endl;
	}

	correctedYaw1 = camYawDeg1 + yawDifference1;

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

//===============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;

	//	cout << "Quad Yaw: " << cflieCopter1->yaw() << endl;
	//	cout << "Camera Yaw: " << camYawDeg1 << endl;

	//	cout << "Yaw Offset: " << yawDifference1 << endl;
	//	cout << "New Desired Yaw: " << yawDesired1 << endl;
	cout << "Frame Count: " << frameCount << endl;
#endif

#if USE_KEYBOARD
	if (cflieCopter1->m_enumFlightMode == LANDING_MODE) //Landing Mode
	{

	//CORRECTS PITCH AND ROLL PID ERRORS TO ACCOUNT FOR QUADS CURRENT YAW DIRECTION
//	xError1 = cos(correctedYaw1 / 57.2958)*(xPositionDesired1 - xPosition1) - sin(correctedYaw1 / 57.2958)*(yPositionDesired1 - yPosition1);	
//	yError1 = sin(correctedYaw1 / 57.2958)*(xPositionDesired1 - xPosition1) + cos(correctedYaw1 / 57.2958)*(yPositionDesired1 - yPosition1);

	xError1 = xPositionDesired1 - xPosition1;
	yError1 = yPositionDesired1 - yPosition1;
	controllerSetXYError(&pidCtrl1, xError1, yError1);

	controllerCorrectAttitudePID(&pidCtrl1, t.pos[0], t.pos[1], zPosition1, correctedYaw1,
		xPositionDesired1, yPositionDesired1, -0.4, yawDesired1,
			&rollControlOutput1, &pitchControlOutput1, &yawControlOutput1,
				&thrustControlOutput1);
		takeOff1 = 0;

	} else if (cflieCopter1->m_enumFlightMode == MIRROR_MODE) {	//Acts as Master
		xPositionDesired1 = -0.012;
		yPositionDesired1 = -0.200;
		zPositionDesired1 = 0.75;

		if( loopTime1Start - lastTime > 4.0 )  //Creates a 'Step' by changing setpoint every 4 seconds
		{
		   toggle = !toggle;
		   lastTime = loopTime1Start;
		}

if(toggle)  
{
xStepPositionError1 = xPositionDesired1 - xPosition1;
yStepPositionError1 = yPositionDesired1 - yPosition1;
controllerSetXYError(&pidCtrl1, xStepPositionError1, yStepPositionError1);

		controllerCorrectAttitudePID(&pidCtrl1, t.pos[0], t.pos[1], zPosition1, correctedYaw1,
				xPositionDesired1, yPositionDesired1, zPositionDesired1,
				yawDesired1, &rollControlOutput1, &pitchControlOutput1,
				&yawControlOutput1, &thrustControlOutput1);
}
else  //Shifts current x and y setpoint 0.65 meters
{
xStepPositionError1 = (xPositionDesired1 + 0.65) - xPosition1;
yStepPositionError1 = (yPositionDesired1 + 0.65) - yPosition1;
controllerSetXYError(&pidCtrl1, xStepPositionError1, yStepPositionError1);

		controllerCorrectAttitudePID(&pidCtrl1, t.pos[0], t.pos[1], zPosition1, correctedYaw1,
				xPositionDesired1 + 0.65, yPositionDesired1 + 0.65, zPositionDesired1,
				yawDesired1, &rollControlOutput1, &pitchControlOutput1,
				&yawControlOutput1, &thrustControlOutput1);

//cout << "Toggled!" << endl;
}

	} else if (cflieCopter1->m_enumFlightMode == TAKEOFF_MODE) {		//correctedYaw1

	//CORRECTS PITCH AND ROLL PID ERRORS TO ACCOUNT FOR QUADS CURRENT YAW DIRECTION
//	xError1 = cos(correctedYaw1 / 57.2958)*(xPositionDesired1 - xPosition1) - sin(correctedYaw1 / 57.2958)*(yPositionDesired1 - yPosition1);	
//	yError1 = sin(correctedYaw1 / 57.2958)*(xPositionDesired1 - xPosition1) + cos(correctedYaw1 / 57.2958)*(yPositionDesired1 - yPosition1);

	xError1 = xPositionDesired1 - xPosition1;
	yError1 = yPositionDesired1 - yPosition1;
	controllerSetXYError(&pidCtrl1, xError1, yError1);

			controllerCorrectAttitudePID(&pidCtrl1, t.pos[0], t.pos[1], zPosition1, correctedYaw1,
					xPositionDesired1, yPositionDesired1, zPositionDesired1,
					yawDesired1, &rollControlOutput1, &pitchControlOutput1,
					&yawControlOutput1, &thrustControlOutput1);

	}
	else if (cflieCopter1->m_enumFlightMode == HAND_MODE) {	

	//CORRECTS PITCH AND ROLL PID ERRORS TO ACCOUNT FOR QUADS CURRENT YAW DIRECTION
//	xError1 = cos(correctedYaw1 / 57.2958)*(xPositionDesired1 - xPosition1) - sin(correctedYaw1 / 57.2958)*(yPositionDesired1 - yPosition1);	
//	yError1 = sin(correctedYaw1 / 57.2958)*(xPositionDesired1 - xPosition1) + cos(correctedYaw1 / 57.2958)*(yPositionDesired1 - yPosition1);

	xError1 = (xPositionHand - 0.5) - xPosition1;
	yError1 = (yPositionHand + 0.5) - yPosition1;
	controllerSetXYError(&pidCtrl1, xError1, yError1);

	controllerCorrectAttitudePID(&pidCtrl1, t.pos[0], t.pos[1], zPosition1, correctedYaw1,
			xPositionDesired1, yPositionDesired1, zPositionDesired1,
			yawDesired1, &rollControlOutput1, &pitchControlOutput1,
			&yawControlOutput1, &thrustControlOutput1);

	}
		else if(flap){                   //Flap Mode (OBSOLETE)
//		cflieCopter1->setThrust(20000);
//		cflieCopter2->setThrust(20000);  //45000 Thrust  of Crazyflie2.0 to match Crazyflie1.0
		
//		cflieCopter2->setYaw(90);


//		lastTime = cflieCopter2->currentTime();
		curTime = cflieCopter2->currentTime();
		
		if( curTime - lastTime > 1.0 )
		{
		   toggle = !toggle;
		   lastTime = curTime;
		}
		
		if( toggle ){
			cflieCopter1->setThrust( 20000 );
			
			printf("Low  %.3lf ", curTime);
		}
		else
		{
			cflieCopter1->setThrust( 40000 );

			printf("High %.3lf ", curTime);
		}

	}

	else if(cflieCopter1->m_enumFlightMode == GROUNDED_MODE){
		controllerResetAllPID(&pidCtrl1);
		rollControlOutput1 = 0;
		pitchControlOutput1 = 0;
		yawControlOutput1 = 0;
		thrustControlOutput1 = 0;
	}

	else{		//Emergency Case (Should never happen)
		controllerResetAllPID(&pidCtrl1);
		rollControlOutput1 = 0;
		pitchControlOutput1 = 0;
		yawControlOutput1 = 0;
		thrustControlOutput1 = 0;
	}

#else  //If keyboard input NOT enabled then just run default Controller
	controllerCorrectAttitudePID(&pidCtrl1, t.pos[0], t.pos[1], -t.pos[2], correctedYaw1,
					xPositionDesired1, yPositionDesired1, zPositionDesired1,
								yawDesired1, &rollControlOutput1, &pitchControlOutput1,
													&yawControlOutput1, &thrustControlOutput1);
#endif
	RunCrazyflie(crRadio1, cflieCopter1, rollControlOutput1,
			pitchControlOutput1, yawControlOutput1, thrustControlOutput1);

#if USE_LOGGING		
#if USE_BASIC_LOGGING
	fprintf(out1, "%.6f\t\t", cflieCopter1->currentTime() - initTime1 );
	fprintf(out1, "%.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%.6f\t\t%d\t\t%d",
				cflieCopter1->roll(), 
				cflieCopter1->pitch(), 
				quadYaw1, 
				t.pos[0], 
				t.pos[1], 
				-t.pos[2],
				camYawDeg1,
				correctedYaw1,
				loopTimeTotal,
				loopTimeTotalDelta,
				loopTime1,
				loopTime1Delta,
				vrpnPacketDelta1,
				vrpnPacketTime1,
				vrpnPacketBackup1,
				cflieCopter1->radioRSSI());	//
	fprintf(out1, "\n");

#else	 

//FULL LOGGING ROUTINE (***CAUSES MAJOR BOTTLENECK IN CODE TIMING THE MORE VARIABLES YOU LOG***)

	fprintf(out1, "%.6f\t\t", cflieCopter1->currentTime() - initTime1 );
	fprintf(out1, "%.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%.6f\t\t%d\t\t%.3f\t\t%.3f\t\t%.3f\t\t%.3f\t\t%.3f\t\t%.3f\t\t%.3f\t\t%.3f\t\t%d\t\t%.3f\t\t%.3f\t\t%.3f\t\t%lu\t\t%lu\t\t%lu\t\t%lu\t\t%.3f\t\t%.3f\t\t%.3f",
				cflieCopter1->roll(), 
				cflieCopter1->pitch(), 
				quadYaw1, 
				t.pos[0], 
				t.pos[1], 
				-t.pos[2],
				camYawDeg1,
				correctedYaw1,
				loopTimeTotal,
				loopTimeTotalDelta,
				loopTime1,
				loopTime1Delta,
				vrpnPacketDelta1,
				vrpnPacketTime1,
				vrpnPacketBackup1,
				xPositionDesired1,
				yPositionDesired1,
				zPositionDesired1,
				yawDesired1,
				pitchControlOutput1,
				rollControlOutput1,
				yawControlOutput1,
				thrustControlOutput1,
				cflieCopter1->radioRSSI(),
				euler[0],
				euler[1],
				euler[2],
				cflieCopter1->motor1(),
				cflieCopter1->motor2(),
				cflieCopter1->motor3(),
				cflieCopter1->motor4(),
				cflieCopter1->gyroX(),
				cflieCopter1->gyroY(),
				cflieCopter1->gyroZ());	//
	fprintf(out1, "\n");
#endif
#endif

#if 0  //(UNUSED)
	if (tcp_client_ON) {
		readMulticast(); //(argc < 1 = Listen Mode)
//		runTCPClient();
	}
#endif

	//cout << "PID Radio 2" << endl << endl;

	loopTime1End = cflieCopter1->currentTime() - initTime1;
	loopTime1 = loopTime1End - loopTime1Start;

}	
else if(vrpnPacketBackup1 < 0){	//Failsafe so can't go below 0 (should never happen)	| || (vrpnPacketBackup1 > 20)?
	vrpnPacketBackup1 = 0;
}
else if(vrpnPacketBackup1 >= 2){
	vrpnPacketBackup1 --;

//	cout << "VRPN Packet Backup 1 Detected: " << vrpnPacketBackup1 << endl;
}//End Packet Backup If check

}	//End Callback 1


Main Directory
Crazyflie Swarm Home | PC Client Software | USB Radio | Firmware | FAQ
Modifications Directory
Controller | Logging | Keyboard Commands | Changing Radio Channel | Flight Modes | Callbacks | Adding a Crazyflie | Firmware
Retrieved from "https://wikis.ece.iastate.edu/distributed-autonomous-and-networked-control-lab/index.php?title=Callbacks&oldid=153"