josh/hj_datalink
1
0
Fork 0
forked from 0xha0/df_datalink
Code Issues Pull Requests Releases Activity

unified indentation

Browse Source
This commit is contained in:
bit-cyber-fh
2024-03-28 15:25:58 +08:00
parent 8779c9855e
commit 16d1ad30a7
14 changed files with 1019 additions and 1030 deletions
Download Patch File Download Diff File Expand all files Collapse all files
df_datalink_sp/src/networking/link_layer/geometry/coord.c
+94 -94
View File
@@ -37,132 +37,132 @@ const COORD COORD_Z_AXIS = {0.0, 0.0, 1.0};
/** @brief Subtracts two coordinate vectors. */
COORD Coord_Sub(COORD a, COORD b)
{
COORD ret;
ret.x = a.x - b.x;
ret.y = a.y - b.y;
ret.z = a.z - b.z;
return ret;
COORD ret;
ret.x = a.x - b.x;
ret.y = a.y - b.y;
ret.z = a.z - b.z;
return ret;
}
/** @brief Adds two coordinate vectors. */
COORD Coord_Add(COORD a, COORD b)
{
COORD ret;
ret.x = a.x + b.x;
ret.y = a.y + b.y;
ret.z = a.z + b.z;
return ret;
COORD ret;
ret.x = a.x + b.x;
ret.y = a.y + b.y;
ret.z = a.z + b.z;
return ret;
}
/** @brief Multiplies a coordinate vector by a real number. */
void Coord_ScalarMult(COORD *a, double t)
{
a->x *= t;
a->y *= t;
a->z *= t;
a->x *= t;
a->y *= t;
a->z *= t;
}
/** @brief Multiplies a coordinate vector by a real number. */
COORD Coord_GetScalarMult(COORD a, double t)
{
COORD ret;
ret.x = a.x * t;
ret.y = a.y * t;
ret.z = a.z * t;
return ret;
COORD ret;
ret.x = a.x * t;
ret.y = a.y * t;
ret.z = a.z * t;
return ret;
}
/** @brief Divides a coordinate vector by a real number. */
void Coord_ScalarDiv(COORD *a, double t)
{
a->x /= t;
a->y /= t;
a->z /= t;
a->x /= t;
a->y /= t;
a->z /= t;
}
/** @brief Divides a coordinate vector by a real number. */
COORD Coord_GetScalarDiv(COORD a, double t)
{
COORD ret;
ret.x = a.x / t;
ret.y = a.y / t;
ret.z = a.z / t;
return ret;
COORD ret;
ret.x = a.x / t;
ret.y = a.y / t;
ret.z = a.z / t;
return ret;
}
/** @brief Dot product. */
inline double Coord_Dot(COORD a, COORD b)
{
return (a.x * b.x + a.y * b.y + a.z * b.z);
return (a.x * b.x + a.y * b.y + a.z * b.z);
}
/** @brief Cross product. */
COORD Coord_Cross(COORD a, COORD b)
{
COORD ret;
ret.x = a.y * b.z - a.z * b.y;
ret.y = a.z * b.x - a.x * b.z;
ret.z = a.x * b.y - a.y * b.x;
return ret;
COORD ret;
ret.x = a.y * b.z - a.z * b.y;
ret.y = a.z * b.x - a.x * b.z;
ret.z = a.x * b.y - a.y * b.x;
return ret;
}
/** @brief Multiplies two coordinate vector in elementwise. */
COORD Coord_ElementwiseMult(COORD a, COORD b)
{
COORD ret;
ret.x = a.x * b.x;
ret.y = a.y * b.y;
ret.z = a.z * b.z;
return ret;
COORD ret;
ret.x = a.x * b.x;
ret.y = a.y * b.y;
ret.z = a.z * b.z;
return ret;
}
/** @brief Divides two coordinate vector in elementwise. */
COORD Coord_ElementwiseDiv(COORD a, COORD b)
{
COORD ret;
ret.x = a.x / b.x;
ret.y = a.y / b.y;
ret.z = a.z / b.z;
return ret;
COORD ret;
ret.x = a.x / b.x;
ret.y = a.y / b.y;
ret.z = a.z / b.z;
return ret;
}
/** @brief Calculate a coordinate vector length^2 (omits calling square root). */
double Coord_SquareLength(COORD a)
{
return (a.x * a.x + a.y * a.y + a.z * a.z);
return (a.x * a.x + a.y * a.y + a.z * a.z);
}
/** @brief Calculate a coordinate vector length. */
double Coord_Length(COORD a)
{
return sqrt(Coord_SquareLength(a));
return sqrt(Coord_SquareLength(a));
}
/** @brief Calculate two coordinates' distance. */
double Coord_Distance(COORD a, COORD b)
{
return Coord_Length(Coord_Sub(a, b));
return Coord_Length(Coord_Sub(a, b));
}
/** @brief Normalize coordinate vector. */
void Coord_Normalize(COORD *a)
{
Coord_ScalarDiv(a, Coord_Length(*a));
Coord_ScalarDiv(a, Coord_Length(*a));
}
/** @brief Normalize coordinate vector. */
COORD Coord_GetNormalized(COORD a)
{
Coord_Normalize(&a);
return a;
Coord_Normalize(&a);
return a;
}
/** @brief Returns the angle between the two vectors. */
double Coord_Angle(COORD a, COORD b)
{
Coord_Normalize(&a);
Coord_Normalize(&b);
return acos(Coord_Dot(a, b));
Coord_Normalize(&a);
Coord_Normalize(&b);
return acos(Coord_Dot(a, b));
}
// Additional Coordinates convert
@@ -170,64 +170,64 @@ double Coord_Angle(COORD a, COORD b)
/** @brief Convert coordinate from Launch coordinate system to ECEF. */
COORD Coord_Launch2Ecef(COORD o, COORD p, double A, double B, double lambda)
{
COORD ret;
double sinA, cosA, sinB, cosB, sinL, cosL;
double S11, S12, S13, S21, S22, S23, S31, S32, S33;
sinA = sin(A);
cosA = cos(A);
sinB = sin(B);
cosB = cos(B);
sinL = sin(lambda);
cosL = cos(lambda);
COORD ret;
double sinA, cosA, sinB, cosB, sinL, cosL;
double S11, S12, S13, S21, S22, S23, S31, S32, S33;
sinA = sin(A);
cosA = cos(A);
sinB = sin(B);
cosB = cos(B);
sinL = sin(lambda);
cosL = cos(lambda);
S11 = -sinA * sinL - cosA * sinB * cosL;
S12 = cosB * cosL;
S13 = -cosA * sinL + sinA * sinB * cosL;
S11 = -sinA * sinL - cosA * sinB * cosL;
S12 = cosB * cosL;
S13 = -cosA * sinL + sinA * sinB * cosL;
S21 = sinA * cosL - cosA * sinB * sinL;
S22 = cosB * sinL;
S23 = cosA * cosL + sinA * sinB * sinL;
S21 = sinA * cosL - cosA * sinB * sinL;
S22 = cosB * sinL;
S23 = cosA * cosL + sinA * sinB * sinL;
S31 = cosA * cosB;
S32 = sinB;
S33 = -sinA * cosB;
S31 = cosA * cosB;
S32 = sinB;
S33 = -sinA * cosB;
ret.x = p.x * (S11 + S21 + S31) + o.x;
ret.y = p.y * (S12 + S22 + S32) + o.y;
ret.z = p.z * (S13 + S23 + S33) + o.z;
ret.x = p.x * (S11 + S21 + S31) + o.x;
ret.y = p.y * (S12 + S22 + S32) + o.y;
ret.z = p.z * (S13 + S23 + S33) + o.z;
return ret;
return ret;
}
/** @brief Convert coordinate from launch coordinate system to Inertia coordinate system. */
COORD Coord_Launch2Inertia(COORD o, COORD p, double A, double B, double we, double t)
{
COORD ret;
double OmgX, OmgY, OmgZ, sinwet, coswet;
double M11, M12, M13, M21, M22, M23, M31, M32, M33;
OmgX = cos(A) * cos(B);
OmgY = sin(B);
OmgZ = -sin(A) * cos(B);
sinwet = sin(we * t);
coswet = cos(we * t);
COORD ret;
double OmgX, OmgY, OmgZ, sinwet, coswet;
double M11, M12, M13, M21, M22, M23, M31, M32, M33;
OmgX = cos(A) * cos(B);
OmgY = sin(B);
OmgZ = -sin(A) * cos(B);
sinwet = sin(we * t);
coswet = cos(we * t);
M11 = 1 - (1 - OmgX * OmgX) * (1 - coswet);
M12 = OmgX * OmgY * (1 - coswet) + OmgZ * sinwet;
M13 = OmgX * OmgZ * (1 - coswet) - OmgY * sinwet;
M11 = 1 - (1 - OmgX * OmgX) * (1 - coswet);
M12 = OmgX * OmgY * (1 - coswet) + OmgZ * sinwet;
M13 = OmgX * OmgZ * (1 - coswet) - OmgY * sinwet;
M21 = OmgY * OmgX * (1 - coswet) - OmgZ * sinwet;
M22 = 1 - (1 - OmgY * OmgY) * (1 - coswet);
M23 = OmgY * OmgZ * (1 - coswet) + OmgX * sinwet;
M21 = OmgY * OmgX * (1 - coswet) - OmgZ * sinwet;
M22 = 1 - (1 - OmgY * OmgY) * (1 - coswet);
M23 = OmgY * OmgZ * (1 - coswet) + OmgX * sinwet;
M31 = OmgZ * OmgX * (1 - coswet) + OmgY * sinwet;
M32 = OmgZ * OmgY * (1 - coswet) - OmgX * sinwet;
M33 = 1 - (1 - OmgZ * OmgZ) * (1 - coswet);
M31 = OmgZ * OmgX * (1 - coswet) + OmgY * sinwet;
M32 = OmgZ * OmgY * (1 - coswet) - OmgX * sinwet;
M33 = 1 - (1 - OmgZ * OmgZ) * (1 - coswet);
ret.x = p.x * (M11 + M21 + M31) + o.x;
ret.y = p.y * (M12 + M22 + M32) + o.y;
ret.z = p.z * (M13 + M23 + M33) + o.z;
ret.x = p.x * (M11 + M21 + M31) + o.x;
ret.y = p.y * (M12 + M22 + M32) + o.y;
ret.z = p.z * (M13 + M23 + M33) + o.z;
return ret;
return ret;
}
/** @} LinkLayerGeometry_Coord */
df_datalink_sp/src/networking/link_layer/geometry/coord.h
+3 -3
View File
@@ -33,9 +33,9 @@
*/
typedef struct
{
double x;
double y;
double z;
double x;
double y;
double z;
} COORD;
extern const COORD COORD_NIL;
df_datalink_sp/src/networking/link_layer/geometry/eulerangles.c
+20 -20
View File
@@ -34,51 +34,51 @@ const EULERANGLES EULERANGLES_ZERO = {0.0, 0.0, 0.0};
/** @brief Add two EulerAngles. */
EULERANGLES EulerAngles_Add(EULERANGLES e1, EULERANGLES e2)
{
EULERANGLES ret;
ret.alpha = e1.alpha + e2.alpha;
ret.beta = e1.beta + e2.beta;
ret.gamma = e1.gamma + e2.gamma;
return ret;
EULERANGLES ret;
ret.alpha = e1.alpha + e2.alpha;
ret.beta = e1.beta + e2.beta;
ret.gamma = e1.gamma + e2.gamma;
return ret;
}
/** @brief Subtracts two EulerAngles. */
EULERANGLES EulerAngles_Sub(EULERANGLES e1, EULERANGLES e2)
{
EULERANGLES ret;
ret.alpha = e1.alpha - e2.alpha;
ret.beta = e1.beta - e2.beta;
ret.gamma = e1.gamma - e2.gamma;
return ret;
EULERANGLES ret;
ret.alpha = e1.alpha - e2.alpha;
ret.beta = e1.beta - e2.beta;
ret.gamma = e1.gamma - e2.gamma;
return ret;
}
/** @brief Multiplies EulerAngles by a real number. */
void EulerAngles_ScalarMult(EULERANGLES *e, double f)
{
e->alpha *= f;
e->beta *= f;
e->gamma *= f;
e->alpha *= f;
e->beta *= f;
e->gamma *= f;
}
/** @brief Multiplies EulerAngles by a real number. */
EULERANGLES EulerAngles_GetScalarMult(EULERANGLES e, double f)
{
EulerAngles_ScalarMult(&e, f);
return e;
EulerAngles_ScalarMult(&e, f);
return e;
}
/** @brief Normalize EulerAngles. */
void EulerAngles_Normalize(EULERANGLES *e)
{
e->alpha = fmod(e->alpha, 2 * EUL_PI);
e->beta = fmod(e->beta, 2 * EUL_PI);
e->gamma = fmod(e->gamma, 2 * EUL_PI);
e->alpha = fmod(e->alpha, 2 * EUL_PI);
e->beta = fmod(e->beta, 2 * EUL_PI);
e->gamma = fmod(e->gamma, 2 * EUL_PI);
}
/** @brief Normalize EulerAngles. */
EULERANGLES EulerAngles_GetNormalize(EULERANGLES e)
{
EulerAngles_Normalize(&e);
return e;
EulerAngles_Normalize(&e);
return e;
}
/** @} LinkLayerGeometry_EulerAngles */
df_datalink_sp/src/networking/link_layer/geometry/quaternion.c
+277 -277
View File
@@ -34,14 +34,14 @@
typedef union
{
float value;
uint32_t raw;
float value;
uint32_t raw;
} SOFT_FLOAT;
typedef union
{
double value;
uint64_t raw;
double value;
uint64_t raw;
} SOFT_DOUBLE;
const SOFT_FLOAT EPS_FLOAT = {.raw = 0x34000000};
@@ -53,424 +53,424 @@ const QUATERNION QUATERNION_NIL = {NAN, NAN, NAN, NAN};
/** @brief Subtracts two quaternions. */
QUATERNION Quaternion_Sub(QUATERNION q1, QUATERNION q2)
{
QUATERNION ret;
ret.s = q1.s - q2.s;
ret.i = q1.i - q2.i;
ret.j = q1.j - q2.j;
ret.k = q1.k - q2.k;
return ret;
QUATERNION ret;
ret.s = q1.s - q2.s;
ret.i = q1.i - q2.i;
ret.j = q1.j - q2.j;
ret.k = q1.k - q2.k;
return ret;
}
/** @brief Adds two quaternions. */
QUATERNION Quaternion_Add(QUATERNION q1, QUATERNION q2)
{
QUATERNION ret;
ret.s = q1.s + q2.s;
ret.i = q1.i + q2.i;
ret.j = q1.j + q2.j;
ret.k = q1.k + q2.k;
return ret;
QUATERNION ret;
ret.s = q1.s + q2.s;
ret.i = q1.i + q2.i;
ret.j = q1.j + q2.j;
ret.k = q1.k + q2.k;
return ret;
}
/** @brief Multiplies a quaternion by a real number. */
void Quaternion_ScalarMult(QUATERNION *q, double t)
{
q->s *= t;
q->i *= t;
q->j *= t;
q->k *= t;
q->s *= t;
q->i *= t;
q->j *= t;
q->k *= t;
}
/** @brief Multiplies a quaternion by a real number. */
QUATERNION Quaternion_GetScalarMult(QUATERNION a, double t)
{
Quaternion_ScalarMult(&a, t);
return a;
Quaternion_ScalarMult(&a, t);
return a;
}
/** @brief Divides a quaternion by a real number. */
void Quaternion_ScalarDiv(QUATERNION *q, double t)
{
q->s /= t;
q->i /= t;
q->j /= t;
q->k /= t;
q->s /= t;
q->i /= t;
q->j /= t;
q->k /= t;
}
/** @brief Divides a quaternion by a real number. */
QUATERNION Quaternion_GetScalarDiv(QUATERNION q, double t)
{
Quaternion_ScalarDiv(&q, t);
return q;
Quaternion_ScalarDiv(&q, t);
return q;
}
/** @brief Multiplies two quaternions. */
QUATERNION Quaternion_Mult(QUATERNION q1, QUATERNION q2)
{
QUATERNION ret;
ret.s = q1.s * q2.s - (q1.i * q2.i + q1.j * q2.j + q1.k * q2.k);
ret.i = q1.j * q2.k - q1.k * q2.j + q1.s * q2.i + q1.i * q2.s;
ret.j = q1.k * q2.i - q1.i * q2.k + q1.s * q2.j + q1.j * q2.s;
ret.k = q1.i * q2.j - q1.j * q2.i + q1.s * q2.k + q1.k * q2.s;
return ret;
QUATERNION ret;
ret.s = q1.s * q2.s - (q1.i * q2.i + q1.j * q2.j + q1.k * q2.k);
ret.i = q1.j * q2.k - q1.k * q2.j + q1.s * q2.i + q1.i * q2.s;
ret.j = q1.k * q2.i - q1.i * q2.k + q1.s * q2.j + q1.j * q2.s;
ret.k = q1.i * q2.j - q1.j * q2.i + q1.s * q2.k + q1.k * q2.s;
return ret;
}
/** @brief Divides two quaternions. */
QUATERNION Quaternion_Div(QUATERNION q1, QUATERNION q2)
{
Quaternion_Inverse(&q2);
return Quaternion_Mult(q1, q2);
Quaternion_Inverse(&q2);
return Quaternion_Mult(q1, q2);
}
/** @brief Dot product. */
inline double Quaternion_Dot(QUATERNION q1, QUATERNION q2)
{
return (q1.s * q2.s + q1.i * q2.i + q1.j * q2.j + q1.k * q2.k);
return (q1.s * q2.s + q1.i * q2.i + q1.j * q2.j + q1.k * q2.k);
}
/** @brief Calculate a quaternion length^2 (omits calling square root). */
inline double Quaternion_SquareLength(QUATERNION q)
{
return (q.s * q.s + q.i * q.i + q.j * q.j + q.k * q.k);
return (q.s * q.s + q.i * q.i + q.j * q.j + q.k * q.k);
}
/** @brief Calculate a quaternion length. */
inline double Quaternion_Length(QUATERNION q)
{
return sqrt(Quaternion_SquareLength(q));
return sqrt(Quaternion_SquareLength(q));
}
/** @brief Normalize quaternion. */
void Quaternion_Normalize(QUATERNION *q)
{
Quaternion_ScalarDiv(q, Quaternion_Length(*q));
Quaternion_ScalarDiv(q, Quaternion_Length(*q));
}
/** @brief Normalize quaternion. */
QUATERNION Quaternion_GetNormalized(QUATERNION q)
{
Quaternion_Normalize(&q);
return q;
Quaternion_Normalize(&q);
return q;
}
/** @brief Conjugated quaternion. */
void Quaternion_Conjugated(QUATERNION *q)
{
q->i = -q->i;
q->j = -q->j;
q->k = -q->k;
q->i = -q->i;
q->j = -q->j;
q->k = -q->k;
}
/** @brief Conjugated quaternion. */
QUATERNION Quaternion_GetConjugated(QUATERNION q)
{
Quaternion_Conjugated(&q);
return q;
Quaternion_Conjugated(&q);
return q;
}
/** @brief Inverse quaternion. */
void Quaternion_Inverse(QUATERNION *q)
{
Quaternion_Conjugated(q);
Quaternion_ScalarDiv(q, Quaternion_SquareLength(*q));
Quaternion_Conjugated(q);
Quaternion_ScalarDiv(q, Quaternion_SquareLength(*q));
}
/** @brief Inverse quaternion. */
QUATERNION Quaternion_GeInverse(QUATERNION q)
{
Quaternion_Inverse(&q);
return q;
Quaternion_Inverse(&q);
return q;
}
inline double Quaternion_GetRotationAngle(QUATERNION q)
{
return (acos(q.s) * 2);
return (acos(q.s) * 2);
}
/** @brief Get axis of quaternion. */
COORD Quaternion_GetRotationAxis(QUATERNION q)
{
double sin_theta_inv;
COORD ret;
sin_theta_inv = 1.0 / sin(acos(q.s));
ret.x = q.i * sin_theta_inv;
ret.y = q.j * sin_theta_inv;
ret.z = q.k * sin_theta_inv;
return ret;
double sin_theta_inv;
COORD ret;
sin_theta_inv = 1.0 / sin(acos(q.s));
ret.x = q.i * sin_theta_inv;
ret.y = q.j * sin_theta_inv;
ret.z = q.k * sin_theta_inv;
return ret;
}
/** @brief Get the axis and rotate angle of a unit Quaternion */
void Quaternion_GetRotationAxisAndAngle(QUATERNION q, COORD *axis, double *angle)
{
double sin_theta_inv;
*angle = acos(q.s);
double sin_theta_inv;
*angle = acos(q.s);
// pre-compute to save time
sin_theta_inv = 1.0 / sin(*angle);
// pre-compute to save time
sin_theta_inv = 1.0 / sin(*angle);
// now the vector
axis->x = q.i * sin_theta_inv;
axis->y = q.j * sin_theta_inv;
axis->z = q.k * sin_theta_inv;
*angle = (*angle) * 2;
// now the vector
axis->x = q.i * sin_theta_inv;
axis->y = q.j * sin_theta_inv;
axis->z = q.k * sin_theta_inv;
*angle = (*angle) * 2;
}
/** @brief Decompose the rotation on to 2 parts. */
void Quaternion_GetSwingAndTwist(QUATERNION q, COORD axis, QUATERNION *swing, QUATERNION *twist)
{
double p;
COORD v;
v.x = q.i;
v.y = q.j;
v.z = q.k;
// return projection v1 on to v2 (parallel component)
p = Coord_Dot(v, axis);
v = Coord_GetScalarMult(axis, p);
twist->s = q.s;
twist->i = v.x;
twist->j = v.y;
twist->k = v.z;
Quaternion_Normalize(twist);
*swing = Quaternion_Mult(q, Quaternion_GetConjugated(*twist));
double p;
COORD v;
v.x = q.i;
v.y = q.j;
v.z = q.k;
// return projection v1 on to v2 (parallel component)
p = Coord_Dot(v, axis);
v = Coord_GetScalarMult(axis, p);
twist->s = q.s;
twist->i = v.x;
twist->j = v.y;
twist->k = v.z;
Quaternion_Normalize(twist);
*swing = Quaternion_Mult(q, Quaternion_GetConjugated(*twist));
}
/** @brief rotates v by this quaternion (quaternion must be unit) */
COORD Quaternion_Rotate(QUATERNION q, COORD v)
{
QUATERNION temp;
COORD ret;
temp.s = 0;
temp.i = v.x;
temp.j = v.y;
temp.k = v.z;
temp = Quaternion_Mult(q, temp);
Quaternion_Conjugated(&q);
temp = Quaternion_Mult(temp, q);
ret.x = temp.i;
ret.y = temp.j;
ret.z = temp.k;
return ret;
QUATERNION temp;
COORD ret;
temp.s = 0;
temp.i = v.x;
temp.j = v.y;
temp.k = v.z;
temp = Quaternion_Mult(q, temp);
Quaternion_Conjugated(&q);
temp = Quaternion_Mult(temp, q);
ret.x = temp.i;
ret.y = temp.j;
ret.z = temp.k;
return ret;
}
/** @brief returns the euler angles from a rotation Quaternion */
EULERANGLES Quaternion_ToEulerAngles(QUATERNION q, EULERANGLES_TYPE type)
{
EULERANGLES ret;
switch (type)
{
case EULERANGLES_ZYX:
{
/* Covert form matlab 2023a */
// Cap all inputs to asin to 1, since values >1 produce complex
// results
// Since the quaternion is of unit length, this should never happen,
// but some code generation configuration seem to hit this edge case
// under some circumstances.
EULERANGLES ret;
switch (type)
{
case EULERANGLES_ZYX:
{
/* Covert form matlab 2023a */
// Cap all inputs to asin to 1, since values >1 produce complex
// results
// Since the quaternion is of unit length, this should never happen,
// but some code generation configuration seem to hit this edge case
// under some circumstances.
double asinValue;
bool mask1, mask2;
double asinValue;
bool mask1, mask2;
asinValue = -2.0 * (q.i * q.k - q.s * q.j);
mask1 = asinValue >= 1.0 - 10 * EPS_DOUBLE.value;
mask2 = asinValue <= -1.0 + 10 * EPS_DOUBLE.value;
if (mask1)
asinValue = 1;
if (mask2)
asinValue = -1;
asinValue = -2.0 * (q.i * q.k - q.s * q.j);
mask1 = asinValue >= 1.0 - 10 * EPS_DOUBLE.value;
mask2 = asinValue <= -1.0 + 10 * EPS_DOUBLE.value;
if (mask1)
asinValue = 1;
if (mask2)
asinValue = -1;
if (mask1 || mask2)
{
ret.alpha = (asinValue > 0) ? (-2.0 * atan2(q.i, q.s)) : (2.0 * atan2(q.i, q.s));
ret.gamma = 0;
}
else
{
ret.alpha = atan2(2.0 * (q.i * q.j + q.s * q.k), q.s * q.s + q.i * q.i - q.j * q.j - q.k * q.k);
ret.gamma = atan2(2.0 * (q.j * q.k + q.s * q.i), q.s * q.s - q.i * q.i - q.j * q.j + q.k * q.k);
}
ret.beta = asin(asinValue);
break;
}
if (mask1 || mask2)
{
ret.alpha = (asinValue > 0) ? (-2.0 * atan2(q.i, q.s)) : (2.0 * atan2(q.i, q.s));
ret.gamma = 0;
}
else
{
ret.alpha = atan2(2.0 * (q.i * q.j + q.s * q.k), q.s * q.s + q.i * q.i - q.j * q.j - q.k * q.k);
ret.gamma = atan2(2.0 * (q.j * q.k + q.s * q.i), q.s * q.s - q.i * q.i - q.j * q.j + q.k * q.k);
}
ret.beta = asin(asinValue);
break;
}
case EULERANGLES_ZYZ:
{
// TODO;
ret = EULERANGLES_NIL;
break;
}
case EULERANGLES_ZYZ:
{
// TODO;
ret = EULERANGLES_NIL;
break;
}
case EULERANGLES_XYZ:
{
/* Covert form matlab 2023a */
// Prevent singularities as done in ZYX case
// Alternative to rotm2eul(quat2rotm(q), 'XYZ') with fewer
// operations
// sin(y) = R13 = 2 * (qx*qz + qy*qw)
// tan(x) = sin(x) / cos(x) = -R23 / R33
// = -2 * (qy*qz - qx*qw) / (1 - 2*(qx^2 + qy^2))
// = -2 * (qy*qz - qx*qw) / (qw^2 - qx^2 - qy^2 + qz^2)
// tan(z) = sin(z) / cos(z) = -R12 / R11
// = -2 * (qx*qy - qz*qw) / (1 - 2*(qy^2 + qz^2))
// = -2 * (qy*qz - qx*qw) / (qw^2 + qx^2 - qy^2 - qz^2)
case EULERANGLES_XYZ:
{
/* Covert form matlab 2023a */
// Prevent singularities as done in ZYX case
// Alternative to rotm2eul(quat2rotm(q), 'XYZ') with fewer
// operations
// sin(y) = R13 = 2 * (qx*qz + qy*qw)
// tan(x) = sin(x) / cos(x) = -R23 / R33
// = -2 * (qy*qz - qx*qw) / (1 - 2*(qx^2 + qy^2))
// = -2 * (qy*qz - qx*qw) / (qw^2 - qx^2 - qy^2 + qz^2)
// tan(z) = sin(z) / cos(z) = -R12 / R11
// = -2 * (qx*qy - qz*qw) / (1 - 2*(qy^2 + qz^2))
// = -2 * (qy*qz - qx*qw) / (qw^2 + qx^2 - qy^2 - qz^2)
double asinValue;
bool mask1, mask2;
double asinValue;
bool mask1, mask2;
asinValue = 2.0 * (q.i * q.k + q.s * q.j);
mask1 = asinValue >= 1.0 - 10 * EPS_DOUBLE.value;
mask2 = asinValue <= -1.0 + 10 * EPS_DOUBLE.value;
if (mask1)
asinValue = 1;
if (mask2)
asinValue = -1;
if (mask1 || mask2)
{
ret.alpha = 0;
ret.gamma = (asinValue > 0) ? (2.0 * atan2(q.i, q.s)) : (-2.0 * atan2(q.i, q.s));
}
else
{
ret.alpha = atan2(-2.0 * (q.j * q.k - q.s * q.i), q.s * q.s - q.i * q.i - q.j * q.j + q.k * q.k);
ret.gamma = atan2(-2.0 * (q.i * q.j - q.s * q.k), q.s * q.s + q.i * q.i - q.j * q.j - q.k * q.k);
}
ret.beta = asin(asinValue);
break;
}
asinValue = 2.0 * (q.i * q.k + q.s * q.j);
mask1 = asinValue >= 1.0 - 10 * EPS_DOUBLE.value;
mask2 = asinValue <= -1.0 + 10 * EPS_DOUBLE.value;
if (mask1)
asinValue = 1;
if (mask2)
asinValue = -1;
if (mask1 || mask2)
{
ret.alpha = 0;
ret.gamma = (asinValue > 0) ? (2.0 * atan2(q.i, q.s)) : (-2.0 * atan2(q.i, q.s));
}
else
{
ret.alpha = atan2(-2.0 * (q.j * q.k - q.s * q.i), q.s * q.s - q.i * q.i - q.j * q.j + q.k * q.k);
ret.gamma = atan2(-2.0 * (q.i * q.j - q.s * q.k), q.s * q.s + q.i * q.i - q.j * q.j - q.k * q.k);
}
ret.beta = asin(asinValue);
break;
}
default:
ret = EULERANGLES_NIL;
}
return ret;
default:
ret = EULERANGLES_NIL;
}
return ret;
}
/** @brief returns the rotation Quaternion from a euler angles */
QUATERNION Quaternion_FromeEulerAngles(EULERANGLES e, EULERANGLES_TYPE type)
{
QUATERNION ret;
double cosx, sinx, cosy, siny, cosz, sinz;
cosx = cos(e.alpha * 0.5);
sinx = sin(e.alpha * 0.5);
cosy = cos(e.beta * 0.5);
siny = sin(e.beta * 0.5);
cosz = cos(e.gamma * 0.5);
sinz = sin(e.gamma * 0.5);
switch (type)
{
case EULERANGLES_ZYX:
ret.s = cosx * cosy * cosz + sinx * siny * sinz;
ret.i = cosx * cosy * sinz - sinx * siny * cosz;
ret.j = cosx * siny * cosz + sinx * cosy * sinz;
ret.k = sinx * cosy * cosz - cosx * siny * sinz;
break;
case EULERANGLES_ZYZ:
ret.s = cosx * cosy * cosz - sinx * cosy * sinz;
ret.i = cosx * siny * sinz - sinx * siny * cosz;
ret.j = cosx * siny * cosz + sinx * siny * sinz;
ret.k = sinx * cosy * cosz + cosx * cosy * sinz;
break;
case EULERANGLES_XYZ:
ret.s = cosx * cosy * cosz - sinx * siny * sinz;
ret.i = sinx * cosy * cosz + cosx * siny * sinz;
ret.j = -sinx * cosy * sinz + cosx * siny * cosz;
ret.k = cosx * cosy * sinz + sinx * siny * cosz;
break;
default:
ret = QUATERNION_NIL;
break;
}
return ret;
QUATERNION ret;
double cosx, sinx, cosy, siny, cosz, sinz;
cosx = cos(e.alpha * 0.5);
sinx = sin(e.alpha * 0.5);
cosy = cos(e.beta * 0.5);
siny = sin(e.beta * 0.5);
cosz = cos(e.gamma * 0.5);
sinz = sin(e.gamma * 0.5);
switch (type)
{
case EULERANGLES_ZYX:
ret.s = cosx * cosy * cosz + sinx * siny * sinz;
ret.i = cosx * cosy * sinz - sinx * siny * cosz;
ret.j = cosx * siny * cosz + sinx * cosy * sinz;
ret.k = sinx * cosy * cosz - cosx * siny * sinz;
break;
case EULERANGLES_ZYZ:
ret.s = cosx * cosy * cosz - sinx * cosy * sinz;
ret.i = cosx * siny * sinz - sinx * siny * cosz;
ret.j = cosx * siny * cosz + sinx * siny * sinz;
ret.k = sinx * cosy * cosz + cosx * cosy * sinz;
break;
case EULERANGLES_XYZ:
ret.s = cosx * cosy * cosz - sinx * siny * sinz;
ret.i = sinx * cosy * cosz + cosx * siny * sinz;
ret.j = -sinx * cosy * sinz + cosx * siny * cosz;
ret.k = cosx * cosy * sinz + sinx * siny * cosz;
break;
default:
ret = QUATERNION_NIL;
break;
}
return ret;
}
/** @brief adapted from https://svn.code.sf.net/p/irrlicht/code/trunk/include/quaternion.h */
QUATERNION Quaternion_RotationFromTo(COORD from, COORD to)
{
// Based on Stan Melax's article in Game Programming Gems
COORD axis;
QUATERNION ret;
double c, s;
Coord_Normalize(&from);
Coord_Normalize(&to);
// Based on Stan Melax's article in Game Programming Gems
COORD axis;
QUATERNION ret;
double c, s;
Coord_Normalize(&from);
Coord_Normalize(&to);
c = Coord_Dot(from, to);
c = Coord_Dot(from, to);
if (c >= 1.0)
{ // If dot == 1, vectors are the same
return QUATERNION_IDENTITY; // identity
}
else if (c <= -1.0)
{ // exactly opposite
axis = Coord_Cross(COORD_X_AXIS, from);
if (c >= 1.0)
{ // If dot == 1, vectors are the same
return QUATERNION_IDENTITY; // identity
}
else if (c <= -1.0)
{ // exactly opposite
axis = Coord_Cross(COORD_X_AXIS, from);
if (Coord_Length(axis) == 0)
axis = Coord_Cross(COORD_Y_AXIS, from);
if (Coord_Length(axis) == 0)
axis = Coord_Cross(COORD_Y_AXIS, from);
// same as fromAxisAngle(axis, PI).normalize();
ret.s = 0;
ret.i = axis.x;
ret.j = axis.y;
ret.k = axis.z;
Quaternion_Normalize(&ret);
return ret;
}
// same as fromAxisAngle(axis, PI).normalize();
ret.s = 0;
ret.i = axis.x;
ret.j = axis.y;
ret.k = axis.z;
Quaternion_Normalize(&ret);
return ret;
}
s = sqrt((1 + c) * 2);
axis = Coord_Cross(from, to);
Coord_ScalarDiv(&axis, s);
ret.s = s * 0.5;
ret.i = axis.x;
ret.j = axis.y;
ret.k = axis.z;
Quaternion_Normalize(&ret);
return ret;
s = sqrt((1 + c) * 2);
axis = Coord_Cross(from, to);
Coord_ScalarDiv(&axis, s);
ret.s = s * 0.5;
ret.i = axis.x;
ret.j = axis.y;
ret.k = axis.z;
Quaternion_Normalize(&ret);
return ret;
}
/** @brief linear quaternion interpolation */
QUATERNION Quaternion_Lerp(QUATERNION q1, QUATERNION q2, double t)
{
QUATERNION ret;
Quaternion_ScalarMult(&q1, 1 - t);
Quaternion_ScalarMult(&q1, t);
ret = Quaternion_Add(q1, q2);
Quaternion_Normalize(&ret);
return ret;
QUATERNION ret;
Quaternion_ScalarMult(&q1, 1 - t);
Quaternion_ScalarMult(&q1, t);
ret = Quaternion_Add(q1, q2);
Quaternion_Normalize(&ret);
return ret;
}
/** @brief spherical linear interpolation */
QUATERNION Quaternion_Slerp(QUATERNION q1, QUATERNION q2, double t)
{
double dot;
QUATERNION q3, ret;
dot = Quaternion_Dot(q1, q2);
/* dot = cos(theta)
if (dot < 0), q1 and q2 are more than 90 degrees apart,
so we can invert one to reduce spinning */
if (dot < 0)
{
dot = -dot;
q3.s = -q2.s;
q3.i = -q2.i;
q3.j = -q2.j;
q3.k = -q2.k;
}
else
{
q3 = q2;
}
double dot;
QUATERNION q3, ret;
dot = Quaternion_Dot(q1, q2);
/* dot = cos(theta)
if (dot < 0), q1 and q2 are more than 90 degrees apart,
so we can invert one to reduce spinning */
if (dot < 0)
{
dot = -dot;
q3.s = -q2.s;
q3.i = -q2.i;
q3.j = -q2.j;
q3.k = -q2.k;
}
else
{
q3 = q2;
}
if (dot < 0.95)
{
double angle = acos(dot);
Quaternion_ScalarMult(&q1, sin(angle * (1 - t)));
Quaternion_ScalarMult(&q3, sin(angle * t));
ret = Quaternion_Add(q1, q3);
Quaternion_ScalarDiv(&ret, sin(angle));
return ret;
}
// if the angle is small, use linear interpolation
else
{
return Quaternion_Lerp(q1, q3, t);
}
if (dot < 0.95)
{
double angle = acos(dot);
Quaternion_ScalarMult(&q1, sin(angle * (1 - t)));
Quaternion_ScalarMult(&q3, sin(angle * t));
ret = Quaternion_Add(q1, q3);
Quaternion_ScalarDiv(&ret, sin(angle));
return ret;
}
// if the angle is small, use linear interpolation
else
{
return Quaternion_Lerp(q1, q3, t);
}
}
/** @} LinkLayerGeometry_Quaternion */
df_datalink_sp/src/networking/link_layer/geometry/quaternion.h
+10 -10
View File
@@ -37,10 +37,10 @@
*/
typedef struct
{
double s;
double i;
double j;
double k;
double s;
double i;
double j;
double k;
} QUATERNION;
extern const QUATERNION QUATERNION_IDENTITY;
@@ -107,13 +107,13 @@ COORD Quaternion_GetRotationAxis(QUATERNION q);
void Quaternion_GetRotationAxisAndAngle(QUATERNION q, COORD *axis, double *angle);
/** @brief Decompose the rotation on to 2 parts.
1. Twist - rotation around the "direction" vector
2. Swing - rotation around axis that is perpendicular to "direction" vector
The rotation can be composed back by rotation = swing * twist
1. Twist - rotation around the "direction" vector
2. Swing - rotation around axis that is perpendicular to "direction" vector
The rotation can be composed back by rotation = swing * twist
has singularity in case of swing_rotation close to 180 degrees rotation.
if the input quaternion is of non-unit length, the outputs are non-unit as well
otherwise, outputs are both unit
has singularity in case of swing_rotation close to 180 degrees rotation.
if the input quaternion is of non-unit length, the outputs are non-unit as well
otherwise, outputs are both unit
*/
void Quaternion_GetSwingAndTwist(QUATERNION q, COORD axis, QUATERNION *swing, QUATERNION *twist);
df_datalink_sp/src/networking/phy_layer/phy_antenna.c
-14
View File
@@ -71,7 +71,6 @@
// Pair by regex
// Regex: (([0-9]+, ){8})
const uint16_t hopPattern_2KBPS_20MS[L1_FRAME_2KBPS_20MS_HOPS] =
{
1500
@@ -97,12 +96,6 @@ const uint16_t hopPattern_10KBPS_10MS[L1_FRAME_10KBPS_10MS_HOPS] =
1500
};
// uint16_t hopPattern_10KBPS_20MS[L1_FRAME_10KBPS_20MS_HOPS] =
//{
// 1500, 1600, 1700, 1800, 1900, 1400, 1300, 1200,
// 1100, 1000, 900
// };
const uint16_t hopPattern_10KBPS_20MS[L1_FRAME_10KBPS_20MS_HOPS] =
{
1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500,
@@ -115,13 +108,6 @@ const uint16_t hopPattern_40KBPS_10MS[L1_FRAME_40KBPS_10MS_HOPS] =
1500
};
// uint16_t hopPattern_40KBPS_20MS[L1_FRAME_40KBPS_20MS_HOPS] =
//{
// 1600, 1650, 1700, 1750, 1800, 1850, 1900, 1550,
// 1500, 1450, 1400, 1350, 1300, 1250, 1200, 1150,
// 1100, 1050, 1000
// };
const uint16_t hopPattern_40KBPS_20MS[L1_FRAME_40KBPS_20MS_HOPS] =
{
1700, 1800, 1900, 2000, 1700, 1700, 1700, 1700,