Diff: Sources/Data_Logger.cpp

Revision:

1:d8c9f6b16279

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/Data_Logger.cpp	Wed Oct 09 13:47:43 2019 +0000
@@ -0,0 +1,107 @@
+#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);
+}
\ No newline at end of file