changed trajectory read-in

boards/px4/fmu-v5/default.cmake

@@ -67,6 +67,7 @@ px4_add_board(
 		events
 		flight_mode_manager
 		fw_att_control
+		fw_dyn_soar_control
 		fw_pos_control_l1
 		gyro_calibration
 		gyro_fft

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp

@@ -76,7 +76,13 @@ FixedwingPositionINDIControl::init()
 		PX4_ERR("vehicle position callback registration failed!");
 		return false;
 	}
-    _read_trajectory_coeffs_csv("trajectory0.csv");
+    //_read_trajectory_coeffs_csv("trajectory5.csv");
+    //_read_trajectory_coeffs_csv("trajectory4.csv");
+    //_read_trajectory_coeffs_csv("trajectory3.csv");
+    //_read_trajectory_coeffs_csv("trajectory2.csv");
+    //_read_trajectory_coeffs_csv("trajectory1.csv");
+    char filename[] = "trajectory0.csv";
+    _read_trajectory_coeffs_csv(filename);
 
     // initialize transformations
     _R_ned_to_enu *= 0.f;
@@ -342,36 +348,47 @@ void
 FixedwingPositionINDIControl::_select_trajectory(float initial_energy)
 {
     // select loiter trajectory for loiter test
-    if (_loiter==0) {
-        _read_trajectory_coeffs_csv("trajectory0.csv");
-    }
-    else if (_loiter==1)  {
-        _read_trajectory_coeffs_csv("trajectory1.csv");
-    }
-    else if (_loiter==2)  {
-        _read_trajectory_coeffs_csv("trajectory2.csv");
-    }
-    else if (_loiter==3)  {
-        _read_trajectory_coeffs_csv("trajectory3.csv");
-    }
-    else if (_loiter==4)  {
-        _read_trajectory_coeffs_csv("trajectory4.csv");
-    }
-    else{
-        _read_trajectory_coeffs_csv("trajectory5.csv");
+    char filename[16];
+    switch (_loiter)
+    {
+    case 0:
+        strcpy(filename,"trajectory0.csv");
+        break;
+    case 1:
+        strcpy(filename,"trajectory1.csv");
+        break;
+    case 2:
+        strcpy(filename,"trajectory2.csv");
+        break;
+    case 3:
+        strcpy(filename,"trajectory3.csv");
+        break;
+    case 4:
+        strcpy(filename,"trajectory4.csv");
+        break;
+    case 5:
+        strcpy(filename,"trajectory5.csv");
+        break;
+
+    default:
+        strcpy(filename,"trajectory0.csv");
     }
+    
+
+    _read_trajectory_coeffs_csv(filename);
 }
 
 void
-FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(string filename)
+FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename)
 {
+    /*
     // File pointer
     std::ifstream fin;
   
     // Open an existing file for testing
-    fin.open("/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/"+filename, ios::in);
+    //fin.open("/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/"+filename, ios::in);
     // Open an existing file from SD card
-    //fin.open("/fs/microsd/trajectories/"+filename);
+    fin.open(PX4_ROOTFSDIR"/fs/microsd/trajectories/"+filename);
   
     // Read the Data from the file
     // as String Vector
@@ -419,6 +436,63 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(string filename)
         }      
     }
     fin.close();
+    */
+
+    // =======================================================================
+    bool error = false;
+
+    char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/";
+    strcat(home_dir,filename);
+    FILE* fp = fopen(home_dir, "r");
+
+    if (!fp) {
+        PX4_ERR("Can't open file");
+        error = true;
+    }
+    else {
+        // Here we have taken size of
+        // array 1024 you can modify it
+        const uint buffersize = 64*_num_basis_funs;
+        char buffer[buffersize];
+
+        int row = 0;
+        int column = 0;
+
+        // loop over rows
+        while (fgets(buffer,
+                     buffersize, fp)) {
+            column = 0;
+ 
+            // Splitting the data
+            char* value = strtok(buffer, ",");
+ 
+            // loop over columns
+            while (value) {
+
+                switch(row){
+                    case 0:
+                        _basis_coeffs_x(column) = (float)atof(value);
+                        break;
+                    case 1:
+                        _basis_coeffs_y(column) = (float)atof(value);
+                        break;
+                    case 2:
+                        _basis_coeffs_z(column) = (float)atof(value);
+                        break;
+
+                    default:
+                        break;
+                }
+                value = strtok(NULL, ",");
+                column++;
+                
+            }
+            row++;
+        }
+    }
+    fclose(fp);
+    // =======================================================================
+
 
     // go back to safety mode loiter circle
     if(error){

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp

@@ -12,10 +12,17 @@
 #define FIXEDWINGPOSITIONINDICONTROL_HPP_
 
 #include <float.h>
-
+/*
+#include <iostream>
+#include <fstream>
 #include <sstream>
 #include <vector>
 #include <array>
+*/
+
+#include <stdio.h>
+#include <string.h>
+
 #include <drivers/drv_hrt.h>
 #include "fw_att_control/ecl_pitch_controller.h"
 #include "fw_att_control/ecl_roll_controller.h"
@@ -69,8 +76,7 @@
 #include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/wind.h>
 #include <uORB/uORB.h>
-#include <iostream>
-#include <fstream>
+
 
 
 using namespace time_literals;
@@ -248,7 +254,7 @@ private:
 
 	// controller methods
 	void _select_trajectory(float initial_energy);				// select the correct trajectory based on available energy
-	void _read_trajectory_coeffs_csv(std::string filename);				// read in the correct coefficients of the appropriate trajectory
+	void _read_trajectory_coeffs_csv(char *filename);				// read in the correct coefficients of the appropriate trajectory
 	void _set_wind_estimate(Vector3f wind);
 	float _get_closest_t(Vector3f pos);				// get the normalized time, at which the reference path is closest to the current position
 	Vector<float, _num_basis_funs> _get_basis_funs(float t=0);			// compute the vector of basis functions at normalized time t in [0,1]