From 4e06b40e2bc90bbbfee562dd8a43db75b81020f4 Mon Sep 17 00:00:00 2001 From: romain-chiap Date: Tue, 11 Jan 2022 08:29:19 +0100 Subject: [PATCH] sih: add tailsitter support, disable UAVCAN --- .../init.d/airframes/1100_rc_quad_x_sih.hil | 2 + .../init.d/airframes/1101_rc_plane_sih.hil | 2 + .../airframes/1102_tailsitter_duo_sih.hil | 61 ++++++++++++++++ .../init.d/airframes/CMakeLists.txt | 1 + ROMFS/px4fmu_common/mixers/CMakeLists.txt | 1 + .../mixers/vtol_tailsitter_duo_sat.main.mix | 39 +++++++++++ Tools/jmavsim_run.sh | 7 +- src/modules/sih/aero.hpp | 41 +++++++---- src/modules/sih/sih.cpp | 70 +++++++++++++++---- src/modules/sih/sih.hpp | 47 +++++++++++-- src/modules/sih/sih_params.c | 5 +- 11 files changed, 239 insertions(+), 37 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil create mode 100644 ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo_sat.main.mix 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 58afd8b169..b0f2b92862 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 @@ -15,6 +15,8 @@ set MIXER quad_x set PWM_OUT 1234 +param set UAVCAN_ENABLE 0 + # set SYS_HITL to 2 to start the SIH and avoid sensors startup param set SYS_HITL 2 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 index 5bc40c1246..b390cde021 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil @@ -15,6 +15,8 @@ set MIXER AERT set PWM_OUT 1234 +param set UAVCAN_ENABLE 0 + # set SYS_HITL to 2 to start the SIH and avoid sensors startup param set-default SYS_HITL 2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil new file mode 100644 index 0000000000..50d2d17fda --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil @@ -0,0 +1,61 @@ +#!/bin/sh +# +# @name SIH Tailsitter Duo +# +# @type Simulation +# @class VTOL +# +# @output MAIN1 motor right +# @output MAIN2 motor left +# @output MAIN5 elevon right +# @output MAIN6 elevon left +# +# @maintainer Romain Chiappinelli +# +# @board px4_fmu-v2 exclude +# + +. ${R}etc/init.d/rc.vtol_defaults + +param set UAVCAN_ENABLE 0 +param set-default VT_ELEV_MC_LOCK 0 +param set-default VT_MOT_COUNT 2 +param set-default VT_TYPE 0 +param set-default VT_FW_DIFTHR_EN 1 +param set-default VT_FW_DIFTHR_SC 0.3 +param set-default MPC_MAN_Y_MAX 60 +param set-default MC_PITCH_P 5 + +param set-default MAV_TYPE 19 +set MAV_TYPE 19 +set MIXER vtol_tailsitter_duo_sat + +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 2.0 +param set SIH_Q_MAX 0.0165 +param set SIH_MASS 0.2 +# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg +param set SIH_IXX 0.00354 +param set SIH_IYY 0.000625 +param set SIH_IZZ 0.00300 +param set SIH_IXZ 0.0 +param set SIH_KDV 0.2 +param set SIH_L_ROLL 0.145 + +# sih as tailsitter +param set SIH_VEHICLE_TYPE 2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 64b114c1aa..d036b5add3 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -40,6 +40,7 @@ px4_add_romfs_files( 1002_standard_vtol.hil 1100_rc_quad_x_sih.hil 1101_rc_plane_sih.hil + 1102_tailsitter_duo_sih.hil # [2000, 2999] Standard planes" 2100_standard_plane diff --git a/ROMFS/px4fmu_common/mixers/CMakeLists.txt b/ROMFS/px4fmu_common/mixers/CMakeLists.txt index 54c6ca32ee..a960ef664e 100644 --- a/ROMFS/px4fmu_common/mixers/CMakeLists.txt +++ b/ROMFS/px4fmu_common/mixers/CMakeLists.txt @@ -90,4 +90,5 @@ px4_add_romfs_files( vtol_convergence.main.mix vtol_delta.aux.mix vtol_tailsitter_duo.main.mix + vtol_tailsitter_duo_sat.main.mix ) diff --git a/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo_sat.main.mix b/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo_sat.main.mix new file mode 100644 index 0000000000..2a9e3197c1 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo_sat.main.mix @@ -0,0 +1,39 @@ +Tailsitter duo mixer +============================ + +This file defines a mixer for a generic duo tailsitter VTOL (eg TBS Caipirinha tailsitter edition). This vehicle +has two motors in total, one attached to each wing. It also has two elevons which +are located in the slipstream of the propellers. This mixer generates 4 PWM outputs +on the main PWM ouput port, two at 400Hz for the motors, and two at 50Hz for the +elevon servos. Channels 1-4 are configured to run at 400Hz, while channels 5-8 run +at the default rate of 50Hz. Note that channels 3 and 4 are assigned but not used. + +Motor mixer +------------ +Channel 1 connects to the right (starboard) motor. +Channel 2 connects to the left (port) motor. + +R: 2- + +Zero mixer (2x) +--------------- +Channels 3,4 are unused. + +Z: + +Z: + +Elevons mixer +-------------- +Channel 5 connects to the right (starboard) elevon. +Channel 6 connects to the left (port) elevon. +Here we saturate the elevons before their full range +to avoid roll-pitch-yaw coupling during faster maneuvers + +M: 2 +S: 1 0 10000 10000 0 -6000 6000 +S: 1 1 10000 10000 0 -6000 6000 + +M: 2 +S: 1 0 10000 10000 0 -6000 6000 +S: 1 1 -10000 -10000 0 -6000 6000 diff --git a/Tools/jmavsim_run.sh b/Tools/jmavsim_run.sh index f848e17531..7413e592d6 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:loa" opt; do +while getopts ":b:d:p:qsr:f:i:loat" opt; do case $opt in b) baudrate=$OPTARG @@ -40,7 +40,10 @@ while getopts ":b:d:p:qsr:f:i:loa" opt; do extra_args="$extra_args -disponly" ;; a) - extra_args="$extra_args -fw" # aircraft + extra_args="$extra_args -fw" # aircraft model + ;; + t) + extra_args="$extra_args -ts" # tailsitter model ;; \?) echo "Invalid option: -$OPTARG" >&2 diff --git a/src/modules/sih/aero.hpp b/src/modules/sih/aero.hpp index c34ce88096..e2905e2dc5 100644 --- a/src/modules/sih/aero.hpp +++ b/src/modules/sih/aero.hpp @@ -124,18 +124,19 @@ private: 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 _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_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 + // 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 + matrix::Vector3f _v_S; // velocity in segment frame public: @@ -144,7 +145,7 @@ public: AeroSeg(1.0f, 0.2f, 0.0f, matrix::Vector3f()); } - /** public explicit constructor + /** public 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) * @@ -161,9 +162,9 @@ public: * 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) + 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}; @@ -217,32 +218,35 @@ public: * 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) + void update_aero(const matrix::Vector3f &v_B, const 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 + _v_S = _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)); + _v_S(0) += sqrtf(2.0f * thrust / (_rho * M_PI_F * _prop_radius * _prop_radius)); } - float vxz2 = vel(0) * vel(0) + vel(2) * vel(2); + float vxz2 = _v_S(0) * _v_S(0) + _v_S(2) * _v_S(2); if (vxz2 < 0.01f) { _Fa = matrix::Vector3f(); _Ma = matrix::Vector3f(); + _alpha = 0.0f; return; } - _alpha = atan2f(vel(2), vel(0)) - _alpha_0; + _alpha = matrix::wrap_pi(atan2f(_v_S(2), _v_S(0)) - _alpha_0); + // _alpha = atan2f(_v_S(2), _v_S(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, @@ -254,6 +258,12 @@ public: // return the air density at current altitude, must be called after update_aero() float get_rho() const { return _rho; } + // return angle of attack in radians + float get_aoa() const {return _alpha;} + + // return the aspect ratio + float get_ar() const {return _ar;} + // 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; } @@ -262,6 +272,9 @@ public: // must be called after update_aero() matrix::Vector3f get_Ma() const { return _Ma; } + // return the velocity in segment frame + matrix::Vector3f get_vS() const { return _v_S; } + private: // low angle of attack and stalling region coefficient based on flat plate @@ -412,8 +425,8 @@ private: + 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 AeroSeg&) const { + // return this; // } // AeroSeg operator+(const States other) const { diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index de41261e5c..46e8396607 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -72,7 +72,7 @@ Sih::Sih() : _gt_time = task_start; _dist_snsr_time = task_start; _vehicle = (VehicleType)constrain(_sih_vtype.get(), static_cast(0), - static_cast(1)); + static_cast(2)); } Sih::~Sih() @@ -199,7 +199,8 @@ void Sih::parameters_updated() _I(0, 2) = _I(2, 0) = _sih_ixz.get(); _I(1, 2) = _I(2, 1) = _sih_iyz.get(); - _Im1 = inv(_I); + // guards against too small determinants + _Im1 = 100.0f * inv(static_cast(100.0f * _I)); _mu_I = Vector3f(_sih_mu_x.get(), _sih_mu_y.get(), _sih_mu_z.get()); @@ -267,7 +268,8 @@ void Sih::read_motors() if (_actuator_out_sub.update(&actuators_out)) { for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals - if (_vehicle == VehicleType::FW && i < 3) { // control surfaces in range [-1,1] + if ((_vehicle == VehicleType::FW && i < 3) || (_vehicle == VehicleType::TS + && 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] @@ -293,12 +295,19 @@ void Sih::generate_force_and_torques() _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(); - } + generate_fw_aerodynamics(); + + } else if (_vehicle == VehicleType::TS) { + _T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (_u[0] + _u[1])); + _Mt_B = Vector3f(_L_ROLL * _T_MAX * (_u[1] - _u[0]), 0.0f, _Q_MAX * (_u[1] - _u[0])); + generate_ts_aerodynamics(); + // _Fa_I = -_KDV * _v_I; // first order drag to slow down the aircraft + // _Ma_B = -_KDW * _w_B; // first order angular damper + } } -void Sih::generate_aerodynamics() +void Sih::generate_fw_aerodynamics() { _v_B = _C_IB.transpose() * _v_I; // velocity in body frame [m/s] float altitude = _H0 - _p_I(2); @@ -309,11 +318,36 @@ void Sih::generate_aerodynamics() _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() + _fuselage.get_Ma() - _KDW * _w_B; // aerodynamic moments } +void Sih::generate_ts_aerodynamics() +{ + _v_B = _C_IB.transpose() * _v_I; // velocity in body frame [m/s] + Vector3f Fa_ts = Vector3f(); + Vector3f Ma_ts = Vector3f(); + Vector3f v_ts = _C_BS.transpose() * + _v_B; // the aerodynamic is resolved in a frame like a standard aircraft (nose-right-belly) + Vector3f w_ts = _C_BS.transpose() * _w_B; + float altitude = _H0 - _p_I(2); + + for (int i = 0; i < NB_TS_SEG; i++) { + if (i <= NB_TS_SEG / 2) { + _ts[i].update_aero(v_ts, w_ts, altitude, _u[5]*TS_DEF_MAX, _T_MAX * _u[1]); + + } else { + _ts[i].update_aero(v_ts, w_ts, altitude, -_u[4]*TS_DEF_MAX, _T_MAX * _u[0]); + } + + Fa_ts += _ts[i].get_Fa(); + Ma_ts += _ts[i].get_Ma(); + } + + _Fa_I = _C_IB * _C_BS * Fa_ts - _KDV * _v_I; // sum of aerodynamic forces + _Ma_B = _C_BS * Ma_ts - _KDW * _w_B; // aerodynamic moments +} + // apply the equations of motion of a rigid body and integrate one step void Sih::equations_of_motion() { @@ -328,7 +362,7 @@ void Sih::equations_of_motion() // fake ground, avoid free fall if (_p_I(2) > 0.0f && (_v_I_dot(2) > 0.0f || _v_I(2) > 0.0f)) { - if (_vehicle == VehicleType::MC) { + if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS) { 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; @@ -356,7 +390,7 @@ void Sih::equations_of_motion() _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 + RPY(1) = radians(0.0f); // pitch slightly up if needed to get some lift _q = Quatf(RPY); _w_B.setZero(); _grounded = true; @@ -366,7 +400,7 @@ void Sih::equations_of_motion() // integration: Euler forward _p_I = _p_I + _p_I_dot * _dt; _v_I = _v_I + _v_I_dot * _dt; - _q = _q * _dq; // as given in attitude_estimator_q_main.cpp + _q = _q * _dq; _q.normalize(); // integration Runge-Kutta 4 // rk4_update(_p_I, _v_I, _q, _w_B); @@ -558,10 +592,16 @@ Vector3f Sih::noiseGauss3f(float stdx, float stdy, float stdz) int Sih::print_status() { if (_vehicle == VehicleType::MC) { - PX4_INFO("Running MC"); + PX4_INFO("Running MultiCopter"); - } else { - PX4_INFO("Running FW"); + } else if (_vehicle == VehicleType::FW) { + PX4_INFO("Running Fixed-Wing"); + + } else if (_vehicle == VehicleType::TS) { + PX4_INFO("Running TailSitter"); + PX4_INFO("aoa [deg]: %d", (int)(degrees(_ts[4].get_aoa()))); + PX4_INFO("v segment (m/s)"); + _ts[4].get_vS().print(); } PX4_INFO("vehicle landed: %d", _grounded); @@ -581,8 +621,8 @@ int Sih::print_status() _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)); + PX4_INFO("Thruster moments in body frame (Nm)"); + _Mt_B.print(); return 0; } diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index f797370cce..705be853c3 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -45,10 +45,13 @@ // 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. +// McGill University (Canada), 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. +// The tailsitter model is from [4] +// [4] Chiappinelli R, supervised by Nahon M, "Modeling and control of a flying wing tailsitter unmanned aerial vehicle." +// McGill University (Canada), Masters Thesis, 2018. #pragma once @@ -143,7 +146,7 @@ private: uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs)}; // hard constants - static constexpr uint16_t NB_MOTORS = 4; + static constexpr uint16_t NB_MOTORS = 6; 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 @@ -165,7 +168,8 @@ private: void send_airspeed(); void send_dist_snsr(); void publish_sih(); - void generate_aerodynamics(); + void generate_fw_aerodynamics(); + void generate_ts_aerodynamics(); 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")}; @@ -197,7 +201,7 @@ private: matrix::Vector3f _w_B_dot; // body rates differential float _u[NB_MOTORS]; // thruster signals - enum class VehicleType {MC, FW}; + enum class VehicleType {MC, FW, TS}; VehicleType _vehicle = VehicleType::MC; // aerodynamic segments for the fixedwing @@ -209,6 +213,41 @@ private: 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); + // aerodynamic segments for the tailsitter + static constexpr const int NB_TS_SEG = 11; + static constexpr const float TS_AR = 3.13f; + static constexpr const float TS_CM = 0.115f; // longitudinal position of the CM from trailing edge + static constexpr const float TS_RP = 0.0625f; // propeller radius [m] + static constexpr const float TS_DEF_MAX = math::radians(39.0f); // max deflection + matrix::Dcmf _C_BS = matrix::Dcmf(matrix::Eulerf(0.0f, math::radians(90.0f), 0.0f)); // segment to body 90 deg pitch + AeroSeg _ts[NB_TS_SEG] = { + AeroSeg(0.0225f, 0.110f, 0.0f, matrix::Vector3f(0.083f - TS_CM, -0.239f, 0.0f), 0.0f, TS_AR), + AeroSeg(0.0383f, 0.125f, 0.0f, matrix::Vector3f(0.094f - TS_CM, -0.208f, 0.0f), 0.0f, TS_AR, 0.063f), + // AeroSeg(0.0884f, 0.148f, 0.0f, matrix::Vector3f(0.111f-TS_CM, -0.143f, 0.0f), 0.0f, TS_AR, 0.063f, TS_RP), + AeroSeg(0.0884f, 0.085f, 0.0f, matrix::Vector3f(0.158f - TS_CM, -0.143f, 0.0f), 0.0f, TS_AR), + AeroSeg(0.0884f, 0.063f, 0.0f, matrix::Vector3f(0.047f - TS_CM, -0.143f, 0.0f), 0.0f, TS_AR, 0.063f, TS_RP), + AeroSeg(0.0633f, 0.176f, 0.0f, matrix::Vector3f(0.132f - TS_CM, -0.068f, 0.0f), 0.0f, TS_AR, 0.063f), + AeroSeg(0.0750f, 0.231f, 0.0f, matrix::Vector3f(0.173f - TS_CM, 0.000f, 0.0f), 0.0f, TS_AR), + AeroSeg(0.0633f, 0.176f, 0.0f, matrix::Vector3f(0.132f - TS_CM, 0.068f, 0.0f), 0.0f, TS_AR, 0.063f), + // AeroSeg(0.0884f, 0.148f, 0.0f, matrix::Vector3f(0.111f-TS_CM, 0.143f, 0.0f), 0.0f, TS_AR, 0.063f, TS_RP), + AeroSeg(0.0884f, 0.085f, 0.0f, matrix::Vector3f(0.158f - TS_CM, 0.143f, 0.0f), 0.0f, TS_AR), + AeroSeg(0.0884f, 0.063f, 0.0f, matrix::Vector3f(0.047f - TS_CM, 0.143f, 0.0f), 0.0f, TS_AR, 0.063f, TS_RP), + AeroSeg(0.0383f, 0.125f, 0.0f, matrix::Vector3f(0.094f - TS_CM, 0.208f, 0.0f), 0.0f, TS_AR, 0.063f), + AeroSeg(0.0225f, 0.110f, 0.0f, matrix::Vector3f(0.083f - TS_CM, 0.239f, 0.0f), 0.0f, TS_AR) + }; + + // AeroSeg _ts[NB_TS_SEG] = { + // AeroSeg(0.0225f, 0.110f, -90.0f, matrix::Vector3f(0.0f, -0.239f, TS_CM-0.083f), 0.0f, TS_AR), + // AeroSeg(0.0383f, 0.125f, -90.0f, matrix::Vector3f(0.0f, -0.208f, TS_CM-0.094f), 0.0f, TS_AR, 0.063f), + // AeroSeg(0.0884f, 0.148f, -90.0f, matrix::Vector3f(0.0f, -0.143f, TS_CM-0.111f), 0.0f, TS_AR, 0.063f, TS_RP), + // AeroSeg(0.0633f, 0.176f, -90.0f, matrix::Vector3f(0.0f, -0.068f, TS_CM-0.132f), 0.0f, TS_AR, 0.063f), + // AeroSeg(0.0750f, 0.231f, -90.0f, matrix::Vector3f(0.0f, 0.000f, TS_CM-0.173f), 0.0f, TS_AR), + // AeroSeg(0.0633f, 0.176f, -90.0f, matrix::Vector3f(0.0f, 0.068f, TS_CM-0.132f), 0.0f, TS_AR, 0.063f), + // AeroSeg(0.0884f, 0.148f, -90.0f, matrix::Vector3f(0.0f, 0.143f, TS_CM-0.111f), 0.0f, TS_AR, 0.063f, TS_RP), + // AeroSeg(0.0383f, 0.125f, -90.0f, matrix::Vector3f(0.0f, 0.208f, TS_CM-0.094f), 0.0f, TS_AR, 0.063f), + // AeroSeg(0.0225f, 0.110f, -90.0f, matrix::Vector3f(0.0f, 0.239f, TS_CM-0.083f), 0.0f, TS_AR) + // }; + // sensors reconstruction matrix::Vector3f _acc; matrix::Vector3f _mag; diff --git a/src/modules/sih/sih_params.c b/src/modules/sih/sih_params.c index 80b6b26e3e..60619885af 100644 --- a/src/modules/sih/sih_params.c +++ b/src/modules/sih/sih_params.c @@ -439,8 +439,9 @@ PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f); /** * Vehicle type * - * @value 0 MC - * @value 1 FW + * @value 0 Multicopter + * @value 1 Fixed-Wing + * @value 2 Tailsitter * @reboot_required true * @group Simulation In Hardware */