mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-06 10:58:52 +08:00
Rebase of changes to sensor_hil_fixedwing branch.
This commit is contained in:
James Goppert
+53
-24
@@ -52,6 +52,23 @@ Dcm::Dcm() :
|
||||
{
|
||||
}
|
||||
|
||||
Dcm::Dcm(float c00, float c01, float c02,
|
||||
float c10, float c11, float c12,
|
||||
float c20, float c21, float c22) :
|
||||
Matrix(3, 3)
|
||||
{
|
||||
Dcm &dcm = *this;
|
||||
dcm(0,0) = c00;
|
||||
dcm(0,1) = c01;
|
||||
dcm(0,2) = c02;
|
||||
dcm(1,0) = c10;
|
||||
dcm(1,1) = c11;
|
||||
dcm(1,2) = c12;
|
||||
dcm(2,0) = c20;
|
||||
dcm(2,1) = c21;
|
||||
dcm(2,2) = c22;
|
||||
}
|
||||
|
||||
Dcm::Dcm(const float *data) :
|
||||
Matrix(3, 3, data)
|
||||
{
|
||||
@@ -61,22 +78,22 @@ Dcm::Dcm(const Quaternion &q) :
|
||||
Matrix(3, 3)
|
||||
{
|
||||
Dcm &dcm = *this;
|
||||
float a = q.getA();
|
||||
float b = q.getB();
|
||||
float c = q.getC();
|
||||
float d = q.getD();
|
||||
float aSq = a * a;
|
||||
float bSq = b * b;
|
||||
float cSq = c * c;
|
||||
float dSq = d * d;
|
||||
double a = q.getA();
|
||||
double b = q.getB();
|
||||
double c = q.getC();
|
||||
double d = q.getD();
|
||||
double aSq = a * a;
|
||||
double bSq = b * b;
|
||||
double cSq = c * c;
|
||||
double dSq = d * d;
|
||||
dcm(0, 0) = aSq + bSq - cSq - dSq;
|
||||
dcm(0, 1) = 2 * (b * c - a * d);
|
||||
dcm(0, 2) = 2 * (a * c + b * d);
|
||||
dcm(1, 0) = 2 * (b * c + a * d);
|
||||
dcm(0, 1) = 2.0 * (b * c - a * d);
|
||||
dcm(0, 2) = 2.0 * (a * c + b * d);
|
||||
dcm(1, 0) = 2.0 * (b * c + a * d);
|
||||
dcm(1, 1) = aSq - bSq + cSq - dSq;
|
||||
dcm(1, 2) = 2 * (c * d - a * b);
|
||||
dcm(2, 0) = 2 * (b * d - a * c);
|
||||
dcm(2, 1) = 2 * (a * b + c * d);
|
||||
dcm(1, 2) = 2.0 * (c * d - a * b);
|
||||
dcm(2, 0) = 2.0 * (b * d - a * c);
|
||||
dcm(2, 1) = 2.0 * (a * b + c * d);
|
||||
dcm(2, 2) = aSq - bSq - cSq + dSq;
|
||||
}
|
||||
|
||||
@@ -84,12 +101,12 @@ Dcm::Dcm(const EulerAngles &euler) :
|
||||
Matrix(3, 3)
|
||||
{
|
||||
Dcm &dcm = *this;
|
||||
float cosPhi = cosf(euler.getPhi());
|
||||
float sinPhi = sinf(euler.getPhi());
|
||||
float cosThe = cosf(euler.getTheta());
|
||||
float sinThe = sinf(euler.getTheta());
|
||||
float cosPsi = cosf(euler.getPsi());
|
||||
float sinPsi = sinf(euler.getPsi());
|
||||
double cosPhi = cos(euler.getPhi());
|
||||
double sinPhi = sin(euler.getPhi());
|
||||
double cosThe = cos(euler.getTheta());
|
||||
double sinThe = sin(euler.getTheta());
|
||||
double cosPsi = cos(euler.getPsi());
|
||||
double sinPsi = sin(euler.getPsi());
|
||||
|
||||
dcm(0, 0) = cosThe * cosPsi;
|
||||
dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
|
||||
@@ -116,11 +133,23 @@ Dcm::~Dcm()
|
||||
int __EXPORT dcmTest()
|
||||
{
|
||||
printf("Test DCM\t\t: ");
|
||||
// default ctor
|
||||
ASSERT(matrixEqual(Dcm(),
|
||||
Matrix::identity(3)));
|
||||
// quaternion ctor
|
||||
ASSERT(matrixEqual(
|
||||
Dcm(Quaternion(0.983347, 0.034271, 0.106021, 0.143572)),
|
||||
Dcm( 0.9362934, -0.2750958, 0.2183507,
|
||||
0.2896295, 0.9564251, -0.0369570,
|
||||
-0.1986693, 0.0978434, 0.9751703)));
|
||||
// euler angle ctor
|
||||
ASSERT(matrixEqual(
|
||||
Dcm(EulerAngles(0.1, 0.2, 0.3)),
|
||||
Dcm( 0.9362934, -0.2750958, 0.2183507,
|
||||
0.2896295, 0.9564251, -0.0369570,
|
||||
-0.1986693, 0.0978434, 0.9751703)));
|
||||
// rotations
|
||||
Vector3 vB(1, 2, 3);
|
||||
ASSERT(matrixEqual(Dcm(Quaternion(1, 0, 0, 0)),
|
||||
Matrix::identity(3)));
|
||||
ASSERT(matrixEqual(Dcm(EulerAngles(0, 0, 0)),
|
||||
Matrix::identity(3)));
|
||||
ASSERT(vectorEqual(Vector3(-2, 1, 3),
|
||||
Dcm(EulerAngles(0, 0, M_PI_2_F))*vB));
|
||||
ASSERT(vectorEqual(Vector3(3, 2, -1),
|
||||
|
||||
@@ -64,6 +64,13 @@ public:
|
||||
*/
|
||||
Dcm();
|
||||
|
||||
/**
|
||||
* scalar ctor
|
||||
*/
|
||||
Dcm(float c00, float c01, float c02,
|
||||
float c10, float c11, float c12,
|
||||
float c20, float c21, float c22);
|
||||
|
||||
/**
|
||||
* data ctor
|
||||
*/
|
||||
|
||||
@@ -97,23 +97,27 @@ EulerAngles::~EulerAngles()
|
||||
int __EXPORT eulerAnglesTest()
|
||||
{
|
||||
printf("Test EulerAngles\t: ");
|
||||
EulerAngles euler(1, 2, 3);
|
||||
EulerAngles euler(0.1, 0.2, 0.3);
|
||||
|
||||
// test ctor
|
||||
ASSERT(vectorEqual(Vector3(1, 2, 3), euler));
|
||||
ASSERT(equal(euler.getPhi(), 1));
|
||||
ASSERT(equal(euler.getTheta(), 2));
|
||||
ASSERT(equal(euler.getPsi(), 3));
|
||||
ASSERT(vectorEqual(Vector3(0.1, 0.2, 0.3), euler));
|
||||
ASSERT(equal(euler.getPhi(), 0.1));
|
||||
ASSERT(equal(euler.getTheta(), 0.2));
|
||||
ASSERT(equal(euler.getPsi(), 0.3));
|
||||
|
||||
// test dcm ctor
|
||||
euler = Dcm(EulerAngles(0.1,0.2,0.3));
|
||||
ASSERT(vectorEqual(Vector3(0.1,0.2,0.3),euler));
|
||||
|
||||
// test quat ctor
|
||||
euler = Quaternion(EulerAngles(0.1,0.2,0.3));
|
||||
ASSERT(vectorEqual(Vector3(0.1,0.2,0.3),euler));
|
||||
|
||||
// test assignment
|
||||
euler.setPhi(4);
|
||||
ASSERT(equal(euler.getPhi(), 4));
|
||||
euler.setTheta(5);
|
||||
ASSERT(equal(euler.getTheta(), 5));
|
||||
euler.setPsi(6);
|
||||
ASSERT(equal(euler.getPsi(), 6));
|
||||
euler.setPhi(0.4);
|
||||
euler.setTheta(0.5);
|
||||
euler.setPsi(0.6);
|
||||
ASSERT(vectorEqual(Vector3(0.4,0.5,0.6),euler));
|
||||
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
|
||||
@@ -79,32 +79,34 @@ Quaternion::Quaternion(const Vector &v) :
|
||||
Quaternion::Quaternion(const Dcm &dcm) :
|
||||
Vector(4)
|
||||
{
|
||||
setA(0.5f * sqrtf(1 + dcm(0, 0) +
|
||||
dcm(1, 1) + dcm(2, 2)));
|
||||
setB((dcm(2, 1) - dcm(1, 2)) /
|
||||
(4 * getA()));
|
||||
setC((dcm(0, 2) - dcm(2, 0)) /
|
||||
(4 * getA()));
|
||||
setD((dcm(1, 0) - dcm(0, 1)) /
|
||||
(4 * getA()));
|
||||
// avoiding singularities by not using
|
||||
// division equations
|
||||
setA(0.5 * sqrt(1.0 +
|
||||
double( dcm(0, 0) + dcm(1, 1) + dcm(2, 2))));
|
||||
setB(0.5 * sqrt(1.0 +
|
||||
double( dcm(0, 0) - dcm(1, 1) - dcm(2, 2))));
|
||||
setC(0.5 * sqrt(1.0 +
|
||||
double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2))));
|
||||
setD(0.5 * sqrt(1.0 +
|
||||
double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2))));
|
||||
}
|
||||
|
||||
Quaternion::Quaternion(const EulerAngles &euler) :
|
||||
Vector(4)
|
||||
{
|
||||
float cosPhi_2 = cosf(euler.getPhi() / 2.0f);
|
||||
float cosTheta_2 = cosf(euler.getTheta() / 2.0f);
|
||||
float cosPsi_2 = cosf(euler.getPsi() / 2.0f);
|
||||
float sinPhi_2 = sinf(euler.getPhi() / 2.0f);
|
||||
float sinTheta_2 = sinf(euler.getTheta() / 2.0f);
|
||||
float sinPsi_2 = sinf(euler.getPsi() / 2.0f);
|
||||
double cosPhi_2 = cos(double(euler.getPhi()) / 2.0);
|
||||
double sinPhi_2 = sin(double(euler.getPhi()) / 2.0);
|
||||
double cosTheta_2 = cos(double(euler.getTheta()) / 2.0);
|
||||
double sinTheta_2 = sin(double(euler.getTheta()) / 2.0);
|
||||
double cosPsi_2 = cos(double(euler.getPsi()) / 2.0);
|
||||
double sinPsi_2 = sin(double(euler.getPsi()) / 2.0);
|
||||
setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
|
||||
sinPhi_2 * sinTheta_2 * sinPsi_2);
|
||||
setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
|
||||
cosPhi_2 * sinTheta_2 * sinPsi_2);
|
||||
setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
|
||||
sinPhi_2 * cosTheta_2 * sinPsi_2);
|
||||
setD(cosPhi_2 * cosTheta_2 * sinPsi_2 +
|
||||
setD(cosPhi_2 * cosTheta_2 * sinPsi_2 -
|
||||
sinPhi_2 * sinTheta_2 * cosPsi_2);
|
||||
}
|
||||
|
||||
@@ -147,33 +149,24 @@ int __EXPORT quaternionTest()
|
||||
ASSERT(equal(q.getC(), 0));
|
||||
ASSERT(equal(q.getD(), 0));
|
||||
// test float ctor
|
||||
q = Quaternion(0, 1, 0, 0);
|
||||
ASSERT(equal(q.getA(), 0));
|
||||
ASSERT(equal(q.getB(), 1));
|
||||
ASSERT(equal(q.getC(), 0));
|
||||
ASSERT(equal(q.getD(), 0));
|
||||
q = Quaternion(0.1825742, 0.3651484, 0.5477226, 0.7302967);
|
||||
ASSERT(equal(q.getA(), 0.1825742));
|
||||
ASSERT(equal(q.getB(), 0.3651484));
|
||||
ASSERT(equal(q.getC(), 0.5477226));
|
||||
ASSERT(equal(q.getD(), 0.7302967));
|
||||
// test euler ctor
|
||||
q = Quaternion(EulerAngles(0, 0, 0));
|
||||
ASSERT(equal(q.getA(), 1));
|
||||
ASSERT(equal(q.getB(), 0));
|
||||
ASSERT(equal(q.getC(), 0));
|
||||
ASSERT(equal(q.getD(), 0));
|
||||
q = Quaternion(EulerAngles(0.1, 0.2, 0.3));
|
||||
ASSERT(vectorEqual(q, Quaternion(0.983347, 0.034271, 0.106021, 0.143572)));
|
||||
// test dcm ctor
|
||||
q = Quaternion(Dcm());
|
||||
ASSERT(equal(q.getA(), 1));
|
||||
ASSERT(equal(q.getB(), 0));
|
||||
ASSERT(equal(q.getC(), 0));
|
||||
ASSERT(equal(q.getD(), 0));
|
||||
ASSERT(vectorEqual(q, Quaternion(1, 0, 0, 0)));
|
||||
// TODO test derivative
|
||||
// test accessors
|
||||
q.setA(0.1);
|
||||
q.setB(0.2);
|
||||
q.setC(0.3);
|
||||
q.setD(0.4);
|
||||
ASSERT(equal(q.getA(), 0.1));
|
||||
ASSERT(equal(q.getB(), 0.2));
|
||||
ASSERT(equal(q.getC(), 0.3));
|
||||
ASSERT(equal(q.getD(), 0.4));
|
||||
ASSERT(vectorEqual(q, Quaternion(0.1, 0.2, 0.3, 0.4)));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
Binary file not shown.
@@ -0,0 +1,63 @@
|
||||
clc
|
||||
clear
|
||||
function out = float_truncate(in, digits)
|
||||
out = round(in*10^digits)
|
||||
out = out/10^digits
|
||||
endfunction
|
||||
|
||||
phi = 0.1
|
||||
theta = 0.2
|
||||
psi = 0.3
|
||||
|
||||
cosPhi = cos(phi)
|
||||
cosPhi_2 = cos(phi/2)
|
||||
sinPhi = sin(phi)
|
||||
sinPhi_2 = sin(phi/2)
|
||||
|
||||
cosTheta = cos(theta)
|
||||
cosTheta_2 = cos(theta/2)
|
||||
sinTheta = sin(theta)
|
||||
sinTheta_2 = sin(theta/2)
|
||||
|
||||
cosPsi = cos(psi)
|
||||
cosPsi_2 = cos(psi/2)
|
||||
sinPsi = sin(psi)
|
||||
sinPsi_2 = sin(psi/2)
|
||||
|
||||
C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi;
|
||||
cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi;
|
||||
-sinTheta, sinPhi*cosTheta, cosPhi*cosTheta]
|
||||
|
||||
disp(C_nb)
|
||||
//C_nb = float_truncate(C_nb,3)
|
||||
//disp(C_nb)
|
||||
|
||||
theta = asin(-C_nb(3,1))
|
||||
phi = atan(C_nb(3,2), C_nb(3,3))
|
||||
psi = atan(C_nb(2,1), C_nb(1,1))
|
||||
printf('phi %f\n', phi)
|
||||
printf('theta %f\n', theta)
|
||||
printf('psi %f\n', psi)
|
||||
|
||||
q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2;
|
||||
sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2;
|
||||
cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2;
|
||||
cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2]
|
||||
|
||||
//q = float_truncate(q,3)
|
||||
|
||||
a = q(1)
|
||||
b = q(2)
|
||||
c = q(3)
|
||||
d = q(4)
|
||||
printf('q: %f %f %f %f\n', a, b, c, d)
|
||||
a2 = a*a
|
||||
b2 = b*b
|
||||
c2 = c*c
|
||||
d2 = d*d
|
||||
|
||||
C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c);
|
||||
2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b);
|
||||
2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2]
|
||||
|
||||
disp(C2_nb)
|
||||
Reference in New Issue
Block a user