Callbacks

From Distributed Autonomous and Networked Control Lab
Revision as of 19:06, 27 July 2016 by Jnoronha (talk | contribs) (→‎Using a Callback in the Swarm Client: changed the full callback example to a lvl 2 heading)
Jump to navigation Jump to search

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.

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


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.

Trigger a Callback

3. Start the mainloop: vrpn_go() (code in vrpn.cpp):


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

	while(!i){  //1 replace 'i<20000' when done testing ******
		readKeyboard();
		nearGround();	
#if USE_HAND
		updateHand();
#endif

		loopTimeTotalStart = cflieCopter1->currentTime() - initTime1;
		loopTimeTotalDelta = loopTimeTotalStart - loopTimeTotalPrev;
		loopTimeTotalPrev = loopTimeTotalStart;

		connection->mainloop();
#if USE_HAND
		trackerHand->mainloop();
#endif
		tracker->mainloop();
		tracker2->mainloop();
		tracker3->mainloop();
		tracker4->mainloop();
		usleep(200);  //Was 200

		//Testing Routines

		//i++;

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

		if (i!=0)
		{
			keystroke_now = fgetc(stdin);
		    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






Full Callback Example

Below is a full Callback example for Crazyflie1: 


#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=149"