mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-23 08:03:35 +08:00
simulation organization and cleanup
- new modules/simulation directory to collect all simulators and related modules - new Tools/simulation directory to collect and organize scattered simulation submodules, scripts, etc - simulation module renamed to simulator_mavlink - sih renamed to simulator_sih (not a great name, but I wanted to be clear it was a simulator) - ignition_simulator renamed to simulator_ignition_bridge - large sitl_target.cmake split by simulation option and in some cases pushed to appropriate modules - sitl targets broken down to what's actually available (eg jmavsim only has 1 model and 1 world) - new Gazebo consistently referred to as Ignition for now (probably the least confusing thing until we fully drop Gazebo classic support someday)
This commit is contained in:
Daniel Agar
@@ -0,0 +1,67 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019-2022 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__simulation__simulator_sih
|
||||
MAIN simulator_sih
|
||||
COMPILE_FLAGS
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
SRCS
|
||||
aero.hpp
|
||||
sih.cpp
|
||||
sih.hpp
|
||||
DEPENDS
|
||||
mathlib
|
||||
drivers_accelerometer
|
||||
drivers_gyroscope
|
||||
drivers_magnetometer
|
||||
)
|
||||
|
||||
if(PX4_PLATFORM MATCHES "posix")
|
||||
# create targets for sihsim
|
||||
set(models
|
||||
airplane
|
||||
quadx
|
||||
xvert
|
||||
)
|
||||
|
||||
foreach(model ${models})
|
||||
add_custom_target(sihsim_${model}
|
||||
COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=${model} PX4_SIMULATOR=sihsim $<TARGET_FILE:px4>
|
||||
WORKING_DIRECTORY ${SITL_WORKING_DIR}
|
||||
USES_TERMINAL
|
||||
DEPENDS px4
|
||||
)
|
||||
endforeach()
|
||||
|
||||
endif()
|
||||
@@ -0,0 +1,12 @@
|
||||
menuconfig MODULES_SIMULATION_SIMULATOR_SIH
|
||||
bool "simulator_sih"
|
||||
default n
|
||||
---help---
|
||||
Enable support for simulator_sih
|
||||
|
||||
menuconfig USER_SIH
|
||||
bool "simulator_sih running as userspace module"
|
||||
default y
|
||||
depends on BOARD_PROTECTED && MODULES_SIMULATION_SIMULATOR_SIH
|
||||
---help---
|
||||
Put sih in userspace memory
|
||||
@@ -0,0 +1,441 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019-2022 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 <romain.chiap@gmail.com>
|
||||
*
|
||||
* 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/matrix/math.hpp> // matrix, vectors, dcm, quaterions
|
||||
#include <conversion/rotation.h> // math::radians,
|
||||
// #include <lib/mathlib/mathlib.h>
|
||||
#include <math.h>
|
||||
|
||||
// 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
|
||||
matrix::Vector3f _v_S; // velocity in segment frame
|
||||
|
||||
public:
|
||||
/** 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)
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
AeroSeg(float span, float mac, float alpha_0_deg, const 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 = 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);
|
||||
}
|
||||
|
||||
|
||||
AeroSeg() : AeroSeg(1.0f, 0.2f, 0.0f, matrix::Vector3f())
|
||||
{}
|
||||
|
||||
/** 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(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;
|
||||
|
||||
_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.
|
||||
_v_S(0) += sqrtf(2.0f * thrust / (_rho * M_PI_F * _prop_radius * _prop_radius));
|
||||
}
|
||||
|
||||
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 = 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,
|
||||
-_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 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; }
|
||||
|
||||
// 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; }
|
||||
|
||||
// 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
|
||||
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 AeroSeg&) const {
|
||||
// return this;
|
||||
// }
|
||||
|
||||
// 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;
|
||||
// }
|
||||
|
||||
}; // ---------------------------------------------------------------------------------------------------------
|
||||
@@ -0,0 +1,826 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019-2022 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 sih.cpp
|
||||
* Simulator in Hardware
|
||||
*
|
||||
* @author Romain Chiappinelli <romain.chiap@gmail.com>
|
||||
*
|
||||
* Coriolis g Corporation - January 2019
|
||||
*/
|
||||
|
||||
#include "aero.hpp"
|
||||
#include "sih.hpp"
|
||||
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
|
||||
#include <drivers/drv_pwm_output.h> // to get PWM flags
|
||||
#include <lib/drivers/device/Device.hpp>
|
||||
|
||||
using namespace math;
|
||||
using namespace matrix;
|
||||
using namespace time_literals;
|
||||
|
||||
Sih::Sih() :
|
||||
ModuleParams(nullptr)
|
||||
{}
|
||||
|
||||
Sih::~Sih()
|
||||
{
|
||||
perf_free(_loop_perf);
|
||||
perf_free(_loop_interval_perf);
|
||||
}
|
||||
|
||||
void Sih::run()
|
||||
{
|
||||
_px4_accel.set_temperature(T1_C);
|
||||
_px4_gyro.set_temperature(T1_C);
|
||||
_px4_mag.set_temperature(T1_C);
|
||||
|
||||
parameters_updated();
|
||||
init_variables();
|
||||
gps_no_fix();
|
||||
|
||||
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(), static_cast<typeof _sih_vtype.get()>(0),
|
||||
static_cast<typeof _sih_vtype.get()>(2));
|
||||
|
||||
if (_sys_ctrl_alloc.get()) {
|
||||
_actuator_out_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)};
|
||||
}
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
lockstep_loop();
|
||||
#else
|
||||
realtime_loop();
|
||||
#endif
|
||||
exit_and_cleanup();
|
||||
}
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
// Get current timestamp in microseconds
|
||||
uint64_t micros()
|
||||
{
|
||||
struct timeval t;
|
||||
gettimeofday(&t, nullptr);
|
||||
return t.tv_sec * ((uint64_t)1000000) + t.tv_usec;
|
||||
}
|
||||
|
||||
void Sih::lockstep_loop()
|
||||
{
|
||||
|
||||
int rate = math::min(_imu_gyro_ratemax.get(), _imu_integration_rate.get());
|
||||
|
||||
// default to 400Hz (2500 us interval)
|
||||
if (rate <= 0) {
|
||||
rate = 400;
|
||||
}
|
||||
|
||||
// 200 - 2000 Hz
|
||||
int sim_interval_us = math::constrain(int(roundf(1e6f / rate)), 500, 5000);
|
||||
|
||||
float speed_factor = 1.f;
|
||||
const char *speedup = getenv("PX4_SIM_SPEED_FACTOR");
|
||||
|
||||
if (speedup) {
|
||||
speed_factor = atof(speedup);
|
||||
}
|
||||
|
||||
int rt_interval_us = int(roundf(sim_interval_us / speed_factor));
|
||||
|
||||
PX4_INFO("Simulation loop with %d Hz (%d us sim time interval)", rate, sim_interval_us);
|
||||
PX4_INFO("Simulation with %.1fx speedup. Loop with (%d us wall time interval)", (double)speed_factor, rt_interval_us);
|
||||
uint64_t pre_compute_wall_time_us;
|
||||
|
||||
while (!should_exit()) {
|
||||
pre_compute_wall_time_us = micros();
|
||||
perf_count(_loop_interval_perf);
|
||||
|
||||
_current_simulation_time_us += sim_interval_us;
|
||||
struct timespec ts;
|
||||
abstime_to_ts(&ts, _current_simulation_time_us);
|
||||
px4_clock_settime(CLOCK_MONOTONIC, &ts);
|
||||
|
||||
perf_begin(_loop_perf);
|
||||
sensor_step();
|
||||
perf_end(_loop_perf);
|
||||
|
||||
// Only do lock-step once we received the first actuator output
|
||||
int sleep_time;
|
||||
uint64_t current_wall_time_us;
|
||||
|
||||
if (_last_actuator_output_time <= 0) {
|
||||
PX4_DEBUG("SIH starting up - no lockstep yet");
|
||||
current_wall_time_us = micros();
|
||||
sleep_time = math::max(0, sim_interval_us - (int)(current_wall_time_us - pre_compute_wall_time_us));
|
||||
|
||||
} else {
|
||||
px4_lockstep_wait_for_components();
|
||||
current_wall_time_us = micros();
|
||||
sleep_time = math::max(0, rt_interval_us - (int)(current_wall_time_us - pre_compute_wall_time_us));
|
||||
}
|
||||
|
||||
_achieved_speedup = 0.99f * _achieved_speedup + 0.01f * ((float)sim_interval_us / (float)(
|
||||
current_wall_time_us - pre_compute_wall_time_us + sleep_time));
|
||||
usleep(sleep_time);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void Sih::realtime_loop()
|
||||
{
|
||||
int rate = _imu_gyro_ratemax.get();
|
||||
|
||||
// default to 250 Hz (4000 us interval)
|
||||
if (rate <= 0) {
|
||||
rate = 250;
|
||||
}
|
||||
|
||||
// 200 - 2000 Hz
|
||||
int interval_us = math::constrain(int(roundf(1e6f / rate)), 500, 5000);
|
||||
|
||||
px4_sem_init(&_data_semaphore, 0, 0);
|
||||
hrt_call_every(&_timer_call, interval_us, interval_us, timer_callback, &_data_semaphore);
|
||||
|
||||
while (!should_exit()) {
|
||||
px4_sem_wait(&_data_semaphore); // periodic real time wakeup
|
||||
perf_begin(_loop_perf);
|
||||
sensor_step();
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
|
||||
hrt_cancel(&_timer_call);
|
||||
px4_sem_destroy(&_data_semaphore);
|
||||
}
|
||||
|
||||
|
||||
void Sih::timer_callback(void *sem)
|
||||
{
|
||||
px4_sem_post((px4_sem_t *)sem);
|
||||
}
|
||||
|
||||
void Sih::sensor_step()
|
||||
{
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
// update parameters from storage
|
||||
updateParams();
|
||||
parameters_updated();
|
||||
}
|
||||
|
||||
perf_begin(_loop_perf);
|
||||
|
||||
_now = hrt_absolute_time();
|
||||
_dt = (_now - _last_run) * 1e-6f;
|
||||
_last_run = _now;
|
||||
|
||||
read_motors();
|
||||
|
||||
generate_force_and_torques();
|
||||
|
||||
equations_of_motion();
|
||||
|
||||
reconstruct_sensors_signals();
|
||||
|
||||
// update IMU every iteration
|
||||
_px4_accel.update(_now, _acc(0), _acc(1), _acc(2));
|
||||
_px4_gyro.update(_now, _gyro(0), _gyro(1), _gyro(2));
|
||||
|
||||
// magnetometer published at 50 Hz
|
||||
if (_now - _mag_time >= 20_ms
|
||||
&& fabs(_mag_offset_x) < 10000
|
||||
&& fabs(_mag_offset_y) < 10000
|
||||
&& fabs(_mag_offset_z) < 10000) {
|
||||
_mag_time = _now;
|
||||
_px4_mag.update(_now, _mag(0), _mag(1), _mag(2));
|
||||
}
|
||||
|
||||
// baro published at 20 Hz
|
||||
if (_now - _baro_time >= 50_ms
|
||||
&& fabs(_baro_offset_m) < 10000) {
|
||||
_baro_time = _now;
|
||||
|
||||
// publish
|
||||
sensor_baro_s sensor_baro{};
|
||||
sensor_baro.timestamp_sample = _now;
|
||||
sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
|
||||
sensor_baro.pressure = _baro_p_mBar * 100.f;
|
||||
sensor_baro.temperature = _baro_temp_c;
|
||||
sensor_baro.error_count = 0;
|
||||
sensor_baro.timestamp = hrt_absolute_time();
|
||||
_sensor_baro_pub.publish(sensor_baro);
|
||||
}
|
||||
|
||||
// gps published at 20Hz
|
||||
if (_now - _gps_time >= 50_ms) {
|
||||
_gps_time = _now;
|
||||
send_gps();
|
||||
}
|
||||
|
||||
if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS) && _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) {
|
||||
_dist_snsr_time = _now;
|
||||
send_dist_snsr();
|
||||
}
|
||||
|
||||
// send groundtruth message every 40 ms
|
||||
if (_now - _gt_time >= 40_ms) {
|
||||
_gt_time = _now;
|
||||
|
||||
publish_sih(); // publish _sih message for debug purpose
|
||||
}
|
||||
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
|
||||
// store the parameters in a more convenient form
|
||||
void Sih::parameters_updated()
|
||||
{
|
||||
_T_MAX = _sih_t_max.get();
|
||||
_Q_MAX = _sih_q_max.get();
|
||||
_L_ROLL = _sih_l_roll.get();
|
||||
_L_PITCH = _sih_l_pitch.get();
|
||||
_KDV = _sih_kdv.get();
|
||||
_KDW = _sih_kdw.get();
|
||||
_H0 = _sih_h0.get();
|
||||
|
||||
_LAT0 = (double)_sih_lat0.get() * 1.0e-7;
|
||||
_LON0 = (double)_sih_lon0.get() * 1.0e-7;
|
||||
_COS_LAT0 = cosl((long double)radians(_LAT0));
|
||||
|
||||
_MASS = _sih_mass.get();
|
||||
|
||||
_W_I = Vector3f(0.0f, 0.0f, _MASS * CONSTANTS_ONE_G);
|
||||
|
||||
_I = diag(Vector3f(_sih_ixx.get(), _sih_iyy.get(), _sih_izz.get()));
|
||||
_I(0, 1) = _I(1, 0) = _sih_ixy.get();
|
||||
_I(0, 2) = _I(2, 0) = _sih_ixz.get();
|
||||
_I(1, 2) = _I(2, 1) = _sih_iyz.get();
|
||||
|
||||
// guards against too small determinants
|
||||
_Im1 = 100.0f * inv(static_cast<typeof _I>(100.0f * _I));
|
||||
|
||||
_mu_I = Vector3f(_sih_mu_x.get(), _sih_mu_y.get(), _sih_mu_z.get());
|
||||
|
||||
_gps_used = _sih_gps_used.get();
|
||||
_baro_offset_m = _sih_baro_offset.get();
|
||||
_mag_offset_x = _sih_mag_offset_x.get();
|
||||
_mag_offset_y = _sih_mag_offset_y.get();
|
||||
_mag_offset_z = _sih_mag_offset_z.get();
|
||||
|
||||
_distance_snsr_min = _sih_distance_snsr_min.get();
|
||||
_distance_snsr_max = _sih_distance_snsr_max.get();
|
||||
_distance_snsr_override = _sih_distance_snsr_override.get();
|
||||
|
||||
_T_TAU = _sih_thrust_tau.get();
|
||||
}
|
||||
|
||||
// initialization of the variables for the simulator
|
||||
void Sih::init_variables()
|
||||
{
|
||||
srand(1234); // initialize the random seed once before calling generate_wgn()
|
||||
|
||||
_p_I = Vector3f(0.0f, 0.0f, 0.0f);
|
||||
_v_I = Vector3f(0.0f, 0.0f, 0.0f);
|
||||
_q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
|
||||
_w_B = Vector3f(0.0f, 0.0f, 0.0f);
|
||||
|
||||
_u[0] = _u[1] = _u[2] = _u[3] = 0.0f;
|
||||
}
|
||||
|
||||
void Sih::gps_fix()
|
||||
{
|
||||
_sensor_gps.fix_type = 3; // 3D fix
|
||||
_sensor_gps.satellites_used = _gps_used;
|
||||
_sensor_gps.heading = NAN;
|
||||
_sensor_gps.heading_offset = NAN;
|
||||
_sensor_gps.s_variance_m_s = 0.5f;
|
||||
_sensor_gps.c_variance_rad = 0.1f;
|
||||
_sensor_gps.eph = 0.9f;
|
||||
_sensor_gps.epv = 1.78f;
|
||||
_sensor_gps.hdop = 0.7f;
|
||||
_sensor_gps.vdop = 1.1f;
|
||||
}
|
||||
|
||||
void Sih::gps_no_fix()
|
||||
{
|
||||
_sensor_gps.fix_type = 0; // 3D fix
|
||||
_sensor_gps.satellites_used = _gps_used;
|
||||
_sensor_gps.heading = NAN;
|
||||
_sensor_gps.heading_offset = NAN;
|
||||
_sensor_gps.s_variance_m_s = 100.f;
|
||||
_sensor_gps.c_variance_rad = 100.f;
|
||||
_sensor_gps.eph = 100.f;
|
||||
_sensor_gps.epv = 100.f;
|
||||
_sensor_gps.hdop = 100.f;
|
||||
_sensor_gps.vdop = 100.f;
|
||||
}
|
||||
|
||||
|
||||
// read the motor signals outputted from the mixer
|
||||
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)) {
|
||||
_last_actuator_output_time = actuators_out.timestamp;
|
||||
|
||||
if (_sys_ctrl_alloc.get()) {
|
||||
for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
|
||||
if ((_vehicle == VehicleType::FW && i < 3) || (_vehicle == VehicleType::TS && i > 3)) {
|
||||
_u[i] = actuators_out.output[i];
|
||||
|
||||
} else {
|
||||
float u_sp = actuators_out.output[i];
|
||||
_u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
|
||||
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]
|
||||
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
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// generate the motors thrust and torque in the body frame
|
||||
void Sih::generate_force_and_torques()
|
||||
{
|
||||
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
|
||||
|
||||
} 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_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_fw_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]);
|
||||
_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.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()
|
||||
{
|
||||
_C_IB = matrix::Dcm<float>(_q); // body to inertial transformation
|
||||
|
||||
// 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
|
||||
_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
|
||||
if (_p_I(2) > 0.0f && (_v_I_dot(2) > 0.0f || _v_I(2) > 0.0f)) {
|
||||
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;
|
||||
|
||||
} 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 if needed to get some lift
|
||||
_q = Quatf(RPY);
|
||||
_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 * _dq;
|
||||
_q.normalize();
|
||||
// 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<float>(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 [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);
|
||||
_gyro = _w_B + noiseGauss3f(0.14f, 0.07f, 0.03f);
|
||||
_mag = _C_IB.transpose() * _mu_I + noiseGauss3f(0.02f, 0.02f, 0.03f);
|
||||
_mag(0) += _mag_offset_x;
|
||||
_mag(1) += _mag_offset_y;
|
||||
_mag(2) += _mag_offset_z;
|
||||
|
||||
// barometer
|
||||
float altitude = (_H0 - _p_I(2)) + _baro_offset_m + generate_wgn() * 0.14f; // altitude with noise
|
||||
_baro_p_mBar = CONSTANTS_STD_PRESSURE_MBAR * // reconstructed pressure in mBar
|
||||
powf((1.0f + altitude * TEMP_GRADIENT / T1_K), -CONSTANTS_ONE_G / (TEMP_GRADIENT * CONSTANTS_AIR_GAS_CONST));
|
||||
_baro_temp_c = T1_K + CONSTANTS_ABSOLUTE_NULL_CELSIUS + TEMP_GRADIENT * altitude; // reconstructed temperture in Celsius
|
||||
|
||||
// GPS
|
||||
_gps_lat_noiseless = _LAT0 + degrees((double)_p_I(0) / CONSTANTS_RADIUS_OF_EARTH);
|
||||
_gps_lon_noiseless = _LON0 + degrees((double)_p_I(1) / CONSTANTS_RADIUS_OF_EARTH) / _COS_LAT0;
|
||||
_gps_alt_noiseless = _H0 - _p_I(2);
|
||||
|
||||
_gps_lat = _gps_lat_noiseless + degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH);
|
||||
_gps_lon = _gps_lon_noiseless + degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH) / _COS_LAT0;
|
||||
_gps_alt = _gps_alt_noiseless + generate_wgn() * 0.5f;
|
||||
_gps_vel = _v_I + noiseGauss3f(0.06f, 0.077f, 0.158f);
|
||||
}
|
||||
|
||||
void Sih::send_gps()
|
||||
{
|
||||
_sensor_gps.timestamp = _now;
|
||||
_sensor_gps.lat = (int32_t)(_gps_lat * 1e7); // Latitude in 1E-7 degrees
|
||||
_sensor_gps.lon = (int32_t)(_gps_lon * 1e7); // Longitude in 1E-7 degrees
|
||||
_sensor_gps.alt = (int32_t)(_gps_alt * 1000.0f); // Altitude in 1E-3 meters above MSL, (millimetres)
|
||||
_sensor_gps.alt_ellipsoid = (int32_t)(_gps_alt * 1000); // Altitude in 1E-3 meters bove Ellipsoid, (millimetres)
|
||||
_sensor_gps.vel_ned_valid = true; // True if NED velocity is valid
|
||||
_sensor_gps.vel_m_s = sqrtf(_gps_vel(0) * _gps_vel(0) + _gps_vel(1) * _gps_vel(
|
||||
1)); // GPS ground speed, (metres/sec)
|
||||
_sensor_gps.vel_n_m_s = _gps_vel(0); // GPS North velocity, (metres/sec)
|
||||
_sensor_gps.vel_e_m_s = _gps_vel(1); // GPS East velocity, (metres/sec)
|
||||
_sensor_gps.vel_d_m_s = _gps_vel(2); // GPS Down velocity, (metres/sec)
|
||||
_sensor_gps.cog_rad = atan2(_gps_vel(1),
|
||||
_gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
|
||||
|
||||
if (_gps_used >= 4) {
|
||||
gps_fix();
|
||||
|
||||
} else {
|
||||
gps_no_fix();
|
||||
}
|
||||
|
||||
// device id
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
|
||||
device_id.devid_s.bus = 0;
|
||||
device_id.devid_s.address = 0;
|
||||
device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;
|
||||
_sensor_gps.device_id = device_id.devid;
|
||||
|
||||
_sensor_gps_pub.publish(_sensor_gps);
|
||||
}
|
||||
|
||||
void Sih::send_airspeed()
|
||||
{
|
||||
airspeed_s airspeed{};
|
||||
airspeed.timestamp_sample = _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.timestamp = hrt_absolute_time();
|
||||
_airspeed_pub.publish(airspeed);
|
||||
}
|
||||
|
||||
void Sih::send_dist_snsr()
|
||||
{
|
||||
_distance_snsr.timestamp = _now;
|
||||
_distance_snsr.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
|
||||
_distance_snsr.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
|
||||
_distance_snsr.min_distance = _distance_snsr_min;
|
||||
_distance_snsr.max_distance = _distance_snsr_max;
|
||||
_distance_snsr.signal_quality = -1;
|
||||
_distance_snsr.device_id = 0;
|
||||
|
||||
if (_distance_snsr_override >= 0.f) {
|
||||
_distance_snsr.current_distance = _distance_snsr_override;
|
||||
|
||||
} else {
|
||||
_distance_snsr.current_distance = -_p_I(2) / _C_IB(2, 2);
|
||||
|
||||
if (_distance_snsr.current_distance > _distance_snsr_max) {
|
||||
// this is based on lightware lw20 behaviour
|
||||
_distance_snsr.current_distance = UINT16_MAX / 100.f;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
_distance_snsr_pub.publish(_distance_snsr);
|
||||
}
|
||||
|
||||
void Sih::publish_sih()
|
||||
{
|
||||
// publish angular velocity groundtruth
|
||||
_vehicle_angular_velocity_gt.timestamp = hrt_absolute_time();
|
||||
_vehicle_angular_velocity_gt.xyz[0] = _w_B(0); // rollspeed;
|
||||
_vehicle_angular_velocity_gt.xyz[1] = _w_B(1); // pitchspeed;
|
||||
_vehicle_angular_velocity_gt.xyz[2] = _w_B(2); // yawspeed;
|
||||
|
||||
_vehicle_angular_velocity_gt_pub.publish(_vehicle_angular_velocity_gt);
|
||||
|
||||
// publish attitude groundtruth
|
||||
_att_gt.timestamp = hrt_absolute_time();
|
||||
_att_gt.q[0] = _q(0);
|
||||
_att_gt.q[1] = _q(1);
|
||||
_att_gt.q[2] = _q(2);
|
||||
_att_gt.q[3] = _q(3);
|
||||
|
||||
_att_gt_pub.publish(_att_gt);
|
||||
|
||||
// publish position groundtruth
|
||||
_gpos_gt.timestamp = hrt_absolute_time();
|
||||
_gpos_gt.lat = _gps_lat_noiseless;
|
||||
_gpos_gt.lon = _gps_lon_noiseless;
|
||||
_gpos_gt.alt = _gps_alt_noiseless;
|
||||
|
||||
_gpos_gt_pub.publish(_gpos_gt);
|
||||
}
|
||||
|
||||
float Sih::generate_wgn() // generate white Gaussian noise sample with std=1
|
||||
{
|
||||
// algorithm 1:
|
||||
// float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f));
|
||||
// return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX);
|
||||
// algorithm 2: from BlockRandGauss.hpp
|
||||
static float V1, V2, S;
|
||||
static bool phase = true;
|
||||
float X;
|
||||
|
||||
if (phase) {
|
||||
do {
|
||||
float U1 = (float)rand() / (float)RAND_MAX;
|
||||
float U2 = (float)rand() / (float)RAND_MAX;
|
||||
V1 = 2.0f * U1 - 1.0f;
|
||||
V2 = 2.0f * U2 - 1.0f;
|
||||
S = V1 * V1 + V2 * V2;
|
||||
} while (S >= 1.0f || fabsf(S) < 1e-8f);
|
||||
|
||||
X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S));
|
||||
|
||||
} else {
|
||||
X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S));
|
||||
}
|
||||
|
||||
phase = !phase;
|
||||
return X;
|
||||
}
|
||||
|
||||
// generate white Gaussian noise sample vector with specified std
|
||||
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 defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
PX4_INFO("Running in lockstep mode");
|
||||
PX4_INFO("Achieved speedup: %.2fX", (double)_achieved_speedup);
|
||||
#endif
|
||||
|
||||
if (_vehicle == VehicleType::MC) {
|
||||
PX4_INFO("Running MultiCopter");
|
||||
|
||||
} 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);
|
||||
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<float, 8> u = Vector<float, 8>(_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("Thruster moments in body frame (Nm)");
|
||||
_Mt_B.print();
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int Sih::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
_task_id = px4_task_spawn_cmd("sih",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX,
|
||||
1250,
|
||||
(px4_main_t)&run_trampoline,
|
||||
(char *const *)argv);
|
||||
|
||||
if (_task_id < 0) {
|
||||
_task_id = -1;
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
Sih *Sih::instantiate(int argc, char *argv[])
|
||||
{
|
||||
Sih *instance = new Sih();
|
||||
|
||||
if (instance == nullptr) {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
return instance;
|
||||
}
|
||||
|
||||
int Sih::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int Sih::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
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
|
||||
signals given by the mixer.
|
||||
|
||||
This simulator publishes the sensors signals corrupted with realistic noise
|
||||
in order to incorporate the state estimator in the loop.
|
||||
|
||||
### Implementation
|
||||
The simulator implements the equations of motion using matrix algebra.
|
||||
Quaternion representation is used for the attitude.
|
||||
Forward Euler is used for integration.
|
||||
Most of the variables are declared global in the .hpp file to avoid stack overflow.
|
||||
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("simulator_sih", "simulation");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int simulator_sih_main(int argc, char *argv[])
|
||||
{
|
||||
return Sih::main(argc, argv);
|
||||
}
|
||||
@@ -0,0 +1,333 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019-2022 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 sih.hpp
|
||||
* Simulator in Hardware
|
||||
*
|
||||
* @author Romain Chiappinelli <romain.chiap@gmail.com>
|
||||
*
|
||||
* 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 (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
|
||||
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
|
||||
#include <matrix/matrix/math.hpp> // matrix, vectors, dcm, quaterions
|
||||
#include <conversion/rotation.h> // math::radians,
|
||||
#include <lib/geo/geo.h> // to get the physical constants
|
||||
#include <drivers/drv_hrt.h> // to get the real time
|
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/sensor_baro.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h> // to publish groundtruth
|
||||
#include <uORB/topics/vehicle_attitude.h> // to publish groundtruth
|
||||
#include <uORB/topics/vehicle_global_position.h> // to publish groundtruth
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
#include <sys/time.h>
|
||||
#endif
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
extern "C" __EXPORT int sih_main(int argc, char *argv[]);
|
||||
|
||||
class Sih : public ModuleBase<Sih>, public ModuleParams
|
||||
{
|
||||
public:
|
||||
Sih();
|
||||
|
||||
virtual ~Sih();
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static Sih *instantiate(int argc, char *argv[]);
|
||||
|
||||
/** @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);
|
||||
|
||||
/** @see ModuleBase::run() */
|
||||
void run() override;
|
||||
|
||||
static float generate_wgn(); // generate white Gaussian noise sample
|
||||
|
||||
// generate white Gaussian noise sample as a 3D vector with specified std
|
||||
static matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz);
|
||||
|
||||
// timer called periodically to post the semaphore
|
||||
static void timer_callback(void *sem);
|
||||
|
||||
private:
|
||||
void parameters_updated();
|
||||
|
||||
// simulated sensor instances
|
||||
PX4Accelerometer _px4_accel{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
|
||||
PX4Gyroscope _px4_gyro{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
|
||||
PX4Magnetometer _px4_mag{197388}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
|
||||
|
||||
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
|
||||
|
||||
// to publish the gps position
|
||||
sensor_gps_s _sensor_gps{};
|
||||
uORB::Publication<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
|
||||
|
||||
// to publish the distance sensor
|
||||
distance_sensor_s _distance_snsr{};
|
||||
uORB::Publication<distance_sensor_s> _distance_snsr_pub{ORB_ID(distance_sensor)};
|
||||
|
||||
// angular velocity groundtruth
|
||||
vehicle_angular_velocity_s _vehicle_angular_velocity_gt{};
|
||||
uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_gt_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
|
||||
|
||||
// attitude groundtruth
|
||||
vehicle_attitude_s _att_gt{};
|
||||
uORB::Publication<vehicle_attitude_s> _att_gt_pub{ORB_ID(vehicle_attitude_groundtruth)};
|
||||
|
||||
// global position groundtruth
|
||||
vehicle_global_position_s _gpos_gt{};
|
||||
uORB::Publication<vehicle_global_position_s> _gpos_gt_pub{ORB_ID(vehicle_global_position_groundtruth)};
|
||||
|
||||
// airspeed
|
||||
uORB::Publication<airspeed_s> _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)};
|
||||
|
||||
// hard constants
|
||||
static constexpr uint16_t NB_MOTORS = 6;
|
||||
static constexpr float T1_C = 15.0f; // ground temperature in Celsius
|
||||
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();
|
||||
void gps_no_fix();
|
||||
void read_motors();
|
||||
void generate_force_and_torques();
|
||||
void equations_of_motion();
|
||||
void reconstruct_sensors_signals();
|
||||
void send_gps();
|
||||
void send_airspeed();
|
||||
void send_dist_snsr();
|
||||
void publish_sih();
|
||||
void generate_fw_aerodynamics();
|
||||
void generate_ts_aerodynamics();
|
||||
void sensor_step();
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
void lockstep_loop();
|
||||
uint64_t _current_simulation_time_us{0};
|
||||
float _achieved_speedup{0.f};
|
||||
#endif
|
||||
|
||||
void realtime_loop();
|
||||
px4_sem_t _data_semaphore;
|
||||
hrt_call _timer_call;
|
||||
|
||||
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")};
|
||||
|
||||
hrt_abstime _last_run{0};
|
||||
hrt_abstime _last_actuator_output_time{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};
|
||||
hrt_abstime _now{0};
|
||||
float _dt{0}; // sampling time [s]
|
||||
bool _grounded{true};// whether the vehicle is on the ground
|
||||
|
||||
matrix::Vector3f _T_B; // thrust force in body frame [N]
|
||||
matrix::Vector3f _Fa_I; // aerodynamic force in inertial frame [N]
|
||||
matrix::Vector3f _Mt_B; // thruster moments in the body frame [Nm]
|
||||
matrix::Vector3f _Ma_B; // aerodynamic moments in the body frame [Nm]
|
||||
matrix::Vector3f _p_I; // inertial position [m]
|
||||
matrix::Vector3f _v_I; // inertial velocity [m/s]
|
||||
matrix::Vector3f _v_B; // body frame velocity [m/s]
|
||||
matrix::Vector3f _p_I_dot; // inertial position differential
|
||||
matrix::Vector3f _v_I_dot; // inertial velocity differential
|
||||
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 _dq; // quaternion differential
|
||||
matrix::Vector3f _w_B_dot; // body rates differential
|
||||
float _u[NB_MOTORS]; // thruster signals
|
||||
|
||||
enum class VehicleType {MC, FW, TS};
|
||||
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.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;
|
||||
matrix::Vector3f _gyro;
|
||||
matrix::Vector3f _gps_vel;
|
||||
double _gps_lat, _gps_lat_noiseless;
|
||||
double _gps_lon, _gps_lon_noiseless;
|
||||
float _gps_alt, _gps_alt_noiseless;
|
||||
float _baro_p_mBar; // reconstructed (simulated) pressure in mBar
|
||||
float _baro_temp_c; // reconstructed (simulated) barometer temperature in degrees Celsius
|
||||
|
||||
// parameters
|
||||
float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _H0, _T_TAU;
|
||||
double _LAT0, _LON0, _COS_LAT0;
|
||||
matrix::Vector3f _W_I; // weight of the vehicle in inertial frame [N]
|
||||
matrix::Matrix3f _I; // vehicle inertia matrix
|
||||
matrix::Matrix3f _Im1; // inverse of the inertia matrix
|
||||
matrix::Vector3f _mu_I; // NED magnetic field in inertial frame [G]
|
||||
|
||||
int _gps_used;
|
||||
float _baro_offset_m, _mag_offset_x, _mag_offset_y, _mag_offset_z;
|
||||
float _distance_snsr_min, _distance_snsr_max, _distance_snsr_override;
|
||||
|
||||
// parameters defined in sih_params.c
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _imu_gyro_ratemax,
|
||||
(ParamInt<px4::params::IMU_INTEG_RATE>) _imu_integration_rate,
|
||||
(ParamFloat<px4::params::SIH_MASS>) _sih_mass,
|
||||
(ParamFloat<px4::params::SIH_IXX>) _sih_ixx,
|
||||
(ParamFloat<px4::params::SIH_IYY>) _sih_iyy,
|
||||
(ParamFloat<px4::params::SIH_IZZ>) _sih_izz,
|
||||
(ParamFloat<px4::params::SIH_IXY>) _sih_ixy,
|
||||
(ParamFloat<px4::params::SIH_IXZ>) _sih_ixz,
|
||||
(ParamFloat<px4::params::SIH_IYZ>) _sih_iyz,
|
||||
(ParamFloat<px4::params::SIH_T_MAX>) _sih_t_max,
|
||||
(ParamFloat<px4::params::SIH_Q_MAX>) _sih_q_max,
|
||||
(ParamFloat<px4::params::SIH_L_ROLL>) _sih_l_roll,
|
||||
(ParamFloat<px4::params::SIH_L_PITCH>) _sih_l_pitch,
|
||||
(ParamFloat<px4::params::SIH_KDV>) _sih_kdv,
|
||||
(ParamFloat<px4::params::SIH_KDW>) _sih_kdw,
|
||||
(ParamInt<px4::params::SIH_LOC_LAT0>) _sih_lat0,
|
||||
(ParamInt<px4::params::SIH_LOC_LON0>) _sih_lon0,
|
||||
(ParamFloat<px4::params::SIH_LOC_H0>) _sih_h0,
|
||||
(ParamFloat<px4::params::SIH_LOC_MU_X>) _sih_mu_x,
|
||||
(ParamFloat<px4::params::SIH_LOC_MU_Y>) _sih_mu_y,
|
||||
(ParamFloat<px4::params::SIH_LOC_MU_Z>) _sih_mu_z,
|
||||
(ParamInt<px4::params::SIH_GPS_USED>) _sih_gps_used,
|
||||
(ParamFloat<px4::params::SIH_BARO_OFFSET>) _sih_baro_offset,
|
||||
(ParamFloat<px4::params::SIH_MAG_OFFSET_X>) _sih_mag_offset_x,
|
||||
(ParamFloat<px4::params::SIH_MAG_OFFSET_Y>) _sih_mag_offset_y,
|
||||
(ParamFloat<px4::params::SIH_MAG_OFFSET_Z>) _sih_mag_offset_z,
|
||||
(ParamFloat<px4::params::SIH_DISTSNSR_MIN>) _sih_distance_snsr_min,
|
||||
(ParamFloat<px4::params::SIH_DISTSNSR_MAX>) _sih_distance_snsr_max,
|
||||
(ParamFloat<px4::params::SIH_DISTSNSR_OVR>) _sih_distance_snsr_override,
|
||||
(ParamFloat<px4::params::SIH_T_TAU>) _sih_thrust_tau,
|
||||
(ParamInt<px4::params::SIH_VEHICLE_TYPE>) _sih_vtype,
|
||||
(ParamBool<px4::params::SYS_CTRL_ALLOC>) _sys_ctrl_alloc
|
||||
)
|
||||
};
|
||||
@@ -0,0 +1,448 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2022 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 sih_params.c
|
||||
* Parameters for quadcopter X simulator in hardware.
|
||||
*
|
||||
* @author Romain Chiappinelli <romain.chiap@gmail.com>
|
||||
* February 2019
|
||||
*/
|
||||
|
||||
/**
|
||||
* Vehicle mass
|
||||
*
|
||||
* This value can be measured by weighting the quad on a scale.
|
||||
*
|
||||
* @unit kg
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.1
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_MASS, 1.0f);
|
||||
|
||||
/**
|
||||
* Vehicle inertia about X axis
|
||||
*
|
||||
* The inertia is a 3 by 3 symmetric matrix.
|
||||
* It represents the difficulty of the vehicle to modify its angular rate.
|
||||
*
|
||||
* @unit kg m^2
|
||||
* @min 0.0
|
||||
* @decimal 3
|
||||
* @increment 0.005
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_IXX, 0.025f);
|
||||
|
||||
/**
|
||||
* Vehicle inertia about Y axis
|
||||
*
|
||||
* The inertia is a 3 by 3 symmetric matrix.
|
||||
* It represents the difficulty of the vehicle to modify its angular rate.
|
||||
*
|
||||
* @unit kg m^2
|
||||
* @min 0.0
|
||||
* @decimal 3
|
||||
* @increment 0.005
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_IYY, 0.025f);
|
||||
|
||||
/**
|
||||
* Vehicle inertia about Z axis
|
||||
*
|
||||
* The inertia is a 3 by 3 symmetric matrix.
|
||||
* It represents the difficulty of the vehicle to modify its angular rate.
|
||||
*
|
||||
* @unit kg m^2
|
||||
* @min 0.0
|
||||
* @decimal 3
|
||||
* @increment 0.005
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_IZZ, 0.030f);
|
||||
|
||||
/**
|
||||
* Vehicle cross term inertia xy
|
||||
*
|
||||
* The inertia is a 3 by 3 symmetric matrix.
|
||||
* This value can be set to 0 for a quad symmetric about its center of mass.
|
||||
*
|
||||
* @unit kg m^2
|
||||
* @decimal 3
|
||||
* @increment 0.005
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_IXY, 0.0f);
|
||||
|
||||
/**
|
||||
* Vehicle cross term inertia xz
|
||||
*
|
||||
* The inertia is a 3 by 3 symmetric matrix.
|
||||
* This value can be set to 0 for a quad symmetric about its center of mass.
|
||||
*
|
||||
* @unit kg m^2
|
||||
* @decimal 3
|
||||
* @increment 0.005
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_IXZ, 0.0f);
|
||||
|
||||
/**
|
||||
* Vehicle cross term inertia yz
|
||||
*
|
||||
* The inertia is a 3 by 3 symmetric matrix.
|
||||
* This value can be set to 0 for a quad symmetric about its center of mass.
|
||||
*
|
||||
* @unit kg m^2
|
||||
* @decimal 3
|
||||
* @increment 0.005
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_IYZ, 0.0f);
|
||||
|
||||
/**
|
||||
* Max propeller thrust force
|
||||
*
|
||||
* This is the maximum force delivered by one propeller
|
||||
* when the motor is running at full speed.
|
||||
*
|
||||
* This value is usually about 5 times the mass of the quadrotor.
|
||||
*
|
||||
* @unit N
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.5
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_T_MAX, 5.0f);
|
||||
|
||||
/**
|
||||
* Max propeller torque
|
||||
*
|
||||
* This is the maximum torque delivered by one propeller
|
||||
* when the motor is running at full speed.
|
||||
*
|
||||
* This value is usually about few percent of the maximum thrust force.
|
||||
*
|
||||
* @unit Nm
|
||||
* @min 0.0
|
||||
* @decimal 3
|
||||
* @increment 0.05
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_Q_MAX, 0.1f);
|
||||
|
||||
/**
|
||||
* Roll arm length
|
||||
*
|
||||
* This is the arm length generating the rolling moment
|
||||
*
|
||||
* This value can be measured with a ruler.
|
||||
* This corresponds to half the distance between the left and right motors.
|
||||
*
|
||||
* @unit m
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.05
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_L_ROLL, 0.2f);
|
||||
|
||||
/**
|
||||
* Pitch arm length
|
||||
*
|
||||
* This is the arm length generating the pitching moment
|
||||
*
|
||||
* This value can be measured with a ruler.
|
||||
* This corresponds to half the distance between the front and rear motors.
|
||||
*
|
||||
* @unit m
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.05
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_L_PITCH, 0.2f);
|
||||
|
||||
/**
|
||||
* First order drag coefficient
|
||||
*
|
||||
* Physical coefficient representing the friction with air particules.
|
||||
* The greater this value, the slower the quad will move.
|
||||
*
|
||||
* Drag force function of velocity: D=-KDV*V.
|
||||
* The maximum freefall velocity can be computed as V=10*MASS/KDV [m/s]
|
||||
*
|
||||
* @unit N/(m/s)
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.05
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_KDV, 1.0f);
|
||||
|
||||
/**
|
||||
* First order angular damper coefficient
|
||||
*
|
||||
* Physical coefficient representing the friction with air particules during rotations.
|
||||
* The greater this value, the slower the quad will rotate.
|
||||
*
|
||||
* Aerodynamic moment function of body rate: Ma=-KDW*W_B.
|
||||
* This value can be set to 0 if unknown.
|
||||
*
|
||||
* @unit Nm/(rad/s)
|
||||
* @min 0.0
|
||||
* @decimal 3
|
||||
* @increment 0.005
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_KDW, 0.025f);
|
||||
|
||||
/**
|
||||
* Initial geodetic latitude
|
||||
*
|
||||
* This value represents the North-South location on Earth where the simulation begins.
|
||||
* A value of 45 deg should be written 450000000.
|
||||
*
|
||||
* LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
|
||||
* to represent a physical ground location on Earth.
|
||||
*
|
||||
* @unit deg*1e7
|
||||
* @min -850000000
|
||||
* @max 850000000
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SIH_LOC_LAT0, 454671160);
|
||||
|
||||
/**
|
||||
* Initial geodetic longitude
|
||||
*
|
||||
* This value represents the East-West location on Earth where the simulation begins.
|
||||
* A value of 45 deg should be written 450000000.
|
||||
*
|
||||
* LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
|
||||
* to represent a physical ground location on Earth.
|
||||
*
|
||||
* @unit deg*1e7
|
||||
* @min -1800000000
|
||||
* @max 1800000000
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SIH_LOC_LON0, -737578370);
|
||||
|
||||
/**
|
||||
* Initial AMSL ground altitude
|
||||
*
|
||||
* This value represents the Above Mean Sea Level (AMSL) altitude where the simulation begins.
|
||||
*
|
||||
* If using FlightGear as a visual animation,
|
||||
* this value can be tweaked such that the vehicle lies on the ground at takeoff.
|
||||
*
|
||||
* LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
|
||||
* to represent a physical ground location on Earth.
|
||||
*
|
||||
*
|
||||
* @unit m
|
||||
* @min -420.0
|
||||
* @max 8848.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_LOC_H0, 32.34f);
|
||||
|
||||
/**
|
||||
* North magnetic field at the initial location
|
||||
*
|
||||
* This value represents the North magnetic field at the initial location.
|
||||
*
|
||||
* A magnetic field calculator can be found on the NOAA website
|
||||
* Note, the values need to be converted from nano Tesla to Gauss
|
||||
*
|
||||
* LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
|
||||
* to represent a physical ground location on Earth.
|
||||
*
|
||||
* @unit gauss
|
||||
* @min -1.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.001
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_LOC_MU_X, 0.179f);
|
||||
|
||||
/**
|
||||
* East magnetic field at the initial location
|
||||
*
|
||||
* This value represents the East magnetic field at the initial location.
|
||||
*
|
||||
* A magnetic field calculator can be found on the NOAA website
|
||||
* Note, the values need to be converted from nano Tesla to Gauss
|
||||
*
|
||||
* LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
|
||||
* to represent a physical ground location on Earth.
|
||||
*
|
||||
* @unit gauss
|
||||
* @min -1.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.001
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_LOC_MU_Y, -0.045f);
|
||||
|
||||
/**
|
||||
* Down magnetic field at the initial location
|
||||
*
|
||||
* This value represents the Down magnetic field at the initial location.
|
||||
*
|
||||
* A magnetic field calculator can be found on the NOAA website
|
||||
* Note, the values need to be converted from nano Tesla to Gauss
|
||||
*
|
||||
* LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
|
||||
* to represent a physical ground location on Earth.
|
||||
*
|
||||
* @unit gauss
|
||||
* @min -1.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.001
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_LOC_MU_Z, 0.504f);
|
||||
|
||||
/**
|
||||
* Number of GPS satellites used
|
||||
*
|
||||
* @min 0
|
||||
* @max 50
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SIH_GPS_USED, 10);
|
||||
|
||||
/**
|
||||
* Barometer offset in meters
|
||||
*
|
||||
* Absolute value superior to 10000 will disable barometer
|
||||
*
|
||||
* @unit m
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_BARO_OFFSET, 0.0f);
|
||||
|
||||
/**
|
||||
* magnetometer X offset in Gauss
|
||||
*
|
||||
* Absolute value superior to 10000 will disable magnetometer
|
||||
*
|
||||
* @unit gauss
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_MAG_OFFSET_X, 0.0f);
|
||||
|
||||
/**
|
||||
* magnetometer Y offset in Gauss
|
||||
*
|
||||
* Absolute value superior to 10000 will disable magnetometer
|
||||
*
|
||||
* @unit gauss
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_MAG_OFFSET_Y, 0.0f);
|
||||
/**
|
||||
* magnetometer Z offset in Gauss
|
||||
*
|
||||
* Absolute value superior to 10000 will disable magnetometer
|
||||
*
|
||||
* @unit gauss
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_MAG_OFFSET_Z, 0.0f);
|
||||
|
||||
/**
|
||||
* distance sensor minimum range
|
||||
*
|
||||
* @unit m
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @decimal 4
|
||||
* @increment 0.01
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MIN, 0.0f);
|
||||
|
||||
/**
|
||||
* distance sensor maximum range
|
||||
*
|
||||
* @unit m
|
||||
* @min 0.0
|
||||
* @max 1000.0
|
||||
* @decimal 4
|
||||
* @increment 0.01
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MAX, 100.0f);
|
||||
|
||||
/**
|
||||
* if >= 0 the distance sensor measures will be overridden by this value
|
||||
*
|
||||
* Absolute value superior to 10000 will disable distance sensor
|
||||
*
|
||||
* @unit m
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_DISTSNSR_OVR, -1.0f);
|
||||
|
||||
/**
|
||||
* thruster time constant tau
|
||||
*
|
||||
* the time taken for the thruster to step from 0 to 100% should be about 4 times tau
|
||||
*
|
||||
* @unit s
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f);
|
||||
|
||||
/**
|
||||
* Vehicle type
|
||||
*
|
||||
* @value 0 Multicopter
|
||||
* @value 1 Fixed-Wing
|
||||
* @value 2 Tailsitter
|
||||
* @reboot_required true
|
||||
* @group Simulation In Hardware
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SIH_VEHICLE_TYPE, 0);
|
||||
Reference in New Issue
Block a user