fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added better roundtrip euler test.

Browse Source
This commit is contained in:
James Goppert 2016-01-24 00:39:21 -05:00
parent cc2804d6a6
commit ffbe58ca10
3 changed files with 35 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
matrix/Dcm.hpp
View File

@ -64,12 +64,12 @@ public:
Dcm(const Euler<Type> & euler) {
Dcm &dcm = *this;
Type cosPhi = cosf(euler.phi());
Type sinPhi = sinf(euler.phi());
Type cosThe = cosf(euler.theta());
Type sinThe = sinf(euler.theta());
Type cosPsi = cosf(euler.psi());
Type sinPsi = sinf(euler.psi());
Type cosPhi = Type(cos(euler.phi()));
Type sinPhi = Type(sin(euler.phi()));
Type cosThe = Type(cos(euler.theta()));
Type sinThe = Type(sin(euler.theta()));
Type cosPsi = Type(cos(euler.psi()));
Type sinPsi = Type(sin(euler.psi()));
dcm(0, 0) = cosThe * cosPsi;
dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;

2
matrix/Matrix.hpp
View File

@ -131,7 +131,7 @@ public:
bool operator==(const Matrix<Type, M, N> &other) const
{
const Matrix<Type, M, N> &self = *this;
static const Type eps = Type(1e-6);
static const Type eps = Type(1e-4);
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {

40
test/attitude.cpp
View File

@ -86,20 +86,36 @@ int main()
Quatf q2(dcm_check);
TEST(isEqual(q2, q_check));
// constants
double deg2rad = M_PI/180.0;
double rad2deg = 180.0/M_PI;
// euler gimbal lock check
// note if theta = pi/2, then roll is set to zero
float pi_2 = float(M_PI_2);
Eulerf euler_gimbal_lock(0.0f, pi_2, 0.2f);
Dcmf dcm_lock(euler_gimbal_lock);
Eulerf euler_gimbal_lock_out(dcm_lock);
TEST(isEqual(euler_gimbal_lock, euler_gimbal_lock_out));
// euler dcm round trip check
for (int roll=-90; roll<90; roll+=1) {
for (int pitch=-90; pitch<90; pitch+=1) {
for (int yaw=0; yaw<360; yaw+=1) {
// note if theta = pi/2, then roll is set to zero
if (pitch == 90 || pitch == -90) {
roll = 0;
}
printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
Euler<double> euler(deg2rad*double(roll),
deg2rad*double(pitch),
deg2rad*double(yaw));
Dcm<double> dcm_from_euler(euler);
Euler<double> euler_out(dcm_from_euler);
TEST(isEqual(rad2deg*euler, rad2deg*euler_out));
// note if theta = pi/2, then roll is set to zero
Eulerf euler_gimbal_lock2(0.0f, -pi_2, 0.2f);
Dcmf dcm_lock2(euler_gimbal_lock2);
Eulerf euler_gimbal_lock_out2(dcm_lock2);
TEST(isEqual(euler_gimbal_lock2, euler_gimbal_lock_out2));
Eulerf eulerf(float(deg2rad)*float(roll),
float(deg2rad)*float(pitch),
float(deg2rad)*float(yaw));
Dcm<float> dcm_from_eulerf(eulerf);
Euler<float> euler_outf(dcm_from_eulerf);
TEST(isEqual(float(rad2deg)*eulerf,
float(rad2deg)*euler_outf));
}
}
}
// quaterion copy ctors
float data_v4[] = {1, 2, 3, 4};