fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 05:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

TESTING CODE

Browse Source
This commit is contained in:
Marvin Harms
2022-08-20 14:37:06 +02:00
parent 48beb3ead0
commit cf53c31d45
3 changed files with 14 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+5 -2
View File
@@ -468,10 +468,12 @@ FixedwingPositionINDIControl::_select_trajectory(float initial_energy)
<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 {
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+3 -1
View File
@@ -14,6 +14,7 @@
#include <float.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <drivers/drv_hrt.h>
@@ -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
src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp
+6 -4
View File
@@ -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<float, 6, 2> tmp1_horizontal = _P_prior_horizontal*_H_horizontal.T();
Matrix<float, 2, 2> 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<float, 1, 1> tmp2_vertical = _H_vertical*_P_prior_vertical*_H_vertical.T() + _R_vertical;
Matrix<float, 1, 1> 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;
}