added trajectory loader

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp

@@ -76,7 +76,7 @@ FixedwingPositionINDIControl::init()
 		PX4_ERR("vehicle position callback registration failed!");
 		return false;
 	}
-    _read_trajectory_coeffs_csv();
+    _read_trajectory_coeffs_csv("trajectory0.csv");
 
     // initialize transformations
     _R_ned_to_enu *= 0.f;
@@ -314,71 +314,117 @@ FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind)
 }
 
 void
-FixedwingPositionINDIControl::_read_trajectory_coeffs_csv()
+FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(string filename)
 {
-        /*
-        std::ifstream file_reader;
-        std::string line;
-        std::vector<float> vars;
-        file_reader.open(model_input_mean_path_, std::ifstream::in);
-        for (int i = 0; i < _num_basis_funs; i++){
-            file_reader >> line;
-            _basis_coeffs_x(i) = std::stof(line);
+    // File pointer
+    std::ifstream fin;
+  
+    // Open an existing file
+    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("/fs/microsd/trajectories/"+filename);
+  
+    // Read the Data from the file
+    // as String Vector
+    string line, word;
+
+    bool error = false;
+    
+    // loop over x, y, z
+    for (int l=0; l<3; l++) {
+        getline(fin, line, '\n');
+
+        // used for breaking words
+        stringstream s(line);
+
+        // read every column data of a row and
+        // store it in a string variable, 'word'
+        uint i = 0;
+        while (getline(s, word, ',')) {
+            // catch error
+            if(i>=_num_basis_funs){
+                PX4_ERR("number of coefficients is too large.");
+                error = true;
+            }
+            else{
+                switch(l){
+                    case 0:
+                        _basis_coeffs_x(i) = stof(word);
+                        break;
+                    case 1:
+                        _basis_coeffs_y(i) = stof(word);
+                        break;
+                    case 2:
+                        _basis_coeffs_z(i) = stof(word);
+                        break;
+                }
+            
+            }
+            //PX4_INFO("coefficient value: %.6f", (double)stof(word));
+            i += 1;
         }
-        file_reader.close();
-        */ 
+        // catch error
+        if(_num_basis_funs-i>1){
+            PX4_ERR("number of coefficients is too small: %.f", (double)(i));
+            error = true;
+        }      
+    }
+    fin.close();
 
-   // 100m radius circle trajec
-    _basis_coeffs_x(0) = -0.000064f;
-    _basis_coeffs_x(1) = -3020.233571f;
-    _basis_coeffs_x(2) = 10609.960177f;
-    _basis_coeffs_x(3) = -17956.458964f;
-    _basis_coeffs_x(4) = 15735.479961f;
-    _basis_coeffs_x(5) = -2399.573434f;
-    _basis_coeffs_x(6) = -11421.854705f;
-    _basis_coeffs_x(7) = 12388.936542f;
-    _basis_coeffs_x(8) = 120.944433f;
-    _basis_coeffs_x(9) = -12530.869640f;
-    _basis_coeffs_x(10) = 11346.431128f;
-    _basis_coeffs_x(11) = 2643.369342f;
-    _basis_coeffs_x(12) = -15999.009519f;
-    _basis_coeffs_x(13) = 18127.094775f;
-    _basis_coeffs_x(14) = -10676.696033f;
-    _basis_coeffs_x(15) = 3032.667571f;
+    // go back to safety mode loiter circle
+    if(error){
+        // 100m radius circle trajec
+        _basis_coeffs_x(0) = -0.000064f;
+        _basis_coeffs_x(1) = -3020.233571f;
+        _basis_coeffs_x(2) = 10609.960177f;
+        _basis_coeffs_x(3) = -17956.458964f;
+        _basis_coeffs_x(4) = 15735.479961f;
+        _basis_coeffs_x(5) = -2399.573434f;
+        _basis_coeffs_x(6) = -11421.854705f;
+        _basis_coeffs_x(7) = 12388.936542f;
+        _basis_coeffs_x(8) = 120.944433f;
+        _basis_coeffs_x(9) = -12530.869640f;
+        _basis_coeffs_x(10) = 11346.431128f;
+        _basis_coeffs_x(11) = 2643.369342f;
+        _basis_coeffs_x(12) = -15999.009519f;
+        _basis_coeffs_x(13) = 18127.094775f;
+        _basis_coeffs_x(14) = -10676.696033f;
+        _basis_coeffs_x(15) = 3032.667571f;
 
-    _basis_coeffs_y(0) = -100.005984f;
-    _basis_coeffs_y(1) = -4686.100637f;
-    _basis_coeffs_y(2) = 21963.998713f;
-    _basis_coeffs_y(3) = -50566.542718f;
-    _basis_coeffs_y(4) = 71908.811359f;
-    _basis_coeffs_y(5) = -61683.065460f;
-    _basis_coeffs_y(6) = 15730.546677f;
-    _basis_coeffs_y(7) = 39386.062413f;
-    _basis_coeffs_y(8) = -63952.599923f;
-    _basis_coeffs_y(9) = 39525.510553f;
-    _basis_coeffs_y(10) = 15526.730604f;
-    _basis_coeffs_y(11) = -61505.752706f;
-    _basis_coeffs_y(12) = 71804.582542f;
-    _basis_coeffs_y(13) = -50525.803330f;
-    _basis_coeffs_y(14) = 21954.858741f;
-    _basis_coeffs_y(15) = -4685.311429f;
+        _basis_coeffs_y(0) = -100.005984f;
+        _basis_coeffs_y(1) = -4686.100637f;
+        _basis_coeffs_y(2) = 21963.998713f;
+        _basis_coeffs_y(3) = -50566.542718f;
+        _basis_coeffs_y(4) = 71908.811359f;
+        _basis_coeffs_y(5) = -61683.065460f;
+        _basis_coeffs_y(6) = 15730.546677f;
+        _basis_coeffs_y(7) = 39386.062413f;
+        _basis_coeffs_y(8) = -63952.599923f;
+        _basis_coeffs_y(9) = 39525.510553f;
+        _basis_coeffs_y(10) = 15526.730604f;
+        _basis_coeffs_y(11) = -61505.752706f;
+        _basis_coeffs_y(12) = 71804.582542f;
+        _basis_coeffs_y(13) = -50525.803330f;
+        _basis_coeffs_y(14) = 21954.858741f;
+        _basis_coeffs_y(15) = -4685.311429f;
+
+        _basis_coeffs_z(0) = 100.0f;
+        _basis_coeffs_z(1) = 0.0f;
+        _basis_coeffs_z(2) = 0.0f;
+        _basis_coeffs_z(3) = 0.0f;
+        _basis_coeffs_z(4) = 0.0f;
+        _basis_coeffs_z(5) = 0.0f;
+        _basis_coeffs_z(6) = 0.0f;
+        _basis_coeffs_z(7) = 0.0f;
+        _basis_coeffs_z(8) = 0.0f;
+        _basis_coeffs_z(9) = 0.0f;
+        _basis_coeffs_z(10) = 0.0f;
+        _basis_coeffs_z(11) = 0.0f;
+        _basis_coeffs_z(12) = 0.0f;
+        _basis_coeffs_z(13) = 0.0f;
+        _basis_coeffs_z(14) = 0.0f;
+        _basis_coeffs_z(15) = 0.0f;
+     }
 
-    _basis_coeffs_z(0) = 100.0f;
-    _basis_coeffs_z(1) = 0.0f;
-    _basis_coeffs_z(2) = 0.0f;
-    _basis_coeffs_z(3) = 0.0f;
-    _basis_coeffs_z(4) = 0.0f;
-    _basis_coeffs_z(5) = 0.0f;
-    _basis_coeffs_z(6) = 0.0f;
-    _basis_coeffs_z(7) = 0.0f;
-    _basis_coeffs_z(8) = 0.0f;
-    _basis_coeffs_z(9) = 0.0f;
-    _basis_coeffs_z(10) = 0.0f;
-    _basis_coeffs_z(11) = 0.0f;
-    _basis_coeffs_z(12) = 0.0f;
-    _basis_coeffs_z(13) = 0.0f;
-    _basis_coeffs_z(14) = 0.0f;
-    _basis_coeffs_z(15) = 0.0f;
 }
 
 void
@@ -512,7 +558,7 @@ FixedwingPositionINDIControl::Run()
             _actuators.control[actuator_controls_s::INDEX_ROLL] = ctrl2(0);
             _actuators.control[actuator_controls_s::INDEX_PITCH] = ctrl2(1);
             _actuators.control[actuator_controls_s::INDEX_YAW] = ctrl2(2);
-            _actuators.control[actuator_controls_s::INDEX_THROTTLE] = 0.3f;
+            _actuators.control[actuator_controls_s::INDEX_THROTTLE] = 0.8f;
             _actuators_0_pub.publish(_actuators);
             //print_message(_actuators);
         }

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp

@@ -13,6 +13,7 @@
 
 #include <float.h>
 
+#include <sstream>
 #include <vector>
 #include <array>
 #include <drivers/drv_hrt.h>
@@ -223,7 +224,8 @@ private:
 	const static size_t _num_basis_funs = 16;			// number of basis functions used for the trajectory approximation
 
 	// controller methods
-	void _read_trajectory_coeffs_csv();				// read in the correct coefficients of the appropriate trajectory
+	
+	void _read_trajectory_coeffs_csv(std::string 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]
@@ -290,11 +292,6 @@ private:
 	float _b2;
 	float _b3;
 
-	// body rate controllers
-	ECL_RollController		_roll_ctrl;
-	ECL_PitchController		_pitch_ctrl;
-	ECL_YawController		_yaw_ctrl;
-
 	bool _airspeed_valid{false};				///< flag if a valid airspeed estimate exists
 	hrt_abstime _airspeed_last_valid{0};			///< last time airspeed was received. Used to detect timeouts.
 	float _airspeed{0.0f};

src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv

@@ -0,0 +1,3 @@
+-0.000064,-3020.233571,10609.960177,-17956.458964,15735.479961,-2399.573434,-11421.854705,12388.936542,120.944433,-12530.869640,11346.431128,2643.369342,-15999.009519,18127.094775,-10676.696033,3032.667571
+-100.005984,-4686.100637,21963.998713,-50566.542718,71908.811359,-61683.065460,15730.546677,39386.062413,-63952.599923,39525.510553,15526.730604,-61505.752706,71804.582542,-50525.803330,21954.858741,-4685.311429
+100.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000

src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv

@@ -0,0 +1,3 @@
+0,1,2,3,4,5
+6,7,8,9,10,11
+12,13,14,15,16,17