Callbacks: Difference between revisions

From Distributed Autonomous and Networked Control Lab
Jump to navigation Jump to search
Newer edit →
Jnoronha (talk | contribs)
203 edits
added registering the Callbacks
 
Jnoronha (talk | contribs)
203 edits
Added full callback example
Line 23: Line 23:
::'''Note:''' We need a ''Tracker_Remote'' object for each constellation we want to track (Hand and each Crazyflie in the Swarm)
::'''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:
:2. Now we create a function to (code in ''vrpn.cpp''):
:#Register the connection to the Server computer
:#Register the connection to the Server computer
:#Assign each ''Tracker_Remote'' object to a valid constellation name on the Server computer
:#Assign each ''Tracker_Remote'' object to a valid constellation name on the Server computer
:#Link that constellation to a callback (code in ''vrpn.cpp''):
:#Link that constellation to a callback


<blockquote>
<blockquote>
Line 64: Line 64:


::'''Note 1:''' We pass in the following as inputs to the function:
::'''Note 1:''' We pass in the following as inputs to the function:
::*''string connectionName'' is the '''IP address''' of the Computer running the Camera System  
::*''string connectionName'' is the "'''IP address:Port'''" of the Computer running the Camera System  
::*A callback pointer for each constellation we want to track (These will be established in the [[#Using a Callback|Using a Callback]] section)
::**(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 [[:Callbacks#Using a Callback in the Swarm Client|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):
::'''Note 2:''' Notice the names of each ''tracker'' '''match the names of the constellations''' in the Camera System Software (shown below):


::[[Image:TrackingToolsCrazyflieConstellationZeroing.png|800px]]
::[[Image:TrackingToolsCrazyflieConstellationParams.png|800px]]


==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''):


<blockquote>
<code>
<pre>
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);
</pre>
</code>
</blockquote>
:2. [[:Callbacks#Registering a Callback|Register the connection]] and constellations to the Callbacks (code in ''eris_vrpn.cpp''):
<blockquote>
<code>
<pre>
#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
</pre>
</code>
</blockquote>
::'''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===
===Full Callback Example===
Below is a full Callback example for Crazyflie1:
<blockquote>
<code>
<pre>
#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
</pre>
</code>
</blockquote>




Revision as of 19:02, 27 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.

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

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=148"