void
UAVSimulation::update3DOF(const double& timestep)
{
if (timestep <= 0)
return;
//! Wind effects
m_velocity(2) = m_wind(2);
calcUAV2AirData();
//==========================================================================
//! Aircraft Dynamics
//==========================================================================
integratePosition(timestep);
/*
//for debug
double vt_velocity1[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
*/
//! Command effect
//! - Airspeed command
m_airspeed = m_airspeed_cmd;
//! - Roll command
m_position(3) = m_bank_cmd;
//! Turn rate
m_velocity(5) = m_g * std::tan(m_position(3))/m_airspeed;
/*
//for debug
double vt_velocity2[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
*/
updateVelocity();
/*
//for debug
double vt_velocity2[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
if (std::abs(vt_velocity1[0] - vt_velocity2[0]) > 0.5*m_airspeed*timestep/m_speed_time_cst ||
std::abs(vt_velocity1[1] - vt_velocity2[1]) > 0.5*m_airspeed*timestep/m_speed_time_cst)
{
std::cout << "Time step: " << timestep << std::endl;
std::cout << "Velocity difference: " << vt_velocity1[0]-vt_velocity2[0] << ", " << vt_velocity1[1]-vt_velocity2[1] << std::endl;
std::cout << "Heading: " << DUNE::Math::Angles::degrees(vt_position1[5]) << "º" << std::endl;
}
*/
/*
//for debug
double vt_velocity3[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
std::cout << "Prev velocity: " << vt_velocity1[0] << ", " << vt_velocity1[1] << std::endl;
std::cout << "Int velocity: " << vt_velocity2[0] << ", " << vt_velocity2[1] << std::endl;
std::cout << "New velocity: " << vt_velocity3[0] << ", " << vt_velocity3[1] << std::endl;
*/
}
bool gol::RendererSFML::render(const std::vector<std::vector<int>> &generation, const int &rows, const int &columns)
{
sf::Text escapePlan("PRESS ESC TO GO BACK TO MAIN MENU", m_golbalFont, 12.f);
escapePlan.setColor(sf::Color::Green);
escapePlan.setPosition(sf::Vector2f(gol::WIDTH - escapePlan.getLocalBounds().width - 2.f , gol::HEIGHT - 15.f));
sf::Vector2i m_position(sf::Vector2i(m_cell.getSize().x + 2, m_cell.getSize().y + 2));
if(m_window.isOpen())
{
sf::Event event;
while(m_window.pollEvent(event))
{
if(event.type == sf::Event::Closed)
{
m_window.close();
return false;
}
if(event.type == sf::Event::KeyPressed)
if(event.key.code == sf::Keyboard::Escape)
return false;
}
m_window.clear(sf::Color::Black);
float width = columns * m_position.x;
float height = rows * m_position.y;
float left = (gol::WIDTH - width) / 2.f;
float top = (gol::HEIGHT - height) / 2.f;
for(int I = 0; I < rows; I++)
{
for(int J = 0; J < columns; J++)
{
if(generation[I][J] == 1)
{
//m_cell.setPosition(sf::Vector2f(left + J * m_position.x + m_position.x, top + I * 10 + 10));
m_cell.setPosition(sf::Vector2f(left + J * m_position.x, top + I * m_position.y));
m_window.draw(m_cell);
}
}
}
m_window.draw(escapePlan);
m_window.display();
return true;
}
return false;
}
void
UAVSimulation::commandAlt(const double& altitude_cmd)
{
//! Control commands
//! - Altitude
m_altitude_cmd = altitude_cmd;
if (m_sim_type.compare("3DOF") == 0 || m_sim_type.compare("4DOF_bank") == 0)
m_position(2) = -altitude_cmd;
//! Control commands initialization flags
//! - Altitude
m_altitude_cmd_ini = 1;
//! - Disallow flight path angle reference
m_fpa_cmd_ini = 0;
//! - Disallow pitch reference
m_pitch_cmd_ini = 0;
}
void
UAVSimulation::command(const double& bank_cmd, const double& airspeed_cmd, const double& altitude_cmd)
{
//! Control commands
//! - Bank
m_bank_cmd = bank_cmd;
//! - Airspeed
m_airspeed_cmd = airspeed_cmd;
//! - Altitude
m_altitude_cmd = altitude_cmd;
if (m_sim_type == "3DOF" || m_sim_type == "4DOF_bank")
m_position(2) = -altitude_cmd;
//! Control commands initialization flags
//! - Airspeed
m_airspeed_cmd_ini = 1;
//! - Altitude
m_altitude_cmd_ini = 1;
}
void
UAVSimulation::setPosition(const DUNE::Math::Matrix& pos)
{
int i_pos_size = pos.rows();
if (i_pos_size < 2 && i_pos_size > 6)
throw Error("Invalid position vector dimension. Vector size must be between 2 and 6.");
// Vehicle position
m_position.set(0, i_pos_size-1, 0, 0, pos);
// Reset the pitch angle for the simulations that do not update it
if (m_sim_type.compare("3DOF") == 0 || m_sim_type.compare("4DOF_bank") == 0)
m_position(4) = 0;
// Simulation variables
m_cos_course = std::cos(m_position(5));
m_sin_course = std::sin(m_position(5));
m_cos_pitch = std::cos(m_position(4));
m_sin_pitch = std::sin(m_position(4));
m_cos_roll = std::cos(m_position(3));
m_sin_roll = std::sin(m_position(3));
}
void
UAVSimulation::setPosition(const DUNE::Math::Matrix& pos)
{
int i_pos_size = pos.rows();
if (i_pos_size < 2 && i_pos_size > 6)
throw Error("Invalid position vector dimension. Vector size must be between 2 and 6.");
//! Vehicle position
m_position.set(0, i_pos_size-1, 0, 0, pos);
//! Simulation variables
m_cos_course = std::cos(m_position(5));
m_sin_course = std::sin(m_position(5));
m_cos_pitch = std::cos(m_position(4));
m_sin_pitch = std::sin(m_position(4));
m_cos_roll = std::cos(m_position(3));
m_sin_roll = std::sin(m_position(3));
}
void
UAVSimulation::update5DOF(const double& timestep)
{
//! Check if model has the required parameters
//! - Model control time constants
if (!m_bank_time_cst_f && !m_speed_time_cst_f && !m_alt_time_cst_f)
{
throw Error("No model parameters defined! The state was not updated.");
//inf("No model parameters defined! The state was not updated.");
return;
}
else if (!m_alt_time_cst_f)
{
throw Error("Model parameter missing (Altitude time constant)! The state was not updated.");
//inf("No model parameters defined! The state was not updated.");
return;
}
if (timestep <= 0)
return;
//! Wind effects
calcUAV2AirData();
//==========================================================================
//! Aircraft Dynamics
//==========================================================================
integratePosition(timestep);
/*
//for debug
double vt_velocity1[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
*/
//! Turn rate
m_velocity(5) = m_g * std::tan(m_position(3))/m_airspeed;
//! Command effect
//! - Horizontal acceleration command
double d_lon_accel = (m_airspeed_cmd - m_airspeed)/m_speed_time_cst;
if (m_lon_accel_lim_f)
d_lon_accel = DUNE::Math::trimValue(d_lon_accel, -m_lon_accel_lim, m_lon_accel_lim);
m_airspeed += d_lon_accel*timestep;
//! - Roll rate command
m_velocity(3) = (m_bank_cmd - m_position(3))/m_bank_time_cst;
if (m_bank_rate_lim_f)
m_velocity(3) = DUNE::Math::trimValue(m_velocity(3), -m_bank_rate_lim, m_bank_rate_lim);
//! - Vertical rate command
m_velocity(2) = (-m_altitude_cmd - m_position(2))/m_alt_time_cst;
if (m_vert_slope_lim_f)
{
double d_vert_rate_lim = m_vert_slope_lim*m_airspeed;
m_velocity(2) = DUNE::Math::trimValue(m_velocity(2), -d_vert_rate_lim, d_vert_rate_lim);
}
else
//! The vertical speed should not exceed the airspeed, even if there is no specified vertical slope limit
m_velocity(2) = DUNE::Math::trimValue(m_velocity(2), -m_airspeed, m_airspeed);
//! - Computing flight path angle
m_sin_pitch = -m_velocity(2)/m_airspeed;
m_cos_pitch = std::sqrt(1 - m_sin_pitch*m_sin_pitch);
m_position(4) = std::asin(m_sin_pitch);
updateVelocity();
/*
//for debug
double vt_velocity2[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
if (std::abs(vt_velocity1[0] - vt_velocity2[0]) > 0.5*m_airspeed*timestep/m_speed_time_cst ||
std::abs(vt_velocity1[1] - vt_velocity2[1]) > 0.5*m_airspeed*timestep/m_speed_time_cst)
{
std::cout << "Time step: " << timestep << std::endl;
std::cout << "Velocity difference: " << vt_velocity1[0]-vt_velocity2[0] << ", " << vt_velocity1[1]-vt_velocity2[1] << std::endl;
std::cout << "Heading: " << DUNE::Math::Angles::degrees(m_position(5)) << "º" << std::endl;
}
*/
}
void
UAVSimulation::update4DOF_Bank(const double& timestep)
{
//! Check if model has the required parameters
//! - Model control time constants
if (!m_bank_time_cst_f)// && !m_speed_time_cst_f)
{
throw Error("No model parameters defined! The state was not updated.");
//inf("No model parameters defined! The state was not updated.");
return;
}
if (timestep <= 0)
return;
calcUAV2AirData();
//==========================================================================
//! Aircraft Dynamics
//==========================================================================
integratePosition(timestep);
/*
//for debug
double vt_velocity1[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
*/
//! Turn rate
m_velocity(5) = m_g * std::tan(m_position(3))/m_airspeed;
//! Command effect
//! - Horizontal acceleration command
double d_lon_accel = (m_airspeed_cmd - m_airspeed)/m_speed_time_cst;
if (m_lon_accel_lim_f)
d_lon_accel = DUNE::Math::trimValue(d_lon_accel, -m_lon_accel_lim, m_lon_accel_lim);
m_airspeed += d_lon_accel*timestep;
//! - Roll rate command
m_velocity(3) = (m_bank_cmd - m_position(3))/m_bank_time_cst;
if (m_bank_rate_lim_f)
m_velocity(3) = DUNE::Math::trimValue(m_velocity(3), -m_bank_rate_lim, m_bank_rate_lim);
//! Wind effects
m_velocity(2) = m_wind(2);
/*
//for debug
double vt_velocity2[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
*/
updateVelocity();
/*
//for debug
double vt_velocity2[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
if (std::abs(vt_velocity1[0] - vt_velocity2[0]) > 0.5*m_airspeed*timestep/m_speed_time_cst ||
std::abs(vt_velocity1[1] - vt_velocity2[1]) > 0.5*m_airspeed*timestep/m_speed_time_cst)
{
std::cout << "Time step: " << timestep << std::endl;
std::cout << "Velocity difference: " << vt_velocity1[0]-vt_velocity2[0] << ", " << vt_velocity1[1]-vt_velocity2[1] << std::endl;
std::cout << "Heading: " << DUNE::Math::Angles::degrees(vt_position1[5]) << "º" << std::endl;
}
*/
/*
//for debug
double vt_velocity3[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
std::cout << "Prev velocity: " << vt_velocity1[0] << ", " << vt_velocity1[1] << std::endl;
std::cout << "Int velocity: " << vt_velocity2[0] << ", " << vt_velocity2[1] << std::endl;
std::cout << "New velocity: " << vt_velocity3[0] << ", " << vt_velocity3[1] << std::endl;
*/
}
void
UAVSimulation::integratePosition(const double& timestep)
{
/*
//for debug
double vt_position1[6] = {m_position(0), m_position(1), m_position(2), m_position(3), m_position(4), m_position(5)};
*/
double d_initial_yaw = m_position(5);
//! Vertical position and Euler angles state update
m_position.set(2, 5, 0, 0, m_position.get(2, 5, 0, 0) + m_velocity.get(2, 5, 0, 0)*timestep);
m_position(5) = DUNE::Math::Angles::normalizeRadian(m_position(5));
// Optimization variables
m_cos_yaw = std::cos(m_position(5));
m_sin_yaw = std::sin(m_position(5));
if (m_sim_type == "5DOF" || m_sim_type == "4DOF_alt")
{
m_cos_pitch = std::cos(m_position(4));
m_sin_pitch = std::sin(m_position(4));
}
else if (m_sim_type == "6DOF_stab")
{
m_cos_roll = std::cos(m_position(3));
m_sin_roll = std::sin(m_position(3));
}
//! Horizontal position state update
if (std::abs(m_position(3)) < 0.1)
{
m_position.set(0, 1, 0, 0, m_position.get(0, 1, 0, 0) + m_velocity.get(0, 1, 0, 0)*timestep);
}
else
{
double d_turn_radius = m_airspeed/m_velocity(5);
m_position(0) += d_turn_radius*(m_sin_yaw - std::sin(d_initial_yaw));
m_position(1) += d_turn_radius*(std::cos(d_initial_yaw) - m_cos_yaw);
}
/*
//for debug
double vt_position2[6] = {m_position(0), m_position(1), m_position(2), m_position(3), m_position(4), m_position(5)};
if (std::abs(vt_position1[0] - vt_position2[0]) > 2*m_airspeed*timestep || std::abs(vt_position1[1] - vt_position2[1]) > 2*m_airspeed*timestep)
{
std::cout << "Time step: " << timestep << std::endl;
std::cout << "Position difference: " << vt_position1[0]-vt_position2[0] << ", " << vt_position1[1]-vt_position2[1] << std::endl;
}
if (std::abs(vt_position1[0] - vt_position2[0]) > 100 || std::abs(vt_position1[1] - vt_position2[1]) > 100)
{
std::cout << "Time step: " << timestep << std::endl;
std::cout << "Position difference: " << vt_position1[0]-vt_position2[0] << ", " << vt_position1[1]-vt_position2[1] << std::endl;
std::cout << "Heading: " << DUNE::Math::Angles::degrees(vt_position1[5]) << "º" << std::endl;
}
*/
/*
//for debug
double vt_position2[6] = {m_position(0), m_position(1), m_position(2), m_position(3), m_position(4), m_position(5)};
std::cout << "Prev position: " << vt_position1[0] << ", " << vt_position1[1] << std::endl;
std::cout << "New position: " << vt_position2[0] << ", " << vt_position2[1] << std::endl;
*/
}