From 571e2077f39687a2dc4b5b2cd655381a9bd9894e Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Fri, 8 Apr 2022 14:25:17 +0200 Subject: [PATCH] backup push --- .../FixedwingPositionINDIControl.cpp | 133 +++++++++++++++--- .../FixedwingPositionINDIControl.hpp | 27 +++- .../fw_dyn_soar_control_params.c | 4 +- 3 files changed, 137 insertions(+), 27 deletions(-) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 3a3141980c..75344c2d6b 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -42,10 +42,9 @@ using math::radians; using matrix::Dcmf; using matrix::Matrix; -using matrix::Eulerf; +using matrix::Euler; using matrix::Quatf; -using matrix::Vector2f; -using matrix::Vector2d; +using matrix::AxisAnglef; using matrix::Vector3f; using matrix::Vector; using matrix::wrap_pi; @@ -149,20 +148,20 @@ FixedwingPositionINDIControl::_get_acceleration_ref(float t, float T) return Vector3f{x, y, z}/powf(T,2); } -Quatf +Dcmf FixedwingPositionINDIControl::_get_attitude_ref(float t, float T) { Vector3f vel = _get_velocity_ref(t,T); Vector3f vel_air = vel - _wind_estimate; Vector3f acc = _get_acceleration_ref(t,T); // add gravity - acc(2) += 9.81; + acc(2) += 9.81f; // compute required force - Vector3f f = FW_MASS*acc; + Vector3f f = _param_fw_mass.get()*acc; // compute force component projected onto lift axis - Vector3f vel_normalized = normalized(vel_air); + Vector3f vel_normalized = vel_air.normalized(); Vector3f f_lift = f - f*vel_normalized; - Vector3f f_lift_normalized = normalized(f_lift); + Vector3f lift_normalized = f_lift.normalized(); Vector3f wing_normalized = -vel_normalized.cross(lift_normalized); // compute rotation matrix Dcmf R_bi; @@ -177,14 +176,11 @@ FixedwingPositionINDIControl::_get_attitude_ref(float t, float T) R_bi(2,2) = lift_normalized(2); // compute required AoA Vector3f f_phi = R_bi*f_lift; - float AoA = (2*f_phi(2))/(1.223*0.4*powf(unit(vel_air),2)) - 0.356)/2.354; + float AoA = ((2.f*f_phi(2))/(1.223f*0.4f*(vel_air*vel_air)) - 0.356f)/2.354f; // compute final rotation matrix - Euler e; - Euler(0, AoA, 0); - Dcmf R_pitch; - R_pitch(e); - Dcmf Rotation; - Rotation(R_pitch*R_bi); + Eulerf e(0.f, AoA, 0.f); + Dcmf R_pitch(e); + Dcmf Rotation(R_pitch*R_bi); // switch from FRD to ENU frame Rotation(1,0) *= -1; Rotation(1,1) *= -1; @@ -192,16 +188,107 @@ FixedwingPositionINDIControl::_get_attitude_ref(float t, float T) Rotation(2,0) *= -1; Rotation(2,1) *= -1; Rotation(2,2) *= -1; - - - - - - - - return 1; + return Rotation.transpose(); + // compute quaternion + //Quatf q; + //q(transpose(Rotation)); + //return q; } +Vector3f +FixedwingPositionINDIControl::_get_angular_velocity_ref(float t, float T) +{ + float dt = 0.001; + float t_lower = fmaxf(0.f,t-dt); + float t_upper = fminf(t+dt,1.f); + Dcmf R_i0 = _get_attitude_ref(t_lower, T); + Dcmf R_i1 = _get_attitude_ref(t_upper, T); + Dcmf R_10 = R_i1.transpose()*R_i0; + AxisAnglef w_01(R_10); + return -w_01.axis()*w_01.angle()/(T*(t_upper-t_lower)); +} + +Vector3f +FixedwingPositionINDIControl::_get_angular_acceleration_ref(float t, float T) +{ + float dt = 0.001; + float t_lower = fmaxf(0.f,t-dt); + float t_upper = fminf(t+dt,1.f); + // compute roational velocity in inertial frame + Dcmf R_i0 = _get_attitude_ref(t_lower, T); + AxisAnglef w_0(R_i0*_get_angular_velocity_ref(t_lower, T)); + // compute roational velocity in inertial frame + Dcmf R_i1 = _get_attitude_ref(t_upper, T); + AxisAnglef w_1(R_i1*_get_angular_velocity_ref(t_upper, T)); + // compute gradient via finite differences + Vector3f dw_dt = (w_1.axis()*w_1.angle() - w_0.axis()*w_0.angle()) / (T*(t_upper-t_lower)); + // transform back to body frame + return R_i0.transpose()*dw_dt; +} + +float +FixedwingPositionINDIControl::_get_closest_t(Vector3f pos) +{ + const uint n = 100; + Vector distances; + // compute all distances + for(uint i=0; i) _param_fw_mass, + (ParamFloat) _param_fw_wing_area, + (ParamFloat) _param_rho + // aerodynamic params + /* + (ParamFloat) _param_fw_c_l0, + (ParamFloat) _param_fw_c_l1, + (ParamFloat) _param_fw_c_d0, + (ParamFloat) _param_fw_c_d1, + (ParamFloat) _param_fw_c_d2, + // filter params + (ParamFloat) _param_filter_a1, + (ParamFloat) _param_filter_a2, + (ParamFloat) _param_filter_b1, + (ParamFloat) _param_filter_b2, + (ParamFloat) _param_filter_b3 + */ + ) + perf_counter_t _loop_perf; ///< loop performance counter @@ -152,11 +174,11 @@ private: Vector3f _get_position_ref(float t=0); // get the reference position on the current path, at normalized time t in [0,1] Vector3f _get_velocity_ref(float t=0, float T=1); // get the reference velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T Vector3f _get_acceleration_ref(float t=0, float T=1); // get the reference acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T - Quatf _get_attitude_ref(float t=0, float T=1); // get the reference attitude on the current path, at normalized time t in [0,1], with an intended cycle time of T + Dcmf _get_attitude_ref(float t=0, float T=1); // get the reference attitude on the current path, at normalized time t in [0,1], with an intended cycle time of T Vector3f _get_angular_velocity_ref(float t=0, float T=1); // get the reference angular velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T Vector3f _get_angular_acceleration_ref(float t=0, float T=1); // get the reference angular acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T float _get_closest_t(Vector3f pos); // get the normalized time, at which the reference path is closest to the current position - Quatf _get_attitude(Vector3f vel, Vector3f f); // get the attitude to produce force f while flying with velocity vel + Dcmf _get_attitude(Vector3f vel, Vector3f f); // get the attitude to produce force f while flying with velocity vel void _compute_NDI_control_input(Vector3f pos, Vector3f vel, Vector3f acc, Quatf att, Vector3f omega, Vector3f alpha); void _compute_INDI_control_input(Vector3f pos, Vector3f vel, Vector3f acc, Quatf att, Vector3f omega, Vector3f alpha); @@ -183,6 +205,7 @@ private: std::array _a_list; std::array _f_lpf_list; std::array _a_lpf_list; + }; 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 96d525659b..6ebf6fefa5 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 @@ -43,8 +43,8 @@ PARAM_DEFINE_FLOAT(FW_WING_AREA, 0.4f); * @unit * @min 0.5 * @max 1.225 - * @decimal 2 - * @increment 0.01 + * @decimal 3 + * @increment 0.001 * @group FW DYN SOAR Control */ PARAM_DEFINE_FLOAT(RHO, 1.223f);