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;
*/
}
void ftPhysicsSystem::step(real dt)
{
integrateVelocity(dt);
auto updateColSystem = [this](ftBody *body) -> void {
for (auto collider = body->colliders; collider != nullptr; collider = collider->next)
{
this->m_collisionSystem.moveShape(collider->collisionHandle, body->transform * collider->transform);
}
};
m_dynamicBodies.forEach(std::cref(updateColSystem));
m_kinematicBodies.forEach(std::cref(updateColSystem));
auto colFilter = [](void *userdataA, void *userdataB) -> bool {
ftCollider *colliderA = (ftCollider *)userdataA;
ftCollider *colliderB = (ftCollider *)userdataB;
ftBody *bodyA = colliderA->body;
ftBody *bodyB = colliderB->body;
if (bodyA == bodyB)
return false;
if (colliderA->group != 0 && colliderA->group == colliderB->group)
return false;
int maskA = colliderA->mask;
int maskB = colliderB->mask;
int categoryA = colliderA->category;
int categoryB = colliderB->category;
if (!(maskA & categoryB))
return false;
if (!(maskB & categoryA))
return false;
return true;
};
ftCollisionCallback callback;
callback.beginContact = &beginContactListener;
callback.updateContact = &updateContactListener;
callback.endContact = &endContactListener;
callback.data = &m_islandSystem;
m_collisionSystem.updateContacts(colFilter, callback);
auto processIsland =
[this, dt](const ftIsland &island) -> void {
bool allSleeping = true;
const ftChunkArray<ftBody *> *bodies = &(island.bodies);
for (uint32 i = 0; i < island.bodies.getSize(); ++i)
{
ftBody *body = (*bodies)[i];
bool isStatic = body->bodyType == STATIC;
bool isKinematic = body->bodyType == KINEMATIC;
bool wantSleep = body->sleepTimer > m_sleepTimeLimit;
if (isKinematic || (!isStatic && !wantSleep))
{
allSleeping = false;
break;
}
}
if (allSleeping)
{
for (uint32 i = 0; i < island.bodies.getSize(); ++i)
{
ftBody *body = (*bodies)[i];
if (body->bodyType == DYNAMIC && body->activationState != SLEEP)
{
body->activationState = SLEEP;
body->velocity.setZero();
body->angularVelocity = 0;
this->m_dynamicBodies.unlink(body);
this->m_sleepingBodies.insert(body);
}
}
}
else
{
for (uint32 i = 0; i < island.bodies.getSize(); ++i)
{
ftBody *body = (*bodies)[i];
if (body->activationState == SLEEP)
{
body->activationState = ACTIVE;
body->sleepTimer = 0;
this->m_sleepingBodies.unlink(body);
this->m_dynamicBodies.insert(body);
}
}
this->m_contactSolver.createConstraints(island);
this->m_contactSolver.preSolve(dt);
this->m_contactSolver.warmStart();
this->m_contactSolver.solve(dt);
this->m_contactSolver.clearConstraints();
}
};
m_islandSystem.buildAndProcessIsland(std::cref(processIsland));
updateBodiesActivation(dt);
integratePosition(dt);
}
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;
*/
}
