From f11ab2bf345f4ac321231993452173acc78db384 Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Mon, 5 Sep 2022 12:13:29 +0200 Subject: [PATCH] cleaned up code, switched wind estimation model --- .../FixedwingPositionINDIControl.cpp | 132 ++++++------------ .../FixedwingPositionINDIControl.hpp | 8 +- .../fw_dyn_soar_control_params.c | 19 ++- .../FixedwingShearEstimator.cpp | 7 - 4 files changed, 69 insertions(+), 97 deletions(-) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index cc59b85660..ce85b38481 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -146,6 +146,7 @@ FixedwingPositionINDIControl::parameters_update() _C_D0 = _param_fw_c_d0.get(); _C_D1 = _param_fw_c_d1.get(); _C_D2 = _param_fw_c_d2.get(); + _C_B1 = _param_fw_c_b1.get(); _aoa_offset = _param_aoa_offset.get(); _stall_speed = _param_stall_speed.get(); @@ -314,9 +315,6 @@ FixedwingPositionINDIControl::vehicle_attitude_poll() // get rotation from FRD to ENU frame (change of basis) Dcmf R_enu_frd(_R_ned_to_enu*R_ned_frd); _att = Quatf(R_enu_frd); - //Eulerf e(R_ned_frd); - //PX4_INFO("attitude euler angles:\t%.4f\t%.4f\t%.4f", (double)e(0),(double)e(1),(double)e(2)); - //PX4_INFO("attitude quaternion:\t%.4f\t%.4f\t%.4f\t%.4f", (double)_att(0),(double)_att(1),(double)_att(2),(double)_att(3)); } if(hrt_absolute_time()-_attitude.timestamp > 20_ms && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD){ PX4_ERR("attitude sample is too old"); @@ -328,7 +326,6 @@ FixedwingPositionINDIControl::vehicle_angular_velocity_poll() { // // no need to check if it was updated as the main loop is fired based on an update... // - //PX4_INFO("angular velocity:\t%.4f\t%.4f\t%.4f", (double)_omega(0),(double)_omega(1),(double)_omega(2)); _omega = Vector3f(_angular_vel.xyz); if(hrt_absolute_time()-_angular_vel.timestamp > 20_ms && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD){ PX4_ERR("angular velocity sample is too old"); @@ -340,7 +337,6 @@ FixedwingPositionINDIControl::vehicle_angular_acceleration_poll() { if (_vehicle_angular_acceleration_sub.update(&_angular_accel)) { _alpha = Vector3f(_angular_accel.xyz); - //PX4_INFO("angular accel:\t%.4f\t%.4f\t%.4f", (double)_alpha(0),(double)_alpha(1),(double)_alpha(2)); } if(hrt_absolute_time()-_angular_accel.timestamp > 20_ms && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD){ PX4_ERR("angular acceleration sample is too old"); @@ -354,13 +350,9 @@ FixedwingPositionINDIControl::vehicle_local_position_poll() if (_vehicle_local_position_sub.update(&_local_pos)){ _pos = _R_ned_to_enu*Vector3f{_local_pos.x,_local_pos.y,_local_pos.z}; _vel = _R_ned_to_enu*Vector3f{_local_pos.vx,_local_pos.vy,_local_pos.vz}; - //_acc = _R_ned_to_enu*Vector3f{_local_pos.ax,_local_pos.ay,_local_pos.az}; // take accel from faster message + // take accel from faster message, since 50Hz is too slow... // transform to soaring frame _pos = _pos - _R_ned_to_enu * Vector3f{_origin_N, _origin_E, _origin_D}; - - //PX4_INFO("local position:\t%.4f\t%.4f\t%.4f", (double)_pos(0),(double)_pos(1),(double)_pos(2)); - //PX4_INFO("local velocity:\t%.4f\t%.4f\t%.4f", (double)_vel(0),(double)_vel(1),(double)_vel(2)); - //PX4_INFO("local acceleration:\t%.4f\t%.4f\t%.4f", (double)_acc(0),(double)_acc(1),(double)_acc(2)); } if(hrt_absolute_time()-_local_pos.timestamp > 50_ms && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD){ PX4_ERR("local position sample is too old"); @@ -427,9 +419,47 @@ FixedwingPositionINDIControl::_compute_trajectory_transform() _vec_enu_to_trajec = Vector3f{0.f,0.f,_shear_h_ref}; } +Vector3f +FixedwingPositionINDIControl::_compute_wind_estimate() +{ + Dcmf R_ib(_att); + Dcmf R_bi(R_ib.transpose()); + // compute expected AoA from g-forces: + Vector3f body_force = _mass*R_bi*(_acc + Vector3f{0.f,0.f,9.81f}); + + // ****************** OLD COMPUTATION, NOT USED ANYMORE **************************** + // approximate lift force, since implicit equation cannot be solved analytically: + // since alpha<<1, we approximate the lift force L = sin(alpha)*Fx - cos(alpha)*Fz + // as L = alpha*Fx - Fz + /* + float Fx = cosf(_aoa_offset)*body_force(0) - sinf(_aoa_offset)*body_force(2); + float Fz = -cosf(_aoa_offset)*body_force(2) - sinf(_aoa_offset)*body_force(0); + float AoA_approx = (((2.f*Fz)/(_rho*_area*(fmaxf(_airspeed*_airspeed,_stall_speed*_stall_speed))+0.001f) - _C_L0)/_C_L1) / + (1 - ((2.f*Fx)/(_rho*_area*(fmaxf(_airspeed*_airspeed,_stall_speed*_stall_speed))+0.001f)/_C_L1)); + AoA_approx = constrain(AoA_approx,-0.2f,0.3f); + Vector3f vel_air = R_ib*(Vector3f{_airspeed,0.f,tanf(AoA_approx-_aoa_offset)*_airspeed}); + */ + + // ***************** NEW COMPUTATION FROM MATLAB CALIBRATION ********************** + float speed = fmaxf(_airspeed, _stall_speed); + float u_approx = _airspeed; + float v_approx = body_force(1) / (0.5f*_rho*speed*_area*_C_B1); + float w_approx = (-body_force(2)/(0.5f*_rho*(speed)*_area)-_C_L0*speed)/_C_L1; + Vector3f vel_air = R_ib*(Vector3f{u_approx, v_approx, w_approx}); + + // compute wind from wind triangle + Vector3f wind = _vel - vel_air; + //PX4_INFO("wind estimate: \t%.1f, \t%.1f, \t%.1f", (double)wind(0), (double)wind(1), (double)wind(2)); + return wind; +} + void FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind) -{ +{ + // apply some filtering + wind(0) = _lp_filter_wind[0].apply(wind(0)); + wind(1) = _lp_filter_wind[1].apply(wind(1)); + wind(2) = _lp_filter_wind[2].apply(wind(2)); _wind_estimate = wind; return; } @@ -778,26 +808,8 @@ FixedwingPositionINDIControl::Run() // =============================== // compute wind pseudo-measurement // =============================== - Dcmf R_ib(_att); - Dcmf R_bi(R_ib.transpose()); - // compute expected AoA from g-forces: - Vector3f body_force = _mass*R_bi*(_acc + Vector3f{0.f,0.f,9.81f}); - // approximate lift force, since implicit equation cannot be solved analytically: - // since alpha<<1, we approximate the lift force L = sin(alpha)*Fx - cos(alpha)*Fz - // as L = alpha*Fx - Fz - float Fx = cosf(_aoa_offset)*body_force(0) - sinf(_aoa_offset)*body_force(2); - float Fz = -cosf(_aoa_offset)*body_force(2) - sinf(_aoa_offset)*body_force(0); - float AoA_approx = (((2.f*Fz)/(_rho*_area*(fmaxf(_airspeed*_airspeed,_stall_speed*_stall_speed))+0.001f) - _C_L0)/_C_L1) / - (1 - ((2.f*Fx)/(_rho*_area*(fmaxf(_airspeed*_airspeed,_stall_speed*_stall_speed))+0.001f)/_C_L1)); - AoA_approx = constrain(AoA_approx,-0.2f,0.2f); - Vector3f vel_air = R_ib*(Vector3f{_airspeed,0.f,tanf(AoA_approx-_aoa_offset)*_airspeed}); - Vector3f wind = _vel - vel_air; - wind(0) = _lp_filter_wind[0].apply(wind(0)); - wind(1) = _lp_filter_wind[1].apply(wind(1)); - wind(2) = _lp_filter_wind[2].apply(wind(2)); + Vector3f wind = _compute_wind_estimate(); _set_wind_estimate(wind); - //PX4_INFO("wind estimate:\t%.4f\t%.4f\t%.4f", (double)_wind_estimate(0),(double)_wind_estimate(1),(double)_wind_estimate(2)); - // only run actuators poll, when our module is not publishing: if (_vehicle_status.nav_state != vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { @@ -829,10 +841,6 @@ FixedwingPositionINDIControl::Run() Quatf q = _get_attitude_ref(t_ref,T); Vector3f omega_ref = _get_angular_velocity_ref(t_ref,T); // body angular velocity Vector3f alpha_ref = _get_angular_acceleration_ref(t_ref,T); // body angular acceleration - //PX4_INFO("local position ref:\t%.4f\t%.4f\t%.4f", (double)pos_ref(0),(double)pos_ref(1),(double)pos_ref(2)); - //PX4_INFO("alpha ref:\t%.4f\t%.4f\t%.4f", (double)alpha_ref(0),(double)alpha_ref(1),(double)alpha_ref(2)); - //PX4_INFO("vel ref:\t%.4f\t%.4f\t%.4f", (double)vel_ref(0),(double)vel_ref(1),(double)vel_ref(2)); - //PX4_INFO("vel:\t%.4f\t%.4f\t%.4f", (double)_vel(0),(double)_vel(1),(double)_vel(2)); // ===================== // compute control input @@ -993,6 +1001,8 @@ FixedwingPositionINDIControl::Run() // ==================== // publish debug values // ==================== + Dcmf R_ib(_att); + Dcmf R_bi(R_ib.transpose()); Vector3f vel_body = R_bi*(_vel - _wind_estimate); _slip = atan2f(vel_body(1), vel_body(0))*180.f/M_PI_2_F; _debug_value.timestamp = hrt_absolute_time(); @@ -1167,33 +1177,7 @@ FixedwingPositionINDIControl::_get_angular_acceleration_ref(float t, float T) float FixedwingPositionINDIControl::_get_closest_t(Vector3f pos) { - /* - const uint n = 100; - Vector distances; - float t_ref; - // compute all distances - for(uint i=0; i distances; @@ -1409,7 +1393,6 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v } */ - // ========================================================================== // get required attitude (assuming we can fly the target velocity), and error // ========================================================================== @@ -1432,30 +1415,12 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v } } - - // ========================================= // apply PD control law on the body attitude // ========================================= Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-_omega) + alpha_ref; - // ========================================== - // input meant for tuning the INDI controller - // ========================================== - /* - if(hrt_absolute_time()%2000000>1000000){ - rot_acc_command = Vector3f{2.0f,1.f,0.f}; - //rot_acc_command = Vector3f{0.f,0.f,0.5f}; - } - else{ - rot_acc_command = Vector3f{-2.0f,-1.f,0.f}; - //rot_acc_command = Vector3f{0.f,0.f,-0.5f}; - } - */ - //PX4_INFO("force command: \t%.2f", (double)(f_command*f_command)); - //PX4_INFO("force command: \t%.2f\t%.2f\t%.2f", (double)f_command(0), (double)f_command(1), (double)f_command(2)); - //PX4_INFO("FRD body frame rotation vec: \t%.2f\t%.2f\t%.2f", (double)w_err(0), (double)w_err(1), (double)w_err(2)); if (sqrtf(w_err*w_err)>M_PI_F){ PX4_ERR("rotation angle larger than pi: \t%.2f, \t%.2f, \t%.2f", (double)sqrtf(w_err*w_err), (double)q_err.angle(), (double)(q_err.axis()*q_err.axis())); } @@ -1525,9 +1490,6 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v // not really an accel command, rather a FF-P command rot_acc_command(2) = _K_q(2,2)*omega_turn_ref(2)*scaler + _K_w(2,2)*(omega_turn_ref(2) - omega_filtered(2))* scaler*scaler; - //PX4_INFO("omega turn ref: \t%.2f\t%.2f\t%.2f", (double)omega_turn_ref(0), (double)omega_turn_ref(1), (double)omega_turn_ref(2)); - //PX4_INFO("omega turn : \t%.2f\t%.2f\t%.2f", (double)_omega(0), (double)_omega(1), (double)_omega(2)); - // ======================================================================================= // filter the stage 1 controller outputs to filter out high-frequency components. // This is desirable as the provided commands might be very noisy otherwise (not feasible) @@ -1549,10 +1511,6 @@ FixedwingPositionINDIControl::_compute_INDI_stage_2(Vector3f ctrl) Dcmf R_ib(_att); Vector3f vel_body = R_ib.transpose()*(_vel-_wind_estimate); float q = fmaxf(0.5f*sqrtf(vel_body*vel_body)*vel_body(0), 0.5f*_stall_speed*_stall_speed); // dynamic pressure, saturates at stall speed - //Vector3f vel_body_2 = Dcmf(Quatf(_attitude.q)).transpose()*Vector3f{_local_pos.vx,_local_pos.vy,_local_pos.vz}; - //PX4_INFO("ENU body frame velocity: \t%.2f\t%.2f\t%.2f", (double)vel_body_2(0), (double)vel_body_2(1), (double)vel_body_2(2)); - //PX4_INFO("FRD body frame velocity: \t%.2f\t%.2f\t%.2f", (double)vel_body(0), (double)vel_body(1), (double)vel_body(2)); - //omega_filtered = _omega; //TODO: remove // compute moments Vector3f moment; moment(0) = _k_ail*q*_actuators.control[actuator_controls_s::INDEX_ROLL] - _k_d_roll*q*_omega(0); diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 7abae2abb3..1ea01f457d 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -185,6 +185,7 @@ private: (ParamFloat) _param_fw_c_d0, (ParamFloat) _param_fw_c_d1, (ParamFloat) _param_fw_c_d2, + (ParamFloat) _param_fw_c_b1, (ParamFloat) _param_aoa_offset, (ParamFloat) _param_stall_speed, // position PD control params @@ -277,6 +278,7 @@ private: 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 + Vector3f _compute_wind_estimate(); Vector _get_basis_funs(float t=0); // compute the vector of basis functions at normalized time t in [0,1] Vector _get_d_dt_basis_funs(float t=0); // compute the vector of basis function gradients at normalized time t in [0,1] Vector _get_d2_dt2_basis_funs(float t=0); // compute the vector of basis function curvatures at normalized time t in [0,1] @@ -353,6 +355,7 @@ private: float _C_D0; float _C_D1; float _C_D2; + float _C_B1; float _aoa_offset; float _stall_speed; // trajectory origin in WGS84 @@ -375,6 +378,9 @@ private: // thrust float _thrust; float _thrust_pos; + // ================== + // controler switches + // ================== // controller mode bool _switch_manual; // soaring mode @@ -383,7 +389,7 @@ private: bool _switch_saturation; // bool _switch_filter; - // + // use shear height from estimator bool _switch_origin_hardcoded; // bool _soaring_feasible; diff --git a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c index 0ae578f563..c1df2b2f2a 100644 --- a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c +++ b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c @@ -181,6 +181,21 @@ PARAM_DEFINE_FLOAT(DS_C_D1, 0.3783f); */ PARAM_DEFINE_FLOAT(DS_C_D2, 1.984f); +/** + * estimated sideslip sensitivity coefficients used for wind estimation + * + * Used as F_y = 0.5 * DS_RHO * ASPD^2 * DS_C_B1, + * where alpha is the angle of attack. + * + * @unit + * @min -100 + * @max -0.01 + * @decimal 4 + * @increment 0.0001 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_C_B1, -3.32f); + /** * offset angle between body frame (Pixhawk) and the wing chord * @@ -329,7 +344,7 @@ PARAM_DEFINE_FLOAT(DS_LIN_FF_Z, 0.5f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_ROT_K_ROLL, 10.0f); +PARAM_DEFINE_FLOAT(DS_ROT_K_ROLL, 30.0f); /** * control gain of attitude PD-controller (body pitch-direction) @@ -341,7 +356,7 @@ PARAM_DEFINE_FLOAT(DS_ROT_K_ROLL, 10.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_ROT_K_PITCH, 10.0f); +PARAM_DEFINE_FLOAT(DS_ROT_K_PITCH, 30.0f); /** * rudder turn coordination FF-gain diff --git a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp index bb43ab4801..2216e584d2 100644 --- a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp +++ b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp @@ -362,9 +362,6 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind) wind_horizontal(0) = _current_wind(0); wind_horizontal(1) = _current_wind(1); identity_1.setIdentity(); - //z_expected_horizontal.print(); - //wind_horizontal.print(); - //(_K_horizontal*(wind_horizontal - z_expected_horizontal)).print(); _X_posterior_horizontal = _X_prior_horizontal + _K_horizontal*(wind_horizontal - z_expected_horizontal); _P_posterior_horizontal = (identity_1 - _K_horizontal*_H_horizontal)*_P_prior_horizontal; @@ -485,8 +482,6 @@ FixedwingShearEstimator::Run() // only update airspeed for trajectory selection _soaring_estimator_shear.aspd = aspd; } - - // publish shear params // ======================================== @@ -523,8 +518,6 @@ bool FixedwingShearEstimator::check_feasibility() float shear_x = _X_posterior_horizontal(0) - _X_posterior_horizontal(2); float shear_y = _X_posterior_horizontal(1) - _X_posterior_horizontal(3); float shear_strength = _X_posterior_horizontal(5); - //float heading = atan2f(shear_x, shear_y); - //float heading_stdev = 0.f; // require shear strength above 8 m/s float shear = sqrtf(powf(shear_x,2) + powf(shear_y,2));