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

Merge pull request #10 from PX4/euler_fix

Browse Source 
fix euler calculation
This commit is contained in:
James Goppert
2016-01-24 01:52:40 -05:00
parent b98ac75452 5aef810e7c
commit d8e397c010
8 changed files with 320 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitmodules
View File
.travis.yml
+1 -1
View File
@@ -3,7 +3,7 @@ sudo: false
install:
- pip install --user cpp-coveralls
script:
- cmake -DCMAKE_BUILD_TYPE=Profile .
- cmake -DCMAKE_BUILD_TYPE=Profile -DTEST=ON -DFORMAT=ON .
- make
- make check_format
- ctest -V
CMakeLists.txt
+30 -22
View File
@@ -13,10 +13,13 @@ endif()
set_property(CACHE CMAKE_BUILD_TYPE PROPERTY
STRINGS "Debug;Release;RelWithDebInfo;MinSizeRel;Profile")
option (SUPPORT_STDIOSTREAM
option(SUPPORT_STDIOSTREAM
"If enabled provides support for << operator (as used with
std::cout)" OFF)
if((SUPPORT_STDIOSTREAM))
option(TEST "Enable testing" OFF)
option(FORMAT "Enable formatting" OFF)
if(SUPPORT_STDIOSTREAM)
add_definitions(-DSUPPORT_STDIOSTREAM)
endif()
@@ -59,29 +62,34 @@ include_directories(${CMAKE_SOURCE_DIR})
file(GLOB_RECURSE COV_SRCS matrix/*.hpp matrix/*.cpp)
add_subdirectory(test)
if(TEST)
enable_testing()
add_subdirectory(test)
endif()
set(astyle_exe ${CMAKE_BINARY_DIR}/astyle/src/bin/astyle)
add_custom_command(OUTPUT ${astyle_exe}
COMMAND wget http://sourceforge.net/projects/astyle/files/astyle/astyle%202.05.1/astyle_2.05.1_linux.tar.gz -O /tmp/astyle.tar.gz
COMMAND tar -xvf /tmp/astyle.tar.gz
COMMAND cd astyle/src && make -f ../build/gcc/Makefile
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
if(FORMAT)
set(astyle_exe ${CMAKE_BINARY_DIR}/astyle/src/bin/astyle)
add_custom_command(OUTPUT ${astyle_exe}
COMMAND wget http://sourceforge.net/projects/astyle/files/astyle/astyle%202.05.1/astyle_2.05.1_linux.tar.gz -O /tmp/astyle.tar.gz
COMMAND tar -xvf /tmp/astyle.tar.gz
COMMAND cd astyle/src && make -f ../build/gcc/Makefile
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
)
add_custom_target(check_format
COMMAND scripts/format.sh ${astyle_exe} 0
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
DEPENDS ${astyle_exe}
VERBATIM
)
add_custom_target(check_format
COMMAND scripts/format.sh ${astyle_exe} 0
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
DEPENDS ${astyle_exe}
VERBATIM
)
add_custom_target(format
COMMAND scripts/format.sh ${astyle_exe} 1
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
VERBATIM
DEPENDS ${astyle_exe}
)
add_custom_target(format
COMMAND scripts/format.sh ${astyle_exe} 1
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
VERBATIM
DEPENDS ${astyle_exe}
)
endif()
set(CPACK_PACKAGE_VERSION_MAJOR ${VERSION_MAJOR})
set(CPACK_PACKAGE_VERSION_MINOR ${VERSION_MINOR})
doc/.gitignore
+1
View File
@@ -0,0 +1 @@
.ipynb_checkpoints/
doc/euler_gimbal_lock.ipynb
+240
View File
File diff suppressed because one or more lines are too long
matrix/Euler.hpp
+15 -14
View File
@@ -48,23 +48,24 @@ public:
Euler(const Dcm<Type> & dcm) : Vector<Type, 3>()
{
Type psi_val = Type(atan(dcm(1, 0)/ dcm(0, 0)));
Type phi_val = Type(atan2(dcm(2,1), dcm(2,2)));
Type theta_val = Type(asin(-dcm(2,0)));
Type phi_val = Type(atan(dcm(2, 1)/ dcm(2, 2)));
Type psi_val = Type(atan2(dcm(1,0), dcm(0,0)));
Type pi = Type(M_PI);
// protection against NaN if dcm(0,0) or dcm(2,2) == 0
psi() = 0;
theta() = 0;
phi() = 0;
if (psi() >= -(Type)M_PI_2 && psi() <= (Type)M_PI_2) {
psi() = psi_val;
}
if (theta() >= -(Type)M_PI_2 && theta() <= (Type)M_PI_2) {
theta() = theta_val;
}
if (phi() >= -(Type)M_PI_2 && phi() <= (Type)M_PI_2) {
phi() = phi_val;
if (fabs(theta_val - pi/2) < 1.0e-3) {
phi_val = Type(0.0);
psi_val = Type(atan2(dcm(1,2), dcm(0,2)));
} else if (Type(fabs(theta_val + pi/2) < Type(1.0e-3))) {
phi_val = Type(0.0);
psi_val = Type(atan2(-dcm(1,2), -dcm(0,2)));
}
if (psi_val < 0) psi_val += 2*pi;
phi() = phi_val;
theta() = theta_val;
psi() = psi_val;
}
Euler(const Quaternion<Type> & q) :
test/CMakeLists.txt
+1 -3
View File
@@ -33,6 +33,4 @@ if (${CMAKE_BUILD_TYPE} STREQUAL "Profile")
WORKING_DIRECTORY ${CMAKE_BUILD_DIR}
DEPENDS test_build
)
endif()
endif()
test/attitude.cpp
+32 -13
View File
@@ -91,27 +91,46 @@ int main()
double rad2deg = 180.0/M_PI;
// 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) {
for (int roll=-90; roll<=90; roll+=90) {
for (int pitch=-90; pitch<=90; pitch+=90) {
for (int yaw=1; yaw<=360; yaw+=90) {
// note if theta = pi/2, then roll is set to zero
if (pitch == 90 || pitch == -90) {
roll = 0;
int roll_expected = roll;
int yaw_expected = yaw;
if (pitch == 90) {
roll_expected = 0;
yaw_expected = yaw - roll;
} else if (pitch == -90) {
roll_expected = 0;
yaw_expected = yaw + roll;
}
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));
if (yaw_expected < 0) yaw_expected += 360;
if (yaw_expected > 360) yaw_expected -= 360;
printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
Euler<double> euler_expected(
deg2rad*double(roll_expected),
deg2rad*double(pitch),
deg2rad*double(yaw_expected));
Euler<double> euler(
deg2rad*double(roll),
deg2rad*double(pitch),
deg2rad*double(yaw));
Dcm<double> dcm_from_euler(euler);
dcm_from_euler.print();
Euler<double> euler_out(dcm_from_euler);
TEST(isEqual(rad2deg*euler_expected, rad2deg*euler_out));
Eulerf eulerf_expected(
float(deg2rad)*float(roll_expected),
float(deg2rad)*float(pitch),
float(deg2rad)*float(yaw_expected));
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,
TEST(isEqual(float(rad2deg)*eulerf_expected,
float(rad2deg)*euler_outf));
}
}