From f328307c41c89bb4ae12b2f2ab77f651139fba5d Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Thu, 8 Sep 2022 15:57:10 +0200 Subject: [PATCH] Testing Code --- .../fw_dyn_soar_control/FixedwingPositionINDIControl.cpp | 8 ++++---- .../fw_dyn_soar_estimator/FixedwingShearEstimator.cpp | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index d534d58b61..d5e7091f36 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -447,9 +447,9 @@ FixedwingPositionINDIControl::_compute_wind_estimate() float AoA_approx = (((2.f*Fz)/(_rho*_area*(fmaxf(_cal_airspeed*_cal_airspeed,_stall_speed*_stall_speed))+0.001f) - _C_L0)/_C_L1) / (1 - ((2.f*Fx)/(_rho*_area*(fmaxf(_cal_airspeed*_cal_airspeed,_stall_speed*_stall_speed))+0.001f)/_C_L1)); AoA_approx = constrain(AoA_approx,-0.2f,0.3f); - //float speed = fmaxf(_cal_airspeed, _stall_speed); + float speed = fmaxf(_cal_airspeed, _stall_speed); float u_approx = _true_airspeed; - float v_approx = 0.f; //body_force(1)*_true_airspeed / (0.5f*_rho*powf(speed,2)*_area*_C_B1); + float v_approx = body_force(1)*_true_airspeed / (0.5f*_rho*powf(speed,2)*_area*_C_B1); float w_approx = tanf(AoA_approx-_aoa_offset)*_true_airspeed; Vector3f vel_air = R_ib*(Vector3f{u_approx, v_approx, w_approx}); @@ -655,8 +655,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"); diff --git a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp index 4c9c6d99fb..2f78591cf5 100644 --- a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp +++ b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp @@ -124,8 +124,8 @@ FixedwingShearEstimator::init() 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/"; strcat(home_dir, "robust/min_aspd_matrix.csv"); FILE* fp = fopen(home_dir, "r");