ALPAKA/sta-peak
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/sta-peak.git synced 2025-06-10 18:16:00 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Added clip for integers and updated attitude from tilt

Browse Source
This commit is contained in:
dario 2024-09-16 15:51:08 +02:00
parent 315abe5d6f
commit 6350091baf
3 changed files with 19 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/sta/math/utils.hpp
View File

@ -9,6 +9,8 @@ namespace sta
{
float fast_inv_sqrt(float);
int clip(int value, int min, int max);
float clip(float value, float min, float max);
} // namespace math
} // namespace sta

4
src/algorithms/attitude/tilt.cpp
View File

@ -24,8 +24,8 @@ namespace sta
az /= norm;
}
float theta = std::atan(ay / az);
float phi = std::atan(-ax / std::sqrt(ay*ay + az*az));
float theta = std::atan2(ay, az);
float phi = std::atan2(-ax, std::sqrt(ay*ay + az*az));
Quaternion tilt(
std::cos(phi / 2.0f) * std::cos(theta / 2.0f),

15
src/utils.cpp
View File

@ -22,6 +22,21 @@ namespace sta
return y;
}
int clip(int value, int min, int max)
{
if (value < min)
{
return min;
}
if (value > max)
{
return max;
}
return value;
}
float clip(float value, float min, float max)
{
if (value < min)