From 92f060bdef00a7e052286ece8727851905cf4fcd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 17 Apr 2017 14:23:15 +1000 Subject: [PATCH] SITL: improved single-motor tailsitter plane give larger control surfaces and take account of thrust airflow over control surfaces --- libraries/SITL/SIM_Plane.cpp | 40 ++++++++++++++++++++++++++++-------- libraries/SITL/SIM_Plane.h | 2 +- 2 files changed, 32 insertions(+), 10 deletions(-) diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp index b977cd588c..2a80cc66de 100644 --- a/libraries/SITL/SIM_Plane.cpp +++ b/libraries/SITL/SIM_Plane.cpp @@ -98,9 +98,23 @@ float Plane::dragCoeff(float alpha) const } // Torque calculation function -Vector3f Plane::getTorque(float inputAileron, float inputElevator, float inputRudder, const Vector3f &force) const +Vector3f Plane::getTorque(float inputAileron, float inputElevator, float inputRudder, float inputThrust, const Vector3f &force) const { - const float alpha = angle_of_attack; + float alpha = angle_of_attack; + + //calculate aerodynamic torque + float effective_airspeed = airspeed; + + if (tailsitter) { + /* + tailsitters get airspeed from prop-wash + */ + effective_airspeed += inputThrust * 20; + + // reduce effective angle of attack as thrust increases + alpha *= constrain_float(1 - inputThrust, 0, 1); + } + const float s = coefficient.s; const float c = coefficient.c; const float b = coefficient.b; @@ -129,10 +143,9 @@ Vector3f Plane::getTorque(float inputAileron, float inputElevator, float inputRu double q = gyro.y; double r = gyro.z; - //calculate aerodynamic torque - double qbar = 1.0/2.0*rho*pow(airspeed,2)*s; //Calculate dynamic pressure + double qbar = 1.0/2.0*rho*pow(effective_airspeed,2)*s; //Calculate dynamic pressure double la, na, ma; - if (is_zero(airspeed)) + if (is_zero(effective_airspeed)) { la = 0; ma = 0; @@ -140,9 +153,9 @@ Vector3f Plane::getTorque(float inputAileron, float inputElevator, float inputRu } else { - la = qbar*b*(c_l_0 + c_l_b*beta + c_l_p*b*p/(2*airspeed) + c_l_r*b*r/(2*airspeed) + c_l_deltaa*inputAileron + c_l_deltar*inputRudder); - ma = qbar*c*(c_m_0 + c_m_a*alpha + c_m_q*c*q/(2*airspeed) + c_m_deltae*inputElevator); - na = qbar*b*(c_n_0 + c_n_b*beta + c_n_p*b*p/(2*airspeed) + c_n_r*b*r/(2*airspeed) + c_n_deltaa*inputAileron + c_n_deltar*inputRudder); + la = qbar*b*(c_l_0 + c_l_b*beta + c_l_p*b*p/(2*effective_airspeed) + c_l_r*b*r/(2*effective_airspeed) + c_l_deltaa*inputAileron + c_l_deltar*inputRudder); + ma = qbar*c*(c_m_0 + c_m_a*alpha + c_m_q*c*q/(2*effective_airspeed) + c_m_deltae*inputElevator); + na = qbar*b*(c_n_0 + c_n_b*beta + c_n_p*b*p/(2*effective_airspeed) + c_n_r*b*r/(2*effective_airspeed) + c_n_deltaa*inputAileron + c_n_deltar*inputRudder); } @@ -254,9 +267,18 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel // calculate angle of attack angle_of_attack = atan2f(velocity_air_bf.z, velocity_air_bf.x); beta = atan2f(velocity_air_bf.y,velocity_air_bf.x); + + if (tailsitter) { + /* + tailsitters get 4x the control surfaces + */ + aileron *= 4; + elevator *= 4; + rudder *= 4; + } Vector3f force = getForce(aileron, elevator, rudder); - rot_accel = getTorque(aileron, elevator, rudder, force); + rot_accel = getTorque(aileron, elevator, rudder, thrust, force); // simulate engine RPM rpm1 = thrust * 7000; diff --git a/libraries/SITL/SIM_Plane.h b/libraries/SITL/SIM_Plane.h index d9b4cc7de6..80b59b3bd0 100644 --- a/libraries/SITL/SIM_Plane.h +++ b/libraries/SITL/SIM_Plane.h @@ -104,7 +104,7 @@ protected: float liftCoeff(float alpha) const; float dragCoeff(float alpha) const; Vector3f getForce(float inputAileron, float inputElevator, float inputRudder) const; - Vector3f getTorque(float inputAileron, float inputElevator, float inputRudder, const Vector3f &force) const; + Vector3f getTorque(float inputAileron, float inputElevator, float inputRudder, float inputThrust, const Vector3f &force) const; void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel); };