Commit 56f70e4e authored by Ian McInerney's avatar Ian McInerney

Made each radio its own thread

parent ec8725c0
......@@ -43,34 +43,13 @@
#include <unistd.h>
#include <iostream>
#include <sstream>
#include <vector>
// Private
#include "structures.h"
#include "CCRTPPacket.h"
#include "CCrazyflie.h"
/*! \brief Power levels to configure the radio dongle with */
enum Power {
/*! \brief Power at -18dbm */
P_M18DBM = 0,
/*! \brief Power at -12dbm */
P_M12DBM = 1,
/*! \brief Power at -6dbm */
P_M6DBM = 2,
/*! \brief Power at 0dbm */
P_0DBM = 3
};
/*! \brief Channel speeds */
enum XferRate {
/*! \brief 250 kbps */
XFER_250K = 0,
/*! \brief 1 mbps */
XFER_1M = 1,
/*! \brief 2 mbps */
XFER_2M = 2,
};
// Backward declaration of the CCrazyflie class to make the compiler happy
class CCrazyflie;
......@@ -103,6 +82,11 @@ private:
float m_fDeviceVersion;
bool m_bAckReceived;
bool m_exitThread;
// Vector to hold the crazyflie objects associated with this radio
std::vector<CCrazyflie*> crazyflies;
// Functions
std::list<libusb_device*> listDevices(int nVendorID, int nProductID);
bool openUSBDongle();
......@@ -115,15 +99,29 @@ private:
bool writeControl(void *vdData, int nLength, uint8_t u8Request, uint16_t u16Value, uint16_t u16Index);
bool readData(void *vdData, int &nMaxLength);
void setAddress(char *cAddress);
void setContCarrier(bool bContCarrier);
public:
/**
* Add a Crazyflie to the radio
*
* @param flie Pointer to the CCrazyflie object
*/
void addCrazyflie(CCrazyflie *flie);
/**
* Main loop for the thread running the radio
*/
void mainLoop();
/**
* Static function to use to start the thread for the radio
*/
static void* startThread(void* args);
void stopThread();
void setDataRate(XferRate rate); //Moved From Private
void setARC(int nARC); //Moved From Private
......
......@@ -45,10 +45,10 @@
#include <pthread.h>
// Private
#include "structures.h"
#include "CCrazyRadio.h"
#include "computations/NetworkForwarding.h"
#include "CTOC.h"
#include "structures.h"
#include "vrpn_Connection.h"
#include "vrpn_Tracker.h"
......@@ -124,10 +124,10 @@ class CTOC;
calculating information based on the current sensor readings. */
class CCrazyflie {
private:
// Variables
// Variables for the specific quadcopter
int m_quadNum;
int m_nRadioChannel; // each crazyflie has its own channel and data rate
std::string m_strDataRate; // "250K", "1M" or "2M"
int m_nRadioChannel;
XferRate m_radioDataRate;
int m_nAckMissTolerance;
int m_nAckMissCounter;
......@@ -186,6 +186,9 @@ class CCrazyflie {
std::queue<userCommands_t> userCommands;
pthread_mutex_t userCommandsMutex;
// Mutex to protect the camera data
pthread_mutex_t cameraDataMutex;
// Control related parameters
/*! \brief Maximum absolute value for the roll that will be sent to
the copter. */
......@@ -254,13 +257,16 @@ class CCrazyflie {
\param crRadio Initialized (and started) instance of the
CCrazyRadio class, denoting the USB dongle to communicate
with. */
CCrazyflie(CCrazyRadio *crRadio, int radioChannel, int quadNum, double startTime);
CCrazyflie(CCrazyRadio *crRadio, int radioChannel, XferRate dataRate, int quadNum, double startTime);
/*! \brief Destructor for the copter convenience class
Destructor, deleting all internal variables (except for the
CCrazyRadio radio instance given in the constructor). */
~CCrazyflie();
int getRadioChannel();
XferRate getRadioDataRate();
/*
* Open a log file for the quadcopter
*
......@@ -280,6 +286,8 @@ class CCrazyflie {
*/
void passUserCommand(userCommands_t command);
int getQuadcopterNumber();
/*
* Callback called whenever there is a new VRPN position packet.
* Note: This function is static in the class.
......
......@@ -3,6 +3,7 @@
#include "vrpn_Connection.h"
#include "vrpn_Tracker.h"
#include "structures.h"
#include "CCrazyflie.h"
#include "computations/NetworkForwarding.h"
......@@ -56,6 +57,7 @@ typedef struct QUADCOPTERS {
CCrazyflie *cflieCopter; // The crazyflie copter object
uint8_t channelNumber; // The channel number of the CrazyFlie
uint8_t radioNumber; // The radio to use for this CrazyFlie
XferRate dataRate; // The data rate of the radio used with this CrazyFlie
double initTime; // Time the crazyflie was initialized
CONTROLLER_DATA_t controllerData; // Data associated with the controller for the quadcopter
......
......@@ -3,6 +3,28 @@
#include <stdint.h>
/*! \brief Power levels to configure the radio dongle with */
enum Power {
/*! \brief Power at -18dbm */
P_M18DBM = 0,
/*! \brief Power at -12dbm */
P_M12DBM = 1,
/*! \brief Power at -6dbm */
P_M6DBM = 2,
/*! \brief Power at 0dbm */
P_0DBM = 3
};
/*! \brief Channel speeds */
typedef enum XferRate {
/*! \brief 250 kbps */
XFER_250K = 0,
/*! \brief 1 mbps */
XFER_1M = 1,
/*! \brief 2 mbps */
XFER_2M = 2,
} XferRate;
typedef struct userCommands {
char type;
float payload;
......
......@@ -31,11 +31,13 @@
#include <fstream>
#include <pthread.h>
#include "CCrazyflie.h"
#include "CCrazyRadio.h"
CCrazyflie::CCrazyflie(CCrazyRadio *crRadio, int nRadioChannel, int quadNum, double startTime) {
CCrazyflie::CCrazyflie(CCrazyRadio *crRadio, int nRadioChannel, XferRate dataRate, int quadNum, double startTime) {
m_quadNum = quadNum;
m_crRadio = crRadio;
m_nRadioChannel = nRadioChannel;
m_radioDataRate = dataRate;
m_network = NULL;
......@@ -101,6 +103,7 @@ CCrazyflie::CCrazyflie(CCrazyRadio *crRadio, int nRadioChannel, int quadNum, dou
// Initialize the mutexes
pthread_mutex_init( &(this->userCommandsMutex), NULL);
pthread_mutex_init( &(this->cameraDataMutex), NULL);
// Open the file
try {
......
#include "CCrazyflie.h"
#include "CCrazyRadio.h"
int CCrazyflie::getControllerType() {
return controllerType;
......@@ -30,20 +31,41 @@ float CCrazyflie::batteryState() {
return this->sensorDoubleValue("pm.state");
}
int CCrazyflie::getRadioChannel() {
return m_nRadioChannel;
}
XferRate CCrazyflie::getRadioDataRate() {
return m_radioDataRate;
}
int CCrazyflie::getQuadcopterNumber() {
return this->m_quadNum;
}
/*
* These functions will return the current quadcopter
* lateral position as reported by the camera system.
*/
float CCrazyflie::getCameraX() {
return this->m_currentCameraPosition.x;
pthread_mutex_lock( &(this->cameraDataMutex) );
float retVal = this->m_currentCameraPosition.x;
pthread_mutex_unlock( &(this->cameraDataMutex) );
return(retVal);
}
float CCrazyflie::getCameraY() {
return this->m_currentCameraPosition.y;
pthread_mutex_lock( &(this->cameraDataMutex) );
float retVal = this->m_currentCameraPosition.y;
pthread_mutex_unlock( &(this->cameraDataMutex) );
return(retVal);
}
float CCrazyflie::getCameraZ() {
return this->m_currentCameraPosition.z;
pthread_mutex_lock( &(this->cameraDataMutex) );
float retVal = this->m_currentCameraPosition.z;
pthread_mutex_unlock( &(this->cameraDataMutex) );
return(retVal);
}
......@@ -69,15 +91,24 @@ float CCrazyflie::getQuadYaw() {
* angular position as reported by the camera system.
*/
float CCrazyflie::getCameraRoll() {
return this->m_currentCameraAngle.roll;
pthread_mutex_lock( &(this->cameraDataMutex) );
float retVal = this->m_currentCameraAngle.roll;
pthread_mutex_unlock( &(this->cameraDataMutex) );
return(retVal);
}
float CCrazyflie::getCameraPitch() {
return this->m_currentCameraAngle.pitch;
pthread_mutex_lock( &(this->cameraDataMutex) );
float retVal = this->m_currentCameraAngle.pitch;
pthread_mutex_unlock( &(this->cameraDataMutex) );
return(retVal);
}
float CCrazyflie::getCameraYaw() {
return this->m_currentCameraAngle.yaw;
pthread_mutex_lock( &(this->cameraDataMutex) );
float retVal = this->m_currentCameraAngle.yaw;
pthread_mutex_unlock( &(this->cameraDataMutex) );
return(retVal);
}
......
......@@ -103,18 +103,23 @@ void CCrazyflie::setXYZYawPosition(float xPosition, float yPosition, float zPosi
}
*/
void CCrazyflie::setCameraPosition(lateralPosition cameraPosition) {
pthread_mutex_lock( &(this->cameraDataMutex) );
this->m_currentCameraPosition = cameraPosition;
this->m_sendPosition = true;
pthread_mutex_unlock( &(this->cameraDataMutex) );
}
void CCrazyflie::setCameraAngle(angularPosition cameraAngle) {
pthread_mutex_lock( &(this->cameraDataMutex) );
this->m_currentCameraAngle = cameraAngle;
this->m_sendPosition = true;
pthread_mutex_unlock( &(this->cameraDataMutex) );
}
void CCrazyflie::setCameraQuat(quaternionPosition cameraQuat) {
pthread_mutex_lock( &(this->cameraDataMutex) );
this->m_currentCameraQuat = cameraQuat;
pthread_mutex_unlock( &(this->cameraDataMutex) );
}
void CCrazyflie::setNetworkForwarding(NetworkForwarding *network) {
......
......@@ -38,6 +38,8 @@ CCrazyRadio::CCrazyRadio(int dongleNumber) {
m_bAckReceived = false;
m_exitThread = false;
/*int nReturn = */libusb_init(&m_ctxContext);
// Do error checking here.
......@@ -127,8 +129,6 @@ bool CCrazyRadio::startRadio() {
return false;
}
bool CCrazyRadio::ackReceived() {
if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__ );
return m_bAckReceived;
......
#include "CCrazyRadio.h"
#include "CCrazyflie.h"
#include <vector>
/**
* Main loop for the thread running the radio
*/
void CCrazyRadio::mainLoop() {
std::cout << "Spawning thread for radio " << this->m_nDongleNumber << std::endl;
// Loop indefinitely until told to exit
while(1) {
for ( std::vector<CCrazyflie*>::iterator it = crazyflies.begin(); it != crazyflies.end(); ++it) {
// Set the channel for the next Crazyflie
this->setChannel( (*it)->getRadioChannel() );
this->setDataRate( (*it)->getRadioDataRate() );
// Run the mainloop for the crazyflie
(*it)->cycle();
// Write the log information for the crazyflie
(*it)->writeLogData();
}
usleep(200);
// Check if the thread has been told to terminate
if (this->m_exitThread) {
std::cout << "Stopping thread for radio " << this->m_nDongleNumber << std::endl;
return;
}
}
}
/**
* Static function to use to start the thread for the radio
*/
void* CCrazyRadio::startThread(void* args) {
CCrazyRadio *radio = (CCrazyRadio*) args;
radio->mainLoop();
return(NULL);
}
/**
* Stop the thread from running
*/
void CCrazyRadio::stopThread() {
this->m_exitThread = true;
}
\ No newline at end of file
#include "CCrazyRadio.h"
#include "CCrazyflie.h"
/**
* Add a Crazyflie to the radio
*
* @param flie Pointer to the CCrazyflie object
*/
void CCrazyRadio::addCrazyflie(CCrazyflie *flie) {
crazyflies.push_back(flie);
std::cout << "Added Crazyflie " << flie->getQuadcopterNumber()+1;
std::cout << " to radio " << this->m_nDongleNumber << std::endl;
}
void CCrazyRadio::setARC(int nARC) {
if( ALL_THE_DEBUG ) printf( "Enter: %s(nARC:%d)\n", __FUNCTION__, nARC );
......
#include "crazyflieGroundStation.h"
#include "CCrazyflie.h"
#include "CCrazyRadio.h"
#include "vrpn.h"
#include "errno.h"
#include "userInput.h"
......@@ -81,7 +82,7 @@ double loopTimeTotalDelta = 0;
//========End Testing Variables============
// For keeping track of the threads that are spawned
pthread_t threads[3];
pthread_t threads[3+NUM_RADIOS];
#define PI 3.14159265
......@@ -129,7 +130,7 @@ int main(int argc, char **argv) {
// Initialize the CrazyFlie quadcopters
for (int i = 0; i < NUM_QUADS; i++) {
cout << "Initializing Crazyflie " << i << endl;
cout << "Initializing Crazyflie " << i+1 << endl;
// Init the radio
radios[crazyflie_info[i].radioNumber].radio->setChannel( crazyflie_info[i].channelNumber );
......@@ -137,12 +138,15 @@ int main(int argc, char **argv) {
// Create the crazyflie object
crazyflie_info[i].cflieCopter = new CCrazyflie(radios[crazyflie_info[i].radioNumber].radio,
crazyflie_info[i].channelNumber,
crazyflie_info[i].dataRate,
i,
startTime);
crazyflie_info[i].cflieCopter->setNetworkForwarding(network);
crazyflie_info[i].cflieCopter->m_enumFlightMode = GROUNDED_MODE;
crazyflie_info[i].initTime = crazyflie_info[i].cflieCopter->currentTime();
// Add the Crazyflie to the radio
radios[crazyflie_info[i].radioNumber].radio->addCrazyflie( crazyflie_info[i].cflieCopter );
// Check the controller type
cout << crazyflie_info[i].cflieCopter->getControllerTypeString() << endl;
......@@ -309,13 +313,17 @@ int main(int argc, char **argv) {
void* status;
pthread_create(&threads[0], NULL, UIThread, (void*)1);
pthread_create(&threads[1], NULL, vrpn_go, (void*)2);
// pthread_create(&threads[2], NULL, displayData, (void*)3);
pthread_join(threads[1], &status);
int i=0;
pthread_create(&threads[i++], NULL, UIThread, (void*)1);
pthread_create(&threads[i++], NULL, vrpn_go, (void*)2);
// Run the VRPN main loop
// 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 );
}
for (int j=0; j < (i+NUM_RADIOS); j++) {
pthread_join(threads[j], &status);
}
// End the program
cout << "Closing program" << endl;
......@@ -329,6 +337,9 @@ int main(int argc, char **argv) {
*/
static void ctrlc_handler(int sig) {
// Send up a flag for the threads to end
for (int i = 0; i < NUM_RADIOS; i++) {
radios[i].radio->stopThread();
}
exitProgram = 1;
}
......
#include "structures.h"
#include "CCrazyflie.h"
#include "quadcopterData.h"
//#include "eris_vrpn.h"
......@@ -138,6 +139,7 @@ QUADCOPTERS_t crazyflie_info[] = {
// 80,
// 45,
0,
XFER_2M,
0,
// The controller data structure
{0, // The loop start time
......@@ -209,6 +211,7 @@ QUADCOPTERS_t crazyflie_info[] = {
NULL, // CrazyFlie copter object (initialized in main)
45, // The channel number of the crazyflie
0, // The radio to use
XFER_2M, // The datarate of the Crazyflie
0, // The init time
// The controller data structure
{0, // The loop start time
......@@ -253,6 +256,7 @@ QUADCOPTERS_t crazyflie_info[] = {
NULL,
45, //swapped radio on 1 and 3
1,
XFER_2M, // The datarate of the Crazyflie
0,
// The controller data structure
{0, // The loop start time
......@@ -296,6 +300,7 @@ QUADCOPTERS_t crazyflie_info[] = {
NULL,
25,
1,
XFER_2M, // The datarate of the Crazyflie
0,
// The controller data structure
{0, // The loop start time
......
......@@ -84,6 +84,7 @@ void* UIThread(void *threadID) {
// Check if the program should terminate
if (exitProgram) {
std::cout << "Stopping thread for user input" << endl;
return(NULL);
}
usleep(1000);
......
......@@ -260,6 +260,7 @@ void* displayData(void* id) {
*/
// Check if the program should terminate
if (exitProgram) {
std::cout << "Stopping thread for user output" << endl;
return(NULL);
}
......
......@@ -98,16 +98,16 @@ void* vrpn_go(void *threadID) {
// Iterate over every single quadcopter
for (int j = 0; j < NUM_QUADS; j++) {
// Get the radio ready for controlling the crazyflie
radios[crazyflie_info[j].radioNumber].radio->setChannel( crazyflie_info[j].channelNumber );
// radios[crazyflie_info[j].radioNumber].radio->setChannel( crazyflie_info[j].channelNumber );
// Go through the main loop for the Crazyflie trackables
crazyflie_info[j].vrpn_tracker->mainloop();
// Call the Crazyflie cycle function to perform its tasks
crazyflie_info[j].cflieCopter->cycle();
// crazyflie_info[j].cflieCopter->cycle();
// Write the log information
crazyflie_info[j].cflieCopter->writeLogData();
// crazyflie_info[j].cflieCopter->writeLogData();
}
#if USE_HAND
......@@ -133,6 +133,7 @@ void* vrpn_go(void *threadID) {
// Check if the program should exit
if (exitProgram) {
std::cout << "Stopping thread for vrpn" << endl;
return(NULL);
}
......
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