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

ADD: added movement tests and updated the calcuation of the updated position in earth centered coordinates

Browse Source
This commit is contained in:
Henry Winkel
2023-11-02 11:07:00 +01:00
parent def866a82b
commit 75efaad6ef
6 changed files with 520 additions and 134 deletions
Download Patch File Download Diff File Expand all files Collapse all files

98
tests/test_MovementClass.cpp
View File

@@ -1,61 +1,32 @@
#include "Entities/Movement.hpp"
#include "SimCore/Position.hpp"
#include "SimCore/SimCore.hpp"
#include <math.h>
#include <cmath>
#include <memory>
#include <string>
#include <thread>
#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>
#include <loguru.hpp>
#include <SimCore/UtilFunctions.hpp>
#include <math.h>
struct KinVec{
double x,y,z;
}
struct MovementVec{
double dSpd,dClb,dCrs;
}
KinVec Generate_Kin_Vect(MovementVec Mov_Dat, LLA_Rad LLA_rCoord)
{
KinVec Result;
//Transform Mov_Dat to topocentric vector
UVW UVW_Coord;
UVW_Coord.dU = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * sin(Mov_Dat.dCrs);
UVW_Coord.dV = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * cos(Mov_Dat.dCrs);
UVW_Coord.dW = Mov_Dat.dSpd * sin(Mov_Dat.dClb);
//Transform topocentric vector to geocentric vector
Result.x = -UVW_Coord.dU * sin(LLA_rCoord.dLon) -
UVW_Coord.dV * sin(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon) +
UVW_Coord.dW * cos(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon);
Result.y = UVW_Coord.dU * cos(LLA_rCoord.dLon) -
UVW_Coord.dV * sin(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon) +
UVW_Coord.dW * cos(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon);
Result.z = UVW_Coord.dV * cos(LLA_rCoord.dLat) +
UVW_Coord.dW * sin(LLA_rCoord.dLat);
return Result;
};
SCENARIO("Testing the SimCore Sensor")
{
GIVEN("different Attributes for a Movement ")
{
SimCore::Position pos1;
std::shared_ptr<SimCore::Position> pos1 = std::make_shared<SimCore::Position>();
WHEN("constructing Track Object with data")
{
pos1.setGeodesicPos(55, 6, 0);
LOG_S(INFO)<<"LAT: "<<pos1.getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< pos1.getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< pos1.getGeodesicPos()(SimCore::HEIGHT);
pos1->setGeodesicPos(55, 6, 0);
LOG_S(INFO)<<"LAT: "<<pos1->getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< pos1->getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< pos1->getGeodesicPos()(SimCore::HEIGHT);
Entities::Movement mov(pos1);
mov.setCourse(90);
@@ -64,7 +35,7 @@ SCENARIO("Testing the SimCore Sensor")
mov.updatePosition(10000);// 10 seconds
LOG_S(INFO)<<"LAT: "<<mov.getPosition().getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov.getPosition().getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov.getPosition().getGeodesicPos()(SimCore::HEIGHT);
LOG_S(INFO)<<"LAT: "<<mov.getPosition()->getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov.getPosition()->getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov.getPosition()->getGeodesicPos()(SimCore::HEIGHT);
GeographicLib::Geodesic geod(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
double h2, lat2,lon2;
@@ -74,36 +45,41 @@ SCENARIO("Testing the SimCore Sensor")
GeographicLib::GeodesicLine line = geod.InverseLine(55,6 , lat2, lon2);
LOG_S(INFO)<<"distance: "<<line.Distance()<< " lat2: "<< lat2 << " lon2: " << lon2;
Entities::Movement mov2(pos1);
mov2.setCourse(0);//degree
mov2.setSpeed(100);//m/s
mov2.setPitch(45);//degree
LOG_S(INFO)<<"LAT: "<<mov2.getPosition().getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov2.getPosition().getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov2.getPosition().getGeodesicPos()(SimCore::HEIGHT);
double pitch = 0;
double course = 360;
double speed = 1;
double Vd = 100 * sin(45* M_PI / 180.0);
double h = Vd*10;
LOG_S(INFO)<<"höhe: "<< h;
double i;
for ( i = 0; i <= 10; ) {
mov2.updatePosition(500);
i += 0.5;
}
Entities::Movement mov2(pos1,course,speed,pitch);
MovementVec movc;
movc.dSpd = 10;
movc.dClb = 0;
movc.dCrs = 0;
// LOG_S(INFO)<< mov.getEulerAngles();
LOG_S(INFO)<<"LAT: "<<mov2.getPosition()->getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov2.getPosition()->getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov2.getPosition()->getGeodesicPos()(SimCore::HEIGHT);
std::shared_ptr<SimCore::Position> pos3 = std::make_shared<SimCore::Position>();
pos3->setGeodesicPos(55, 6, 0);
Entities::Movement mov3(pos3,0,1,90);
mov3.updatePosition(1000);
LOG_S(INFO)<<"LAT: "<<mov3.getPosition()->getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov3.getPosition()->getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov3.getPosition()->getGeodesicPos()(SimCore::HEIGHT);
THEN("check if Track attributes are correct")
{
// LOG_S(INFO)<<mov.getPosition().getGeodesicPos()(SimCore::LATITUDE);
REQUIRE((pos2.getGeocentricPos() - mov.getPosition().getGeocentricPos()).norm() <= 0.1);
REQUIRE((h - mov2.getPosition().getGeodesicPos()(SimCore::HEIGHT)) <= 0.1);
// REQUIRE((pos2.getGeocentricPos() - mov.getPosition()->getGeocentricPos()).norm() <= 0.1);
REQUIRE((pos2.getGeodesicPos() - pos1->getGeodesicPos()).norm() <= 0.1);
REQUIRE(round(mov3.getPosition()->getGeodesicPos()(SimCore::HEIGHT)) == 1);
REQUIRE(mov2.getCourse() == SimCore::UtilFunctions::DegToRad(course));
REQUIRE(mov2.getPitch() == SimCore::UtilFunctions::DegToRad(pitch));
REQUIRE(mov2.getSpeed() == speed);