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

Updated quaternion class, added attitude from tilt, started madgwick filter

Browse Source
This commit is contained in:
dario 2024-12-11 20:12:29 +01:00
parent 47ca310551
commit 3b11501f3b
2 changed files with 8 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/algorithms/attitude/madgwick.cpp
View File

@ -6,15 +6,13 @@
*/ */
#include <sta/math/algorithms/attitude/madgwick.hpp> #include <sta/math/algorithms/attitude/madgwick.hpp>
#include <sta/math/linalg/matrix.hpp>
namespace sta namespace sta
{ {
namespace math namespace math
{ {
MadgwickFilter::MadgwickFilter(Quaternion state, Quaternion direction, uint32_t n, float alpha) MadgwickFilter::MadgwickFilter(Quaternion state, uint32_t n, float alpha)
: state_{state}, : state_{state.normalized()},
direction_{direction},
n_{n}, n_{n},
alpha_{alpha} alpha_{alpha}
{ {

12
src/quaternion.cpp
View File

@ -71,14 +71,14 @@ namespace sta
return Quaternion(w / n, x / n, y / n, z / n); return Quaternion(w / n, x / n, y / n, z / n);
} }
Quaternion Quaternion::conjugate() Quaternion Quaternion::operator*(const Quaternion& quat)
{ {
return Quaternion(w, -x, -y, -z); float rw = w * quat.w - x * quat.x + y * quat.y + z * quat.z;
} float rx = w * quat.x + x * quat.w + y * quat.z - z * quat.y;
float ry = w * quat.y - x * quat.z + y * quat.w + z * quat.x;
float rz = w * quat.z - x * quat.y - y * quat.x + z * quat.w;
Quaternion operator-(const Quaternion& q1, const Quaternion& q2) return Quaternion(rw, rx, ry, rz);
{
return Quaternion(q1.w - q2.w, q1.x - q2.x, q1.y - q2.y, q1.z - q2.z);
} }
Quaternion operator*(const Quaternion& q1, const Quaternion& q2) Quaternion operator*(const Quaternion& q1, const Quaternion& q2)