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

sih: add tailsitter support, disable UAVCAN

Browse Source
This commit is contained in:
romain-chiap
2022-01-11 08:29:19 +01:00
committed by GitHub
parent 6ed48ad0c0
commit 4e06b40e2b
11 changed files with 239 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sih/aero.hpp
+27 -14
View File
@@ -124,18 +124,19 @@ private:
float _alpha_min; // min angle of attack (stall angle)
float _alpha_max; // min angle of attack (stall angle)
float _alf0eff; // effective zero lift angle of attack
float _alfmeff; // effective maximum lift angle of attack
// float _alfmeff; // effective maximum lift angle of attack
float _alpha_eff; // effectie angle of attack
float _alpha_eff_dot; // effectie angle of attack derivative
// float _alpha_eff_dot; // effectie angle of attack derivative
float _alpha_eff_old; // angle of attack [rad]
float _pressure; // pressure in Pa at current altitude
float _temperature; // temperature in K at current altitude
float _prop_radius; // propeller radius [m], used to create the slipstream
float _v_slipstream; // slipstream velocity [m/s], computed from momentum theory
// float _v_slipstream; // slipstream velocity [m/s], computed from momentum theory
matrix::Vector3f _Fa; // aerodynamic force
matrix::Vector3f _Ma; // aerodynamic moment computed at _CM directly
matrix::Vector3f _v_S; // velocity in segment frame
public:
@@ -144,7 +145,7 @@ public:
AeroSeg(1.0f, 0.2f, 0.0f, matrix::Vector3f());
}
/** public explicit constructor
/** public constructor
* AeroSeg(float span, float mac, float alpha_0_deg, matrix::Vector3f p_B, float dihedral_deg = 0.0f,
* float AR = -1.0f, float cf = 0.0f, float prop_radius=-1.0f, float cl_alpha=2.0f*M_PI_F, float alpha_max_deg=0.0f, float alpha_min_deg=0.0f)
*
@@ -161,9 +162,9 @@ public:
* alpha_max_deg: maximum angle of attack before stall. Setting to 0 (default) will compute it from a table for flat plate.
* alpha_min_deg: maximum negative angle of attack before stall. Setting to 0 (default) will compute it from a table for flat plate.
*/
explicit AeroSeg(float span, float mac, float alpha_0_deg, matrix::Vector3f p_B, float dihedral_deg = 0.0f,
float AR = -1.0f, float cf = 0.0f, float prop_radius = -1.0f, float cl_alpha = 2.0f * M_PI_F,
float alpha_max_deg = 0.0f, float alpha_min_deg = 0.0f)
AeroSeg(float span, float mac, float alpha_0_deg, matrix::Vector3f p_B, float dihedral_deg = 0.0f,
float AR = -1.0f, float cf = 0.0f, float prop_radius = -1.0f, float cl_alpha = 2.0f * M_PI_F,
float alpha_max_deg = 0.0f, float alpha_min_deg = 0.0f)
{
static const float AR_tab[N_TAB] = {0.1666f, 0.333f, 0.4f, 0.5f, 1.0f, 1.25f, 2.0f, 3.0f, 4.0f, 6.0f};
static const float ale_tab[N_TAB] = {3.00f, 3.64f, 4.48f, 7.18f, 10.20f, 13.38f, 14.84f, 14.49f, 9.95f, 12.93f, 15.00f, 15.00f};
@@ -217,32 +218,35 @@ public:
* def: flap deflection angle [rad], default is 0.
* thrust: thrust force [N] from the propeller to compute the slipstream velocity, default is 0.
*/
void update_aero(matrix::Vector3f v_B, matrix::Vector3f w_B, float alt = 0.0f, float def = 0.0f, float thrust = 0.0f)
void update_aero(const matrix::Vector3f &v_B, const matrix::Vector3f &w_B, float alt = 0.0f, float def = 0.0f,
float thrust = 0.0f)
{
// ISA model taken from Mustafa Cavcar, Anadolu University, Turkey
_pressure = P0 * powf(1.0f - 0.0065f * alt / T0_K, 5.2561f);
_temperature = T0_K + TEMP_GRADIENT * alt;
_rho = _pressure / R / _temperature;
matrix::Vector3f vel = _C_BS.transpose() * (v_B + w_B % _p_B); // velocity in segment frame
_v_S = _C_BS.transpose() * (v_B + w_B % _p_B); // velocity in segment frame
if (_prop_radius > 1e-4f) {
// Add velocity generated from the propeller and thrust force.
// Computed from momentum theory.
// For info, the diameter of the slipstream is sqrt(2)*_prop_radius,
// this should be the width of the segment in the slipstream.
vel(0) += sqrtf(2.0f * thrust / (_rho * M_PI_F * _prop_radius * _prop_radius));
_v_S(0) += sqrtf(2.0f * thrust / (_rho * M_PI_F * _prop_radius * _prop_radius));
}
float vxz2 = vel(0) * vel(0) + vel(2) * vel(2);
float vxz2 = _v_S(0) * _v_S(0) + _v_S(2) * _v_S(2);
if (vxz2 < 0.01f) {
_Fa = matrix::Vector3f();
_Ma = matrix::Vector3f();
_alpha = 0.0f;
return;
}
_alpha = atan2f(vel(2), vel(0)) - _alpha_0;
_alpha = matrix::wrap_pi(atan2f(_v_S(2), _v_S(0)) - _alpha_0);
// _alpha = atan2f(_v_S(2), _v_S(0));
aoa_coeff(_alpha, sqrtf(vxz2), def);
_Fa = _C_BS * (0.5f * _rho * vxz2 * _span * _mac) * matrix::Vector3f(_CL * sinf(_alpha) - _CD * cosf(_alpha),
0.0f,
@@ -254,6 +258,12 @@ public:
// return the air density at current altitude, must be called after update_aero()
float get_rho() const { return _rho; }
// return angle of attack in radians
float get_aoa() const {return _alpha;}
// return the aspect ratio
float get_ar() const {return _ar;}
// return the sum of aerodynamic forces of the segment in the body frame, taken at the _CM,
// must be called after update_aero()
matrix::Vector3f get_Fa() const { return _Fa; }
@@ -262,6 +272,9 @@ public:
// must be called after update_aero()
matrix::Vector3f get_Ma() const { return _Ma; }
// return the velocity in segment frame
matrix::Vector3f get_vS() const { return _v_S; }
private:
// low angle of attack and stalling region coefficient based on flat plate
@@ -412,8 +425,8 @@ private:
+ 0.17f * _fle * _fle * KV * fabsf(sinf(a)) * sinf(a);
}
// AeroSeg operator*(const float k) const {
// return AeroSeg(p_I*k, v_I*k, q*k, w_B*k);
// AeroSeg operator=(const AeroSeg&) const {
// return this;
// }
// AeroSeg operator+(const States other) const {
src/modules/sih/sih.cpp
+55 -15
View File
@@ -72,7 +72,7 @@ Sih::Sih() :
_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()>(1));
static_cast<typeof _sih_vtype.get()>(2));
}
Sih::~Sih()
@@ -199,7 +199,8 @@ void Sih::parameters_updated()
_I(0, 2) = _I(2, 0) = _sih_ixz.get();
_I(1, 2) = _I(2, 1) = _sih_iyz.get();
_Im1 = inv(_I);
// guards against too small determinants
_Im1 = 100.0f * inv(static_cast<typeof _I>(100.0f * _I));
_mu_I = Vector3f(_sih_mu_x.get(), _sih_mu_y.get(), _sih_mu_z.get());
@@ -267,7 +268,8 @@ void Sih::read_motors()
if (_actuator_out_sub.update(&actuators_out)) {
for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
if (_vehicle == VehicleType::FW && i < 3) { // control surfaces in range [-1,1]
if ((_vehicle == VehicleType::FW && i < 3) || (_vehicle == VehicleType::TS
&& i > 3)) { // control surfaces in range [-1,1]
_u[i] = constrain(2.0f * (actuators_out.output[i] - pwm_middle) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), -1.0f, 1.0f);
} else { // throttle signals in range [0,1]
@@ -293,12 +295,19 @@ void Sih::generate_force_and_torques()
_T_B = Vector3f(_T_MAX * _u[3], 0.0f, 0.0f); // forward thruster
// _Mt_B = Vector3f(_Q_MAX*_u[3], 0.0f,0.0f); // thruster torque
_Mt_B = Vector3f();
generate_aerodynamics();
}
generate_fw_aerodynamics();
} else if (_vehicle == VehicleType::TS) {
_T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (_u[0] + _u[1]));
_Mt_B = Vector3f(_L_ROLL * _T_MAX * (_u[1] - _u[0]), 0.0f, _Q_MAX * (_u[1] - _u[0]));
generate_ts_aerodynamics();
// _Fa_I = -_KDV * _v_I; // first order drag to slow down the aircraft
// _Ma_B = -_KDW * _w_B; // first order angular damper
}
}
void Sih::generate_aerodynamics()
void Sih::generate_fw_aerodynamics()
{
_v_B = _C_IB.transpose() * _v_I; // velocity in body frame [m/s]
float altitude = _H0 - _p_I(2);
@@ -309,11 +318,36 @@ void Sih::generate_aerodynamics()
_fuselage.update_aero(_v_B, _w_B, altitude);
_Fa_I = _C_IB * (_wing_l.get_Fa() + _wing_r.get_Fa() + _tailplane.get_Fa() + _fin.get_Fa() + _fuselage.get_Fa())
- _KDV * _v_I; // sum of aerodynamic forces
// _Ma_B = wing_l.Ma + wing_r.Ma + tailplane.Ma + fin.Ma + flap_moments() -_KDW * _w_B; // aerodynamic moments
_Ma_B = _wing_l.get_Ma() + _wing_r.get_Ma() + _tailplane.get_Ma() + _fin.get_Ma() + _fuselage.get_Ma() - _KDW *
_w_B; // aerodynamic moments
}
void Sih::generate_ts_aerodynamics()
{
_v_B = _C_IB.transpose() * _v_I; // velocity in body frame [m/s]
Vector3f Fa_ts = Vector3f();
Vector3f Ma_ts = Vector3f();
Vector3f v_ts = _C_BS.transpose() *
_v_B; // the aerodynamic is resolved in a frame like a standard aircraft (nose-right-belly)
Vector3f w_ts = _C_BS.transpose() * _w_B;
float altitude = _H0 - _p_I(2);
for (int i = 0; i < NB_TS_SEG; i++) {
if (i <= NB_TS_SEG / 2) {
_ts[i].update_aero(v_ts, w_ts, altitude, _u[5]*TS_DEF_MAX, _T_MAX * _u[1]);
} else {
_ts[i].update_aero(v_ts, w_ts, altitude, -_u[4]*TS_DEF_MAX, _T_MAX * _u[0]);
}
Fa_ts += _ts[i].get_Fa();
Ma_ts += _ts[i].get_Ma();
}
_Fa_I = _C_IB * _C_BS * Fa_ts - _KDV * _v_I; // sum of aerodynamic forces
_Ma_B = _C_BS * Ma_ts - _KDW * _w_B; // aerodynamic moments
}
// apply the equations of motion of a rigid body and integrate one step
void Sih::equations_of_motion()
{
@@ -328,7 +362,7 @@ void Sih::equations_of_motion()
// fake ground, avoid free fall
if (_p_I(2) > 0.0f && (_v_I_dot(2) > 0.0f || _v_I(2) > 0.0f)) {
if (_vehicle == VehicleType::MC) {
if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS) {
if (!_grounded) { // if we just hit the floor
// for the accelerometer, compute the acceleration that will stop the vehicle in one time step
_v_I_dot = -_v_I / _dt;
@@ -356,7 +390,7 @@ void Sih::equations_of_motion()
_v_I = _v_I + _v_I_dot * _dt;
Eulerf RPY = Eulerf(_q);
RPY(0) = 0.0f; // no roll
RPY(1) = radians(0.0f); // pitch slightly up to get some lift
RPY(1) = radians(0.0f); // pitch slightly up if needed to get some lift
_q = Quatf(RPY);
_w_B.setZero();
_grounded = true;
@@ -366,7 +400,7 @@ void Sih::equations_of_motion()
// integration: Euler forward
_p_I = _p_I + _p_I_dot * _dt;
_v_I = _v_I + _v_I_dot * _dt;
_q = _q * _dq; // as given in attitude_estimator_q_main.cpp
_q = _q * _dq;
_q.normalize();
// integration Runge-Kutta 4
// rk4_update(_p_I, _v_I, _q, _w_B);
@@ -558,10 +592,16 @@ Vector3f Sih::noiseGauss3f(float stdx, float stdy, float stdz)
int Sih::print_status()
{
if (_vehicle == VehicleType::MC) {
PX4_INFO("Running MC");
PX4_INFO("Running MultiCopter");
} else {
PX4_INFO("Running FW");
} else if (_vehicle == VehicleType::FW) {
PX4_INFO("Running Fixed-Wing");
} else if (_vehicle == VehicleType::TS) {
PX4_INFO("Running TailSitter");
PX4_INFO("aoa [deg]: %d", (int)(degrees(_ts[4].get_aoa())));
PX4_INFO("v segment (m/s)");
_ts[4].get_vS().print();
}
PX4_INFO("vehicle landed: %d", _grounded);
@@ -581,8 +621,8 @@ int Sih::print_status()
_Fa_I.print();
PX4_INFO("Aerodynamic moments body frame (Nm)");
_Ma_B.print();
PX4_INFO("v_I.z: %f", (double)_v_I(2));
PX4_INFO("v_I_dot.z: %f", (double)_v_I_dot(2));
PX4_INFO("Thruster moments in body frame (Nm)");
_Mt_B.print();
return 0;
}
src/modules/sih/sih.hpp
+43 -4
View File
@@ -45,10 +45,13 @@
// In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018.
// The aerodynamic model is from [2]
// [2] Khan W, supervised by Nahon M, "Dynamics modeling of agile fixed-wing unmanned aerial vehicles."
// McGill University, PhD thesis, 2016.
// McGill University (Canada), PhD thesis, 2016.
// The quaternion integration are from [3]
// [3] Sveier A, Sjøberg AM, Egeland O. "Applied Runge–Kutta–Munthe-Kaas Integration for the Quaternion Kinematics."
// Journal of Guidance, Control, and Dynamics. 2019 Dec;42(12):2747-54.
// The tailsitter model is from [4]
// [4] Chiappinelli R, supervised by Nahon M, "Modeling and control of a flying wing tailsitter unmanned aerial vehicle."
// McGill University (Canada), Masters Thesis, 2018.
#pragma once
@@ -143,7 +146,7 @@ private:
uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs)};
// hard constants
static constexpr uint16_t NB_MOTORS = 4;
static constexpr uint16_t NB_MOTORS = 6;
static constexpr float T1_C = 15.0f; // ground temperature in celcius
static constexpr float T1_K = T1_C - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // ground temperature in Kelvin
static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre
@@ -165,7 +168,8 @@ private:
void send_airspeed();
void send_dist_snsr();
void publish_sih();
void generate_aerodynamics();
void generate_fw_aerodynamics();
void generate_ts_aerodynamics();
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
@@ -197,7 +201,7 @@ private:
matrix::Vector3f _w_B_dot; // body rates differential
float _u[NB_MOTORS]; // thruster signals
enum class VehicleType {MC, FW};
enum class VehicleType {MC, FW, TS};
VehicleType _vehicle = VehicleType::MC;
// aerodynamic segments for the fixedwing
@@ -209,6 +213,41 @@ private:
AeroSeg _fin = AeroSeg(0.25, 0.18, 0.0f, matrix::Vector3f(-0.45f, 0.0f, -0.1f), -90.0f, -1.0f, 0.12f, RP);
AeroSeg _fuselage = AeroSeg(0.2, 0.8, 0.0f, matrix::Vector3f(0.0f, 0.0f, 0.0f), -90.0f);
// aerodynamic segments for the tailsitter
static constexpr const int NB_TS_SEG = 11;
static constexpr const float TS_AR = 3.13f;
static constexpr const float TS_CM = 0.115f; // longitudinal position of the CM from trailing edge
static constexpr const float TS_RP = 0.0625f; // propeller radius [m]
static constexpr const float TS_DEF_MAX = math::radians(39.0f); // max deflection
matrix::Dcmf _C_BS = matrix::Dcmf(matrix::Eulerf(0.0f, math::radians(90.0f), 0.0f)); // segment to body 90 deg pitch
AeroSeg _ts[NB_TS_SEG] = {
AeroSeg(0.0225f, 0.110f, 0.0f, matrix::Vector3f(0.083f - TS_CM, -0.239f, 0.0f), 0.0f, TS_AR),
AeroSeg(0.0383f, 0.125f, 0.0f, matrix::Vector3f(0.094f - TS_CM, -0.208f, 0.0f), 0.0f, TS_AR, 0.063f),
// AeroSeg(0.0884f, 0.148f, 0.0f, matrix::Vector3f(0.111f-TS_CM, -0.143f, 0.0f), 0.0f, TS_AR, 0.063f, TS_RP),
AeroSeg(0.0884f, 0.085f, 0.0f, matrix::Vector3f(0.158f - TS_CM, -0.143f, 0.0f), 0.0f, TS_AR),
AeroSeg(0.0884f, 0.063f, 0.0f, matrix::Vector3f(0.047f - TS_CM, -0.143f, 0.0f), 0.0f, TS_AR, 0.063f, TS_RP),
AeroSeg(0.0633f, 0.176f, 0.0f, matrix::Vector3f(0.132f - TS_CM, -0.068f, 0.0f), 0.0f, TS_AR, 0.063f),
AeroSeg(0.0750f, 0.231f, 0.0f, matrix::Vector3f(0.173f - TS_CM, 0.000f, 0.0f), 0.0f, TS_AR),
AeroSeg(0.0633f, 0.176f, 0.0f, matrix::Vector3f(0.132f - TS_CM, 0.068f, 0.0f), 0.0f, TS_AR, 0.063f),
// AeroSeg(0.0884f, 0.148f, 0.0f, matrix::Vector3f(0.111f-TS_CM, 0.143f, 0.0f), 0.0f, TS_AR, 0.063f, TS_RP),
AeroSeg(0.0884f, 0.085f, 0.0f, matrix::Vector3f(0.158f - TS_CM, 0.143f, 0.0f), 0.0f, TS_AR),
AeroSeg(0.0884f, 0.063f, 0.0f, matrix::Vector3f(0.047f - TS_CM, 0.143f, 0.0f), 0.0f, TS_AR, 0.063f, TS_RP),
AeroSeg(0.0383f, 0.125f, 0.0f, matrix::Vector3f(0.094f - TS_CM, 0.208f, 0.0f), 0.0f, TS_AR, 0.063f),
AeroSeg(0.0225f, 0.110f, 0.0f, matrix::Vector3f(0.083f - TS_CM, 0.239f, 0.0f), 0.0f, TS_AR)
};
// AeroSeg _ts[NB_TS_SEG] = {
// AeroSeg(0.0225f, 0.110f, -90.0f, matrix::Vector3f(0.0f, -0.239f, TS_CM-0.083f), 0.0f, TS_AR),
// AeroSeg(0.0383f, 0.125f, -90.0f, matrix::Vector3f(0.0f, -0.208f, TS_CM-0.094f), 0.0f, TS_AR, 0.063f),
// AeroSeg(0.0884f, 0.148f, -90.0f, matrix::Vector3f(0.0f, -0.143f, TS_CM-0.111f), 0.0f, TS_AR, 0.063f, TS_RP),
// AeroSeg(0.0633f, 0.176f, -90.0f, matrix::Vector3f(0.0f, -0.068f, TS_CM-0.132f), 0.0f, TS_AR, 0.063f),
// AeroSeg(0.0750f, 0.231f, -90.0f, matrix::Vector3f(0.0f, 0.000f, TS_CM-0.173f), 0.0f, TS_AR),
// AeroSeg(0.0633f, 0.176f, -90.0f, matrix::Vector3f(0.0f, 0.068f, TS_CM-0.132f), 0.0f, TS_AR, 0.063f),
// AeroSeg(0.0884f, 0.148f, -90.0f, matrix::Vector3f(0.0f, 0.143f, TS_CM-0.111f), 0.0f, TS_AR, 0.063f, TS_RP),
// AeroSeg(0.0383f, 0.125f, -90.0f, matrix::Vector3f(0.0f, 0.208f, TS_CM-0.094f), 0.0f, TS_AR, 0.063f),
// AeroSeg(0.0225f, 0.110f, -90.0f, matrix::Vector3f(0.0f, 0.239f, TS_CM-0.083f), 0.0f, TS_AR)
// };
// sensors reconstruction
matrix::Vector3f _acc;
matrix::Vector3f _mag;
src/modules/sih/sih_params.c
+3 -2
View File
@@ -439,8 +439,9 @@ PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f);
/**
* Vehicle type
*
* @value 0 MC
* @value 1 FW
* @value 0 Multicopter
* @value 1 Fixed-Wing
* @value 2 Tailsitter
* @reboot_required true
* @group Simulation In Hardware
*/