From 5bce20e06a5ad130f71a43aab9fb8b744bd16bf2 Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Fri, 5 Aug 2022 09:29:06 +0200 Subject: [PATCH] increased buffer size and added trajec loader --- .../fw_dyn_soar_control/CMakeLists.txt | 2 +- .../FixedwingPositionINDIControl.cpp | 21 +++++++++---------- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/src/modules/fw_dyn_soar_control/CMakeLists.txt b/src/modules/fw_dyn_soar_control/CMakeLists.txt index 3cb4fd5da3..c0f1c1edb9 100644 --- a/src/modules/fw_dyn_soar_control/CMakeLists.txt +++ b/src/modules/fw_dyn_soar_control/CMakeLists.txt @@ -36,7 +36,7 @@ px4_add_module( MODULE modules__fw_dyn_soar_control MAIN fw_dyn_soar_control - STACK_MAIN 2048 + STACK_MAIN 4096 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 bf3516d32f..489578c325 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -419,7 +419,7 @@ FixedwingPositionINDIControl::_findClosest(float arr[], int n, float target) while (i < j) { mid = (i + j) / 2; - if (fabs(arr[mid]-target) < 0.000001f) + if ((float)fabs(arr[mid]-target) < (float)0.0001f) return arr[mid]; /* If target is less than array element, @@ -465,24 +465,24 @@ FixedwingPositionINDIControl::_select_trajectory(float initial_energy) in [0.10, 1.00] in [E_min, E_max] */ - /* + float v = _findClosest(_v_max_arr, _gridsize, _shear_v_max); // wind velocity float a = _findClosest(_alpha_arr, _gridsize, _shear_alpha); // shear strength float e = _findClosest(_energy_arr, _gridsize, _shear_energy); // initial energy PX4_INFO("V, A, E: \t%.2f\t%.2f\t%.2f", double(v), double(a), double(e)); char file[30] = "nominal"; - char v_str[10]; - char a_str[10]; - char e_str[10]; + char v_str[6]; + char a_str[6]; + char e_str[6]; strcat(file,"_"); - strcat(file,gcvt(v, 3, v_str)); + strcat(file,gcvt(v, 2, v_str)); strcat(file,"_"); strcat(file,gcvt(a, 3, a_str)); strcat(file,"_"); - strcat(file,gcvt(e, 3, e_str)); + strcat(file,gcvt(e, 2, e_str)); strcat(file,".csv"); PX4_INFO("filename: \t%.30s", file); - */ + // select loiter trajectory for loiter test char filename[16]; @@ -525,7 +525,6 @@ FixedwingPositionINDIControl::_select_trajectory(float initial_energy) Therefore we define a new transform between the soaring frame and the trajectory frame. */ - _read_trajectory_coeffs_csv(filename); } @@ -536,8 +535,8 @@ 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] = PX4_ROOTFSDIR"/fs/microsd/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");