Commit c7cb01b8 authored by Ian McInerney's avatar Ian McInerney

Added VRPN received time delta calculation and logging

parent a4901e09
......@@ -172,6 +172,10 @@ class CCrazyflie {
bool m_resetController;
// Last time a position packet was received
double lastPositionTime;
double positionTimeDelta;
// File stream for the console output and log file
std::ofstream file_console;
std::ofstream file_log;
......@@ -351,6 +355,9 @@ class CCrazyflie {
float getSetpointYaw();
uint16_t getBaseThrust();
// Return the time delta for the most recent position packet
double getPositionTimeDelta();
/**
* These functions will set the setpoints of the quadcopter
*/
......
......@@ -101,6 +101,10 @@ CCrazyflie::CCrazyflie(CCrazyRadio *crRadio, int nRadioChannel, XferRate dataRat
m_startTime = startTime;
// Variables used to compute the sampling time of position information
lastPositionTime = 0;
positionTimeDelta = 0;
// Initialize the mutexes
pthread_mutex_init( &(this->userCommandsMutex), NULL);
pthread_mutex_init( &(this->cameraDataMutex), NULL);
......
......@@ -134,3 +134,7 @@ float CCrazyflie::getSetpointYaw() {
uint16_t CCrazyflie::getBaseThrust() {
return this->m_baseThrust;
}
double CCrazyflie::getPositionTimeDelta() {
return this->positionTimeDelta;
}
\ No newline at end of file
......@@ -13,7 +13,7 @@
#include <string.h>
// Various headers for the files
static const char *variables = "%Time\t\tBattery\t\t"
static const char *variables = "%Time\t\tPositionDelta\t\tBattery\t\t"
"xSetpoint\t\tySetpoint\t\tzSetpoint\t\tyawSetpoint\t\t"
"xPosition\t\tyPosition\t\tzPosition\t\t"
"quadPitch\t\tquadRoll\t\tquadYaw\t\t\t\tcamPitch\t\tcamRoll\t\tcamYaw\t\t"
......@@ -25,7 +25,7 @@ static const char *variables = "%Time\t\tBattery\t\t"
"baseThrust\t\tcontrollerReset\t\t"
"Ut\t\tUa\t\tUe\t\tUr";
static const char *units = "&sec\t\tVolt\t\t"
static const char *units = "&sec\t\tsec\t\tVolt\t\t"
"meters\t\tmeters\t\tmeters\t\tmeters\t\t"
"meters\t\tmeters\t\tmeters\t\t"
"degrees\t\tdegrees\t\tdegrees\t\tdegrees\t\tdegrees\t\tdegrees\t\t"
......@@ -84,6 +84,9 @@ void CCrazyflie::writeLogData() {
// Print the time
file_log << this->currentTime();
// Print the time delta for the position information
file_log << "\t\t" << this->getPositionTimeDelta();
// Print the battery
file_log << "\t\t" << this->batteryLevel();
......
......@@ -104,6 +104,17 @@ void CCrazyflie::setXYZYawPosition(float xPosition, float yPosition, float zPosi
*/
void CCrazyflie::setCameraPosition(lateralPosition cameraPosition) {
pthread_mutex_lock( &(this->cameraDataMutex) );
// Compute the VRPN sample time
double currentTime = this->currentTime();
if (this->lastPositionTime == 0) {
// First sample, just ignore
this->positionTimeDelta = 0;
} else {
this->positionTimeDelta = currentTime - this->lastPositionTime;
}
this->lastPositionTime = currentTime;
this->m_currentCameraPosition = cameraPosition;
this->m_sendPosition = true;
pthread_mutex_unlock( &(this->cameraDataMutex) );
......
......@@ -244,7 +244,7 @@ void* displayData(void* id) {
// Print the loop time
cout << "Loop T\t";
for (int i = 0; i < NUM_QUADS; i++) {
printf("%1.5f", crazyflie_info[i].controllerData.timeBetweenLoop);
printf("%1.5f", crazyflie_info[i].cflieCopter->getPositionTimeDelta());
cout << "\t\t";
}
cout << endl << 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