From d91cd653536ecb131402833b26c79b6686ac25ac Mon Sep 17 00:00:00 2001
From: zeisele <zeisele@iastate.edu>
Date: Mon, 28 Mar 2022 20:43:28 +0200
Subject: [PATCH] log nan if value has not been updated recently
---
crazyflie_groundstation/inc/CTOC.h | 2 ++
crazyflie_groundstation/src/CTOC.cpp | 24 ++++++++++++++++++-
.../ccrazyflie/CCrazyflie_loggingFuncs.cpp | 13 ++++++++--
3 files changed, 36 insertions(+), 3 deletions(-)
diff --git a/crazyflie_groundstation/inc/CTOC.h b/crazyflie_groundstation/inc/CTOC.h
index b326be5e8..c07aed490 100644
--- a/crazyflie_groundstation/inc/CTOC.h
+++ b/crazyflie_groundstation/inc/CTOC.h
@@ -60,6 +60,7 @@ struct TOCElement {
std::string strIdentifier;
bool bIsLogging;
double dValue;
+ double timeLastUpdated;
};
@@ -140,6 +141,7 @@ class CTOC {
int elementIDinBlock(int nBlockID, int nElementIndex);
bool setFloatValueForElementID(int nElementID, float fValue);
+ bool setLastUpdateForElementID(int nElementID);
bool addElementToBlock(int nBlockID, int nElementID);
bool unregisterLoggingBlockID(int nID);
diff --git a/crazyflie_groundstation/src/CTOC.cpp b/crazyflie_groundstation/src/CTOC.cpp
index 4e484aad8..039b0d951 100644
--- a/crazyflie_groundstation/src/CTOC.cpp
+++ b/crazyflie_groundstation/src/CTOC.cpp
@@ -216,7 +216,8 @@ bool CTOC::processItem(CCRTPPacket* crtpItem) {
teNew.nID = nID;
teNew.nType = nType;
teNew.bIsLogging = false;
- teNew.dValue = 1;
+ teNew.dValue = 0;
+ teNew.timeLastUpdated = 0;
if(ALL_THE_DEBUG) printf("Group.Name: %s.%s ID: %i Type: %i ", strGroup.c_str(), strIdentifier.c_str(), nID, nType);
@@ -377,6 +378,7 @@ bool CTOC::processNextItem(CCRTPPacket* crtpItem) {
teNew.nType = nType;
teNew.bIsLogging = false;
teNew.dValue = 0;
+ teNew.timeLastUpdated = 0;
m_lstTOCElements.push_back(teNew);
@@ -935,6 +937,7 @@ void CTOC::processPackets(CCRTPPacket *crtpPacket) {
}
this->setFloatValueForElementID(nElementID, fValue);
+ this->setLastUpdateForElementID(nElementID);
nOffset += nByteLength;
nIndex++;
} else {
@@ -990,6 +993,25 @@ bool CTOC::setFloatValueForElementID(int nElementID, float fValue) {
return false;
}
+bool CTOC::setLastUpdateForElementID(int nElementID) {
+ if( ALL_THE_DEBUG ) printf( "%s\n", __FUNCTION__);
+ int nIndex = 0;
+ for (std::list<struct TOCElement>::iterator itElement =
+ m_lstTOCElements.begin(); itElement != m_lstTOCElements.end();
+ itElement++, nIndex++) {
+ struct TOCElement teCurrent = *itElement;
+
+ if (teCurrent.nID == nElementID) {
+ teCurrent.timeLastUpdated = m_crazyflie->currentTime(); // We store floats as doubles
+ (*itElement) = teCurrent;
+ // std::cout << fValue << std::endl;
+ return true;
+ }
+ }
+
+ return false;
+}
+
/*
* Open a log file for the quadcopter
*
diff --git a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_loggingFuncs.cpp b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_loggingFuncs.cpp
index 32fcc188a..d0caf4141 100644
--- a/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_loggingFuncs.cpp
+++ b/crazyflie_groundstation/src/ccrazyflie/CCrazyflie_loggingFuncs.cpp
@@ -112,10 +112,19 @@ void CCrazyflie::writeLogData() {
file_log << "\t\t" << this->sensorDoubleValue("testStand");
*/
+ bool bFound;
+ TOCElement response;
file_log << std::endl;
- file_log << this->currentTime();
+ double currentTime = this->currentTime();
+ file_log << currentTime;
for(int i = 0; i < m_tocLogs->sizeOfActiveList(); i++) {
- file_log << "\t" << this->sensorDoubleValue(m_tocLogs->activeLogName(i));
+ response = m_tocLogs->elementForName(m_tocLogs->activeLogName(i), bFound);
+ if(response.timeLastUpdated > currentTime - 0.01) {
+ file_log << "\t" << this->sensorDoubleValue(m_tocLogs->activeLogName(i));
+ }
+ else {
+ file_log << "\t" << std::numeric_limits<double>::quiet_NaN();
+ }
}
file_log << "\t" << this->sensorDoubleValue("testStand");
}
--
GitLab