diff --git a/crazyflie_groundstation/inc/CCrazyflie.h b/crazyflie_groundstation/inc/CCrazyflie.h
index 62281cefe605649fa2dda02e1b3936608f747665..b1a738f6b2891df219c50a728d299781d5a9a477 100644
--- a/crazyflie_groundstation/inc/CCrazyflie.h
+++ b/crazyflie_groundstation/inc/CCrazyflie.h
@@ -313,6 +313,8 @@ class CCrazyflie {
*/
void openLogFile(char *baseFileName);
+ void printUpdatedHeader();
+
bool loadLoggingBlocksFromFile(std::string blockFileName);
bool resetLoggingBlocks();
diff --git a/crazyflie_groundstation/inc/CTOC.h b/crazyflie_groundstation/inc/CTOC.h
index 22da31fdc714abab28da81ba2704772eadc341d9..fd96360f7b41207504cf9f149e7e4c25063dac5c 100644
--- a/crazyflie_groundstation/inc/CTOC.h
+++ b/crazyflie_groundstation/inc/CTOC.h
@@ -39,6 +39,7 @@
#include <cstdlib>
#include <iostream>
#include <fstream>
+#include <vector>
// Private
#include "CCrazyRadio.h"
@@ -67,6 +68,7 @@ struct LoggingBlock {
int nID;
double dFrequency;
std::list<int> lstElementIDs;
+ bool isActive;
};
// Backward declaration of the CCrazyflie and CCrazyradio class to make the compiler happy
@@ -82,6 +84,8 @@ class CTOC {
int m_nItemCount;
std::list<struct TOCElement> m_lstTOCElements;
std::list<struct LoggingBlock> m_lstLoggingBlocks;
+ std::vector<std::string> m_lstActiveLogging;
+ int updateAmount = 0;
std::ofstream param_toc_file;
std::ofstream log_toc_file;
std::string paramfilename;
@@ -116,6 +120,7 @@ class CTOC {
// For loggable variables only
bool registerLoggingBlock(std::string strName, double dFrequency);
bool unregisterLoggingBlock(std::string strName);
+ bool unregisterLoggingBlocks();
struct LoggingBlock loggingBlockForName(std::string strName, bool& bFound);
struct LoggingBlock loggingBlockForID(int nID, bool& bFound);
@@ -127,6 +132,8 @@ class CTOC {
bool enableLogging(std::string strBlockName);
bool disableLogging(std::string strBlockName);
+ void setBlockActive(int nID, bool activity);
+ std::string createActiveHeader();
// void processPackets(std::list<CCRTPPacket*> lstPackets);
void processPackets(CCRTPPacket *crtpPacket);
@@ -140,6 +147,9 @@ class CTOC {
void openParamTOCFile(char *baseFileName);
std::string getLogTOCFile();
std::string getParamTOCFile();
+
+ std::string activeLogName(int index);
+ int sizeOfActiveList();
};
diff --git a/crazyflie_groundstation/src/CTOC.cpp b/crazyflie_groundstation/src/CTOC.cpp
index bf93a6fb9ba5cacb90fbb2f8291b075ad4645cda..27c1cf444cd3b99d808785d828cb0716fb62b803 100644
--- a/crazyflie_groundstation/src/CTOC.cpp
+++ b/crazyflie_groundstation/src/CTOC.cpp
@@ -563,6 +563,45 @@ void CTOC::printLoggingBlocksInitialized() {
}
}
+bool compare_id(const struct LoggingBlock& first, const struct LoggingBlock& second) {
+ return (first.nID < second.nID);
+}
+
+std::string CTOC::createActiveHeader() {
+ std::string header = "#" + std::to_string(updateAmount) + ": " + "\t";
+ header = header + "time" + "\t";
+ std::string element = "";
+ m_lstActiveLogging.clear();
+ m_lstLoggingBlocks.sort(compare_id);
+ bool bFound = false;
+ for (std::list<struct LoggingBlock>::iterator itBlock = m_lstLoggingBlocks.begin();
+ itBlock != m_lstLoggingBlocks.end();
+ itBlock++) {
+ struct LoggingBlock lbCurrent = *itBlock;
+
+ if(lbCurrent.isActive) {
+ for(int i = 0; i < lbCurrent.lstElementIDs.size(); i++) {
+ int elementID = this->elementIDinBlock(lbCurrent.nID, i);
+ struct TOCElement teCurrent = this->elementForID(elementID,
+ bFound);
+ element = teCurrent.strGroup + "." + teCurrent.strIdentifier;
+ header = header + element + "\t";
+ m_lstActiveLogging.push_back(element);
+ }
+ }
+ }
+ updateAmount++;
+ return header;
+}
+
+std::string CTOC::activeLogName(int index) {
+ return m_lstActiveLogging.at(index);
+}
+
+int CTOC::sizeOfActiveList() {
+ return m_lstActiveLogging.size();
+}
+
struct LoggingBlock CTOC::loggingBlockForName(std::string strName,
bool& bFound) {
if( ALL_THE_DEBUG ) printf( "%s\n", __FUNCTION__);
@@ -669,6 +708,7 @@ bool CTOC::enableLogging(std::string strBlockName) {
struct LoggingBlock lbCurrent = this->loggingBlockForName(strBlockName,
bFound);
if (bFound) {
+ this->setBlockActive(lbCurrent.nID, true);
uint8_t logPeriod= (uint8_t) ( (1 / lbCurrent.dFrequency) * 1000 );
char cPayload[3] = { 0x03, (char)lbCurrent.nID, logPeriod }; // JRB: A little concerned about this cast from double to char
@@ -694,6 +734,7 @@ bool CTOC::disableLogging(std::string strBlockName) {
bFound);
if (bFound) {
+ this->setBlockActive(lbCurrent.nID, false);
char cPayload[2] = { 0x04, (char)lbCurrent.nID }; // JRB: A little concerned about this cast from double to char
CCRTPPacket* crtpEnable = new CCRTPPacket(cPayload, 2, 1);
@@ -723,6 +764,31 @@ bool CTOC::unregisterLoggingBlock(std::string strName) {
return false;
}
+bool CTOC::unregisterLoggingBlocks() {
+ if( ALL_THE_DEBUG ) printf( "%s\n", __FUNCTION__);
+ bool bFound;
+
+ char cPayload[1] = { 0x05 };
+
+ CCRTPPacket* resetBlock = new CCRTPPacket(cPayload, 1, 1);
+ resetBlock->setPort(0x05);
+ resetBlock->setChannel(1);
+
+ CCRTPPacket* crtpReceived = m_crRadio->sendAndReceive(resetBlock, m_nRadioChannel, m_crazyflie);
+
+ delete resetBlock;
+
+ if (crtpReceived) {
+ delete crtpReceived;
+ for(std::list<struct LoggingBlock>::iterator itBlock = m_lstLoggingBlocks.begin(); itBlock != m_lstLoggingBlocks.end(); itBlock++) {
+ itBlock = m_lstLoggingBlocks.erase(itBlock);
+ }
+ return true;
+ }
+
+ return false;
+}
+
bool CTOC::unregisterLoggingBlockID(int nID) {
if( ALL_THE_DEBUG ) printf( "%s\n", __FUNCTION__);
char cPayload[2] = { 0x02, (char)nID };
@@ -1025,4 +1091,15 @@ std::string CTOC::getParamTOCFile() {
else {
return this->paramfilename;
}
+}
+
+void CTOC::setBlockActive(int nID, bool activity) {
+ if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__);
+ for(std::list<struct LoggingBlock>::iterator itBlock = m_lstLoggingBlocks.begin(); itBlock != m_lstLoggingBlocks.end(); itBlock++) {
+ struct LoggingBlock &lbCurrent = *itBlock;
+ if (nID == lbCurrent.nID) {
+ lbCurrent.isActive = activity;
+ return;
+ }
+ }
}
\ No newline at end of file
diff --git a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_commands.cpp b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_commands.cpp
index 95ba1c91dd54593c892b760f489fd01281eb509e..2a3e4cf021544c214007bf32ddb47470b752e08f 100644
--- a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_commands.cpp
+++ b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_commands.cpp
@@ -422,34 +422,40 @@ bool CCrazyflie::parseReceivedUserCommand() {
case 0:
{
this->resetLoggingBlocks();
+ this->printUpdatedHeader();
break;
}
case 1:
{
this->resetLoggingBlocks();
this->loadLoggingBlocksFromFile("loggingBlocks.txt");
+ this->printUpdatedHeader();
break;
}
case 2:
{
this->loadLoggingBlocksFromFile("loggingBlocks.txt");
+ this->printUpdatedHeader();
break;
}
case 3:
{
this->m_tocLogs->unregisterLoggingBlockID(id);
+ this->printUpdatedHeader();
break;
}
case 4:
{
struct LoggingBlock block = this->m_tocLogs->loggingBlockForID(id, found);
this->enableLogging(block.strName);
+ this->printUpdatedHeader();
break;
}
case 5:
{
struct LoggingBlock block = this->m_tocLogs->loggingBlockForID(id, found);
this->disableLogging(block.strName);
+ this->printUpdatedHeader();
break;
}
}
diff --git a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_loggingFuncs.cpp b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_loggingFuncs.cpp
index bec1dd0f9c49778fb2c6fc0499221c30587b408b..a786c9f4eaa67e0aa673f1623ea16aff62e63398 100644
--- a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_loggingFuncs.cpp
+++ b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_loggingFuncs.cpp
@@ -69,9 +69,9 @@ void CCrazyflie::openLogFile(char *baseFileName) {
file_log << "#Crazyflie" << std::endl;
// Add the controller type to the logfile
- file_log << "#" << this->getControllerTypeString() << std::endl;
- file_log << variables << std::endl;
- file_log << units << std::endl;
+ file_log << "#" << this->getControllerTypeString();
+ //file_log << variables << std::endl;
+ //file_log << units << std::endl;
std::cout << " Complete" << std::endl;
}
@@ -80,7 +80,7 @@ void CCrazyflie::openLogFile(char *baseFileName) {
* Write data to the log file
*/
void CCrazyflie::writeLogData() {
- // Make a new line
+ /* Make a new line
file_log << std::endl;
// Print the time
@@ -111,6 +111,12 @@ void CCrazyflie::writeLogData() {
file_log << "\t\t" << this->sensorDoubleValue("ctrlStdnt.rollRate");
file_log << "\t\t" << this->sensorDoubleValue("testStand");
+ */
+ file_log << std::endl;
+ file_log << this->currentTime();
+ for(int i = 0; i < m_tocLogs->sizeOfActiveList(); i++) {
+ file_log << "\t" << this->sensorDoubleValue(m_tocLogs->activeLogName(i));
+ }
}
@@ -166,22 +172,8 @@ bool CCrazyflie::addLoggingBlock(const char *name, uint16_t frequency) {
bool CCrazyflie::resetLoggingBlocks() {
if( ALL_THE_DEBUG ) printf( "%s\n", __FUNCTION__);
- char cPayload[1] = { 0x05 };
-
- CCRTPPacket* resetBlock = new CCRTPPacket(cPayload, 2, 1);
- resetBlock->setPort(0x05);
- resetBlock->setChannel(1);
-
- CCRTPPacket* crtpReceived = m_crRadio->sendAndReceive(resetBlock, m_nRadioChannel, this);
-
- delete resetBlock;
-
- if (crtpReceived) {
- delete crtpReceived;
- return true;
- }
-
- return false;
+ bool result = m_tocLogs->unregisterLoggingBlocks();
+ return result;
}
void CCrazyflie::removeLoggingBlock(const char *name) {
@@ -296,7 +288,7 @@ bool CCrazyflie::loadLoggingBlocksFromFile(std::string blockFileName) {
else if(!line.compare("END BLOCK")) {
enableLogging(curName);
blockLinesRead = 0;
- int curId = -1;
+ curId = -1;
curFreq = 0;
curName = "";
curEntryName = "";
@@ -305,6 +297,16 @@ bool CCrazyflie::loadLoggingBlocksFromFile(std::string blockFileName) {
else if(blockLinesRead == 1) {
try {
curId = std::stoi(line);
+ bool found = false;
+ m_tocLogs->loggingBlockForID(curId, found);
+ if(found) {
+ blockLinesRead = 0;
+ curId = -1;
+ curFreq = 0;
+ curName = "";
+ curEntryName = "";
+ continue;
+ }
}
catch(std::invalid_argument& e) {
std::cout << "Error: invalid ID. Skipping Block" << std::endl;
@@ -344,4 +346,9 @@ bool CCrazyflie::loadLoggingBlocksFromFile(std::string blockFileName) {
}
blockFile.close();
return true;
+}
+
+void CCrazyflie::printUpdatedHeader() {
+ file_log << std::endl;
+ file_log << m_tocLogs->createActiveHeader();
}
\ No newline at end of file
diff --git a/crazyflie_groundstation/src/crazyflieGroundStation.cpp b/crazyflie_groundstation/src/crazyflieGroundStation.cpp
index acf54773031a0250c67da9d145339f0cea419a96..c3f2a930fb523a7a47683886dcb80e8fa7036336 100644
--- a/crazyflie_groundstation/src/crazyflieGroundStation.cpp
+++ b/crazyflie_groundstation/src/crazyflieGroundStation.cpp
@@ -368,6 +368,7 @@ int main(int argc, char **argv) {
*/
crazyflie_info[i].cflieCopter->loadLoggingBlocksFromFile("loggingBlocks.txt");
+ crazyflie_info[i].cflieCopter->printUpdatedHeader();
crazyflie_info[i].cflieCopter->displayLoggingBlocksInitialized();