logging file changes

crazyflie_groundstation/inc/CCrazyflie.h

@@ -313,6 +313,8 @@ class CCrazyflie {
    */
   void openLogFile(char *baseFileName);
 
+  void printUpdatedHeader();
+
   bool loadLoggingBlocksFromFile(std::string blockFileName);
 
   bool resetLoggingBlocks();

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();
 };
 
 

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

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;
 				}
 			}

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

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();