p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

IMU: Fix some weird cases when the imu gives nan values

This commit is contained in:
Daniel Santos-Olivan 2021-04-23 12:53:25 +02:00 committed by DSantosO
parent 12e4148def
commit 051a33c505
1 changed files with 7 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/InertialMeasurementUnit.cpp
View File

@ -167,13 +167,16 @@ float AInertialMeasurementUnit::ComputeCompass()
{
// Magnetometer: orientation with respect to the North in rad
const FVector ForwVect = GetActorForwardVector().GetSafeNormal2D();
float Compass = std::acos(FVector::DotProduct(CarlaNorthVector, ForwVect));
const float DotProd = FVector::DotProduct(CarlaNorthVector, ForwVect)
// We check if the dot product is higher than 1.0 due to numerical error
if (DotProd >= 1.00f)
return 0.0f;
const float Compass = std::acos(DotProd);
// Keep the angle between [0, 2pi)
if (FVector::CrossProduct(CarlaNorthVector, ForwVect).Z < 0.0f)
{
Compass = carla::geom::Math::Pi2<float>() - Compass;
}
return carla::geom::Math::Pi2<float>() - Compass;
return Compass;
}