From c8346534f1b832846e713b442a511a262deaffa0 Mon Sep 17 00:00:00 2001 From: romain-chiap Date: Thu, 12 Aug 2021 16:10:03 +0200 Subject: [PATCH] sih: using Quatf::expq(), fuselage added --- src/modules/sih/sih.cpp | 28 +++++++--------------------- src/modules/sih/sih.hpp | 7 ++----- 2 files changed, 9 insertions(+), 26 deletions(-) diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index f150713e5e..f5fb30b834 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -305,10 +305,12 @@ void Sih::generate_aerodynamics() _wing_r.update_aero(_v_B, _w_B, altitude, -_u[0]*FLAP_MAX); _tailplane.update_aero(_v_B, _w_B, altitude, _u[1]*FLAP_MAX, _T_MAX * _u[3]); _fin.update_aero(_v_B, _w_B, altitude, _u[2]*FLAP_MAX, _T_MAX * _u[3]); - _Fa_I = _C_IB * (_wing_l.get_Fa() + _wing_r.get_Fa() + _tailplane.get_Fa() + _fin.get_Fa()) - _KDV * - _v_I; // sum of aerodynamic forces + _fuselage.update_aero(_v_B, _w_B, altitude); + _Fa_I = _C_IB * (_wing_l.get_Fa() + _wing_r.get_Fa() + _tailplane.get_Fa() + _fin.get_Fa() + _fuselage.get_Fa()) + - _KDV * _v_I; // sum of aerodynamic forces // _Ma_B = wing_l.Ma + wing_r.Ma + tailplane.Ma + fin.Ma + flap_moments() -_KDW * _w_B; // aerodynamic moments - _Ma_B = _wing_l.get_Ma() + _wing_r.get_Ma() + _tailplane.get_Ma() + _fin.get_Ma() - _KDW * _w_B; // aerodynamic moments + _Ma_B = _wing_l.get_Ma() + _wing_r.get_Ma() + _tailplane.get_Ma() + _fin.get_Ma() + _fuselage.get_Ma() - _KDW * + _w_B; // aerodynamic moments } // apply the equations of motion of a rigid body and integrate one step @@ -320,7 +322,7 @@ void Sih::equations_of_motion() _p_I_dot = _v_I; // position differential _v_I_dot = (_W_I + _Fa_I + _C_IB * _T_B) / _MASS; // conservation of linear momentum // _q_dot = _q.derivative1(_w_B); // attitude differential - _dq = expq(0.5f * _dt * _w_B); + _dq = Quatf::expq(0.5f * _dt * _w_B); _w_B_dot = _Im1 * (_Mt_B + _Ma_B - _w_B.cross(_I * _w_B)); // conservation of angular momentum // fake ground, avoid free fall @@ -517,22 +519,6 @@ void Sih::publish_sih() _gpos_gt_pub.publish(_gpos_gt); } -// quaternion exponential as defined in [3] -Quatf Sih::expq(const matrix::Vector3f &u) -{ - float u_norm = u.norm(); - Vector3f v; - - if (u_norm < 1.0e-6f) { // error will be smaller than 1e-18 - v = (1.0f - u_norm * u_norm / 6.0f) * u; // first taylor serie term of sin(x)/x - - } else { - v = sinf(u_norm) / u_norm * u; - } - - return Quatf(cosf(u_norm), v(0), v(1), v(2)); -} - float Sih::generate_wgn() // generate white Gaussian noise sample with std=1 { // algorithm 1: @@ -636,7 +622,7 @@ int Sih::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -This module provide a simulator for quadrotors running fully +This module provide a simulator for quadrotors and fixed-wings running fully inside the hardware autopilot. This simulator subscribes to "actuator_outputs" which are the actuator pwm diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index 4718f07b0a..f797370cce 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -166,10 +166,6 @@ private: void send_dist_snsr(); void publish_sih(); void generate_aerodynamics(); - float sincf(float x); // sin cardinal = sin(x)/x - matrix::Quatf expq(const matrix::Vector3f &u); // quaternion exponential as defined in [3] - // States eom_f(States); // equations of motion f: x'=f(x) - perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; @@ -210,7 +206,8 @@ private: AeroSeg _wing_r = AeroSeg(SPAN / 2.0f, MAC, -4.0f, matrix::Vector3f(0.0f, SPAN / 4.0f, 0.0f), -3.0f, SPAN / MAC, MAC / 3.0f); AeroSeg _tailplane = AeroSeg(0.3f, 0.1f, 0.0f, matrix::Vector3f(-0.4f, 0.0f, 0.0f), 0.0f, -1.0f, 0.05f, RP); - AeroSeg _fin = AeroSeg(0.25, 0.15, 0.0f, matrix::Vector3f(-0.45f, 0.0f, -0.1f), -90.0f, -1.0f, 0.08f, RP); + AeroSeg _fin = AeroSeg(0.25, 0.18, 0.0f, matrix::Vector3f(-0.45f, 0.0f, -0.1f), -90.0f, -1.0f, 0.12f, RP); + AeroSeg _fuselage = AeroSeg(0.2, 0.8, 0.0f, matrix::Vector3f(0.0f, 0.0f, 0.0f), -90.0f); // sensors reconstruction matrix::Vector3f _acc;