diff --git a/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil index 9011987384..58afd8b169 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil @@ -26,3 +26,5 @@ param set-default CBRK_SUPPLY_CHK 894281 # - without safety switch param set-default COM_PREARM_MODE 0 param set-default CBRK_IO_SAFETY 22027 + +param set SIH_VEHICLE_TYPE 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil new file mode 100644 index 0000000000..57bbbc1d4a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil @@ -0,0 +1,40 @@ +#!/bin/sh +# +# @name SIH plane AERT +# +# @type Simulation +# @class Plane +# +# @maintainer Romain Chiappinelli +# +# @board px4_fmu-v2 exclude +# + +. ${R}etc/init.d/rc.fw_defaults + +set MIXER AERT +set PWM_OUT 1234 + +# set SYS_HITL to 2 to start the SIH and avoid sensors startup +param set-default SYS_HITL 2 + +# disable some checks to allow to fly: +# - with usb +param set-default CBRK_USB_CHK 197848 +# - without real battery +param set-default CBRK_SUPPLY_CHK 894281 +# - without safety switch +param set-default COM_PREARM_MODE 0 +param set-default CBRK_IO_SAFETY 22027 + +param set-default BAT_N_CELLS 3 + +param set SIH_T_MAX 6.0 +param set SIH_MASS 0.3 +param set SIH_IXX 0.00402 +param set SIH_IYY 0.0144 +param set SIH_IZZ 0.0177 +param set SIH_IXZ 0.00046 +param set SIH_KDV 0.2 + +param set SIH_VEHICLE_TYPE 1 # sih as fixed wing diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 6368cee989..f70aba3460 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -39,6 +39,7 @@ px4_add_romfs_files( 1001_rc_quad_x.hil 1002_standard_vtol.hil 1100_rc_quad_x_sih.hil + 1101_rc_plane_sih.hil # [2000, 2999] Standard planes" 2100_standard_plane diff --git a/Tools/jmavsim_run.sh b/Tools/jmavsim_run.sh index 06b9923189..f848e17531 100755 --- a/Tools/jmavsim_run.sh +++ b/Tools/jmavsim_run.sh @@ -10,7 +10,7 @@ extra_args= baudrate=921600 device= ip="127.0.0.1" -while getopts ":b:d:p:qsr:f:i:lo" opt; do +while getopts ":b:d:p:qsr:f:i:loa" opt; do case $opt in b) baudrate=$OPTARG @@ -39,6 +39,9 @@ while getopts ":b:d:p:qsr:f:i:lo" opt; do o) extra_args="$extra_args -disponly" ;; + a) + extra_args="$extra_args -fw" # aircraft + ;; \?) echo "Invalid option: -$OPTARG" >&2 exit 1 diff --git a/src/modules/sih/CMakeLists.txt b/src/modules/sih/CMakeLists.txt index fce939c732..bb103d089f 100644 --- a/src/modules/sih/CMakeLists.txt +++ b/src/modules/sih/CMakeLists.txt @@ -37,6 +37,7 @@ px4_add_module( COMPILE_FLAGS ${MAX_CUSTOM_OPT_LEVEL} SRCS + aero.hpp sih.cpp sih.hpp DEPENDS diff --git a/src/modules/sih/aero.hpp b/src/modules/sih/aero.hpp new file mode 100644 index 0000000000..c34ce88096 --- /dev/null +++ b/src/modules/sih/aero.hpp @@ -0,0 +1,430 @@ +/**************************************************************************** +* +* Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +****************************************************************************/ + +/** + * @file aero.hpp + * Aerodynamic class for modeling wing, tailaplane, fin. + * Captures the effect of partial stall, low aspect ratio, control surfaces deflection, + * propeller slipstream. + * + * @author Romain Chiappinelli + * + * Altitude R&D inc, Montreal - July 2021 + * + * The aerodynamic model is inspired from [2] + * [2] Khan, Waqas, supervised by Meyer Nahon "Dynamics modeling of agile fixed-wing unmanned aerial vehicles." + * McGill University, PhD thesis, 2016. Sections 3.1, 3.2, and 3.3 + * + * Capabilities and limitations + * This class can model + * - full 360 deg angle of attack lift, drag, and pitching moment. + * - wings with aspect ratio from 0.1666 up to 6, (gliders would perform poorly for instance). + * - control surface (flap) deflection up to 70 degrees. + * - unlimited flap chord, (the elevator could take the entire tailplane). + * - stall angle function of the flap deflection. + * - effect of the flap deflection on lift, drag, and pitching moment. + * - any dihedral angle, the fin is modeled as a wing with dihedral angle of 90 deg. + * - slipstream velocity (velocity from the propeller) using momentum theory. + */ + + + +#pragma once + +#include // matrix, vectors, dcm, quaterions +#include // math::radians, +// #include +#include + +// class Aerodynamic Segment ------------------------------------------------------------------------ +class AeroSeg +{ +private: + // Table 3.1 SEMI-EMPIRICAL COEFFICIENTS FOR RECTANGULAR FLAT PLATES + static constexpr const int N_TAB = 12; + // static constexpr const float AR_tab[N_TAB] = {0.1666f,0.333f,0.4f,0.5f,1.0f,1.25f, 2.0f, 3.0f, 4.0f, 6.0f}; + // static constexpr const float ale_tab[N_TAB] = {3.00f,3.64f,4.48f,7.18f,10.20f,13.38f,14.84f,14.49f,9.95f,12.93f,15.00f,15.00f}; + // static constexpr const float ate_tab[N_TAB] = {5.90f,15.51f,32.57f,39.44f,48.22f,59.29f,21.55f,7.74f,7.05f,5.26f,6.50f,6.50f}; + // static constexpr const float afle_tab[N_TAB] = {59.00f,58.60f,58.20f,50.00f,41.53f,26.70f,23.44f,21.00f,18.63f,14.28f,11.60f,10.00f}; + // static constexpr const float afte_tab[N_TAB] = {59.00f,58.60f,58.20f,51.85f,41.46f,28.09f,39.40f,35.86f,26.76f,19.76f,16.43f,14.00f}; + // static constexpr const float afs_tab[N_TAB] = {49.00f,54.00f,56.00f,48.00f,40.00f,29.00f,27.00f,25.00f,24.00f,22.00f,22.00f,20.00f}; + + // deflection effectiveness curve fitted as second order + static constexpr const float ETA_POLY[] = {0.0535f, -0.2688f, 0.5817f}; // 1/rad + + // aerodynamic and physical constants + static constexpr const float P0 = 101325.0f; // _pressure at sea level [N/m^2]=[Pa] + static constexpr const float R = 287.04f; // real gas constant for air [J/kg/K] + static constexpr const float T0_K = 288.15f; // _temperature at sea level [K] + static constexpr const float TEMP_GRADIENT = -6.5e-3f; // _temperature gradient in degrees per metre + + static constexpr const float KV = M_PI_F; // total vortex lift parameter + static constexpr float CD0 = 0.04f; // no lift drag coefficient + static constexpr float CD90 = 1.98f; // 90 deg angle of attack drag coefficient + static constexpr float AF_DOT_MAX = M_PI_F / 2.0f; + static constexpr float ALPHA_BLEND = M_PI_F / 18.0f; // 10 degrees + + // here we make the distinction of the plate (i.e. wing, or tailplane, or fin) and the segment + // the segment can be a portion of the wing, but the aspect ratio (AR) of the wing needs to be used + float _alpha; // angle of attack [rad] + float _CL, _CD, _CM; // aerodynamic coefficients + float _CL_, _CD_, _CM_; // low aoa coeffs + float _f_blend; // blending function + matrix::Vector3f _p_B; // position of the aerodynamic center of the segment from _CM in body frame [m] + matrix::Dcmf _C_BS; // dcm from segment frame to body frame + float _ar; // aspect ratio of the plate + float _span; // _span of the segment + float _mac; // mean aerodynamic chord of the segment + float _alpha_0; // zero lift angle of attack [rad] + float _kp, _kn; + float _ate, _ale, _afte, _afle; // semi empirical coefficients for flat plates function of AR + float _tau_te, _tau_le, _fte, _fle; // leading and trailing edge functions + float _rho = 1.225f; // air density at current altitude [kg/m^3] + float _kD; // for parabolic drag model + const float K0 = 0.87f; // Oswald efficiency factor + // variables for flap model + float _eta_f; // flap effectiveness + float _def_a; // absolute value of the deflection angle + float _cf; // flap chord (control surface chord length) + float _theta_f, _tau_f; // check 3.2.3 in [2] + float _deltaCL, _dCLmax; // increase in lift coefficient + float _CLmax, _CLmin; // max and min lift value + float _alpha_eff_min; // min effective angle of attack + float _alpha_eff_max; // max effective angle of attack + float _alpha_min; // min angle of attack (stall angle) + float _alpha_max; // min angle of attack (stall angle) + float _alf0eff; // effective zero lift angle of attack + float _alfmeff; // effective maximum lift angle of attack + float _alpha_eff; // effectie angle of attack + float _alpha_eff_dot; // effectie angle of attack derivative + float _alpha_eff_old; // angle of attack [rad] + + float _pressure; // pressure in Pa at current altitude + float _temperature; // temperature in K at current altitude + float _prop_radius; // propeller radius [m], used to create the slipstream + float _v_slipstream; // slipstream velocity [m/s], computed from momentum theory + + matrix::Vector3f _Fa; // aerodynamic force + matrix::Vector3f _Ma; // aerodynamic moment computed at _CM directly + +public: + + AeroSeg() + { + AeroSeg(1.0f, 0.2f, 0.0f, matrix::Vector3f()); + } + + /** public explicit constructor + * AeroSeg(float span, float mac, float alpha_0_deg, matrix::Vector3f p_B, float dihedral_deg = 0.0f, + * float AR = -1.0f, float cf = 0.0f, float prop_radius=-1.0f, float cl_alpha=2.0f*M_PI_F, float alpha_max_deg=0.0f, float alpha_min_deg=0.0f) + * + * span_: span of the segment [m] + * mac_: mean aerodynamic chord of the segment [m] + * alpha_0_deg: zero lift angle of attack of the segment [deg], negative number represents a segment oriented up + * p_B_: position of the segment (mean aerodynamic center) in the body frame from the center of mass [m,m,m] + * dihedral_deg: dihedral angle of the segment [deg], set to 0 for tailplane, set to -90 for the fin. default is 0. + * AR: Aspect Ratio of the wing, or tailplane, or fin (not the segment). + * If the aspect ratio is negative, the aspect ratio is computed from the _span and MAC + * cf_: flap chord [m], this is the chord length of the control surface, default is zero (no flap). + * prop_radius_: radius of the propeller for slipstream computation. Setting to -1 (default) will assume no slipstream. + * cl_alpha: 2D lift curve slope (1/rad), default it 2*pi for a flat plate. This can be computed from http://airfoiltools.com for instance. + * alpha_max_deg: maximum angle of attack before stall. Setting to 0 (default) will compute it from a table for flat plate. + * alpha_min_deg: maximum negative angle of attack before stall. Setting to 0 (default) will compute it from a table for flat plate. + */ + explicit AeroSeg(float span, float mac, float alpha_0_deg, matrix::Vector3f p_B, float dihedral_deg = 0.0f, + float AR = -1.0f, float cf = 0.0f, float prop_radius = -1.0f, float cl_alpha = 2.0f * M_PI_F, + float alpha_max_deg = 0.0f, float alpha_min_deg = 0.0f) + { + static const float AR_tab[N_TAB] = {0.1666f, 0.333f, 0.4f, 0.5f, 1.0f, 1.25f, 2.0f, 3.0f, 4.0f, 6.0f}; + static const float ale_tab[N_TAB] = {3.00f, 3.64f, 4.48f, 7.18f, 10.20f, 13.38f, 14.84f, 14.49f, 9.95f, 12.93f, 15.00f, 15.00f}; + static const float ate_tab[N_TAB] = {5.90f, 15.51f, 32.57f, 39.44f, 48.22f, 59.29f, 21.55f, 7.74f, 7.05f, 5.26f, 6.50f, 6.50f}; + static const float afle_tab[N_TAB] = {59.00f, 58.60f, 58.20f, 50.00f, 41.53f, 26.70f, 23.44f, 21.00f, 18.63f, 14.28f, 11.60f, 10.00f}; + static const float afte_tab[N_TAB] = {59.00f, 58.60f, 58.20f, 51.85f, 41.46f, 28.09f, 39.40f, 35.86f, 26.76f, 19.76f, 16.43f, 14.00f}; + static const float afs_tab[N_TAB] = {49.00f, 54.00f, 56.00f, 48.00f, 40.00f, 29.00f, 27.00f, 25.00f, 24.00f, 22.00f, 22.00f, 20.00f}; + + _span = span; + _mac = mac; + _alpha_0 = math::radians(alpha_0_deg); + _p_B = matrix::Vector3f(p_B); + _ar = (AR <= 0.0f) ? _span / _mac : AR; // setting AR<=0 will compute it from _span and _mac + _alpha_eff = 0.0f; + _alpha_eff_old = 0.0f; + _kp = cl_alpha / (1.0f + 2.0f * (_ar + 4.0f) / (_ar * (_ar + 2.0f))); + _kn = 0.41f * (1.0f - expf(-17.0f / _ar)); + _ale = lin_interp_lkt(AR_tab, ale_tab, _ar, N_TAB); + _ate = lin_interp_lkt(AR_tab, ate_tab, _ar, N_TAB); + _afle = lin_interp_lkt(AR_tab, afle_tab, _ar, N_TAB); + _afte = lin_interp_lkt(AR_tab, afte_tab, _ar, N_TAB); + float afs_rad = math::radians(lin_interp_lkt(AR_tab, afs_tab, _ar, N_TAB)); + + if (fabsf(alpha_max_deg) < 1.0e-3f) { + _alpha_max = afs_rad; + + } else { + _alpha_max = math::radians(alpha_max_deg); + } + + if (fabsf(alpha_min_deg) < 1.0e-3f) { + _alpha_min = -afs_rad; + + } else { + _alpha_min = math::radians(alpha_min_deg); + } + + _cf = math::constrain(cf, 0.0f, mac); + _C_BS = matrix::Dcmf(matrix::Eulerf(math::radians(dihedral_deg), 0.0f, 0.0f)); + _prop_radius = prop_radius; + _kD = 1.0f / (M_PI_F * K0 * _ar); + } + + /** aerodynamic force and moments of a generic flate plate segment + * void update_aero(matrix::Vector3f v_B, matrix::Vector3f w_B, float alt = 0.0f, + * float def = 0.0f, float thrust=0.0f, float dt = -1.0f) + * + * v_B: 3D velocity in body frame [m/s], (front, right, down FRD frame) + * w_B: 3D body rates in body frame [rad/s], FRD frame. + * alt: altitude above mean sea level for computing air density [m], default is 0. + * def: flap deflection angle [rad], default is 0. + * thrust: thrust force [N] from the propeller to compute the slipstream velocity, default is 0. + */ + void update_aero(matrix::Vector3f v_B, matrix::Vector3f w_B, float alt = 0.0f, float def = 0.0f, float thrust = 0.0f) + { + // ISA model taken from Mustafa Cavcar, Anadolu University, Turkey + _pressure = P0 * powf(1.0f - 0.0065f * alt / T0_K, 5.2561f); + _temperature = T0_K + TEMP_GRADIENT * alt; + _rho = _pressure / R / _temperature; + + matrix::Vector3f vel = _C_BS.transpose() * (v_B + w_B % _p_B); // velocity in segment frame + + if (_prop_radius > 1e-4f) { + // Add velocity generated from the propeller and thrust force. + // Computed from momentum theory. + // For info, the diameter of the slipstream is sqrt(2)*_prop_radius, + // this should be the width of the segment in the slipstream. + vel(0) += sqrtf(2.0f * thrust / (_rho * M_PI_F * _prop_radius * _prop_radius)); + } + + float vxz2 = vel(0) * vel(0) + vel(2) * vel(2); + + if (vxz2 < 0.01f) { + _Fa = matrix::Vector3f(); + _Ma = matrix::Vector3f(); + return; + } + + _alpha = atan2f(vel(2), vel(0)) - _alpha_0; + aoa_coeff(_alpha, sqrtf(vxz2), def); + _Fa = _C_BS * (0.5f * _rho * vxz2 * _span * _mac) * matrix::Vector3f(_CL * sinf(_alpha) - _CD * cosf(_alpha), + 0.0f, + -_CL * cosf(_alpha) - _CD * sinf(_alpha)); + _Ma = _C_BS * (0.5f * _rho * vxz2 * _span * _mac * _mac) * matrix::Vector3f(0.0f, _CM, + 0.0f) + _p_B % _Fa; // computed at vehicle _CM + } + + // return the air density at current altitude, must be called after update_aero() + float get_rho() const { return _rho; } + + // return the sum of aerodynamic forces of the segment in the body frame, taken at the _CM, + // must be called after update_aero() + matrix::Vector3f get_Fa() const { return _Fa; } + + // return the sum of aerodynamic moments of the segment in the body frame, taken at the _CM, + // must be called after update_aero() + matrix::Vector3f get_Ma() const { return _Ma; } + +private: + + // low angle of attack and stalling region coefficient based on flat plate + void aoa_coeff(float a, float vxz, float def) + { + _alpha_eff_old = _alpha_eff; + _tau_te = (vxz > 0.01f) ? 4.5f * _mac / vxz : 0.0f; + _tau_le = (vxz > 0.01f) ? 0.5f * _mac / vxz : 0.0f; + + // model for the control surface deflection + if (_cf / _mac < 0.999f) { + _def_a = fminf(fabsf(def), math::radians(70.0f)); + _eta_f = _def_a * _def_a * ETA_POLY[0] + _def_a * ETA_POLY[1] + ETA_POLY[2]; // second order fit + _theta_f = acosf(2.0f * _cf / _mac - 1.0f); + _tau_f = 1.0f - (_theta_f - sinf(_theta_f)) / M_PI_F; + _deltaCL = _kp * _tau_f * _eta_f * def; + _dCLmax = (1.0f - _cf / _mac) * _deltaCL; + _alf0eff = solve_alpha_eff(_kp, KV, _deltaCL, _alpha_0); + _alpha_eff = a - _alf0eff; + + // this doesn't seem to work, so let's comment it + // _alpha_eff_dot = math::constrain((_alpha_eff - _alpha_eff_old) / dt, -AF_DOT_MAX, AF_DOT_MAX); + // _fte = 0.5f * (1.0f - tanhf(_ate * ((_alpha_eff) - _tau_te * (_alpha_eff_dot) + // - math::radians(_afte)))); // normalized trailing edge separation + // _fle = 0.5f * (1.0f - tanhf(_ale * ((_alpha_eff) - _tau_le * (_alpha_eff_dot) + // - math::radians(_afle)))); // normalized leading edge separation + + _fte = 1.0f; + _fle = 1.0f; + _CLmax = fCL(_alpha_max - _alpha_0) + _dCLmax; + _alpha_eff_max = _alf0eff - solve_alpha_eff(_kp, KV * _fle * _fle, + _CLmax / (0.25f * (1.0f + sqrtf(_fte)) * (1.0f + sqrtf(_fte))), _alpha_max - _alpha_0); + _CLmin = fCL(_alpha_min - _alpha_0) + _dCLmax; + _alpha_eff_min = _alf0eff - solve_alpha_eff(_kp, KV * _fle * _fle, + _CLmin / (0.25f * (1.0f + sqrtf(_fte)) * (1.0f + sqrtf(_fte))), _alpha_min - _alpha_0); + + } else { // this segment is a full flap + _alpha_eff = a + def; + + // this doesn't seem to work, so let's comment it + // _alpha_eff_dot = math::constrain((_alpha_eff - _alpha_eff_old) / dt, -AF_DOT_MAX, AF_DOT_MAX); + // _fte = 0.5f * (1.0f - tanhf(_ate * ((_alpha_eff) - _tau_te * (_alpha_eff_dot) + // - math::radians(_afte)))); // normalized trailing edge separation + // _fle = 0.5f * (1.0f - tanhf(_ale * ((_alpha_eff) - _tau_le * (_alpha_eff_dot) + // - math::radians(_afle)))); // normalized leading edge separation + + _fte = 1.0f; + _fle = 1.0f; + _alpha_eff_max = _alpha_max; + _alpha_eff_min = _alpha_min; + } + + // compute the aerodynamic coefficients + high_aoa_coeff(_alpha_eff, def); + _CL_ = fCL(_alpha_eff); + _CD_ = CD0 + fabsf(_CL * tanf(_alpha_eff)); + // _CD_ = CD0 + _kD*_CL_*_CL_; // alternative method + _CM_ = -fCM(_alpha_eff); + + // blending function + if (_alpha_eff > 0.0f) { + _f_blend = 0.5f * (1.0f - tanhf(4.0f * (_alpha_eff - _alpha_eff_max) / ALPHA_BLEND)); + + } else { + _f_blend = 0.5f * (1.0f - tanhf(-4.0f * (_alpha_eff - _alpha_eff_min) / ALPHA_BLEND)); + } + + _CL = _CL_ * _f_blend + _CL * (1.0f - _f_blend); + _CD = _CD_ * _f_blend + _CD * (1.0f - _f_blend); + _CM = _CM_ * _f_blend + _CM * (1.0f - _f_blend); + } + + // high angle of attack coefficient based on flat plate + void high_aoa_coeff(float a, float def = 0.0f) + { + float mac_eff = sqrtf((_mac - _cf) * (_mac - _cf) + _cf * _cf + 2.0f * (_mac - _cf) * _cf * cosf(fabsf(def))); + a += asinf(_cf / mac_eff * sinf(def)); + float cd90_eff = CD90 + 0.21f * def - 0.0426f * def * + def; // this might not be accurate for lower flap chord to chord ratio + // normal coeff + float CN = cd90_eff * sinf(a) * (1.0f / (0.56f + 0.44f * sinf(fabsf(a))) - _kn); + // tengential coeff + float CT = 0.5f * CD0 * cosf(a); + _CL = CN * cosf(a) - CT * sinf(a); + _CD = CN * sinf(a) + CT * cosf(a); + _CM = -CN * (0.25f - 7.0f / 40.0f * (1.0f - 2.0f / M_PI_F * fabsf(a))); + } + + // linear interpolation between 2 points + static float lin_interp(const float x0, const float y0, const float x1, const float y1, const float x) + { + if (x < x0) { + return y0; + } + + if (x > x1) { + return y1; + } + + float slope = (y1 - y0) / (x1 - x0); + return y0 + slope * (x - x0); + } + + // lookup table linear interpolation + static float lin_interp_lkt(const float x_tab [], const float y_tab [], const float x, const int length) + { + if (x < x_tab[0]) { + return y_tab[0]; + } + + if (x > x_tab[length - 1]) { + return y_tab [length - 1]; + } + + int i = length - 2; + + while (x_tab[i] > x) { + i--; + } + + return lin_interp(x_tab[i], y_tab[i], x_tab[i + 1], y_tab[i + 1], x); + } + + float solve_alpha_eff(const float Kp, const float Kv, const float dCL, const float a0) + { + // we use here the Newton method with explicit derivative to find the root of equation 3.15 + float a = a0; // init the search + + for (int i = 0; i < 3; i++) { + a = a - (-Kp * sinf(a) * cosf(a) * cosf(a) - Kv * fabsf(sinf(a)) * sinf(a) * cosf(a) - dCL) / + (Kv * fabsf(sinf(a)) * sinf(a) * sinf(a) - Kv * fabsf(sinf(a)) * cosf(a) * cosf(a) - Kp * cosf(a) * cosf(a) * cosf( + a) + 2 * Kp * cosf(a) * sinf(a) * sinf(a) - Kv * matrix::sign(sinf(a)) * cosf(a) * cosf(a) * sinf(a)); + } + + return a; + } + + float fCL(float a) + { + return 0.25f * (1.0f + sqrtf(_fte)) * (1.0f + sqrtf(_fte)) * (_kp * sinf(a) * cosf(a) * cosf(a) + + _fle * _fle * KV * fabsf(sinf(a)) * sinf(a) * cosf(a)); + } + + float fCM(float a) + { + return -0.25f * (1.0f + sqrtf(_fte)) * (1.0f + sqrtf(_fte)) * 0.0625f * (-1.0f + 6.0f * sqrtf( + _fte) - 5.0f * _fte) * _kp * sinf(a) * cosf(a) + + 0.17f * _fle * _fle * KV * fabsf(sinf(a)) * sinf(a); + } + + // AeroSeg operator*(const float k) const { + // return AeroSeg(p_I*k, v_I*k, q*k, w_B*k); + // } + + // AeroSeg operator+(const States other) const { + // return AeroSeg(p_I+other.p_I, v_I+other.v_I, q+other.q, w_B+other.w_B); + // } + + // void unwrap_states(matrix::Vector3f &p, matrix::Vector3f &v, matrix::Quatf &q_, matrix::Vector3f &w) { + // p=p_I; + // v=v_I; + // q_=q; + // w=w_B; + // } + +}; // --------------------------------------------------------------------------------------------------------- diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index 3cdac7302f..f150713e5e 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -40,6 +40,7 @@ * Coriolis g Corporation - January 2019 */ +#include "aero.hpp" #include "sih.hpp" #include @@ -67,8 +68,10 @@ Sih::Sih() : const hrt_abstime task_start = hrt_absolute_time(); _last_run = task_start; _gps_time = task_start; + _airspeed_time = task_start; _gt_time = task_start; _dist_snsr_time = task_start; + _vehicle = (VehicleType)constrain(_sih_vtype.get(), 0, 1); } Sih::~Sih() @@ -149,6 +152,11 @@ void Sih::Run() send_gps(); } + if (_vehicle == VehicleType::FW && _now - _airspeed_time >= 50_ms) { + _airspeed_time = _now; + send_airspeed(); + } + // distance sensor published at 50 Hz if (_now - _dist_snsr_time >= 20_ms && fabs(_distance_snsr_override) < 10000) { @@ -254,10 +262,17 @@ void Sih::read_motors() { actuator_outputs_s actuators_out; + float pwm_middle = 0.5f * (PWM_DEFAULT_MIN + PWM_DEFAULT_MAX); + if (_actuator_out_sub.update(&actuators_out)) { for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals - float u_sp = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), 0.0f, 1.0f); - _u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau + if (_vehicle == VehicleType::FW && i < 3) { // control surfaces in range [-1,1] + _u[i] = constrain(2.0f * (actuators_out.output[i] - pwm_middle) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), -1.0f, 1.0f); + + } else { // throttle signals in range [0,1] + float u_sp = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), 0.0f, 1.0f); + _u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau + } } } } @@ -265,13 +280,35 @@ void Sih::read_motors() // generate the motors thrust and torque in the body frame void Sih::generate_force_and_torques() { - _T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3])); - _Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]), - _L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]), - _Q_MAX * (+_u[0] + _u[1] - _u[2] - _u[3])); + if (_vehicle == VehicleType::MC) { + _T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3])); + _Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]), + _L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]), + _Q_MAX * (+_u[0] + _u[1] - _u[2] - _u[3])); + _Fa_I = -_KDV * _v_I; // first order drag to slow down the aircraft + _Ma_B = -_KDW * _w_B; // first order angular damper - _Fa_I = -_KDV * _v_I; // first order drag to slow down the aircraft - _Ma_B = -_KDW * _w_B; // first order angular damper + } else if (_vehicle == VehicleType::FW) { + _T_B = Vector3f(_T_MAX * _u[3], 0.0f, 0.0f); // forward thruster + // _Mt_B = Vector3f(_Q_MAX*_u[3], 0.0f,0.0f); // thruster torque + _Mt_B = Vector3f(); + generate_aerodynamics(); + } + +} + +void Sih::generate_aerodynamics() +{ + _v_B = _C_IB.transpose() * _v_I; // velocity in body frame [m/s] + float altitude = _H0 - _p_I(2); + _wing_l.update_aero(_v_B, _w_B, altitude, _u[0]*FLAP_MAX); + _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 + // _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 } // apply the equations of motion of a rigid body and integrate one step @@ -282,40 +319,79 @@ void Sih::equations_of_motion() // Equations of motion of a rigid body _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 + // _q_dot = _q.derivative1(_w_B); // attitude differential + _dq = 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 if (_p_I(2) > 0.0f && (_v_I_dot(2) > 0.0f || _v_I(2) > 0.0f)) { - if (!_grounded) { // if we just hit the floor - // for the accelerometer, compute the acceleration that will stop the vehicle in one time step - _v_I_dot = -_v_I / _dt; + if (_vehicle == VehicleType::MC) { + if (!_grounded) { // if we just hit the floor + // for the accelerometer, compute the acceleration that will stop the vehicle in one time step + _v_I_dot = -_v_I / _dt; - } else { - _v_I_dot.setZero(); + } else { + _v_I_dot.setZero(); + } + + _v_I.setZero(); + _w_B.setZero(); + _grounded = true; + + } else if (_vehicle == VehicleType::FW) { + if (!_grounded) { // if we just hit the floor + // for the accelerometer, compute the acceleration that will stop the vehicle in one time step + _v_I_dot(2) = -_v_I(2) / _dt; + + } else { + // we only allow negative acceleration in order to takeoff + _v_I_dot(2) = fminf(_v_I_dot(2), 0.0f); + } + + // integration: Euler forward + _p_I = _p_I + _p_I_dot * _dt; + _v_I = _v_I + _v_I_dot * _dt; + Eulerf RPY = Eulerf(_q); + RPY(0) = 0.0f; // no roll + RPY(1) = radians(0.0f); // pitch slightly up to get some lift + _q = Quatf(RPY); + _w_B.setZero(); + _grounded = true; } - _v_I.setZero(); - _w_B.setZero(); - _grounded = true; - } else { // integration: Euler forward _p_I = _p_I + _p_I_dot * _dt; _v_I = _v_I + _v_I_dot * _dt; - _q = _q + _q_dot * _dt; // as given in attitude_estimator_q_main.cpp + _q = _q * _dq; // as given in attitude_estimator_q_main.cpp _q.normalize(); - _w_B = _w_B + _w_B_dot * _dt; + // integration Runge-Kutta 4 + // rk4_update(_p_I, _v_I, _q, _w_B); + _w_B = constrain(_w_B + _w_B_dot * _dt, -6.0f * M_PI_F, 6.0f * M_PI_F); _grounded = false; } } +// Sih::States Sih::eom_f(States x) // equations of motion f: x'=f(x) +// { +// States x_dot{}; // dx/dt + +// Dcmf C_IB = matrix::Dcm(x.q); // body to inertial transformation +// // Equations of motion of a rigid body +// x_dot.p_I = x.v_I; // position differential +// x_dot.v_I = (_W_I + _Fa_I + C_IB * _T_B) / _MASS; // conservation of linear momentum +// x_dot.q = x.q.derivative1(x.w_B); // attitude differential +// x_dot.w_B = _Im1 * (_Mt_B + _Ma_B - x.w_B.cross(_I * x.w_B)); // conservation of angular momentum + +// return x_dot; +// } + // reconstruct the noisy sensor signals void Sih::reconstruct_sensors_signals() { - // The sensor signals reconstruction and noise levels are from - // Bulka, Eitan, and Meyer Nahon. "Autonomous fixed-wing aerobatics: from theory to flight." - // In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018. + // The sensor signals reconstruction and noise levels are from [1] + // [1] Bulka, Eitan, and Meyer Nahon. "Autonomous fixed-wing aerobatics: from theory to flight." + // In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018. // IMU _acc = _C_IB.transpose() * (_v_I_dot - Vector3f(0.0f, 0.0f, CONSTANTS_ONE_G)) + noiseGauss3f(0.5f, 1.7f, 1.4f); @@ -376,6 +452,17 @@ void Sih::send_gps() _sensor_gps_pub.publish(_sensor_gps); } +void Sih::send_airspeed() +{ + airspeed_s airspeed{}; + airspeed.timestamp = _now; + airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_B(0) + generate_wgn() * 0.2f); + airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO); + airspeed.air_temperature_celsius = _baro_temp_c; + airspeed.confidence = 0.7f; + _airspeed_pub.publish(airspeed); +} + void Sih::send_dist_snsr() { _distance_snsr.timestamp = _now; @@ -430,6 +517,22 @@ 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: @@ -465,6 +568,37 @@ Vector3f Sih::noiseGauss3f(float stdx, float stdy, float stdz) return Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz); } +int Sih::print_status() +{ + if (_vehicle == VehicleType::MC) { + PX4_INFO("Running MC"); + + } else { + PX4_INFO("Running FW"); + } + + PX4_INFO("vehicle landed: %d", _grounded); + PX4_INFO("dt [us]: %d", (int)(_dt * 1e6f)); + PX4_INFO("inertial position NED (m)"); + _p_I.print(); + PX4_INFO("inertial velocity NED (m/s)"); + _v_I.print(); + PX4_INFO("attitude roll-pitch-yaw (deg)"); + (Eulerf(_q) * 180.0f / M_PI_F).print(); + PX4_INFO("angular acceleration roll-pitch-yaw (deg/s)"); + (_w_B * 180.0f / M_PI_F).print(); + PX4_INFO("actuator signals"); + Vector u = Vector(_u); + u.transpose().print(); + PX4_INFO("Aerodynamic forces NED inertial (N)"); + _Fa_I.print(); + PX4_INFO("Aerodynamic moments body frame (Nm)"); + _Ma_B.print(); + PX4_INFO("v_I.z: %f", (double)_v_I(2)); + PX4_INFO("v_I_dot.z: %f", (double)_v_I_dot(2)); + return 0; +} + int Sih::task_spawn(int argc, char *argv[]) { Sih *instance = new Sih(); diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index 87bbd3ee0e..4718f07b0a 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -31,6 +31,25 @@ * ****************************************************************************/ +/** + * @file sih.hpp + * Simulator in Hardware + * + * @author Romain Chiappinelli + * + * Coriolis g Corporation - January 2019 + */ + +// The sensor signals reconstruction and noise levels are from [1] +// [1] Bulka E, and Nahon M, "Autonomous fixed-wing aerobatics: from theory to flight." +// In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018. +// The aerodynamic model is from [2] +// [2] Khan W, supervised by Nahon M, "Dynamics modeling of agile fixed-wing unmanned aerial vehicles." +// McGill University, PhD thesis, 2016. +// The quaternion integration are from [3] +// [3] Sveier A, Sjøberg AM, Egeland O. "Applied Runge–Kutta–Munthe-Kaas Integration for the Quaternion Kinematics." +// Journal of Guidance, Control, and Dynamics. 2019 Dec;42(12):2747-54. + #pragma once #include @@ -57,6 +76,7 @@ #include // to publish groundtruth #include // to publish groundtruth #include +#include using namespace time_literals; @@ -72,6 +92,9 @@ public: /** @see ModuleBase */ static int custom_command(int argc, char *argv[]); + /** @see ModuleBase::print_status() */ + int print_status() override; + /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); @@ -113,6 +136,9 @@ private: vehicle_global_position_s _gpos_gt{}; uORB::Publication _gpos_gt_pub{ORB_ID(vehicle_global_position_groundtruth)}; + // airspeed + uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs)}; @@ -121,6 +147,12 @@ private: static constexpr float T1_C = 15.0f; // ground temperature in celcius static constexpr float T1_K = T1_C - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // ground temperature in Kelvin static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre + // Aerodynamic coefficients + static constexpr float RHO = 1.225f; // air density at sea level [kg/m^3] + static constexpr float SPAN = 0.86f; // wing span [m] + static constexpr float MAC = 0.21f; // wing mean aerodynamic chord [m] + static constexpr float RP = 0.1f; // radius of the propeller [m] + static constexpr float FLAP_MAX = M_PI_F / 12.0f; // 15 deg, maximum control surface deflection void init_variables(); void gps_fix(); @@ -130,8 +162,14 @@ private: void equations_of_motion(); void reconstruct_sensors_signals(); void send_gps(); + void send_airspeed(); 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")}; @@ -139,6 +177,7 @@ private: hrt_abstime _last_run{0}; hrt_abstime _baro_time{0}; hrt_abstime _gps_time{0}; + hrt_abstime _airspeed_time{0}; hrt_abstime _mag_time{0}; hrt_abstime _gt_time{0}; hrt_abstime _dist_snsr_time{0}; @@ -158,10 +197,20 @@ private: matrix::Quatf _q; // quaternion attitude matrix::Dcmf _C_IB; // body to inertial transformation matrix::Vector3f _w_B; // body rates in body frame [rad/s] - matrix::Quatf _q_dot; // quaternion differential + matrix::Quatf _dq; // quaternion differential matrix::Vector3f _w_B_dot; // body rates differential - float _u[NB_MOTORS]; // thruster signals + float _u[NB_MOTORS]; // thruster signals + enum class VehicleType {MC, FW}; + VehicleType _vehicle = VehicleType::MC; + + // aerodynamic segments for the fixedwing + AeroSeg _wing_l = AeroSeg(SPAN / 2.0f, MAC, -4.0f, matrix::Vector3f(0.0f, -SPAN / 4.0f, 0.0f), 3.0f, + SPAN / MAC, MAC / 3.0f); + 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); // sensors reconstruction matrix::Vector3f _acc; @@ -217,6 +266,7 @@ private: (ParamFloat) _sih_distance_snsr_min, (ParamFloat) _sih_distance_snsr_max, (ParamFloat) _sih_distance_snsr_override, - (ParamFloat) _sih_thrust_tau + (ParamFloat) _sih_thrust_tau, + (ParamInt) _sih_vtype ) }; diff --git a/src/modules/sih/sih_params.c b/src/modules/sih/sih_params.c index 92542715dc..80b6b26e3e 100644 --- a/src/modules/sih/sih_params.c +++ b/src/modules/sih/sih_params.c @@ -435,3 +435,13 @@ PARAM_DEFINE_FLOAT(SIH_DISTSNSR_OVR, -1.0f); * @group Simulation In Hardware */ PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f); + +/** + * Vehicle type + * + * @value 0 MC + * @value 1 FW + * @reboot_required true + * @group Simulation In Hardware + */ +PARAM_DEFINE_INT32(SIH_VEHICLE_TYPE, 0);