Commit 11111ee2 authored by Ian McInerney's avatar Ian McInerney

Working computation framework

parent ed4db91d
......@@ -5,6 +5,8 @@
#include <fstream>
#include <string.h>
#include "vrpn_Connection.h"
#include "vrpn_Tracker.h"
#include "CCRTPPacket.h"
......@@ -16,7 +18,7 @@ typedef struct computation_localization_shared_node_data {
float alpha;
float beta;
float gamma;
} computation_localization_node_data;
} __attribute__((packed)) computation_localization_node_data;
class LocalizationLogger{
......@@ -31,10 +33,14 @@ private:
double startTime;
double nextLogTime;
double logInterval;
std::ofstream file_log;
public:
LocalizationLogger(int numAgents, char* filename);
LocalizationLogger(int numAgents, char* filename, double logInterval);
~LocalizationLogger();
......@@ -49,6 +55,8 @@ public:
float getBeta(int agent);
float getGamma(int agent);
static void VRPN_CALLBACK vrpnTrackerCallback(void *logger, const vrpn_TRACKERCB t);
void setTargetPosition(float x, float y, float z);
void logData();
......
......@@ -29,7 +29,6 @@ private:
pthread_mutex_t queueMutex;
public:
// The constructor
NetworkForwarding(int numNodes);
......@@ -39,6 +38,9 @@ public:
// The destructor
~NetworkForwarding();
void clearAllNetworkQueues();
void clearNetworkQueue(int node);
void setNetworkLogger(LocalizationLogger *logger);
/*
......
......@@ -11,6 +11,7 @@
#include "vrpn_Connection.h"
#include "vrpn_Tracker.h"
#include "computations/LocalizationLogger.h"
#include <quat.h>
......@@ -25,6 +26,8 @@ extern Bridge *bridge;
//#define USE_BRIDGE 1
#define USE_LOCALIZATION
#define USE_VRPN 1
#define USE_KEYBOARD 1
#define USE_PRINTOUT 0
......@@ -33,6 +36,8 @@ extern Bridge *bridge;
#define USE_PR_YAW_CORRECT 0 //DO NOT NEED (NOW IMPLEMENTED ON FIRMWARE)
#define USE_HAND 0
extern LocalizationLogger *compLogger;
void query_vrpn();
/**
......
......@@ -11,10 +11,10 @@
#include <fstream>
// The number of radios to use
#define NUM_RADIOS 1
#define NUM_RADIOS 2
// The number of quadcopters to initialize
#define NUM_QUADS 2
#define NUM_QUADS 4
#define BASE_THRUST 42000
#define Kp 5000
......
......@@ -33,7 +33,7 @@ typedef struct computation_localization_config_packet {
float k1;
float k2;
float k3;
} computation_localization_config_packet;
} __attribute__((packed)) computation_localization_config_packet;
typedef enum coordinateSystem {
coor_global = 0,
......
......@@ -4,14 +4,20 @@
#include <time.h>
#include <stdlib.h>
#include "vrpn.h"
#include "computations/LocalizationLogger.h"
#include "CCRTPPacket.h"
#define NSEC_PER_SEC 1000000000L
LocalizationLogger::LocalizationLogger(int numAgents, char* fileName) {
LocalizationLogger::LocalizationLogger(int numAgents, char* fileName, double logInterval) {
this->numAgents = numAgents;
this->logInterval = logInterval;
std::cout << "Logging computation at " << this->logInterval << " second intervals" << std::endl;
// Init the target position to 0
this->targetX = 0;
this->targetY = 0;
......@@ -22,13 +28,23 @@ LocalizationLogger::LocalizationLogger(int numAgents, char* fileName) {
clock_gettime(CLOCK_MONOTONIC, &tsTime);
this->startTime = (tsTime.tv_sec + double(tsTime.tv_nsec) / NSEC_PER_SEC);
this->nextLogTime = this->startTime;
// Create the array to hold the data from the nodes
// Create the array to hold the data from the nodes and zero out the data
this->nodeData = new computation_localization_node_data[numAgents]();
for (int i=0; i < numAgents; i++) {
this->nodeData[i].measuredRadius = 0;
this->nodeData[i].estimatedX = 0;
this->nodeData[i].estimatedY = 0;
this->nodeData[i].estimatedZ = 0;
this->nodeData[i].alpha = 0;
this->nodeData[i].beta = 0;
this->nodeData[i].gamma = 0;
}
// Open the file
try {
char logFileName[40];
char logFileName[255];
char timeString[40];
time_t rawtime;
......@@ -39,9 +55,11 @@ LocalizationLogger::LocalizationLogger(int numAgents, char* fileName) {
// Create the log file name (including date)
sprintf(logFileName, "%s_%s.txt", fileName, timeString);
this->file_log.open(fileName, std::ios_base::out);
this->file_log.open(logFileName, std::ios_base::out);
std::cout << "Opened computation log file: " << logFileName << std::endl;
} catch (...) {
std::cout << "Error opening log file for localization logger";
std::cout << "Error opening log file for localization logger" << std::endl;
exit(-1);
}
......@@ -85,6 +103,18 @@ LocalizationLogger::~LocalizationLogger() {
this->file_log.close();
}
void VRPN_CALLBACK LocalizationLogger::vrpnTrackerCallback(void *logger, const vrpn_TRACKERCB t) {
// Make a new variable so the methods can be called
LocalizationLogger *log = (LocalizationLogger *) logger;
// Pass the position into the logger
float x = t.pos[0];
float y = t.pos[1];
float z = t.pos[2];
log->setTargetPosition(x, y, z);
}
void LocalizationLogger::setTargetPosition(float x, float y, float z) {
this->targetX = x;
this->targetY = y;
......@@ -97,10 +127,19 @@ void LocalizationLogger::logData() {
struct timespec tsTime;
clock_gettime(CLOCK_MONOTONIC, &tsTime);
double currentTime = (tsTime.tv_sec + double(tsTime.tv_nsec) / NSEC_PER_SEC) - this->startTime;
double currentTime = (tsTime.tv_sec + double(tsTime.tv_nsec) / NSEC_PER_SEC);
if ( currentTime < this->nextLogTime ) {
// Not time to log yet, break out
return;
}
double timeSinceStart = currentTime - this->startTime;
this->nextLogTime = currentTime + this->logInterval;
// Log the time since start
this->file_log << currentTime;
this->file_log << timeSinceStart;
// Log the target data
this->file_log << "\t\t" << this->targetX;
......
......@@ -52,6 +52,24 @@ NetworkForwarding::~NetworkForwarding() {
delete[] edges;
}
void NetworkForwarding::clearAllNetworkQueues() {
for (int i=0; i<numNodes; i++) {
this->clearNetworkQueue(i);
}
}
void NetworkForwarding::clearNetworkQueue(int node) {
pthread_mutex_lock( &(this->queueMutex) );
// Clear out the queue
while (!packetQueues[node]->empty()) {
packetQueues[node]->pop();
}
// Release the data mutex
pthread_mutex_unlock( &(this->queueMutex) );
}
void NetworkForwarding::setNetworkLogger(LocalizationLogger *logger) {
this->networkLogger = logger;
}
......
......@@ -7,6 +7,7 @@
#include "userOutput.h"
#include "Bridge.h"
#include "computations/NetworkForwarding.h"
#include "computations/LocalizationLogger.h"
#include "quadcopterData.h"
#include <sys/socket.h>
......@@ -29,6 +30,9 @@ Bridge *bridge;
// The computation graph
NetworkForwarding *network;
// The logger for the localization system
LocalizationLogger *compLogger;
using namespace std;
void initMulticast();
......@@ -90,6 +94,10 @@ int main(int argc, char **argv) {
signal(SIGINT, &ctrlc_handler);
// Initialize the computation logger
char compFileName[] = "logs/comp_localization";
compLogger = new LocalizationLogger(networkNumNodes, compFileName, 1.0/100.0);
// Initialize the computation graph
network = new NetworkForwarding(networkNumNodes, networkEdges, networkNumEdges);
/* network->addEdge({1, 2});
......@@ -101,6 +109,7 @@ int main(int argc, char **argv) {
network->addEdge({4, 1});
network->addEdge({4, 3});
*/
network->setNetworkLogger(compLogger);
network->displayAdjacency();
// Initialize the crazyflie radios
......@@ -134,6 +143,9 @@ int main(int argc, char **argv) {
// Init the radio
radios[crazyflie_info[i].radioNumber].radio->setChannel( crazyflie_info[i].channelNumber );
// Clear the packets on the network destined for this quadcopter
network->clearNetworkQueue(i);
// Create the crazyflie object
crazyflie_info[i].cflieCopter = new CCrazyflie(radios[crazyflie_info[i].radioNumber].radio,
crazyflie_info[i].channelNumber,
......@@ -146,6 +158,9 @@ int main(int argc, char **argv) {
// Add the Crazyflie to the radio
radios[crazyflie_info[i].radioNumber].radio->addCrazyflie( crazyflie_info[i].cflieCopter );
// Disable the computation (just in case)
crazyflie_info[i].cflieCopter->disableComputation();
// Check the controller type
cout << crazyflie_info[i].cflieCopter->getControllerTypeString() << endl;
......@@ -163,10 +178,10 @@ int main(int argc, char **argv) {
config.nodeNumber = i;
config.numTotalNodes = NUM_QUADS;
config.stepSize = 0.01;
config.anchor = 1;
config.k1 = 10;
config.k2 = 10;
config.k3 = 10;
config.anchor = 0;
config.k1 = 1;
config.k2 = 1;
config.k3 = 1;
crazyflie_info[i].cflieCopter->configureComputation( (void*) &config, sizeof(computation_localization_config_packet));
......@@ -327,6 +342,9 @@ int main(int argc, char **argv) {
//===========End Logfile Init==============
// Clear all the queues for good measure
network->clearAllNetworkQueues();
cout << "The Init has completed, press enter key to continue" << endl;
getchar();
......@@ -385,14 +403,13 @@ void closeoutProgram() {
delete radios[i].radio;
}
delete network;
delete compLogger;
// Close out of the log files
#if USE_LOGGING
#if USE_HAND
fclose(outHand);
#endif
for (int i = 0; i < NUM_QUADS; i++) {
logging_CloseLog(i);
}
#endif
}
......@@ -128,14 +128,14 @@ QUADCOPTERS_t crazyflie_info[] = {
},
*/
// Quadcopter 1
{"Crazyflie22",
{"Crazyflie21",
//"Bridge1",
//"UAV",
// "BigQuad7",
NULL,
NULL,
// 75,
100,//swapped 1,2
25,//swapped 1,2
// 80,
// 45,
0,
......@@ -251,10 +251,10 @@ QUADCOPTERS_t crazyflie_info[] = {
},
// Quadcopter 3
{"Crazyflie22",
{"Crazyflie23",
NULL,
NULL,
45, //swapped radio on 1 and 3
65, //swapped radio on 1 and 3
1,
XFER_2M, // The datarate of the Crazyflie
0,
......@@ -295,10 +295,10 @@ QUADCOPTERS_t crazyflie_info[] = {
},
// Quadcopter 4
{"Crazyflie23",
{"Crazyflie24",
NULL,
NULL,
25,
85,
1,
XFER_2M, // The datarate of the Crazyflie
0,
......
......@@ -16,6 +16,8 @@ vrpn_Tracker_Remote *trackerHand;
vrpn_Tracker_Remote *trackerBridge1;
vrpn_Tracker_Remote *trackerBridge2;
vrpn_Tracker_Remote *trackerLocalizationTarget;
char keystroke_now = 0;
char keystroke_prev = 0;
int keyPressed = 0;
......@@ -70,6 +72,11 @@ void vrpn_init(std::string connectionName, void (*callbackHand)(void*, const vrp
trackerBridge2 = new vrpn_Tracker_Remote("Bridge2", connection);
trackerBridge2->register_change_handler(bridge, Bridge::end2Callback);
#endif
#ifdef USE_LOCALIZATION
trackerLocalizationTarget = new vrpn_Tracker_Remote("Target", connection);
trackerLocalizationTarget->register_change_handler(compLogger, LocalizationLogger::vrpnTrackerCallback);
#endif
// Initialize all the quadcopter trackables
for (int i = 0; i < NUM_QUADS; i++) {
......@@ -97,17 +104,9 @@ 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 );
// 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();
// Write the log information
// crazyflie_info[j].cflieCopter->writeLogData();
}
#if USE_HAND
......@@ -129,8 +128,14 @@ void* vrpn_go(void *threadID) {
trackerBridge2->mainloop();
#endif // END USE_BRIDGE
#ifdef USE_LOCALIZATION
trackerLocalizationTarget->mainloop();
#endif
usleep(200); //Was 200
compLogger->logData();
// Check if the program should exit
if (exitProgram) {
std::cout << "Stopping thread for vrpn" << endl;
......
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