diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 66ff7e01d4..42b89a5af0 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/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 diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 510cb3220f..7684ea0c82 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/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){ diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 4fd6448d80..e3666d76ec 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -12,10 +12,17 @@ #define FIXEDWINGPOSITIONINDICONTROL_HPP_ #include - +/* +#include +#include #include #include #include +*/ + +#include +#include + #include #include "fw_att_control/ecl_pitch_controller.h" #include "fw_att_control/ecl_roll_controller.h" @@ -69,8 +76,7 @@ #include #include #include -#include -#include + 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 _get_basis_funs(float t=0); // compute the vector of basis functions at normalized time t in [0,1]