Adding a Crazyflie

Adding a Crazyflie to the Swarm is simple, but tedious. It's basically just replicating a copy of the other Crazyflies code, but there are multiple files you will need to make changes in. The following list should help you locate all the code you need to replicate:

Changes in vrpn.cpp

vrpn_Tracker_Remote *tracker#;

extern float xPositionDesired#;
extern float yPositionDesired#;
extern float zPositionDesired#;
extern float yawDesired#;

extern float xPosition#;
extern float yPosition#;
extern float zPosition#;
	
extern CCrazyflie *cflieCopter#;
	
extern double initTime#;
	
extern char takeOff#;
	
extern double takeOffTime#;

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();

Changes in nearGround()

void nearGround()			//During Landing Mode checks how close to ground quad is for shutdown
{
...
...
...
	if(cflieCopter#){
		if((cflieCopter#->m_enumFlightMode == LANDING_MODE)  && (zPosition# < 0.05))
		{
			cflieCopter#->m_enumFlightMode = GROUNDED_MODE;
		}
        }
}

Changes in updateHand()

void updateHand()
{
	if(handMode == 1)
	{
	//	cout << "Hand Roll: " << rollHand << endl;
		if(killCount > 5){
		...
		...
			cflieCopter#->m_enumFlightMode = GROUNDED_MODE;
		}
		else if(trackCount > 5){
		...
		...
			cflieCopter#->m_enumFlightMode = HAND_MODE;

		}
		else{
		...
		...
			if(zPositionHand > 1.6)
			{
				...
				...
				cflieCopter#->m_enumFlightMode = TAKEOFF_MODE;


				cout << "Hand Mode: Launching Quads" << endl;
			}
			else if(zPositionHand < 0.4)
			{
				...
				...
				cflieCopter#->m_enumFlightMode = LANDING_MODE;
				...
			}
		}
	}
}

Changes in 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);

Changes in eris_vrpn.cpp

Global Variables

	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;

	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;
	
	float xError# = 0.0;
	float yError# = 0.0;
	float xStepPositionError# = 0.0;
	float yStepPositionError# = 0.0;
	
	float xPosition# = -0.012;
	float yPosition# = -0.008;	//Default Crazyflie location setpoints
	float zPosition# = 0.75;
	
	char takeOff# = 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;

Changes in main()

	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\t........

	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#);
	}
	
	crRadio#->setARDTime(2000); //Sets Wait Time between Packet Retries (**MUST BE IN INCREMENTS OF 250**)
	crRadio#->setARC(0);		//Sets Number of times Retries Packet Send
	crRadio#->setPower(P_0DBM);

	cflieCopter#->setSendSetpoints( true );
	initTime# = cflieCopter#->currentTime();
	cflieCopter#->m_enumFlightMode = GROUNDED_MODE;

	fclose(out#);

	delete cflieCopter#;
	delete crRadio#;

Creating a New Callback

Note: To make it easier for this section you can:

1. COPY and PASTE the callback code directly after the last callback in the Swarm Client
2. Use FIND and REPLACE to replace all '#' signs with the Crazyflie number desired

• IMPORTANT: Be careful, some of the '#' symbols are used for the Radio Dongle number also.

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() );
	
	frameCount++;
	
	// Get the euler angles
	q_vec_type euler;
	q_to_euler(euler, t.quat);
	
	xPosition# = t.pos[0];
	yPosition# = t.pos[1];			//Stores X, Y, and Z Position for use in other Callbacks (external)
	zPosition# = -t.pos[2] + 0.05;
	
#if USE_PRINTOUT
	cout << "PID Radio #" << endl << endl;
#endif
#if 0   //UNUSED (old way of storing camera data)
	//cout << "\033[2J\033[1;1H";  //Clears Terminal Window to display clean output

	cfliePitch = cflieCopter#->pitch();
	cflieRoll = cflieCopter#->roll();
	cflieYaw = cflieCopter#->yaw();
	cflieThrust = cflieCopter#->thrust();
	
	/*
	 * 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  //END UNUSED

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

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


#if USE_KEYBOARD
	if (cflieCopter#->m_enumFlightMode == LANDING_MODE) //Landing Mode (TODO Change to be a keystroke when implemented)
	{

	//CORRECTS PITCH AND ROLL PID ERRORS TO ACCOUNT FOR QUADS CURRENT YAW DIRECTION
//	xError# = cos(correctedYaw# / 57.2958)*(xPositionDesired# - xPosition#) - sin(correctedYaw# / 57.2958)*(yPositionDesired# - yPosition#);	
//	yError# = sin(correctedYaw# / 57.2958)*(xPositionDesired# - xPosition#) + cos(correctedYaw# / 57.2958)*(yPositionDesired# - yPosition#);

	xError# = xPositionDesired# - xPosition#;
	yError# = yPositionDesired# - yPosition#;
	controllerSetXYError(&pidCtrl#, xError#, yError#);

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

	} else if (cflieCopter#->m_enumFlightMode == MIRROR_MODE) {		//Acts as Master
		xPositionDesired# = -0.012;
		yPositionDesired# = -0.200;
		zPositionDesired# = 0.75;

		if( loopTime#Start - lastTime > 4.0 )
		{
		   toggle = !toggle;
		   lastTime = loopTime#Start;
		}

#if USE_PRINTOUT
		cout << "New xPositionDesired: " << xPositionDesired# << endl;
#endif
		if(toggle)
		{
		xStepPositionError# = xPositionDesired# - xPosition#;
		yStepPositionError# = yPositionDesired# - yPosition#;
		controllerSetXYError(&pidCtrl#, xStepPositionError#, yStepPositionError#);

				controllerCorrectAttitudePID(&pidCtrl#, t.pos[0], t.pos[1], zPosition#, correctedYaw#,
						xPositionDesired#, yPositionDesired#, zPositionDesired#,
						yawDesired#, &rollControlOutput#, &pitchControlOutput#,
						&yawControlOutput#, &thrustControlOutput#);
		}
		else
		{
		xStepPositionError# = (xPositionDesired# + 0.65) - xPosition#;
		yStepPositionError# = (yPositionDesired# + 0.65) - yPosition#;
		controllerSetXYError(&pidCtrl#, xStepPositionError#, yStepPositionError#);

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

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

	} else if (cflieCopter#->m_enumFlightMode == TAKEOFF_MODE) {		//correctedYaw#

	//CORRECTS PITCH AND ROLL PID ERRORS TO ACCOUNT FOR QUADS CURRENT YAW DIRECTION
//	xError# = cos(correctedYaw# / 57.2958)*(xPositionDesired# - xPosition#) - sin(correctedYaw# / 57.2958)*(yPositionDesired# - yPosition#);	
//	yError# = sin(correctedYaw# / 57.2958)*(xPositionDesired# - xPosition#) + cos(correctedYaw# / 57.2958)*(yPositionDesired# - yPosition#);

	xError# = xPositionDesired# - xPosition#;
	yError# = yPositionDesired# - yPosition#;
	controllerSetXYError(&pidCtrl#, xError#, yError#);

	controllerCorrectAttitudePID(&pidCtrl#, t.pos[0], t.pos[1], zPosition#, correctedYaw#,
			xPositionDesired#, yPositionDesired#, zPositionDesired#,
			yawDesired#, &rollControlOutput#, &pitchControlOutput#,
			&yawControlOutput#, &thrustControlOutput#);
	}
	
	 else if (cflieCopter#->m_enumFlightMode == HAND_MODE) {		//correctedYaw#

	//CORRECTS PITCH AND ROLL PID ERRORS TO ACCOUNT FOR QUADS CURRENT YAW DIRECTION
//	xError# = cos(correctedYaw# / 57.2958)*(xPositionDesired# - xPosition#) - sin(correctedYaw# / 57.2958)*(yPositionDesired# - yPosition#);	
//	yError# = sin(correctedYaw# / 57.2958)*(xPositionDesired# - xPosition#) + cos(correctedYaw# / 57.2958)*(yPositionDesired# - yPosition#);

	xError# = (xPositionHand - 0.5) - xPosition#;
	yError# = (yPositionHand + 0.5) - yPosition#;
	controllerSetXYError(&pidCtrl#, xError#, yError#);

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

	}

	else if(cflieCopter#->m_enumFlightMode == GROUNDED_MODE){
		controllerResetAllPID(&pidCtrl#);
		rollControlOutput# = 0;
		pitchControlOutput# = 0;
		yawControlOutput# = 0;
		thrustControlOutput# = 0;
	}

	else{		//Emergency Case (Should never happen)
		controllerResetAllPID(&pidCtrl#);
		rollControlOutput# = 0;
		pitchControlOutput# = 0;
		yawControlOutput# = 0;
		thrustControlOutput# = 0;
	}

#else
	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		
#if USE_BASIC_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%.6f\t\t%d\t\t%d",
				cflieCopter#->roll(), 
				cflieCopter#->pitch(), 
				quadYaw#, 
				t.pos[0], 
				t.pos[1], 
				-t.pos[2],
				camYawDeg#,
				correctedYaw#,
				loopTimeTotal,
				loopTimeTotalDelta,
				loopTime#,
				loopTime#Delta,
				vrpnPacketDelta#,
				vrpnPacketTime#,
				vrpnPacketBackup#,
				cflieCopter#->radioRSSI());	//
	fprintf(out#, "\n");

#else	
	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%.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",
				cflieCopter#->roll(), 
				cflieCopter#->pitch(), 
				quadYaw#, 
				t.pos[0], 
				t.pos[1], 
				-t.pos[2],
				camYawDeg#,
				correctedYaw#,
				loopTimeTotal,
				loopTimeTotalDelta,
				loopTime#,
				loopTime#Delta,
				vrpnPacketDelta#,
				vrpnPacketTime#,
				vrpnPacketBackup#,
				xPositionDesired#,
				yPositionDesired#,
				zPositionDesired#,
				yawDesired#,
				pitchControlOutput#,
				rollControlOutput#,
				yawControlOutput#,
				thrustControlOutput#,
				cflieCopter#->radioRSSI(),
				euler[0],
				euler[1],
				euler[2],
				cflieCopter#->motor1(),
				cflieCopter#->motor2(),
				cflieCopter#->motor3(),
				cflieCopter#->motor4(),
				cflieCopter#->gyroX(),
				cflieCopter#->gyroY(),
				cflieCopter#->gyroZ());	//
	fprintf(out#, "\n");
#endif
#endif

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

}	//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 #

Changes in CTRL+C Handler

---

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