fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 02:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

sih: add fixed-wing support

Browse Source
This commit is contained in:
romain-chiap
2021-08-05 09:16:14 +02:00
committed by GitHub
parent 6060ec8c9a
commit 718e5b5b5d
9 changed files with 698 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sih/CMakeLists.txt
+1
View File
@@ -37,6 +37,7 @@ px4_add_module(
COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
SRCS
aero.hpp
sih.cpp
sih.hpp
DEPENDS
src/modules/sih/aero.hpp
+430
View File
@@ -0,0 +1,430 @@
/****************************************************************************
*
* Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file aero.hpp
* Aerodynamic class for modeling wing, tailaplane, fin.
* Captures the effect of partial stall, low aspect ratio, control surfaces deflection,
* propeller slipstream.
*
* @author Romain Chiappinelli <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
public:
AeroSeg()
{
AeroSeg(1.0f, 0.2f, 0.0f, matrix::Vector3f());
}
/** public explicit constructor
* AeroSeg(float span, float mac, float alpha_0_deg, matrix::Vector3f p_B, float dihedral_deg = 0.0f,
* float AR = -1.0f, float cf = 0.0f, float prop_radius=-1.0f, float cl_alpha=2.0f*M_PI_F, float alpha_max_deg=0.0f, float alpha_min_deg=0.0f)
*
* span_: span of the segment [m]
* mac_: mean aerodynamic chord of the segment [m]
* alpha_0_deg: zero lift angle of attack of the segment [deg], negative number represents a segment oriented up
* p_B_: position of the segment (mean aerodynamic center) in the body frame from the center of mass [m,m,m]
* dihedral_deg: dihedral angle of the segment [deg], set to 0 for tailplane, set to -90 for the fin. default is 0.
* AR: Aspect Ratio of the wing, or tailplane, or fin (not the segment).
* If the aspect ratio is negative, the aspect ratio is computed from the _span and MAC
* cf_: flap chord [m], this is the chord length of the control surface, default is zero (no flap).
* prop_radius_: radius of the propeller for slipstream computation. Setting to -1 (default) will assume no slipstream.
* cl_alpha: 2D lift curve slope (1/rad), default it 2*pi for a flat plate. This can be computed from http://airfoiltools.com for instance.
* alpha_max_deg: maximum angle of attack before stall. Setting to 0 (default) will compute it from a table for flat plate.
* alpha_min_deg: maximum negative angle of attack before stall. Setting to 0 (default) will compute it from a table for flat plate.
*/
explicit AeroSeg(float span, float mac, float alpha_0_deg, matrix::Vector3f p_B, float dihedral_deg = 0.0f,
float AR = -1.0f, float cf = 0.0f, float prop_radius = -1.0f, float cl_alpha = 2.0f * M_PI_F,
float alpha_max_deg = 0.0f, float alpha_min_deg = 0.0f)
{
static const float AR_tab[N_TAB] = {0.1666f, 0.333f, 0.4f, 0.5f, 1.0f, 1.25f, 2.0f, 3.0f, 4.0f, 6.0f};
static const float ale_tab[N_TAB] = {3.00f, 3.64f, 4.48f, 7.18f, 10.20f, 13.38f, 14.84f, 14.49f, 9.95f, 12.93f, 15.00f, 15.00f};
static const float ate_tab[N_TAB] = {5.90f, 15.51f, 32.57f, 39.44f, 48.22f, 59.29f, 21.55f, 7.74f, 7.05f, 5.26f, 6.50f, 6.50f};
static const float afle_tab[N_TAB] = {59.00f, 58.60f, 58.20f, 50.00f, 41.53f, 26.70f, 23.44f, 21.00f, 18.63f, 14.28f, 11.60f, 10.00f};
static const float afte_tab[N_TAB] = {59.00f, 58.60f, 58.20f, 51.85f, 41.46f, 28.09f, 39.40f, 35.86f, 26.76f, 19.76f, 16.43f, 14.00f};
static const float afs_tab[N_TAB] = {49.00f, 54.00f, 56.00f, 48.00f, 40.00f, 29.00f, 27.00f, 25.00f, 24.00f, 22.00f, 22.00f, 20.00f};
_span = span;
_mac = mac;
_alpha_0 = math::radians(alpha_0_deg);
_p_B = matrix::Vector3f(p_B);
_ar = (AR <= 0.0f) ? _span / _mac : AR; // setting AR<=0 will compute it from _span and _mac
_alpha_eff = 0.0f;
_alpha_eff_old = 0.0f;
_kp = cl_alpha / (1.0f + 2.0f * (_ar + 4.0f) / (_ar * (_ar + 2.0f)));
_kn = 0.41f * (1.0f - expf(-17.0f / _ar));
_ale = lin_interp_lkt(AR_tab, ale_tab, _ar, N_TAB);
_ate = lin_interp_lkt(AR_tab, ate_tab, _ar, N_TAB);
_afle = lin_interp_lkt(AR_tab, afle_tab, _ar, N_TAB);
_afte = lin_interp_lkt(AR_tab, afte_tab, _ar, N_TAB);
float afs_rad = math::radians(lin_interp_lkt(AR_tab, afs_tab, _ar, N_TAB));
if (fabsf(alpha_max_deg) < 1.0e-3f) {
_alpha_max = afs_rad;
} else {
_alpha_max = math::radians(alpha_max_deg);
}
if (fabsf(alpha_min_deg) < 1.0e-3f) {
_alpha_min = -afs_rad;
} else {
_alpha_min = math::radians(alpha_min_deg);
}
_cf = math::constrain(cf, 0.0f, mac);
_C_BS = matrix::Dcmf(matrix::Eulerf(math::radians(dihedral_deg), 0.0f, 0.0f));
_prop_radius = prop_radius;
_kD = 1.0f / (M_PI_F * K0 * _ar);
}
/** aerodynamic force and moments of a generic flate plate segment
* void update_aero(matrix::Vector3f v_B, matrix::Vector3f w_B, float alt = 0.0f,
* float def = 0.0f, float thrust=0.0f, float dt = -1.0f)
*
* v_B: 3D velocity in body frame [m/s], (front, right, down FRD frame)
* w_B: 3D body rates in body frame [rad/s], FRD frame.
* alt: altitude above mean sea level for computing air density [m], default is 0.
* def: flap deflection angle [rad], default is 0.
* thrust: thrust force [N] from the propeller to compute the slipstream velocity, default is 0.
*/
void update_aero(matrix::Vector3f v_B, matrix::Vector3f w_B, float alt = 0.0f, float def = 0.0f, float thrust = 0.0f)
{
// ISA model taken from Mustafa Cavcar, Anadolu University, Turkey
_pressure = P0 * powf(1.0f - 0.0065f * alt / T0_K, 5.2561f);
_temperature = T0_K + TEMP_GRADIENT * alt;
_rho = _pressure / R / _temperature;
matrix::Vector3f vel = _C_BS.transpose() * (v_B + w_B % _p_B); // velocity in segment frame
if (_prop_radius > 1e-4f) {
// Add velocity generated from the propeller and thrust force.
// Computed from momentum theory.
// For info, the diameter of the slipstream is sqrt(2)*_prop_radius,
// this should be the width of the segment in the slipstream.
vel(0) += sqrtf(2.0f * thrust / (_rho * M_PI_F * _prop_radius * _prop_radius));
}
float vxz2 = vel(0) * vel(0) + vel(2) * vel(2);
if (vxz2 < 0.01f) {
_Fa = matrix::Vector3f();
_Ma = matrix::Vector3f();
return;
}
_alpha = atan2f(vel(2), vel(0)) - _alpha_0;
aoa_coeff(_alpha, sqrtf(vxz2), def);
_Fa = _C_BS * (0.5f * _rho * vxz2 * _span * _mac) * matrix::Vector3f(_CL * sinf(_alpha) - _CD * cosf(_alpha),
0.0f,
-_CL * cosf(_alpha) - _CD * sinf(_alpha));
_Ma = _C_BS * (0.5f * _rho * vxz2 * _span * _mac * _mac) * matrix::Vector3f(0.0f, _CM,
0.0f) + _p_B % _Fa; // computed at vehicle _CM
}
// return the air density at current altitude, must be called after update_aero()
float get_rho() const { return _rho; }
// return the sum of aerodynamic forces of the segment in the body frame, taken at the _CM,
// must be called after update_aero()
matrix::Vector3f get_Fa() const { return _Fa; }
// return the sum of aerodynamic moments of the segment in the body frame, taken at the _CM,
// must be called after update_aero()
matrix::Vector3f get_Ma() const { return _Ma; }
private:
// low angle of attack and stalling region coefficient based on flat plate
void aoa_coeff(float a, float vxz, float def)
{
_alpha_eff_old = _alpha_eff;
_tau_te = (vxz > 0.01f) ? 4.5f * _mac / vxz : 0.0f;
_tau_le = (vxz > 0.01f) ? 0.5f * _mac / vxz : 0.0f;
// model for the control surface deflection
if (_cf / _mac < 0.999f) {
_def_a = fminf(fabsf(def), math::radians(70.0f));
_eta_f = _def_a * _def_a * ETA_POLY[0] + _def_a * ETA_POLY[1] + ETA_POLY[2]; // second order fit
_theta_f = acosf(2.0f * _cf / _mac - 1.0f);
_tau_f = 1.0f - (_theta_f - sinf(_theta_f)) / M_PI_F;
_deltaCL = _kp * _tau_f * _eta_f * def;
_dCLmax = (1.0f - _cf / _mac) * _deltaCL;
_alf0eff = solve_alpha_eff(_kp, KV, _deltaCL, _alpha_0);
_alpha_eff = a - _alf0eff;
// this doesn't seem to work, so let's comment it
// _alpha_eff_dot = math::constrain((_alpha_eff - _alpha_eff_old) / dt, -AF_DOT_MAX, AF_DOT_MAX);
// _fte = 0.5f * (1.0f - tanhf(_ate * ((_alpha_eff) - _tau_te * (_alpha_eff_dot)
// - math::radians(_afte)))); // normalized trailing edge separation
// _fle = 0.5f * (1.0f - tanhf(_ale * ((_alpha_eff) - _tau_le * (_alpha_eff_dot)
// - math::radians(_afle)))); // normalized leading edge separation
_fte = 1.0f;
_fle = 1.0f;
_CLmax = fCL(_alpha_max - _alpha_0) + _dCLmax;
_alpha_eff_max = _alf0eff - solve_alpha_eff(_kp, KV * _fle * _fle,
_CLmax / (0.25f * (1.0f + sqrtf(_fte)) * (1.0f + sqrtf(_fte))), _alpha_max - _alpha_0);
_CLmin = fCL(_alpha_min - _alpha_0) + _dCLmax;
_alpha_eff_min = _alf0eff - solve_alpha_eff(_kp, KV * _fle * _fle,
_CLmin / (0.25f * (1.0f + sqrtf(_fte)) * (1.0f + sqrtf(_fte))), _alpha_min - _alpha_0);
} else { // this segment is a full flap
_alpha_eff = a + def;
// this doesn't seem to work, so let's comment it
// _alpha_eff_dot = math::constrain((_alpha_eff - _alpha_eff_old) / dt, -AF_DOT_MAX, AF_DOT_MAX);
// _fte = 0.5f * (1.0f - tanhf(_ate * ((_alpha_eff) - _tau_te * (_alpha_eff_dot)
// - math::radians(_afte)))); // normalized trailing edge separation
// _fle = 0.5f * (1.0f - tanhf(_ale * ((_alpha_eff) - _tau_le * (_alpha_eff_dot)
// - math::radians(_afle)))); // normalized leading edge separation
_fte = 1.0f;
_fle = 1.0f;
_alpha_eff_max = _alpha_max;
_alpha_eff_min = _alpha_min;
}
// compute the aerodynamic coefficients
high_aoa_coeff(_alpha_eff, def);
_CL_ = fCL(_alpha_eff);
_CD_ = CD0 + fabsf(_CL * tanf(_alpha_eff));
// _CD_ = CD0 + _kD*_CL_*_CL_; // alternative method
_CM_ = -fCM(_alpha_eff);
// blending function
if (_alpha_eff > 0.0f) {
_f_blend = 0.5f * (1.0f - tanhf(4.0f * (_alpha_eff - _alpha_eff_max) / ALPHA_BLEND));
} else {
_f_blend = 0.5f * (1.0f - tanhf(-4.0f * (_alpha_eff - _alpha_eff_min) / ALPHA_BLEND));
}
_CL = _CL_ * _f_blend + _CL * (1.0f - _f_blend);
_CD = _CD_ * _f_blend + _CD * (1.0f - _f_blend);
_CM = _CM_ * _f_blend + _CM * (1.0f - _f_blend);
}
// high angle of attack coefficient based on flat plate
void high_aoa_coeff(float a, float def = 0.0f)
{
float mac_eff = sqrtf((_mac - _cf) * (_mac - _cf) + _cf * _cf + 2.0f * (_mac - _cf) * _cf * cosf(fabsf(def)));
a += asinf(_cf / mac_eff * sinf(def));
float cd90_eff = CD90 + 0.21f * def - 0.0426f * def *
def; // this might not be accurate for lower flap chord to chord ratio
// normal coeff
float CN = cd90_eff * sinf(a) * (1.0f / (0.56f + 0.44f * sinf(fabsf(a))) - _kn);
// tengential coeff
float CT = 0.5f * CD0 * cosf(a);
_CL = CN * cosf(a) - CT * sinf(a);
_CD = CN * sinf(a) + CT * cosf(a);
_CM = -CN * (0.25f - 7.0f / 40.0f * (1.0f - 2.0f / M_PI_F * fabsf(a)));
}
// linear interpolation between 2 points
static float lin_interp(const float x0, const float y0, const float x1, const float y1, const float x)
{
if (x < x0) {
return y0;
}
if (x > x1) {
return y1;
}
float slope = (y1 - y0) / (x1 - x0);
return y0 + slope * (x - x0);
}
// lookup table linear interpolation
static float lin_interp_lkt(const float x_tab [], const float y_tab [], const float x, const int length)
{
if (x < x_tab[0]) {
return y_tab[0];
}
if (x > x_tab[length - 1]) {
return y_tab [length - 1];
}
int i = length - 2;
while (x_tab[i] > x) {
i--;
}
return lin_interp(x_tab[i], y_tab[i], x_tab[i + 1], y_tab[i + 1], x);
}
float solve_alpha_eff(const float Kp, const float Kv, const float dCL, const float a0)
{
// we use here the Newton method with explicit derivative to find the root of equation 3.15
float a = a0; // init the search
for (int i = 0; i < 3; i++) {
a = a - (-Kp * sinf(a) * cosf(a) * cosf(a) - Kv * fabsf(sinf(a)) * sinf(a) * cosf(a) - dCL) /
(Kv * fabsf(sinf(a)) * sinf(a) * sinf(a) - Kv * fabsf(sinf(a)) * cosf(a) * cosf(a) - Kp * cosf(a) * cosf(a) * cosf(
a) + 2 * Kp * cosf(a) * sinf(a) * sinf(a) - Kv * matrix::sign(sinf(a)) * cosf(a) * cosf(a) * sinf(a));
}
return a;
}
float fCL(float a)
{
return 0.25f * (1.0f + sqrtf(_fte)) * (1.0f + sqrtf(_fte)) * (_kp * sinf(a) * cosf(a) * cosf(a)
+ _fle * _fle * KV * fabsf(sinf(a)) * sinf(a) * cosf(a));
}
float fCM(float a)
{
return -0.25f * (1.0f + sqrtf(_fte)) * (1.0f + sqrtf(_fte)) * 0.0625f * (-1.0f + 6.0f * sqrtf(
_fte) - 5.0f * _fte) * _kp * sinf(a) * cosf(a)
+ 0.17f * _fle * _fle * KV * fabsf(sinf(a)) * sinf(a);
}
// AeroSeg operator*(const float k) const {
// return AeroSeg(p_I*k, v_I*k, q*k, w_B*k);
// }
// AeroSeg operator+(const States other) const {
// return AeroSeg(p_I+other.p_I, v_I+other.v_I, q+other.q, w_B+other.w_B);
// }
// void unwrap_states(matrix::Vector3f &p, matrix::Vector3f &v, matrix::Quatf &q_, matrix::Vector3f &w) {
// p=p_I;
// v=v_I;
// q_=q;
// w=w_B;
// }
}; // ---------------------------------------------------------------------------------------------------------
src/modules/sih/sih.cpp
+157 -23
View File
@@ -40,6 +40,7 @@
* Coriolis g Corporation - January 2019
*/
#include "aero.hpp"
#include "sih.hpp"
#include <px4_platform_common/getopt.h>
@@ -67,8 +68,10 @@ Sih::Sih() :
const hrt_abstime task_start = hrt_absolute_time();
_last_run = task_start;
_gps_time = task_start;
_airspeed_time = task_start;
_gt_time = task_start;
_dist_snsr_time = task_start;
_vehicle = (VehicleType)constrain(_sih_vtype.get(), 0, 1);
}
Sih::~Sih()
@@ -149,6 +152,11 @@ void Sih::Run()
send_gps();
}
if (_vehicle == VehicleType::FW && _now - _airspeed_time >= 50_ms) {
_airspeed_time = _now;
send_airspeed();
}
// distance sensor published at 50 Hz
if (_now - _dist_snsr_time >= 20_ms
&& fabs(_distance_snsr_override) < 10000) {
@@ -254,10 +262,17 @@ void Sih::read_motors()
{
actuator_outputs_s actuators_out;
float pwm_middle = 0.5f * (PWM_DEFAULT_MIN + PWM_DEFAULT_MAX);
if (_actuator_out_sub.update(&actuators_out)) {
for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
float u_sp = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), 0.0f, 1.0f);
_u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau
if (_vehicle == VehicleType::FW && i < 3) { // control surfaces in range [-1,1]
_u[i] = constrain(2.0f * (actuators_out.output[i] - pwm_middle) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), -1.0f, 1.0f);
} else { // throttle signals in range [0,1]
float u_sp = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), 0.0f, 1.0f);
_u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau
}
}
}
}
@@ -265,13 +280,35 @@ void Sih::read_motors()
// generate the motors thrust and torque in the body frame
void Sih::generate_force_and_torques()
{
_T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3]));
_Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]),
_L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]),
_Q_MAX * (+_u[0] + _u[1] - _u[2] - _u[3]));
if (_vehicle == VehicleType::MC) {
_T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3]));
_Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]),
_L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]),
_Q_MAX * (+_u[0] + _u[1] - _u[2] - _u[3]));
_Fa_I = -_KDV * _v_I; // first order drag to slow down the aircraft
_Ma_B = -_KDW * _w_B; // first order angular damper
_Fa_I = -_KDV * _v_I; // first order drag to slow down the aircraft
_Ma_B = -_KDW * _w_B; // first order angular damper
} else if (_vehicle == VehicleType::FW) {
_T_B = Vector3f(_T_MAX * _u[3], 0.0f, 0.0f); // forward thruster
// _Mt_B = Vector3f(_Q_MAX*_u[3], 0.0f,0.0f); // thruster torque
_Mt_B = Vector3f();
generate_aerodynamics();
}
}
void Sih::generate_aerodynamics()
{
_v_B = _C_IB.transpose() * _v_I; // velocity in body frame [m/s]
float altitude = _H0 - _p_I(2);
_wing_l.update_aero(_v_B, _w_B, altitude, _u[0]*FLAP_MAX);
_wing_r.update_aero(_v_B, _w_B, altitude, -_u[0]*FLAP_MAX);
_tailplane.update_aero(_v_B, _w_B, altitude, _u[1]*FLAP_MAX, _T_MAX * _u[3]);
_fin.update_aero(_v_B, _w_B, altitude, _u[2]*FLAP_MAX, _T_MAX * _u[3]);
_Fa_I = _C_IB * (_wing_l.get_Fa() + _wing_r.get_Fa() + _tailplane.get_Fa() + _fin.get_Fa()) - _KDV *
_v_I; // sum of aerodynamic forces
// _Ma_B = wing_l.Ma + wing_r.Ma + tailplane.Ma + fin.Ma + flap_moments() -_KDW * _w_B; // aerodynamic moments
_Ma_B = _wing_l.get_Ma() + _wing_r.get_Ma() + _tailplane.get_Ma() + _fin.get_Ma() - _KDW * _w_B; // aerodynamic moments
}
// apply the equations of motion of a rigid body and integrate one step
@@ -282,40 +319,79 @@ void Sih::equations_of_motion()
// Equations of motion of a rigid body
_p_I_dot = _v_I; // position differential
_v_I_dot = (_W_I + _Fa_I + _C_IB * _T_B) / _MASS; // conservation of linear momentum
_q_dot = _q.derivative1(_w_B); // attitude differential
// _q_dot = _q.derivative1(_w_B); // attitude differential
_dq = expq(0.5f * _dt * _w_B);
_w_B_dot = _Im1 * (_Mt_B + _Ma_B - _w_B.cross(_I * _w_B)); // conservation of angular momentum
// fake ground, avoid free fall
if (_p_I(2) > 0.0f && (_v_I_dot(2) > 0.0f || _v_I(2) > 0.0f)) {
if (!_grounded) { // if we just hit the floor
// for the accelerometer, compute the acceleration that will stop the vehicle in one time step
_v_I_dot = -_v_I / _dt;
if (_vehicle == VehicleType::MC) {
if (!_grounded) { // if we just hit the floor
// for the accelerometer, compute the acceleration that will stop the vehicle in one time step
_v_I_dot = -_v_I / _dt;
} else {
_v_I_dot.setZero();
} else {
_v_I_dot.setZero();
}
_v_I.setZero();
_w_B.setZero();
_grounded = true;
} else if (_vehicle == VehicleType::FW) {
if (!_grounded) { // if we just hit the floor
// for the accelerometer, compute the acceleration that will stop the vehicle in one time step
_v_I_dot(2) = -_v_I(2) / _dt;
} else {
// we only allow negative acceleration in order to takeoff
_v_I_dot(2) = fminf(_v_I_dot(2), 0.0f);
}
// integration: Euler forward
_p_I = _p_I + _p_I_dot * _dt;
_v_I = _v_I + _v_I_dot * _dt;
Eulerf RPY = Eulerf(_q);
RPY(0) = 0.0f; // no roll
RPY(1) = radians(0.0f); // pitch slightly up to get some lift
_q = Quatf(RPY);
_w_B.setZero();
_grounded = true;
}
_v_I.setZero();
_w_B.setZero();
_grounded = true;
} else {
// integration: Euler forward
_p_I = _p_I + _p_I_dot * _dt;
_v_I = _v_I + _v_I_dot * _dt;
_q = _q + _q_dot * _dt; // as given in attitude_estimator_q_main.cpp
_q = _q * _dq; // as given in attitude_estimator_q_main.cpp
_q.normalize();
_w_B = _w_B + _w_B_dot * _dt;
// integration Runge-Kutta 4
// rk4_update(_p_I, _v_I, _q, _w_B);
_w_B = constrain(_w_B + _w_B_dot * _dt, -6.0f * M_PI_F, 6.0f * M_PI_F);
_grounded = false;
}
}
// Sih::States Sih::eom_f(States x) // equations of motion f: x'=f(x)
// {
// States x_dot{}; // dx/dt
// Dcmf C_IB = matrix::Dcm<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
// Bulka, Eitan, and Meyer Nahon. "Autonomous fixed-wing aerobatics: from theory to flight."
// In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018.
// The sensor signals reconstruction and noise levels are from [1]
// [1] Bulka, Eitan, and Meyer Nahon. "Autonomous fixed-wing aerobatics: from theory to flight."
// In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018.
// IMU
_acc = _C_IB.transpose() * (_v_I_dot - Vector3f(0.0f, 0.0f, CONSTANTS_ONE_G)) + noiseGauss3f(0.5f, 1.7f, 1.4f);
@@ -376,6 +452,17 @@ void Sih::send_gps()
_sensor_gps_pub.publish(_sensor_gps);
}
void Sih::send_airspeed()
{
airspeed_s airspeed{};
airspeed.timestamp = _now;
airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_B(0) + generate_wgn() * 0.2f);
airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO);
airspeed.air_temperature_celsius = _baro_temp_c;
airspeed.confidence = 0.7f;
_airspeed_pub.publish(airspeed);
}
void Sih::send_dist_snsr()
{
_distance_snsr.timestamp = _now;
@@ -430,6 +517,22 @@ void Sih::publish_sih()
_gpos_gt_pub.publish(_gpos_gt);
}
// quaternion exponential as defined in [3]
Quatf Sih::expq(const matrix::Vector3f &u)
{
float u_norm = u.norm();
Vector3f v;
if (u_norm < 1.0e-6f) { // error will be smaller than 1e-18
v = (1.0f - u_norm * u_norm / 6.0f) * u; // first taylor serie term of sin(x)/x
} else {
v = sinf(u_norm) / u_norm * u;
}
return Quatf(cosf(u_norm), v(0), v(1), v(2));
}
float Sih::generate_wgn() // generate white Gaussian noise sample with std=1
{
// algorithm 1:
@@ -465,6 +568,37 @@ Vector3f Sih::noiseGauss3f(float stdx, float stdy, float stdz)
return Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz);
}
int Sih::print_status()
{
if (_vehicle == VehicleType::MC) {
PX4_INFO("Running MC");
} else {
PX4_INFO("Running FW");
}
PX4_INFO("vehicle landed: %d", _grounded);
PX4_INFO("dt [us]: %d", (int)(_dt * 1e6f));
PX4_INFO("inertial position NED (m)");
_p_I.print();
PX4_INFO("inertial velocity NED (m/s)");
_v_I.print();
PX4_INFO("attitude roll-pitch-yaw (deg)");
(Eulerf(_q) * 180.0f / M_PI_F).print();
PX4_INFO("angular acceleration roll-pitch-yaw (deg/s)");
(_w_B * 180.0f / M_PI_F).print();
PX4_INFO("actuator signals");
Vector<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("v_I.z: %f", (double)_v_I(2));
PX4_INFO("v_I_dot.z: %f", (double)_v_I_dot(2));
return 0;
}
int Sih::task_spawn(int argc, char *argv[])
{
Sih *instance = new Sih();
src/modules/sih/sih.hpp
+53 -3
View File
@@ -31,6 +31,25 @@
*
****************************************************************************/
/**
* @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, PhD thesis, 2016.
// The quaternion integration are from [3]
// [3] Sveier A, Sjøberg AM, Egeland O. "Applied Runge–Kutta–Munthe-Kaas Integration for the Quaternion Kinematics."
// Journal of Guidance, Control, and Dynamics. 2019 Dec;42(12):2747-54.
#pragma once
#include <px4_platform_common/module.h>
@@ -57,6 +76,7 @@
#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>
using namespace time_literals;
@@ -72,6 +92,9 @@ public:
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase::print_status() */
int print_status() override;
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
@@ -113,6 +136,9 @@ private:
vehicle_global_position_s _gpos_gt{};
uORB::Publication<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)};
@@ -121,6 +147,12 @@ private:
static constexpr float T1_C = 15.0f; // ground temperature in celcius
static constexpr float T1_K = T1_C - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // ground temperature in Kelvin
static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre
// Aerodynamic coefficients
static constexpr float RHO = 1.225f; // air density at sea level [kg/m^3]
static constexpr float SPAN = 0.86f; // wing span [m]
static constexpr float MAC = 0.21f; // wing mean aerodynamic chord [m]
static constexpr float RP = 0.1f; // radius of the propeller [m]
static constexpr float FLAP_MAX = M_PI_F / 12.0f; // 15 deg, maximum control surface deflection
void init_variables();
void gps_fix();
@@ -130,8 +162,14 @@ private:
void equations_of_motion();
void reconstruct_sensors_signals();
void send_gps();
void send_airspeed();
void send_dist_snsr();
void publish_sih();
void generate_aerodynamics();
float sincf(float x); // sin cardinal = sin(x)/x
matrix::Quatf expq(const matrix::Vector3f &u); // quaternion exponential as defined in [3]
// States eom_f(States); // equations of motion f: x'=f(x)
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
@@ -139,6 +177,7 @@ private:
hrt_abstime _last_run{0};
hrt_abstime _baro_time{0};
hrt_abstime _gps_time{0};
hrt_abstime _airspeed_time{0};
hrt_abstime _mag_time{0};
hrt_abstime _gt_time{0};
hrt_abstime _dist_snsr_time{0};
@@ -158,10 +197,20 @@ private:
matrix::Quatf _q; // quaternion attitude
matrix::Dcmf _C_IB; // body to inertial transformation
matrix::Vector3f _w_B; // body rates in body frame [rad/s]
matrix::Quatf _q_dot; // quaternion differential
matrix::Quatf _dq; // quaternion differential
matrix::Vector3f _w_B_dot; // body rates differential
float _u[NB_MOTORS]; // thruster signals
float _u[NB_MOTORS]; // thruster signals
enum class VehicleType {MC, FW};
VehicleType _vehicle = VehicleType::MC;
// aerodynamic segments for the fixedwing
AeroSeg _wing_l = AeroSeg(SPAN / 2.0f, MAC, -4.0f, matrix::Vector3f(0.0f, -SPAN / 4.0f, 0.0f), 3.0f,
SPAN / MAC, MAC / 3.0f);
AeroSeg _wing_r = AeroSeg(SPAN / 2.0f, MAC, -4.0f, matrix::Vector3f(0.0f, SPAN / 4.0f, 0.0f), -3.0f,
SPAN / MAC, MAC / 3.0f);
AeroSeg _tailplane = AeroSeg(0.3f, 0.1f, 0.0f, matrix::Vector3f(-0.4f, 0.0f, 0.0f), 0.0f, -1.0f, 0.05f, RP);
AeroSeg _fin = AeroSeg(0.25, 0.15, 0.0f, matrix::Vector3f(-0.45f, 0.0f, -0.1f), -90.0f, -1.0f, 0.08f, RP);
// sensors reconstruction
matrix::Vector3f _acc;
@@ -217,6 +266,7 @@ private:
(ParamFloat<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
(ParamFloat<px4::params::SIH_T_TAU>) _sih_thrust_tau,
(ParamInt<px4::params::SIH_VEHICLE_TYPE>) _sih_vtype
)
};
src/modules/sih/sih_params.c
+10
View File
@@ -435,3 +435,13 @@ PARAM_DEFINE_FLOAT(SIH_DISTSNSR_OVR, -1.0f);
* @group Simulation In Hardware
*/
PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f);
/**
* Vehicle type
*
* @value 0 MC
* @value 1 FW
* @reboot_required true
* @group Simulation In Hardware
*/
PARAM_DEFINE_INT32(SIH_VEHICLE_TYPE, 0);