From cf53c31d45c01e473dc57f895e132323d00be351 Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Sat, 20 Aug 2022 14:37:06 +0200 Subject: [PATCH] TESTING CODE --- .../FixedwingPositionINDIControl.cpp | 7 +++++-- .../FixedwingPositionINDIControl.hpp | 4 +++- .../fw_dyn_soar_estimator/FixedwingShearEstimator.cpp | 10 ++++++---- 3 files changed, 14 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 4d9ae4b195..9cd2687bbe 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -468,10 +468,12 @@ FixedwingPositionINDIControl::_select_trajectory(float initial_energy) 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[6]; char a_str[6]; @@ -483,7 +485,8 @@ FixedwingPositionINDIControl::_select_trajectory(float initial_energy) strcat(file,"_"); strcat(file,gcvt(e, 2, e_str)); strcat(file,".csv"); - PX4_INFO("filename: \t%.30s", file); + */ + //PX4_INFO("filename: \t%.30s", file); // select loiter trajectory for loiter test @@ -879,7 +882,7 @@ FixedwingPositionINDIControl::Run() _soaring_controller_wind.position[2] = _pos(2); Eulerf e(Quatf(_attitude.q)); float bank = e(0); - if (fabs(bank)<0.5f) { + if ((float)fabs(bank)<0.5f) { _soaring_controller_wind.valid = true; } else { diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 8bc11a7f3c..848a378f1a 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -379,10 +380,11 @@ private: // vectors defining the gridding for trajectory selection: initial velocities, wind speed and shear strength const static size_t _gridsize = 11; + /* float _energy_arr[_gridsize] = {14.f,16.f,18.f,20.f,22.f,24.f,26.f,28.f,30.f,32.f,34.f}; float _v_max_arr[_gridsize] = {10.f,11.f,12.f,13.f,14.f,15.f,16.f,17.f,18.f,19.f,20.f}; float _alpha_arr[_gridsize] = {0.1f,0.2f,0.3f,0.4f,0.5f,0.6f,0.7f,0.8f,0.9f,1.0f,1.1f}; - + */ // helper variables Dcmf _R_ned_to_enu; // rotation matrix from NED to ENU frame diff --git a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp index 3979462b47..6a5a20058f 100644 --- a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp +++ b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp @@ -170,6 +170,8 @@ FixedwingShearEstimator::reset_filter() // set height to enable convergence _X_prior_horizontal(4) = _init_height; _X_posterior_horizontal(4) = _init_height; + _X_prior_horizontal(5) = 0.5f/_unit_a; + _X_posterior_horizontal(5) = 0.5f/_unit_a; // increment counter _reset_counter += 1; @@ -223,11 +225,11 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind) Matrix tmp1_horizontal = _P_prior_horizontal*_H_horizontal.T(); Matrix tmp2_horizontal = _H_horizontal*(_P_prior_horizontal*_H_horizontal.T()) + _R_horizontal; float determinant_horizontal = tmp2_horizontal(0,0)*tmp2_horizontal(1,1) - tmp2_horizontal(1,0)*tmp2_horizontal(0,1); - if (fabs(determinant_horizontal)>0.000001f){ + if ((float)fabs(determinant_horizontal)>=0.000001f){ _K_horizontal = tmp1_horizontal*geninv(tmp2_horizontal); } else{ - PX4_WARN("singular horizontal matrix, resetting filter"); + //PX4_WARN("singular horizontal matrix, resetting filter"); error = true; } @@ -241,12 +243,12 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind) Matrix tmp2_vertical = _H_vertical*_P_prior_vertical*_H_vertical.T() + _R_vertical; Matrix inv_vertical; float determinant_vertical = tmp2_vertical(0,0); - if (fabs(determinant_vertical)>0.000001f){ + if ((float)fabs(determinant_vertical)>0.000001f){ inv_vertical(0,0) = 1.f / determinant_vertical; _K_vertical = tmp1_vertical*inv_vertical; } else{ - PX4_WARN("singular vertical matrix, resetting filter"); + //PX4_WARN("singular vertical matrix, resetting filter"); error = true; }