forked from CPL/Simulator
254 lines
6.3 KiB
C++
254 lines
6.3 KiB
C++
#include "orientation.h"
|
||
#include <math.h>
|
||
|
||
static const float PI = 3.14159265359f;
|
||
static const float TO_DEG = 180.0f / PI;
|
||
static const float TO_RAD = PI / 180.0f;
|
||
static const float R = 8.314f;
|
||
static const float M = 0.029f;
|
||
static const float g = 9.80665f;
|
||
static const float ro = 1.189f;
|
||
|
||
struct Quaternion
|
||
{
|
||
float w, x, y, z;
|
||
};
|
||
|
||
static Quaternion qCurrent = { 1, 0, 0, 0 };
|
||
|
||
static const float period = 0.005f;
|
||
static bool isFirst = true;
|
||
|
||
static void vecNormalize(Vec3& v)
|
||
{
|
||
float n = sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
|
||
if (n > 1e-9f)
|
||
{
|
||
v.x /= n;
|
||
v.y /= n;
|
||
v.z /= n;
|
||
}
|
||
}
|
||
|
||
static Vec3 vecCross(const Vec3& a, const Vec3& b)
|
||
{
|
||
return
|
||
{
|
||
a.y * b.z - a.z * b.y,
|
||
a.z * b.x - a.x * b.z,
|
||
a.x * b.y - a.y * b.x
|
||
};
|
||
}
|
||
|
||
static void normalizeQuaternion(Quaternion& q)
|
||
{
|
||
float norm = sqrtf(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
|
||
if (norm > 1e-12f)
|
||
{
|
||
q.w /= norm;
|
||
q.x /= norm;
|
||
q.y /= norm;
|
||
q.z /= norm;
|
||
}
|
||
}
|
||
|
||
static Quaternion quaternionMultiply(const Quaternion& q1, const Quaternion& q2)
|
||
{
|
||
Quaternion r;
|
||
r.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
|
||
r.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
|
||
r.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
|
||
r.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
|
||
return r;
|
||
}
|
||
|
||
static Quaternion rotateVectorByQuaternion(const Quaternion& q, const Vec3& In)
|
||
{
|
||
Quaternion r = quaternionMultiply(quaternionMultiply(q, Quaternion{ 0, In.x, In.y, In.z }), Quaternion{ q.w, -q.x, -q.y, -q.z });
|
||
|
||
return r;
|
||
}
|
||
|
||
static Vec3 backRotateVectorByQuaternion(const Quaternion& q, const Vec3& In)
|
||
{
|
||
Quaternion r = quaternionMultiply(quaternionMultiply(Quaternion{ q.w, -q.x, -q.y, -q.z }, Quaternion{ 0, In.x, In.y, In.z }), q);
|
||
|
||
return { r.x, r.y, r.z };
|
||
}
|
||
|
||
static Quaternion backRotateVectorByQuaternion2(const Quaternion& q, const Quaternion& In)
|
||
{
|
||
Quaternion r = quaternionMultiply(quaternionMultiply(Quaternion{ q.w, -q.x, -q.y, -q.z }, In), q);
|
||
|
||
return r;
|
||
}
|
||
|
||
static Quaternion createYawQuaternion(float angle)
|
||
{
|
||
Quaternion q;
|
||
|
||
q.w = cosf(angle);
|
||
q.x = 0.0f;
|
||
q.y = 0.0f;
|
||
q.z = sinf(angle);
|
||
|
||
return q;
|
||
}
|
||
|
||
static Quaternion AccQuaternion(Quaternion& current, Vec3 a, float gravity)
|
||
{
|
||
float acc = sqrtf(a.x * a.x + a.y * a.y + a.z * a.z);
|
||
if (acc > (1.0f + gravity) || acc < (1.0f - gravity)) return current;
|
||
|
||
vecNormalize(a);
|
||
|
||
Vec3 g{ 0, 0, 1 };
|
||
Vec3 axis = vecCross(g, a);
|
||
float w = 1 + (g.x * a.x + g.y * a.y + g.z * a.z);
|
||
Quaternion q = { w, axis.x, axis.y, axis.z };
|
||
normalizeQuaternion(q);
|
||
|
||
Quaternion qYaw{ current.w, 0, 0, current.z };
|
||
|
||
return quaternionMultiply(q, qYaw); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> Z
|
||
}
|
||
|
||
static Quaternion GyroQuaternion(Quaternion& current, float wx, float wy, float wz)
|
||
{
|
||
Quaternion Mapp = current;
|
||
Quaternion Spd{ 0, wx, wy, wz };
|
||
Quaternion aq = quaternionMultiply(Spd, Mapp);
|
||
|
||
Mapp.w -= 0.5f * aq.w;
|
||
Mapp.x -= 0.5f * aq.x;
|
||
Mapp.y -= 0.5f * aq.y;
|
||
Mapp.z -= 0.5f * aq.z;
|
||
|
||
normalizeQuaternion(Mapp);
|
||
return Mapp;
|
||
}
|
||
|
||
static Quaternion nlerp(const Quaternion& q1, const Quaternion& q2, float alpha)
|
||
{
|
||
|
||
float dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
|
||
|
||
Quaternion q2_ = q2;
|
||
if (dot < 0)
|
||
{
|
||
q2_.w = -q2.w;
|
||
q2_.x = -q2.x;
|
||
q2_.y = -q2.y;
|
||
q2_.z = -q2.z;
|
||
}
|
||
|
||
Quaternion r;
|
||
r.w = (1.0f - alpha) * q1.w + alpha * q2_.w;
|
||
r.x = (1.0f - alpha) * q1.x + alpha * q2_.x;
|
||
r.y = (1.0f - alpha) * q1.y + alpha * q2_.y;
|
||
r.z = (1.0f - alpha) * q1.z + alpha * q2_.z;
|
||
|
||
normalizeQuaternion(r);
|
||
|
||
return r;
|
||
}
|
||
|
||
inline float GetAngle(float a1, float a2, float az)
|
||
{
|
||
if (a2 == 0.0f && az == 0.0f) return a1 > 0.0f ? 90.0f : -90.0f;
|
||
return atanf(a1 / sqrtf(a2 * a2 + az * az)) * TO_DEG;
|
||
}
|
||
|
||
static Vec3 quaternionToPitchRollYaw(const Quaternion& q, float& Upside)
|
||
{
|
||
Quaternion pry = rotateVectorByQuaternion(q, { 0, 0, 1 });
|
||
|
||
Upside = (pry.z > 0.0f) ? 1.0f : -1.0f;
|
||
|
||
float yaw = 2 * atan2f(q.z, q.w) * TO_DEG;
|
||
if (yaw < 0.0f) yaw = 360.0f + yaw;
|
||
|
||
return // Sovereign orientation
|
||
{
|
||
GetAngle(pry.y, pry.x, pry.z), // Pitch
|
||
GetAngle(-pry.x, pry.y, pry.z), // Roll
|
||
yaw // Yaw
|
||
};
|
||
}
|
||
|
||
static void addMagneto(Quaternion& q, Vec3 mag, float alpha, const float shift)
|
||
{
|
||
static Quaternion yq = createYawQuaternion(shift * TO_RAD);
|
||
|
||
vecNormalize(mag);
|
||
|
||
Quaternion mQ = { 0, mag.x, mag.y, mag.z };
|
||
|
||
Quaternion mW = backRotateVectorByQuaternion2(q, mQ);
|
||
|
||
mW = quaternionMultiply(mW, yq); // Shifting the axes to the true north
|
||
|
||
float gamma = mW.x * mW.x + mW.y * mW.y;
|
||
float beta = sqrtf(gamma + mW.x * sqrtf(gamma));
|
||
|
||
Quaternion mD
|
||
{
|
||
beta / (sqrtf(2.0f * gamma)),
|
||
0.0f,
|
||
0.0f,
|
||
mW.y / (sqrtf(2.0f) * beta),
|
||
};
|
||
|
||
mD.w = (1.0f - alpha) + alpha * mD.w;
|
||
mD.z = alpha * mD.z;
|
||
|
||
if (mD.w != mD.w || mD.x != mD.x || mD.y != mD.y || mD.z != mD.z) return;
|
||
|
||
q = quaternionMultiply(q, mD);
|
||
normalizeQuaternion(q);
|
||
}
|
||
|
||
// WorkAccGyro({DataIMU.Acc.X, DataIMU.Acc.Y, DataIMU.Acc.Z}, {DataIMU.Gyr.X, DataIMU.Gyr.Y, DataIMU.Gyr.Z}, {-DataIMU.Mag.X, DataIMU.Mag.Y, DataIMU.Mag.Z}, 0.01);
|
||
ORI WorkAccGyroMag(const Vec3 acc, const Vec3 gyr, const Vec3 mag, const float mag_shift, const float alpha)
|
||
{
|
||
float wx = gyr.x * 500 / 32768 * 1.21f * (PI / 180) * period;
|
||
float wy = gyr.y * 500 / 32768 * 1.21f * (PI / 180) * period;
|
||
float wz = gyr.z * 500 / 32768 * 1.21f * (PI / 180) * period;
|
||
|
||
Vec3 aB = acc;
|
||
|
||
Quaternion qAcc = AccQuaternion(qCurrent, aB, 0.05f); // Tolerance for gravity deviation 5%
|
||
|
||
qCurrent = GyroQuaternion(qCurrent, wx, wy, wz);
|
||
|
||
Quaternion qFused = nlerp(qCurrent, qAcc, alpha);
|
||
|
||
if (isFirst)
|
||
{
|
||
qFused = qAcc;
|
||
isFirst = false;
|
||
}
|
||
|
||
qCurrent = qFused;
|
||
|
||
addMagneto(qCurrent, mag, alpha, mag_shift);
|
||
|
||
float up;
|
||
Vec3 pry = quaternionToPitchRollYaw(qCurrent, up);
|
||
|
||
Vec3 ine = backRotateVectorByQuaternion(qCurrent, aB);
|
||
return
|
||
{
|
||
sqrtf(pry.x * pry.x + pry.y * pry.y) * up,
|
||
pry.x, pry.y, pry.z,
|
||
ine.x, ine.y, ine.z
|
||
|
||
};
|
||
}
|
||
|
||
float calculateHeight(float bar, float temp)
|
||
{
|
||
static double firstBar = bar;
|
||
return (R * (temp + 273) / (M * g)) * log(firstBar / bar);
|
||
|
||
} |