altb_pmic / Mbed OS IndNav_QK3_T265

branch for tests with T265

Dependencies:   Lib_Cntrl AHRS Lib_Misc

Sources/Data_Logger.cpp@1:d8c9f6b16279, 2019-10-09 (annotated)

Committer:
altb2
Date:
Wed Oct 09 13:47:43 2019 +0000
Revision:
1:d8c9f6b16279
Newly designed CopterCode Okt 2019, based on Quadcopt_QK2, this Version flies in Stabilized mode, Alt hold not tested yet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 1:d8c9f6b16279 1 #include "Data_Logger.h"
altb2 1:d8c9f6b16279 2 using namespace std;
altb2 1:d8c9f6b16279 3
altb2 1:d8c9f6b16279 4 // Class Data logger
altb2 1:d8c9f6b16279 5 // -----------------------------------------------------------------------------
altb2 1:d8c9f6b16279 6 Data_Logger::Data_Logger(uint8_t nb, float Ts) : sd(PC_12, PC_11, PC_10, PD_2),fs("sd"),thread(osPriorityBelowNormal, 4096)
altb2 1:d8c9f6b16279 7 {
altb2 1:d8c9f6b16279 8 sd.init(); // init SD-card
altb2 1:d8c9f6b16279 9 fs.mount(&sd); // mount drive
altb2 1:d8c9f6b16279 10 ti.reset();
altb2 1:d8c9f6b16279 11 this->Ts = Ts;
altb2 1:d8c9f6b16279 12 fp = fopen("/sd/copter_QK2/number.txt", "r"); // read ff number for next log
altb2 1:d8c9f6b16279 13 log_number = 0;
altb2 1:d8c9f6b16279 14 if (fp != NULL) {
altb2 1:d8c9f6b16279 15 fscanf(fp,"%d",&log_number);
altb2 1:d8c9f6b16279 16 fclose(fp);
altb2 1:d8c9f6b16279 17 fp = fopen("/sd/copter_QK2/number.txt", "w");
altb2 1:d8c9f6b16279 18 fprintf(fp,"%d\r\n",++log_number); // write incremented number
altb2 1:d8c9f6b16279 19 fclose(fp);
altb2 1:d8c9f6b16279 20 } else {
altb2 1:d8c9f6b16279 21 printf("\r\nfailed to open logging number file!\r\n");
altb2 1:d8c9f6b16279 22 }
altb2 1:d8c9f6b16279 23 //-----------------------------------------------
altb2 1:d8c9f6b16279 24 printf("Log Data on log_%d.txt\r\n",log_number);
altb2 1:d8c9f6b16279 25 char str[30];
altb2 1:d8c9f6b16279 26 //sprintf(str, "/sd/copter_altb1/log_%d.txt", log_number);
altb2 1:d8c9f6b16279 27 // Write Textfile for description:
altb2 1:d8c9f6b16279 28 sprintf(str, "/sd/copter_QK2/log_%d.txt", log_number);
altb2 1:d8c9f6b16279 29 fp = fopen(str, "w");
altb2 1:d8c9f6b16279 30 if(fp == NULL) {
altb2 1:d8c9f6b16279 31 error("Could not open file for write\r\n");
altb2 1:d8c9f6b16279 32 file_is_open = false;
altb2 1:d8c9f6b16279 33 }
altb2 1:d8c9f6b16279 34 else{
altb2 1:d8c9f6b16279 35 //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
altb2 1:d8c9f6b16279 36 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
altb2 1:d8c9f6b16279 37 fclose(fp);
altb2 1:d8c9f6b16279 38 sprintf(str, "/sd/copter_QK2/log_%d.bin", log_number);
altb2 1:d8c9f6b16279 39 fp = fopen(str, "wb");
altb2 1:d8c9f6b16279 40 file_is_open = true;
altb2 1:d8c9f6b16279 41 fwrite(&nb, 1, 1, fp); // write number of columns once!!!
altb2 1:d8c9f6b16279 42 }
altb2 1:d8c9f6b16279 43 this->nb_cols = nb;
altb2 1:d8c9f6b16279 44 }
altb2 1:d8c9f6b16279 45 // ------------------- start logging ----------------
altb2 1:d8c9f6b16279 46 void Data_Logger::start_logging(void){
altb2 1:d8c9f6b16279 47 ti.reset();
altb2 1:d8c9f6b16279 48 ti.start();
altb2 1:d8c9f6b16279 49 pc.printf("DataLogger Thread started\r\n");
altb2 1:d8c9f6b16279 50 if(file_is_open){
altb2 1:d8c9f6b16279 51 log_is_active = true;
altb2 1:d8c9f6b16279 52 thread.start(callback(this, &Data_Logger::write_line));
altb2 1:d8c9f6b16279 53 ticker.attach(callback(this, &Data_Logger::sendSignal), Ts);
altb2 1:d8c9f6b16279 54 }
altb2 1:d8c9f6b16279 55 }
altb2 1:d8c9f6b16279 56 // ------------------- stop logging ----------------
altb2 1:d8c9f6b16279 57 void Data_Logger::stop_logging(void){
altb2 1:d8c9f6b16279 58 log_is_active = false;
altb2 1:d8c9f6b16279 59 ticker.detach();
altb2 1:d8c9f6b16279 60 thread.terminate();
altb2 1:d8c9f6b16279 61 }
altb2 1:d8c9f6b16279 62 // ------------------- stop logging ----------------
altb2 1:d8c9f6b16279 63 void Data_Logger::pause_logging(void){
altb2 1:d8c9f6b16279 64 log_is_active = false;
altb2 1:d8c9f6b16279 65 }
altb2 1:d8c9f6b16279 66 // ------------------- stop logging ----------------
altb2 1:d8c9f6b16279 67 void Data_Logger::continue_logging(void){
altb2 1:d8c9f6b16279 68 log_is_active = true;
altb2 1:d8c9f6b16279 69 }
altb2 1:d8c9f6b16279 70 // ------------------- destructor -----------------
altb2 1:d8c9f6b16279 71 Data_Logger::~Data_Logger() {
altb2 1:d8c9f6b16279 72 ticker.detach();
altb2 1:d8c9f6b16279 73 }
altb2 1:d8c9f6b16279 74 // ------------------ write line -------------
altb2 1:d8c9f6b16279 75 void Data_Logger::write_line(void){
altb2 1:d8c9f6b16279 76 uint8_t kk=0;
altb2 1:d8c9f6b16279 77 printf("THREAD LOG loop\r\n");
altb2 1:d8c9f6b16279 78 while(true){
altb2 1:d8c9f6b16279 79 thread.signal_wait(signal);
altb2 1:d8c9f6b16279 80 if(log_is_active && file_is_open)
altb2 1:d8c9f6b16279 81 {
altb2 1:d8c9f6b16279 82 mutex.lock();
altb2 1:d8c9f6b16279 83 float t_temp = ti.read();
altb2 1:d8c9f6b16279 84 fwrite(&t_temp, 4, 1, fp);
altb2 1:d8c9f6b16279 85 //fwrite(data_vector, 4, nb_cols, fp);
altb2 1:d8c9f6b16279 86 fwrite(data.sens_gyr, 4, 3, fp);
altb2 1:d8c9f6b16279 87 fwrite(data.sens_acc, 4, 3, fp);
altb2 1:d8c9f6b16279 88 fwrite(&data.sens_Lidar[0], 4, 1, fp);
altb2 1:d8c9f6b16279 89 fwrite(data.sens_OF, 4, 4, fp);
altb2 1:d8c9f6b16279 90 fwrite(data.est_RPY, 4, 3, fp);
altb2 1:d8c9f6b16279 91 fwrite(data.cntrl_Mxyz, 4, 3, fp);
altb2 1:d8c9f6b16279 92 fwrite(&data.F_Thrust, 4, 1, fp);
altb2 1:d8c9f6b16279 93 fwrite(data.wMot, 4, 4, fp);
altb2 1:d8c9f6b16279 94 mutex.unlock();
altb2 1:d8c9f6b16279 95 }
altb2 1:d8c9f6b16279 96 }
altb2 1:d8c9f6b16279 97 }
altb2 1:d8c9f6b16279 98 // ------------- close file -----------------
altb2 1:d8c9f6b16279 99 void Data_Logger::close_file(void){
altb2 1:d8c9f6b16279 100 if(log_is_active)
altb2 1:d8c9f6b16279 101 stop_logging();
altb2 1:d8c9f6b16279 102 fclose(fp);
altb2 1:d8c9f6b16279 103 }
altb2 1:d8c9f6b16279 104 // this is for realtime OS
altb2 1:d8c9f6b16279 105 void Data_Logger::sendSignal() {
altb2 1:d8c9f6b16279 106 thread.signal_set(signal);
altb2 1:d8c9f6b16279 107 }