Commit 2fc5016b authored by Ian McInerney's avatar Ian McInerney

Added network topology forwarding to the CCrazyflie class

parent fdb7c272
......@@ -44,6 +44,7 @@
// Private
#include "CCrazyRadio.h"
#include "computations/NetworkForwarding.h"
#include "CTOC.h"
#include "structures.h"
......@@ -156,6 +157,8 @@ class CCrazyflie {
float m_desiredYaw;
uint16_t m_baseThrust;
// The network structure for forwarding packets
NetworkForwarding *m_network;
// An array to hold the takeoff profile
takeoffProfileStep *takeoffProfile;
......@@ -255,6 +258,8 @@ class CCrazyflie {
*/
static void VRPN_CALLBACK vrpnTrackerCallback(void *crazyflie, const vrpn_TRACKERCB t);
void setNetworkForwarding(NetworkForwarding *network);
/**
* Query the type of controller that the Crazyflie is using
*
......
......@@ -58,6 +58,14 @@ public:
*/
void displayAdjacency();
/**
* Retrieve the next packet to send
*
* @param quadNum The number of the quadcopter to get the next packet for
* @param pk Pointer to store the packet
*/
bool retrievePacket(int quadNum, CCRTPPacket *pk);
/*
* Callback to be called when a packet is received from a quadcopter
*
......@@ -65,6 +73,7 @@ public:
* @param pk Pointer to the CRTP packet that was received
*/
void packetReceived(int quadNum, CCRTPPacket *pk);
static void packetCallback(NetworkForwarding *network, int quadNum, CCRTPPacket *pk);
/**
*
......
......@@ -36,6 +36,8 @@ CCrazyflie::CCrazyflie(CCrazyRadio *crRadio, int nRadioChannel, int quadNum, dou
m_crRadio = crRadio;
m_nRadioChannel = nRadioChannel;
m_network = NULL;
// Review these values
m_fMaxAbsRoll = 45.0f;
m_fMaxAbsPitch = m_fMaxAbsRoll;
......@@ -212,6 +214,16 @@ bool CCrazyflie::cycle() {
default:
break;
}
// If there is a packet from the computation network to send, send it
// Place a timeout of 5 packets on the sending
int count = 0;
CCRTPPacket *pk = NULL;
if( m_network->retrievePacket(m_quadNum, pk) && (count < 5) ) {
this->m_crRadio->sendPacket( m_nRadioChannel, pk, this);
count++;
}
delete pk;
if (m_bufferEnd > 0) {
// Send PID constants
......
......@@ -100,6 +100,16 @@ void CCrazyflie::callback_Packet(CCRTPPacket *crtpPacket) {
delete crtpLog;
}
break;
case CRTP_PORT_COMP:
// The computation port
if (m_network == NULL) {
// The network hasn't been enabled, so just ignore the packet
break;
}
// Pass the packet into the callback so it can be processed
m_network->packetReceived(m_quadNum, crtpPacket);
break;
}
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
......
......@@ -116,3 +116,7 @@ void CCrazyflie::setCameraAngle(angularPosition cameraAngle) {
void CCrazyflie::setCameraQuat(quaternionPosition cameraQuat) {
this->m_currentCameraQuat = cameraQuat;
}
void CCrazyflie::setNetworkForwarding(NetworkForwarding *network) {
this->m_network = network;
}
\ No newline at end of file
......@@ -123,4 +123,34 @@ void NetworkForwarding::displayAdjacency() {
}
std::cout << std::endl;
}
}
\ No newline at end of file
}
/**
* Retrieve the next packet to send
*
* @param quadNum The number of the quadcopter to get the next packet for
* @param pk Pointer to store the packet
*/
bool NetworkForwarding::retrievePacket(int quadNum, CCRTPPacket *pk) {
if ( packetQueues[quadNum]->size() == 0 ) {
return(false);
}
*pk = packetQueues[quadNum]->front();
packetQueues[quadNum]->pop();
return(true);
}
/**
* Callback to be called when a packet is received from a quadcopter
*
* @param quadNum The number of the quadcopter the packet was received from
* @param pk Pointer to the CRTP packet that was received
*/
void NetworkForwarding::packetReceived(int quadNum, CCRTPPacket *pk) {
// Determine the quads to get the packet
for (std::vector<int>::iterator it = edges[quadNum]->begin(); it != edges[quadNum]->end(); ++it) {
// Add it to the queue of the adjacent quadcopter
packetQueues[*it]->push(*pk);
}
}
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment