Commit f6733668 authored by Ian McInerney's avatar Ian McInerney

Moved logging functions into the CCrazyflie class

parent 7a36760c
......@@ -41,6 +41,7 @@
#include <stdbool.h>
#include <iostream>
#include <fstream>
#include <string.h>
// Private
#include "CCrazyRadio.h"
......@@ -170,8 +171,9 @@ class CCrazyflie {
bool m_resetController;
// File stream for the console output
// File stream for the console output and log file
std::ofstream file_console;
std::ofstream file_log;
// Controller type
int controllerType;
......@@ -246,6 +248,18 @@ class CCrazyflie {
CCrazyRadio radio instance given in the constructor). */
~CCrazyflie();
/*
* Open a log file for the quadcopter
*
* @param baseFileName The base filename for the logfile
*/
void openLogFile(char *baseFileName);
/*
* Write data to the log file
*/
void writeLogData();
/*
* Callback called whenever there is a new VRPN position packet.
* Note: This function is static in the class.
......
#ifndef LOGDATA_H_
#define LOGDATA_H_
#include "stdint.h"
/*
* Open a log file for a quadcopter
*
* @param quadNum The quadcopter to make the file for
*/
void logging_OpenLog( uint8_t quadNum);
/*
* Write data to the log file
*
* @param quadNum The quadcopter to log data for
*/
void logging_WriteData( uint8_t quadNum);
/*
* Close the log file
*
* @param quadNum The quadcopter to close the file for
*/
void logging_CloseLog( uint8_t quadNum);
#endif
\ No newline at end of file
......@@ -3,7 +3,6 @@
#include "callbacks.h"
#include "vrpn_Tracker.h"
#include <quat.h>
#include "logData.h"
#include "helperFunctions.h"
......
......@@ -109,9 +109,22 @@ CCrazyflie::CCrazyflie(CCrazyRadio *crRadio, int nRadioChannel, int quadNum, dou
}
this->queryControllerType();
// Open the log file
char fileName[255];
sprintf(fileName, "logs/cflie%d", m_quadNum+1);
this->openLogFile(fileName);
}
CCrazyflie::~CCrazyflie() {
if (file_log != NULL) {
file_log.close();
}
if (file_console != NULL) {
file_console.close();
}
delete m_tocParameters;
delete m_tocLogs;
}
......
......@@ -6,6 +6,142 @@
#include "CCrazyflie.h"
#include "CTOC.h"
#include <time.h>
#include <iostream>
#include <stdio.h>
#include <errno.h>
#include <string.h>
// Various headers for the files
static const char *variables = "%Time\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"
"gyroX\t\tgyroY\t\tgyroZ\t\taccelX\t\taccelY\t\taccelZ\t\t"
"motor1\t\tmotor2\t\tmotor3\t\tmotor4\t\t"
"xOut\t\tyOut\t\tzOut\t\t"
"Pitch_Out\t\tRoll_Out\t\tYaw_Out\t\t"
"PitchRate_Out\t\tRollRate_Out\t\tYawRate_Out\t\t"
"baseThrust\t\tcontrollerReset\t\t"
"Ut\t\tUa\t\tUe\t\tUr";
static const char *units = "&sec\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"
"deg\\sec\t\tdeg\\sec\t\tdeg\\sec\t\tg\t\tg\t\tg\t\t"
"raw\t\traw\t\traw\t\traw\t\t"
"raw\t\traw\t\traw\t\t"
"raw\t\traw\t\traw\t\t"
"raw\t\traw\t\traw\t\t"
"raw\t\tboolean\t\t"
"raw\t\traw\t\traw\t\traw";
/*
* Open a log file for the quadcopter
*
* @param baseFileName The base filename for the logfile
*/
void CCrazyflie::openLogFile(char *baseFileName) {
char logFileName[40];
char timeString[40];
time_t rawtime;
time( &rawtime );
struct tm *timeinfo = localtime( &rawtime );
strftime(timeString, 40, "%Y_%m_%d_%H:%M:%S", timeinfo);
// Create the log file name (including date)
sprintf(logFileName, "%s_%s.txt", baseFileName, timeString);
printf("Opening log file %s for quadcopter %d...", logFileName, (m_quadNum + 1));
try {
file_log.open(logFileName, std::ios_base::out);
} catch (...) {
std::cout << "Error opening logfile for quadcopter " << m_quadNum;
exit(-1);
}
// Place the header information into the log file
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;
std::cout << " Complete" << std::endl;
}
/*
* Write data to the log file
*/
void CCrazyflie::writeLogData() {
// Make a new line
file_log << std::endl;
// Print the time
file_log << this->currentTime();
// Print the battery
file_log << "\t\t" << this->batteryLevel();
// Print the setpoints
file_log << "\t\t" << this->getSetpointX();
file_log << "\t\t" << this->getSetpointY();
file_log << "\t\t" << this->getSetpointZ();
file_log << "\t\t" << this->getSetpointYaw();
// Print the current position
file_log << "\t\t" << this->getCameraX();
file_log << "\t\t" << this->getCameraY();
file_log << "\t\t" << this->getCameraZ();
// Print the angles
file_log << "\t\t" << this->getQuadPitch();
file_log << "\t\t" << this->getQuadRoll();
file_log << "\t\t" << this->getQuadYaw();
file_log << "\t\t" << this->getCameraPitch();
file_log << "\t\t" << this->getCameraRoll();
file_log << "\t\t" << this->getCameraYaw();
// Print the sensor data
file_log << "\t\t" << this->sensorDoubleValue("gyro.x");
file_log << "\t\t" << this->sensorDoubleValue("gyro.y");
file_log << "\t\t" << this->sensorDoubleValue("gyro.z");
file_log << "\t\t" << this->sensorDoubleValue("accel.x");
file_log << "\t\t" << this->sensorDoubleValue("accel.y");
file_log << "\t\t" << this->sensorDoubleValue("accel.z");
file_log << "\t\t" << this->sensorDoubleValue("motor.m1");
file_log << "\t\t" << this->sensorDoubleValue("motor.m2");
file_log << "\t\t" << this->sensorDoubleValue("motor.m3");
file_log << "\t\t" << this->sensorDoubleValue("motor.m4");
file_log << "\t\t" << this->sensorDoubleValue("posCtlAlt.outz");
file_log << "\t\t" << this->sensorDoubleValue("posCtlAlt.outy");
file_log << "\t\t" << this->sensorDoubleValue("posCtlAlt.outz");
file_log << "\t\t" << this->sensorDoubleValue("pitchPID.output");
file_log << "\t\t" << this->sensorDoubleValue("rollPID.output");
file_log << "\t\t" << this->sensorDoubleValue("yawPID.output");
file_log << "\t\t" << this->sensorDoubleValue("pitchRatePID.output");
file_log << "\t\t" << this->sensorDoubleValue("rollRatePID.output");
file_log << "\t\t" << this->sensorDoubleValue("yawRatePID.output");
file_log << "\t\t" << this->getBaseThrust();
file_log << "\t\t" << "0";//quad->controllerData.resetController;
file_log << "\t\t" << this->sensorDoubleValue("mixer.ctr_thrust");
file_log << "\t\t" << this->sensorDoubleValue("mixer.ctr_roll");
file_log << "\t\t" << this->sensorDoubleValue("mixer.ctr_pitch");
file_log << "\t\t" << this->sensorDoubleValue("mixer.ctr_yaw");
}
bool CCrazyflie::readTOCParameters() {
if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__ );
......
......@@ -4,7 +4,6 @@
#include "errno.h"
#include "userInput.h"
#include "userOutput.h"
#include "logData.h"
#include "Bridge.h"
#include "computations/NetworkForwarding.h"
......@@ -300,10 +299,6 @@ int main(int argc, char **argv) {
fprintf(hand.logfile, "%s\n", logHeaderHand);
#endif // END USE_HAND
#endif // END USE_LOGGING
// Initialize the logging structure
for (int i = 0; i < NUM_QUADS; i++) {
logging_OpenLog(i);
}
//===========End Logfile Init==============
......
#include "logData.h"
#include "quadcopterData.h"
#include <time.h>
#include <iostream>
#include <stdio.h>
#include <errno.h>
using namespace std;
std::ofstream logfiles[NUM_QUADS];
// Various headers for the files
static const char *variables = "%Time\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"
"gyroX\t\tgyroY\t\tgyroZ\t\taccelX\t\taccelY\t\taccelZ\t\t"
"motor1\t\tmotor2\t\tmotor3\t\tmotor4\t\t"
"xOut\t\tyOut\t\tzOut\t\t"
"Pitch_Out\t\tRoll_Out\t\tYaw_Out\t\t"
"PitchRate_Out\t\tRollRate_Out\t\tYawRate_Out\t\t"
"baseThrust\t\tcontrollerReset\t\t"
"Ut\t\tUa\t\tUe\t\tUr";
static const char *units = "&sec\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"
"deg\\sec\t\tdeg\\sec\t\tdeg\\sec\t\tg\t\tg\t\tg\t\t"
"raw\t\traw\t\traw\t\traw\t\t"
"raw\t\traw\t\traw\t\t"
"raw\t\traw\t\traw\t\t"
"raw\t\traw\t\traw\t\t"
"raw\t\tboolean\t\t"
"raw\t\traw\t\traw\t\traw";
/*
* Open a log file for a quadcopter
*
* @param quadNum The quadcopter to make the file for
*/
void logging_OpenLog( uint8_t quadNum) {
char logFileName[40];
char timeString[40];
time_t rawtime;
time( &rawtime );
struct tm *timeinfo = localtime( &rawtime );
strftime(timeString, 40, "%Y_%m_%d_%H:%M:%S", timeinfo);
// Create the log file name (including date)
sprintf(logFileName, "%s_%s.txt", crazyflie_info[quadNum].baselogfileName, timeString);
printf("Opening log file %s for quadcopter %d...", logFileName, (quadNum + 1));
try {
logfiles[quadNum].open(logFileName, ios_base::out);
} catch (...) {
cout << "Error opening logfile for quadcopter " << quadNum;
exit(-1);
}
// Place the header information into the log file
logfiles[quadNum] << "#Crazyflie" << endl;
// Add the controller type to the logfile
logfiles[quadNum] << "#" << crazyflie_info[quadNum].cflieCopter->getControllerTypeString() << endl;
logfiles[quadNum] << variables << endl;
logfiles[quadNum] << units << endl;
cout << " Complete" << endl;
}
/*
* Write data to the log file
*
* @param quadNum The quadcopter to log data for
*/
void logging_WriteData( uint8_t quadNum) {
ofstream *logfile = &logfiles[quadNum];
QUADCOPTERS_t *quad = &crazyflie_info[quadNum];
CCrazyflie *cflieCopter = crazyflie_info[quadNum].cflieCopter;
// Make a new line
*logfile << endl;
// Print the time
*logfile << cflieCopter->currentTime() << "\t\t";
// Print the battery
*logfile << "\t\t" << cflieCopter->batteryLevel();
// Print the setpoints
*logfile << "\t\t" << cflieCopter->getSetpointX();
*logfile << "\t\t" << cflieCopter->getSetpointY();
*logfile << "\t\t" << cflieCopter->getSetpointZ();
*logfile << "\t\t" << cflieCopter->getSetpointYaw();
// Print the current position
*logfile << "\t\t" << cflieCopter->getCameraX();
*logfile << "\t\t" << cflieCopter->getCameraY();
*logfile << "\t\t" << cflieCopter->getCameraZ();
// Print the angles
*logfile << "\t\t" << cflieCopter->getQuadPitch();
*logfile << "\t\t" << cflieCopter->getQuadRoll();
*logfile << "\t\t" << cflieCopter->getQuadYaw();
*logfile << "\t\t" << cflieCopter->getCameraPitch();
*logfile << "\t\t" << cflieCopter->getCameraRoll();
*logfile << "\t\t" << cflieCopter->getCameraYaw();
// Print the sensor data
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("gyro.x");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("gyro.y");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("gyro.z");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("accel.x");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("accel.y");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("accel.z");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("motor.m1");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("motor.m2");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("motor.m3");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("motor.m4");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("posCtlAlt.outz");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("posCtlAlt.outy");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("posCtlAlt.outz");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("pitchPID.output");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("rollPID.output");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("yawPID.output");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("pitchRatePID.output");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("rollRatePID.output");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("yawRatePID.output");
*logfile << "\t\t" << cflieCopter->getBaseThrust();
*logfile << "\t\t" << quad->controllerData.resetController;
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("mixer.ctr_thrust");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("mixer.ctr_roll");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("mixer.ctr_pitch");
*logfile << "\t\t" << cflieCopter->sensorDoubleValue("mixer.ctr_yaw");
}
/*
* Close the log file
*
* @param quadNum The quadcopter to close the file for
*/
void logging_CloseLog( uint8_t quadNum) {
logfiles[quadNum].close();
}
\ No newline at end of file
......@@ -7,7 +7,6 @@
#include "callbacks.h"
#include "userInput.h"
#include "userOutput.h"
#include "logData.h"
#include <iostream>
using namespace std;
......@@ -108,6 +107,9 @@ void* vrpn_go(void *threadID) {
// 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
......@@ -136,11 +138,6 @@ void* vrpn_go(void *threadID) {
return(NULL);
}
//
for (int j = 0; j < NUM_QUADS; j++) {
logging_WriteData(i);
}
}//END WHILE
loopTimeTotalEnd = crazyflie_info[0].cflieCopter->currentTime() - crazyflie_info[0].initTime;
......
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