Commit a64f6900 authored by Ian McInerney's avatar Ian McInerney

Computation groundstation control functionality

parent 8a500e4e
......@@ -64,7 +64,7 @@ enum CRTPPort {
CRTP_PORT_XYZ_SET = 0x06,
CRTP_PORT_XYZ_POS = 0x07,
CRTP_PORT_CONTROLLER_UPDATE = 0x08,
CRTP_PORT_COMP = 0x09,
CRTP_PORT_COMPUTATION = 0x09,
CRTP_PORT_PLATFORM = 0x0D,
CRTP_PORT_LINK = 0x0F,
};
......@@ -76,6 +76,19 @@ enum controllerUpdate_Channels {
CONTROLLER_SWITCH_THROTTLE_CTRL
};
// Channels in the CRTP packet for the computation framework
enum Computation_channels {
COMP_CHAN_QUERY = 0,
COMP_CHAN_COMPUTE_DATA,
COMP_CHAN_CONFIG,
};
// These are various codes used to configure the computation framework in the config packet
enum comp_config_codes {
COMP_CONFIG_ENABLE = 0,
COMP_CONFIG_FUNC,
};
// Numbers specified here must match the quadcopter ID numbers
enum PID_Controller_IDs {
PID_X = 0,
......@@ -169,6 +182,10 @@ class CCrazyflie {
// The network structure for forwarding packets
NetworkForwarding *m_network;
// Stuff for the computation
int computationType;
bool computationStarted;
// An array to hold the takeoff profile
takeoffProfileStep *takeoffProfile;
int numTakeoffSteps;
......@@ -380,6 +397,17 @@ class CCrazyflie {
void setCoordinateSystem(coordinateSystem coor);
coordinateSystem getCoordinateSystem();
/*
* Computation functions
*/
int getComputationType();
int queryComputationType();
std::string getComputationTypeString();
void enableComputation();
void disableComputation();
void configureComputation( void *config, uint8_t length);
/**
* Reset the controller on the quadcopter
*/
......
......@@ -59,11 +59,12 @@ public:
void displayAdjacency();
/**
* Retrieve the next packet to send
* 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
*/
* @param quadNum The number of the quadcopter to get the next packet for
* @param pk Pointer to the packet object to return the next packet in
* @return If there was a packet to send
*/
bool retrievePacket(int quadNum, CCRTPPacket *pk);
/*
......
......@@ -14,7 +14,7 @@
#define NUM_RADIOS 1
// The number of quadcopters to initialize
#define NUM_QUADS 1
#define NUM_QUADS 2
#define BASE_THRUST 42000
#define Kp 5000
......
......@@ -25,6 +25,16 @@ typedef enum XferRate {
XFER_2M = 2,
} XferRate;
typedef struct computation_localization_config_packet {
uint8_t nodeNumber;
uint8_t numTotalNodes;
float stepSize;
uint8_t anchor;
float k1;
float k2;
float k3;
} computation_localization_config_packet;
typedef enum coordinateSystem {
coor_global = 0,
coor_local,
......
......@@ -18,6 +18,7 @@ enum commandType {
CMD_GOTO_DEFAULT = 'd',
CMD_START_MOTORS = 's',
CMD_TOGGLE_COOR = 'f',
CMD_TOGGLE_COMP = 'c',
};
extern const std::string CLEAR; // When sent to cout, this clears the terminal screen
......
......@@ -40,6 +40,7 @@ CCrazyflie::CCrazyflie(CCrazyRadio *crRadio, int nRadioChannel, XferRate dataRat
m_radioDataRate = dataRate;
m_network = NULL;
computationStarted = false;
// Review these values
m_fMaxAbsRoll = 45.0f;
......@@ -144,6 +145,7 @@ CCrazyflie::CCrazyflie(CCrazyRadio *crRadio, int nRadioChannel, XferRate dataRat
}
this->queryControllerType();
this->queryComputationType();
// Open the log file
char fileName[255];
......@@ -264,7 +266,7 @@ bool CCrazyflie::cycle() {
// 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;
CCRTPPacket *pk = new CCRTPPacket(0);
while( m_network->retrievePacket(m_quadNum, pk) && (count < 5) ) {
this->m_crRadio->sendPacket( m_nRadioChannel, pk, this);
count++;
......
......@@ -100,15 +100,33 @@ void CCrazyflie::callback_Packet(CCRTPPacket *crtpPacket) {
delete crtpLog;
}
break;
case CRTP_PORT_COMP:
case CRTP_PORT_COMPUTATION:
// The computation port
if (m_network == NULL) {
// The network hasn't been enabled, so just ignore the packet
break;
{
if (crtpPacket->channel() == COMP_CHAN_COMPUTE_DATA) {
// This is the channel that contains data to pass along the network
if (m_network == NULL) {
// The network hasn't been enabled, so just ignore the packet
break;
}
if ( nLength < 1) {
// See if the length is 0, if it is leave
break;
}
// Pass the packet into the network so it can be processed
CCRTPPacket *crtpComp = new CCRTPPacket(cData, nLength,
crtpPacket->channel());
crtpComp->setChannel(crtpPacket->channel());
crtpComp->setPort(crtpPacket->port());
m_network->packetReceived(m_quadNum, crtpComp);
delete crtpComp;
}
}
// Pass the packet into the callback so it can be processed
m_network->packetReceived(m_quadNum, crtpPacket);
break;
}
......
......@@ -154,9 +154,19 @@ bool CCrazyflie::parseReceivedUserCommand() {
this->setSetpointZ( this->getSetpointZ(coor_global) - this->m_localOrigin.z , coor_local);
std::cout << "Crazyflie " << (m_quadNum+1) << "Using local system" << std::endl;
std::cout << "Crazyflie " << (m_quadNum+1) << " Using local system" << std::endl;
}
break;
case CMD_TOGGLE_COMP:
// Toggle the computation on and off
if (this->computationStarted) {
this->disableComputation();
std::cout << "Crazyflie " << (m_quadNum+1) << " Disabling computation" << std::endl;
} else {
this->enableComputation();
std::cout << "Crazyflie " << (m_quadNum+1) << " Enabling computation" << std::endl;
}
}
// A command was parsed
......
#include "CCrazyflie.h"
#include "CCRTPPacket.h"
int CCrazyflie::queryComputationType() {
// Create just random data to send (Unsure how the packet behaves if there is no data)
int nSize = 1;
char cBuffer[1];
cBuffer[0] = 1;
// Create the packet to send
CCRTPPacket *crtpPacket = new CCRTPPacket(cBuffer, nSize, CRTP_PORT_COMPUTATION);
crtpPacket->setChannel(COMP_CHAN_QUERY);
CCRTPPacket *crtpReceived = m_crRadio->sendAndReceive(crtpPacket, m_nRadioChannel, this);
// Parse the response
int dataLength = crtpReceived->dataLength();
int retVal = -1;
if (dataLength >= 1) {
// Parse the one data byte to get controller type
char *cData = crtpReceived->data();
this->computationType = cData[0];
retVal = this->computationType;
} else {
// This isn't good...
retVal = -1;
}
// Clear variables and return
delete crtpPacket;
delete crtpReceived;
return( retVal );
}
void CCrazyflie::enableComputation() {
// Populate the packet
int nSize = 2;
char cBuffer[2];
cBuffer[0] = COMP_CONFIG_ENABLE;
cBuffer[1] = 1;
// Create the packet to send
CCRTPPacket *crtpPacket = new CCRTPPacket(cBuffer, nSize, CRTP_PORT_COMPUTATION);
crtpPacket->setChannel(COMP_CHAN_CONFIG);
CCRTPPacket *crtpReceived = m_crRadio->sendAndReceive(crtpPacket, m_nRadioChannel, this);
// Clear variables and return
delete crtpPacket;
delete crtpReceived;
this->computationStarted = true;
}
void CCrazyflie::disableComputation() {
// Populate the packet
int nSize = 2;
char cBuffer[2];
cBuffer[0] = COMP_CONFIG_ENABLE;
cBuffer[1] = 0;
// Create the packet to send
CCRTPPacket *crtpPacket = new CCRTPPacket(cBuffer, nSize, CRTP_PORT_COMPUTATION);
crtpPacket->setChannel(COMP_CHAN_CONFIG);
CCRTPPacket *crtpReceived = m_crRadio->sendAndReceive(crtpPacket, m_nRadioChannel, this);
// Clear variables and return
delete crtpPacket;
delete crtpReceived;
this->computationStarted = false;
}
void CCrazyflie::configureComputation( void *config, uint8_t length) {
// Populate the packet
int nSize = length+1;
char *cBuffer;
cBuffer = (char*) malloc(nSize);
cBuffer[0] = COMP_CONFIG_FUNC; // Add the appropriate opcode
memcpy(&cBuffer[1], config, length);
// Create the packet to send
CCRTPPacket *crtpPacket = new CCRTPPacket(cBuffer, nSize, CRTP_PORT_COMPUTATION);
crtpPacket->setChannel(COMP_CHAN_CONFIG);
CCRTPPacket *crtpReceived = m_crRadio->sendAndReceive(crtpPacket, m_nRadioChannel, this);
// Clear variables and return
delete crtpPacket;
delete crtpReceived;
}
\ No newline at end of file
......@@ -5,6 +5,22 @@ int CCrazyflie::getControllerType() {
return controllerType;
}
int CCrazyflie::getComputationType() {
return this->computationType;
}
std::string CCrazyflie::getComputationTypeString() {
switch ( this->getComputationType() ) {
case 0:
return( "Computation:None" );
break;
case 1:
return( "Computation:Localization" );
break;
}
return( "Computation:Unknown" );
}
std::string CCrazyflie::getControllerTypeString() {
switch (this->getControllerType() ) {
case 0:
......
......@@ -129,15 +129,20 @@ void NetworkForwarding::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
* @param pk Pointer to the packet object to return the next packet in
* @return If there was a packet to send
*/
bool NetworkForwarding::retrievePacket(int quadNum, CCRTPPacket *pk) {
if ( packetQueues[quadNum]->size() == 0 ) {
return(false);
}
// Pull the next packet from the queue
*pk = packetQueues[quadNum]->front();
packetQueues[quadNum]->pop();
std::cout << "Sending comp packet to quad " << quadNum << std::endl;
return(true);
}
......@@ -148,6 +153,7 @@ bool NetworkForwarding::retrievePacket(int quadNum, CCRTPPacket *pk) {
* @param pk Pointer to the CRTP packet that was received
*/
void NetworkForwarding::packetReceived(int quadNum, CCRTPPacket *pk) {
std::cout << "Received comp packet from quad " << quadNum << std::endl;
// 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
......
......@@ -149,12 +149,28 @@ int main(int argc, char **argv) {
// Check the controller type
cout << crazyflie_info[i].cflieCopter->getControllerTypeString() << endl;
// Check the computation type
cout << crazyflie_info[i].cflieCopter->getComputationTypeString() << endl;
lateralPosition localOrigin;
localOrigin.x = -0.5;
localOrigin.y = 0.5;
localOrigin.z = -1;
crazyflie_info[i].cflieCopter->setLocalOrigin( localOrigin);
cout << "Configuring the computation subsystem" << endl;
computation_localization_config_packet config;
config.nodeNumber = i;
config.numTotalNodes = NUM_QUADS;
config.stepSize = 0.01;
config.anchor = 1;
config.k1 = 10;
config.k2 = 10;
config.k3 = 10;
crazyflie_info[i].cflieCopter->configureComputation( (void*) &config, sizeof(computation_localization_config_packet));
// Change the PID controller values
cout << "Sending PID Constants" << endl;
usleep(1000);
......@@ -322,7 +338,7 @@ int main(int argc, char **argv) {
pthread_create(&threads[i++], NULL, UIThread, (void*)1);
pthread_create(&threads[i++], NULL, vrpn_go, (void*)2);
pthread_create(&threads[i++], NULL, displayData, (void*)3);
// pthread_create(&threads[i++], NULL, displayData, (void*)3);
for (int j=0; j<NUM_RADIOS; j++) {
pthread_create(&threads[i+j], NULL, CCrazyRadio::startThread, (void*) radios[j].radio );
}
......
......@@ -78,7 +78,14 @@ void* UIThread(void *threadID) {
command.payload = atof(&tempbuf[2]);
// Pass the command to the Crazyflie
crazyflie_info[quadcopterNumber-1].cflieCopter->passUserCommand(command);
if (quadcopterNumber == 0) {
// This command is for every quadcopter
for (int i=0; i < NUM_QUADS; i++) {
crazyflie_info[i].cflieCopter->passUserCommand(command);
}
} else {
crazyflie_info[quadcopterNumber-1].cflieCopter->passUserCommand(command);
}
// Print it out and then reset the counter
printf("\nReceived command: %s\n", tempbuf);
......
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