henry/EntityLibrary
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ADD: added use of round to position calculation

Browse Source
This commit is contained in:
Henry Winkel
2024-02-15 14:23:18 +01:00
parent 2a279f59ec
commit f839ea4815
3 changed files with 19 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
include/Entities/Movement.hpp
View File

@@ -145,6 +145,7 @@ namespace Entities
// Eigen::Matrix3d getRotationMatrix(double lat, double lon); // Eigen::Matrix3d getRotationMatrix(double lat, double lon);
/** /**
* @brief transforms from local ENU (East North Up) Vector to ECEF (Earth Centred Earth Fixed) * @brief transforms from local ENU (East North Up) Vector to ECEF (Earth Centred Earth Fixed)
* @param Eigen::Vector3d ENU * @param Eigen::Vector3d ENU
@@ -197,7 +198,7 @@ namespace Entities
double speed_; double speed_;
double climbAngle_; double climbAngle_;
const double precision = 0.01;
mutable std::mutex mx; mutable std::mutex mx;
std::condition_variable c; std::condition_variable c;

2
libs/OrderLibrary

Submodule libs/OrderLibrary updated: 9d892fdcb6...13f3c07dbc

18
src/Entities/Movement.cpp
View File

@@ -1,4 +1,5 @@
#include "GeographicLib/Constants.hpp" #include "GeographicLib/Constants.hpp"
#include "GeographicLib/LocalCartesian.hpp"
#include "SimCore/Position.hpp" #include "SimCore/Position.hpp"
#include "SimCore/SimCore.hpp" #include "SimCore/SimCore.hpp"
#include "SimCore/UtilFunctions.hpp" #include "SimCore/UtilFunctions.hpp"
@@ -6,6 +7,8 @@
#include <Eigen/Geometry> #include <Eigen/Geometry>
#include <memory> #include <memory>
#include <tuple> #include <tuple>
#include <GeographicLib/Geocentric.hpp>
using namespace Eigen; using namespace Eigen;
@@ -96,6 +99,8 @@ namespace Entities
void Movement::updatePosition(double dt) void Movement::updatePosition(double dt)
{ {
if (ownPosition_->isValid()) { if (ownPosition_->isValid()) {
auto ecefVelocityVector = this->ToECEFFromENU(); auto ecefVelocityVector = this->ToECEFFromENU();
@@ -110,6 +115,9 @@ namespace Entities
// kinematicVec_.x() += accelerationVec_.x() * dt / 1000.0; // kinematicVec_.x() += accelerationVec_.x() * dt / 1000.0;
// kinematicVec_.y() += accelerationVec_.y() * dt / 1000.0; // kinematicVec_.y() += accelerationVec_.y() * dt / 1000.0;
// kinematicVec_.z() += accelerationVec_.z() * dt / 1000.0; // kinematicVec_.z() += accelerationVec_.z() * dt / 1000.0;
X = SimCore::UtilFunctions::round(X,precision);
Y = SimCore::UtilFunctions::round(Y,precision);
Z = SimCore::UtilFunctions::round(Z,precision);
ownPosition_->setGeocentricPos(X, Y, Z); ownPosition_->setGeocentricPos(X, Y, Z);
// c.notify_one(); // c.notify_one();
@@ -140,7 +148,7 @@ namespace Entities
geod.Direct(lat,lon,this->course_,movedDistance,lat2,lon2); geod.Direct(lat,lon,this->course_,movedDistance,lat2,lon2);
ownPosition_->setGeodesicPos(lat2,lon2,climedHeight); ownPosition_->setGeodesicPos(lat2,lon2,alt + climedHeight);
} }
@@ -162,8 +170,13 @@ namespace Entities
cos(lon), - sin(lon)* sin(lat), sin(lon) *cos(lat), cos(lon), - sin(lon)* sin(lat), sin(lon) *cos(lat),
0 , cos(lat) , sin(lat); 0 , cos(lat) , sin(lat);
auto vec = LocalRotationMatrix * ENU;
return LocalRotationMatrix * ENU; return Eigen::Vector3d(
SimCore::UtilFunctions::round(vec[0],precision ),
SimCore::UtilFunctions::round(vec[1],precision ),
SimCore::UtilFunctions::round(vec[2],precision )
);
} }
Eigen::Vector3d Movement::ENUFromCourseSpeedClbToLocalSpeedVector() Eigen::Vector3d Movement::ENUFromCourseSpeedClbToLocalSpeedVector()
@@ -172,7 +185,6 @@ namespace Entities
ENU[0] = this->speed_ * cos(this->climbAngle_) * sin(this->course_); ENU[0] = this->speed_ * cos(this->climbAngle_) * sin(this->course_);
ENU[1] = this->speed_ * cos(this->climbAngle_) * cos(this->course_); ENU[1] = this->speed_ * cos(this->climbAngle_) * cos(this->course_);
ENU[2] = this->speed_ * sin(this->climbAngle_); ENU[2] = this->speed_ * sin(this->climbAngle_);
this->ENUVec_ = ENU; this->ENUVec_ = ENU;
return ENU; return ENU;
} }