altb_pmic / Mbed OS IndNav_QK3_T265

branch for tests with T265

Dependencies:   Lib_Cntrl AHRS Lib_Misc

Threads_and_main/Data_Logger.cpp@3:bc24fee36ba3, 2019-10-28 (annotated)

Committer:
altb2
Date:
Mon Oct 28 07:54:10 2019 +0000
Revision:
3:bc24fee36ba3
Parent:
2:e7874762cc25 
- with complementFilter for Yaw (Magneteometer), Lidar, AltHold running, Flow sensor reading, control sth wrong, Yaw not tested in detail

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 3:bc24fee36ba3 36 fprintf(fp,"gyr, 1:3\r\nacc, 4:6\r\nLidar, 7\r\nOF, 8:13\r\nRPY, 14:16\r\nMxyz, 17:19\r\nFT, 20\r\nwMot, 21:24\r\ndes_z, 25\r\ndes_yaw, 26\r\nest_v, 27:29\r\nMag_raw, 30:32\r\nRP_RPY, 33:37\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 printf("THREAD LOG loop\r\n");
altb2 1:d8c9f6b16279 77 while(true){
altb2 1:d8c9f6b16279 78 thread.signal_wait(signal);
altb2 1:d8c9f6b16279 79 if(log_is_active && file_is_open)
altb2 1:d8c9f6b16279 80 {
altb2 1:d8c9f6b16279 81 mutex.lock();
altb2 1:d8c9f6b16279 82 float t_temp = ti.read();
altb2 1:d8c9f6b16279 83 fwrite(&t_temp, 4, 1, fp);
altb2 1:d8c9f6b16279 84 //fwrite(data_vector, 4, nb_cols, fp);
altb2 1:d8c9f6b16279 85 fwrite(data.sens_gyr, 4, 3, fp);
altb2 1:d8c9f6b16279 86 fwrite(data.sens_acc, 4, 3, fp);
altb2 1:d8c9f6b16279 87 fwrite(&data.sens_Lidar[0], 4, 1, fp);
altb2 2:e7874762cc25 88 fwrite(data.sens_OF, 4, 6, fp);
altb2 1:d8c9f6b16279 89 fwrite(data.est_RPY, 4, 3, fp);
altb2 1:d8c9f6b16279 90 fwrite(data.cntrl_Mxyz, 4, 3, fp);
altb2 1:d8c9f6b16279 91 fwrite(&data.F_Thrust, 4, 1, fp);
altb2 1:d8c9f6b16279 92 fwrite(data.wMot, 4, 4, fp);
altb2 2:e7874762cc25 93 fwrite(&data.cntrl_pos_xyz_des[2], 4, 1, fp);
altb2 3:bc24fee36ba3 94 fwrite(&data.cntrl_att_rpy_des[2], 4, 1, fp);
altb2 3:bc24fee36ba3 95 fwrite(data.est_Vxyz, 4, 3, fp);
altb2 3:bc24fee36ba3 96 fwrite(data.sens_mag_raw, 4, 3, fp);
altb2 2:e7874762cc25 97 fwrite(data.est_RP_RPY, 4, 5, fp);
altb2 1:d8c9f6b16279 98 mutex.unlock();
altb2 1:d8c9f6b16279 99 }
altb2 1:d8c9f6b16279 100 }
altb2 1:d8c9f6b16279 101 }
altb2 1:d8c9f6b16279 102 // ------------- close file -----------------
altb2 1:d8c9f6b16279 103 void Data_Logger::close_file(void){
altb2 1:d8c9f6b16279 104 if(log_is_active)
altb2 1:d8c9f6b16279 105 stop_logging();
altb2 1:d8c9f6b16279 106 fclose(fp);
altb2 1:d8c9f6b16279 107 }
altb2 1:d8c9f6b16279 108 // this is for realtime OS
altb2 1:d8c9f6b16279 109 void Data_Logger::sendSignal() {
altb2 1:d8c9f6b16279 110 thread.signal_set(signal);
altb2 1:d8c9f6b16279 111 }