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; */ }