hongshaorou/YOPO
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Simplify the code and remove drone dynamics (unused)

Browse Source
This commit is contained in:
Lu Junjie
2024-10-22 11:13:26 +08:00
parent 004ff2fea9
commit e9aa4e406d
19 changed files with 13 additions and 794 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
flightlib/src/common/command.cpp
View File

@@ -1,29 +0,0 @@
#include "flightlib/common/command.hpp"
namespace flightlib {
Command::Command() {}
Command::Command(const Scalar t, const Scalar thrust, const Vector<3>& omega)
: t(t), collective_thrust(thrust), omega(omega) {}
Command::Command(const Scalar t, const Vector<4>& thrusts)
: t(t), thrusts(thrusts) {}
bool Command::valid() const {
return std::isfinite(t) &&
((std::isfinite(collective_thrust) && omega.allFinite()) ^
thrusts.allFinite());
}
bool Command::isSingleRotorThrusts() const {
return std::isfinite(t) && thrusts.allFinite();
}
bool Command::isRatesThrust() const {
return std::isfinite(t) && std::isfinite(collective_thrust) &&
omega.allFinite();
}
} // namespace flightlib

33
flightlib/src/common/integrator_base.cpp
View File

@@ -1,33 +0,0 @@
#include "flightlib/common/integrator_base.hpp"
namespace flightlib {
IntegratorBase::IntegratorBase(IntegratorBase::DynamicsFunction function,
const Scalar dt_max)
: dynamics_(function), dt_max_(dt_max) {}
bool IntegratorBase::integrate(const QuadState& initial,
QuadState* const final) const {
if (std::isnan(initial.t) || std::isnan(final->t)) return false;
if (initial.t >= final->t) return false;
return integrate(initial.x, final->t - initial.t, final->x);
}
bool IntegratorBase::integrate(const Ref<const Vector<>> initial,
const Scalar dt, Ref<Vector<>> final) const {
Scalar dt_remaining = dt;
Vector<> state = initial;
do {
const Scalar dt_this = std::min(dt_remaining, dt_max_);
if (!step(state, dt_this, final)) return false;
state = final;
dt_remaining -= dt_this;
} while (dt_remaining > 0.0);
return true;
}
Scalar IntegratorBase::dtMax() const { return dt_max_; }
} // namespace flightlib

15
flightlib/src/common/integrator_euler.cpp
View File

@@ -1,15 +0,0 @@
#include "flightlib/common/integrator_euler.hpp"
namespace flightlib {
bool IntegratorEuler::step(const Ref<const Vector<>> initial, const Scalar dt,
Ref<Vector<>> final) const {
Vector<> derivative(initial.rows());
if (!this->dynamics_(initial, derivative)) return false;
final = initial + dt * derivative;
return true;
}
} // namespace flightlib

34
flightlib/src/common/integrator_rk4.cpp
View File

@@ -1,34 +0,0 @@
#include "flightlib/common/integrator_rk4.hpp"
namespace flightlib {
bool IntegratorRK4::step(const Ref<const Vector<>> initial, const Scalar dt,
Ref<Vector<>> final) const {
static const Vector<4> rk4_sum_vec{1.0 / 6.0, 2.0 / 6.0, 2.0 / 6.0,
1.0 / 6.0};
Matrix<> k = Matrix<>::Zero(initial.rows(), 4);
final = initial;
// k_1
if (!this->dynamics_(final, k.col(0))) return false;
// k_2
final = initial + 0.5 * dt * k.col(0);
if (!this->dynamics_(final, k.col(1))) return false;
// k_3
final = initial + 0.5 * dt * k.col(1);
if (!this->dynamics_(final, k.col(2))) return false;
// k_4
final = initial + dt * k.col(2);
if (!this->dynamics_(final, k.col(3))) return false;
final = initial + dt * k * rk4_sum_vec;
return true;
}
} // namespace flightlib