forked from Archive/PX4-Autopilot
sih: add tailsitter support, disable UAVCAN
This commit is contained in:
romain-chiap
GitHub
parent
6ed48ad0c0
commit
4e06b40e2b
|
|
@ -15,6 +15,8 @@
|
|||
set MIXER quad_x
|
||||
set PWM_OUT 1234
|
||||
|
||||
param set UAVCAN_ENABLE 0
|
||||
|
||||
# set SYS_HITL to 2 to start the SIH and avoid sensors startup
|
||||
param set SYS_HITL 2
|
||||
|
||||
|
|
|
|||
|
|
@ -15,6 +15,8 @@
|
|||
set MIXER AERT
|
||||
set PWM_OUT 1234
|
||||
|
||||
param set UAVCAN_ENABLE 0
|
||||
|
||||
# set SYS_HITL to 2 to start the SIH and avoid sensors startup
|
||||
param set-default SYS_HITL 2
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,61 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# @name SIH Tailsitter Duo
|
||||
#
|
||||
# @type Simulation
|
||||
# @class VTOL
|
||||
#
|
||||
# @output MAIN1 motor right
|
||||
# @output MAIN2 motor left
|
||||
# @output MAIN5 elevon right
|
||||
# @output MAIN6 elevon left
|
||||
#
|
||||
# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
|
||||
#
|
||||
# @board px4_fmu-v2 exclude
|
||||
#
|
||||
|
||||
. ${R}etc/init.d/rc.vtol_defaults
|
||||
|
||||
param set UAVCAN_ENABLE 0
|
||||
param set-default VT_ELEV_MC_LOCK 0
|
||||
param set-default VT_MOT_COUNT 2
|
||||
param set-default VT_TYPE 0
|
||||
param set-default VT_FW_DIFTHR_EN 1
|
||||
param set-default VT_FW_DIFTHR_SC 0.3
|
||||
param set-default MPC_MAN_Y_MAX 60
|
||||
param set-default MC_PITCH_P 5
|
||||
|
||||
param set-default MAV_TYPE 19
|
||||
set MAV_TYPE 19
|
||||
set MIXER vtol_tailsitter_duo_sat
|
||||
|
||||
set PWM_OUT 1234
|
||||
|
||||
# set SYS_HITL to 2 to start the SIH and avoid sensors startup
|
||||
param set-default SYS_HITL 2
|
||||
|
||||
# disable some checks to allow to fly:
|
||||
# - with usb
|
||||
param set-default CBRK_USB_CHK 197848
|
||||
# - without real battery
|
||||
param set-default CBRK_SUPPLY_CHK 894281
|
||||
# - without safety switch
|
||||
param set-default COM_PREARM_MODE 0
|
||||
param set-default CBRK_IO_SAFETY 22027
|
||||
|
||||
param set-default BAT_N_CELLS 3
|
||||
|
||||
param set SIH_T_MAX 2.0
|
||||
param set SIH_Q_MAX 0.0165
|
||||
param set SIH_MASS 0.2
|
||||
# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg
|
||||
param set SIH_IXX 0.00354
|
||||
param set SIH_IYY 0.000625
|
||||
param set SIH_IZZ 0.00300
|
||||
param set SIH_IXZ 0.0
|
||||
param set SIH_KDV 0.2
|
||||
param set SIH_L_ROLL 0.145
|
||||
|
||||
# sih as tailsitter
|
||||
param set SIH_VEHICLE_TYPE 2
|
||||
|
|
@ -40,6 +40,7 @@ px4_add_romfs_files(
|
|||
1002_standard_vtol.hil
|
||||
1100_rc_quad_x_sih.hil
|
||||
1101_rc_plane_sih.hil
|
||||
1102_tailsitter_duo_sih.hil
|
||||
|
||||
# [2000, 2999] Standard planes"
|
||||
2100_standard_plane
|
||||
|
|
|
|||
|
|
@ -90,4 +90,5 @@ px4_add_romfs_files(
|
|||
vtol_convergence.main.mix
|
||||
vtol_delta.aux.mix
|
||||
vtol_tailsitter_duo.main.mix
|
||||
vtol_tailsitter_duo_sat.main.mix
|
||||
)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,39 @@
|
|||
Tailsitter duo mixer
|
||||
============================
|
||||
|
||||
This file defines a mixer for a generic duo tailsitter VTOL (eg TBS Caipirinha tailsitter edition). This vehicle
|
||||
has two motors in total, one attached to each wing. It also has two elevons which
|
||||
are located in the slipstream of the propellers. This mixer generates 4 PWM outputs
|
||||
on the main PWM ouput port, two at 400Hz for the motors, and two at 50Hz for the
|
||||
elevon servos. Channels 1-4 are configured to run at 400Hz, while channels 5-8 run
|
||||
at the default rate of 50Hz. Note that channels 3 and 4 are assigned but not used.
|
||||
|
||||
Motor mixer
|
||||
------------
|
||||
Channel 1 connects to the right (starboard) motor.
|
||||
Channel 2 connects to the left (port) motor.
|
||||
|
||||
R: 2-
|
||||
|
||||
Zero mixer (2x)
|
||||
---------------
|
||||
Channels 3,4 are unused.
|
||||
|
||||
Z:
|
||||
|
||||
Z:
|
||||
|
||||
Elevons mixer
|
||||
--------------
|
||||
Channel 5 connects to the right (starboard) elevon.
|
||||
Channel 6 connects to the left (port) elevon.
|
||||
Here we saturate the elevons before their full range
|
||||
to avoid roll-pitch-yaw coupling during faster maneuvers
|
||||
|
||||
M: 2
|
||||
S: 1 0 10000 10000 0 -6000 6000
|
||||
S: 1 1 10000 10000 0 -6000 6000
|
||||
|
||||
M: 2
|
||||
S: 1 0 10000 10000 0 -6000 6000
|
||||
S: 1 1 -10000 -10000 0 -6000 6000
|
||||
|
|
@ -10,7 +10,7 @@ extra_args=
|
|||
baudrate=921600
|
||||
device=
|
||||
ip="127.0.0.1"
|
||||
while getopts ":b:d:p:qsr:f:i:loa" opt; do
|
||||
while getopts ":b:d:p:qsr:f:i:loat" opt; do
|
||||
case $opt in
|
||||
b)
|
||||
baudrate=$OPTARG
|
||||
|
|
@ -40,7 +40,10 @@ while getopts ":b:d:p:qsr:f:i:loa" opt; do
|
|||
extra_args="$extra_args -disponly"
|
||||
;;
|
||||
a)
|
||||
extra_args="$extra_args -fw" # aircraft
|
||||
extra_args="$extra_args -fw" # aircraft model
|
||||
;;
|
||||
t)
|
||||
extra_args="$extra_args -ts" # tailsitter model
|
||||
;;
|
||||
\?)
|
||||
echo "Invalid option: -$OPTARG" >&2
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
*/
|
||||
|
|
|
|||
Loading…
Reference in New Issue