branch for tests with T265
Dependencies: Lib_Cntrl AHRS Lib_Misc
Sources/Data_Logger.cpp
- Committer:
- altb2
- Date:
- 2019-10-09
- Revision:
- 1:d8c9f6b16279
File content as of revision 1:d8c9f6b16279:
#include "Data_Logger.h" using namespace std; // Class Data logger // ----------------------------------------------------------------------------- Data_Logger::Data_Logger(uint8_t nb, float Ts) : sd(PC_12, PC_11, PC_10, PD_2),fs("sd"),thread(osPriorityBelowNormal, 4096) { sd.init(); // init SD-card fs.mount(&sd); // mount drive ti.reset(); this->Ts = Ts; fp = fopen("/sd/copter_QK2/number.txt", "r"); // read ff number for next log log_number = 0; if (fp != NULL) { fscanf(fp,"%d",&log_number); fclose(fp); fp = fopen("/sd/copter_QK2/number.txt", "w"); fprintf(fp,"%d\r\n",++log_number); // write incremented number fclose(fp); } else { printf("\r\nfailed to open logging number file!\r\n"); } //----------------------------------------------- printf("Log Data on log_%d.txt\r\n",log_number); char str[30]; //sprintf(str, "/sd/copter_altb1/log_%d.txt", log_number); // Write Textfile for description: sprintf(str, "/sd/copter_QK2/log_%d.txt", log_number); fp = fopen(str, "w"); if(fp == NULL) { error("Could not open file for write\r\n"); file_is_open = false; } else{ //fprintf(fp,"gyr, 1:3\r\nEKF, 4:7\r\nOptFl, 8:11\r\nLidar, 12\r\nMxyz, 14:16\r\nFT, 17\r\nwMot, 18:21\r\nUSens, 22\r\nASens, 23\r\nCS, 24\r\ndt, 25\r\nRPYdes, 26:28\r\n"); // Description fprintf(fp,"gyr, 1:3\r\nacc, 4:6\r\nLidar, 7\r\nOF, 8:11\r\nRPY, 12:14\r\nMxyz, 15:17\r\nFT, 18\r\nwMot, 19:22\r\n"); // Description fclose(fp); sprintf(str, "/sd/copter_QK2/log_%d.bin", log_number); fp = fopen(str, "wb"); file_is_open = true; fwrite(&nb, 1, 1, fp); // write number of columns once!!! } this->nb_cols = nb; } // ------------------- start logging ---------------- void Data_Logger::start_logging(void){ ti.reset(); ti.start(); pc.printf("DataLogger Thread started\r\n"); if(file_is_open){ log_is_active = true; thread.start(callback(this, &Data_Logger::write_line)); ticker.attach(callback(this, &Data_Logger::sendSignal), Ts); } } // ------------------- stop logging ---------------- void Data_Logger::stop_logging(void){ log_is_active = false; ticker.detach(); thread.terminate(); } // ------------------- stop logging ---------------- void Data_Logger::pause_logging(void){ log_is_active = false; } // ------------------- stop logging ---------------- void Data_Logger::continue_logging(void){ log_is_active = true; } // ------------------- destructor ----------------- Data_Logger::~Data_Logger() { ticker.detach(); } // ------------------ write line ------------- void Data_Logger::write_line(void){ uint8_t kk=0; printf("THREAD LOG loop\r\n"); while(true){ thread.signal_wait(signal); if(log_is_active && file_is_open) { mutex.lock(); float t_temp = ti.read(); fwrite(&t_temp, 4, 1, fp); //fwrite(data_vector, 4, nb_cols, fp); fwrite(data.sens_gyr, 4, 3, fp); fwrite(data.sens_acc, 4, 3, fp); fwrite(&data.sens_Lidar[0], 4, 1, fp); fwrite(data.sens_OF, 4, 4, fp); fwrite(data.est_RPY, 4, 3, fp); fwrite(data.cntrl_Mxyz, 4, 3, fp); fwrite(&data.F_Thrust, 4, 1, fp); fwrite(data.wMot, 4, 4, fp); mutex.unlock(); } } } // ------------- close file ----------------- void Data_Logger::close_file(void){ if(log_is_active) stop_logging(); fclose(fp); } // this is for realtime OS void Data_Logger::sendSignal() { thread.signal_set(signal); }