fixedwing_forces_and_moments.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2017 Daniel Koch, James Jackson and Gary Ellingson, BYU MAGICC Lab.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * * Redistributions of source code must retain the above copyright notice, this
 *   list of conditions and the following disclaimer.
 *
 * * 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.
 *
 * * Neither the name of the copyright holder 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 HOLDER 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.
 */

#include <rosflight_sim/fixedwing_forces_and_moments.h>

namespace rosflight_sim
{
Fixedwing::Fixedwing(ros::NodeHandle* nh)
{
  nh_ = nh;

  // physical parameters
  mass_ = nh_->param<double>("mass", 13.5);
  Jx_ = nh_->param<double>("Jx", 0.8244);
  Jy_ = nh_->param<double>("Jy", 1.135);
  Jz_ = nh_->param<double>("Jz", 1.759);
  Jxz_ = nh_->param<double>("Jxz", .1204);
  rho_ = nh_->param<double>("rho", 1.2682);

  // Wing Geometry
  wing_.S = nh_->param<double>("wing_s", 0.55);
  wing_.b = nh_->param<double>("wing_b", 2.8956);
  wing_.c = nh_->param<double>("wing_c", 0.18994);
  wing_.M = nh_->param<double>("wing_M", 0.55);
  wing_.epsilon = nh_->param<double>("wing_epsilon", 2.8956);
  wing_.alpha0 = nh_->param<double>("wing_alpha0", 0.18994);

  // Propeller Coefficients
  prop_.k_motor = nh_->param<double>("k_motor", 80.0);
  prop_.k_T_P = nh_->param<double>("k_T_P", 0.0);
  prop_.k_Omega = nh_->param<double>("k_Omega", 0.0);
  prop_.e = nh_->param<double>("prop_e", 0.9);
  prop_.S = nh_->param<double>("prop_S", 0.202);
  prop_.C = nh_->param<double>("prop_C", 1.0);

  // Lift Params
  CL_.O = nh_->param<double>("C_L_O", 0.28);
  CL_.alpha = nh_->param<double>("C_L_alpha", 3.45);
  CL_.beta = nh_->param<double>("C_L_beta", 0.0);
  CL_.p = nh_->param<double>("C_L_p", 0.0);
  CL_.q = nh_->param<double>("C_L_q", 0.0);
  CL_.r = nh_->param<double>("C_L_r", 0.0);
  CL_.delta_a = nh_->param<double>("C_L_delta_a", 0.0);
  CL_.delta_e = nh_->param<double>("C_L_delta_e", -0.36);
  CL_.delta_r = nh_->param<double>("C_L_delta_r", 0.0);

  // Drag Params
  CD_.O = nh_->param<double>("C_D_O", 0.03);
  CD_.alpha = nh_->param<double>("C_D_alpha", 0.30);
  CD_.beta = nh_->param<double>("C_D_beta", 0.0);
  CD_.p = nh_->param<double>("C_D_p", 0.0437);
  CD_.q = nh_->param<double>("C_D_q", 0.0);
  CD_.r = nh_->param<double>("C_D_r", 0.0);
  CD_.delta_a = nh_->param<double>("C_D_delta_a", 0.0);
  CD_.delta_e = nh_->param<double>("C_D_delta_e", 0.0);
  CD_.delta_r = nh_->param<double>("C_D_delta_r", 0.0);

  // ell Params (x axis moment)
  Cell_.O = nh_->param<double>("C_ell_O", 0.0);
  Cell_.alpha = nh_->param<double>("C_ell_alpha", 0.00);
  Cell_.beta = nh_->param<double>("C_ell_beta", -0.12);
  Cell_.p = nh_->param<double>("C_ell_p", -0.26);
  Cell_.q = nh_->param<double>("C_ell_q", 0.0);
  Cell_.r = nh_->param<double>("C_ell_r", 0.14);
  Cell_.delta_a = nh_->param<double>("C_ell_delta_a", 0.08);
  Cell_.delta_e = nh_->param<double>("C_ell_delta_e", 0.0);
  Cell_.delta_r = nh_->param<double>("C_ell_delta_r", 0.105);

  // m Params (y axis moment)
  Cm_.O = nh_->param<double>("C_m_O", -0.02338);
  Cm_.alpha = nh_->param<double>("C_m_alpha", -0.38);
  Cm_.beta = nh_->param<double>("C_m_beta", 0.0);
  Cm_.p = nh_->param<double>("C_m_p", 0.0);
  Cm_.q = nh_->param<double>("C_m_q", -3.6);
  Cm_.r = nh_->param<double>("C_m_r", 0.0);
  Cm_.delta_a = nh_->param<double>("C_m_delta_a", 0.0);
  Cm_.delta_e = nh_->param<double>("C_m_delta_e", -0.5);
  Cm_.delta_r = nh_->param<double>("C_m_delta_r", 0.0);

  // n Params (z axis moment)
  Cn_.O = nh_->param<double>("C_n_O", 0.0);
  Cn_.alpha = nh_->param<double>("C_n_alpha", 0.0);
  Cn_.beta = nh_->param<double>("C_n_beta", 0.25);
  Cn_.p = nh_->param<double>("C_n_p", 0.022);
  Cn_.q = nh_->param<double>("C_n_q", 0.0);
  Cn_.r = nh_->param<double>("C_n_r", -0.35);
  Cn_.delta_a = nh_->param<double>("C_n_delta_a", 0.06);
  Cn_.delta_e = nh_->param<double>("C_n_delta_e", 0.0);
  Cn_.delta_r = nh_->param<double>("C_n_delta_r", -0.032);

  // Y Params (Sideslip Forces)
  CY_.O = nh_->param<double>("C_Y_O", 0.0);
  CY_.alpha = nh_->param<double>("C_Y_alpha", 0.00);
  CY_.beta = nh_->param<double>("C_Y_beta", -0.98);
  CY_.p = nh_->param<double>("C_Y_p", 0.0);
  CY_.q = nh_->param<double>("C_Y_q", 0.0);
  CY_.r = nh_->param<double>("C_Y_r", 0.0);
  CY_.delta_a = nh_->param<double>("C_Y_delta_a", 0.0);
  CY_.delta_e = nh_->param<double>("C_Y_delta_e", 0.0);
  CY_.delta_r = nh_->param<double>("C_Y_delta_r", -0.017);

  wind_ = Eigen::Vector3d::Zero();
}

Eigen::Matrix<double, 6, 1> Fixedwing::updateForcesAndTorques(Current_State x, const int act_cmds[])
{
  delta_.a = (act_cmds[0] - 1500.0) / 500.0;
  delta_.e = -(act_cmds[1] - 1500.0) / 500.0;
  delta_.t = (act_cmds[2] - 1000.0) / 1000.0;
  delta_.r = -(act_cmds[3] - 1500.0) / 500.0;

  double p = x.omega(0);
  double q = x.omega(1);
  double r = x.omega(2);

  // Calculate airspeed
  Eigen::Vector3d V_airspeed = x.vel + x.rot.inverse() * wind_;
  double ur = V_airspeed(0);
  double vr = V_airspeed(1);
  double wr = V_airspeed(2);

  double Va = V_airspeed.norm();

  Eigen::Matrix<double, 6, 1> forces;

  // Be sure that we have some significant airspeed before we run aerodynamics, and don't let NaNs get through
  if (Va > 1.0 && std::isfinite(Va))
  {
    /*
     * The following math follows the method described in chapter 4 of
     * Small Unmanned Aircraft: Theory and Practice
     * By Randy Beard and Tim McLain.
     * Look there for a detailed explanation of each line in the rest of this function
     */
    double alpha = atan2(wr, ur);
    double beta = asin(vr / Va);

    double ca = cos(alpha);
    double sa = sin(alpha);

    double sign = (alpha >= 0 ? 1 : -1); // Sigmoid function
    double sigma_a = (1 + exp(-(wing_.M * (alpha - wing_.alpha0))) + exp((wing_.M * (alpha + wing_.alpha0))))
                     / ((1 + exp(-(wing_.M * (alpha - wing_.alpha0)))) * (1 + exp((wing_.M * (alpha + wing_.alpha0)))));
    double CL_a = (1 - sigma_a) * (CL_.O + CL_.alpha * alpha) + sigma_a * (2 * sign * sa * sa * ca);
    double AR = (pow(wing_.b, 2.0)) / wing_.S;
    double CD_a =
        CD_.p
        + ((pow((CL_.O + CL_.alpha * (alpha)), 2.0))
           / (3.14159 * 0.9
              * AR)); // the const 0.9 in this equation replaces the e (Oswald Factor) variable and may be inaccurate

    double CX_a = -CD_a * ca + CL_a * sa;
    double CX_q_a = -CD_.q * ca + CL_.q * sa;
    double CX_deltaE_a = -CD_.delta_e * ca + CL_.delta_e * sa;

    double CZ_a = -CD_a * sa - CL_a * ca;
    double CZ_q_a = -CD_.q * sa - CL_.q * ca;
    double CZ_deltaE_a = -CD_.delta_e * sa - CL_.delta_e * ca;

    forces(0) = 0.5 * (rho_)*Va * Va * wing_.S * (CX_a + (CX_q_a * wing_.c * q) / (2.0 * Va) + CX_deltaE_a * delta_.e)
                + 0.5 * rho_ * prop_.S * prop_.C * (pow((prop_.k_motor * delta_.t), 2.0) - Va * Va);
    forces(1) = 0.5 * (rho_)*Va * Va * wing_.S
                * (CY_.O + CY_.beta * beta + ((CY_.p * wing_.b * p) / (2.0 * Va)) + ((CY_.r * wing_.b * r) / (2.0 * Va))
                   + CY_.delta_a * delta_.a + CY_.delta_r * delta_.r);
    forces(2) = 0.5 * (rho_)*Va * Va * wing_.S * (CZ_a + (CZ_q_a * wing_.c * q) / (2.0 * Va) + CZ_deltaE_a * delta_.e);

    forces(3) = 0.5 * (rho_)*Va * Va * wing_.S * wing_.b
                    * (Cell_.O + Cell_.beta * beta + (Cell_.p * wing_.b * p) / (2.0 * Va)
                       + (Cell_.r * wing_.b * r) / (2.0 * Va) + Cell_.delta_a * delta_.a + Cell_.delta_r * delta_.r)
                - prop_.k_T_P * pow((prop_.k_Omega * delta_.t), 2.0);
    forces(4) = 0.5 * (rho_)*Va * Va * wing_.S * wing_.c
                * (Cm_.O + Cm_.alpha * alpha + (Cm_.q * wing_.c * q) / (2.0 * Va) + Cm_.delta_e * delta_.e);
    forces(5) = 0.5 * (rho_)*Va * Va * wing_.S * wing_.b
                * (Cn_.O + Cn_.beta * beta + (Cn_.p * wing_.b * p) / (2.0 * Va) + (Cn_.r * wing_.b * r) / (2.0 * Va)
                   + Cn_.delta_a * delta_.a + Cn_.delta_r * delta_.r);
  }
  else
  {
    forces(0) = 0.5 * rho_ * prop_.S * prop_.C * ((prop_.k_motor * delta_.t * prop_.k_motor * delta_.t));
    forces(1) = 0.0;
    forces(2) = 0.0;
    forces(3) = 0.0;
    forces(4) = 0.0;
    forces(5) = 0.0;
  }

  return forces;
}

void Fixedwing::set_wind(Eigen::Vector3d wind)
{
  wind_ = wind;
}

} // namespace rosflight_sim