diff --git a/src/modules/fw_dyn_soar_control/CMakeLists.txt b/src/modules/fw_dyn_soar_control/CMakeLists.txt
index 24a25b3c5d..3cb4fd5da3 100644
--- a/src/modules/fw_dyn_soar_control/CMakeLists.txt
+++ b/src/modules/fw_dyn_soar_control/CMakeLists.txt
@@ -36,6 +36,7 @@
 px4_add_module(
 	MODULE modules__fw_dyn_soar_control
 	MAIN fw_dyn_soar_control
+	STACK_MAIN 2048
 	SRCS
 		FixedwingPositionINDIControl.cpp
 		FixedwingPositionINDIControl.hpp
diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
index 7684ea0c82..7edde4ca4e 100644
--- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
@@ -81,8 +81,8 @@ FixedwingPositionINDIControl::init()
     //_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);
+    //char filename[] = "trajectory0.csv";
+    //_read_trajectory_coeffs_csv(filename);
 
     // initialize transformations
     _R_ned_to_enu *= 0.f;
@@ -441,18 +441,20 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename)
     // =======================================================================
     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/";
+    //char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/";
+    char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/";
+    //PX4_ERR(home_dir);
     strcat(home_dir,filename);
     FILE* fp = fopen(home_dir, "r");
 
-    if (!fp) {
+    if (fp == nullptr) {
         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;
+        const uint buffersize = _num_basis_funs*32;
         char buffer[buffersize];
 
         int row = 0;
@@ -465,7 +467,7 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename)
  
             // Splitting the data
             char* value = strtok(buffer, ",");
- 
+            
             // loop over columns
             while (value) {
 
@@ -483,6 +485,7 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename)
                     default:
                         break;
                 }
+                PX4_INFO("row: %d, col: %d, read value: %.3f", row, column, (double)atof(value));
                 value = strtok(NULL, ",");
                 column++;
                 
diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
index e3666d76ec..b4874da1fb 100644
--- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
@@ -12,13 +12,6 @@
 #define FIXEDWINGPOSITIONINDICONTROL_HPP_
 
 #include <float.h>
-/*
-#include <iostream>
-#include <fstream>
-#include <sstream>
-#include <vector>
-#include <array>
-*/
 
 #include <stdio.h>
 #include <string.h>