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

Fix the test data matrix script (#24745)

Browse Source 
Co-authored-by: jmackay2 <jmackay2@gmail.com>
This commit is contained in:
jmackay2 2026-01-13 17:47:24 -05:00 committed by GitHub
parent 473ef5fd06
commit e371c4edd9
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/lib/matrix/test/test_data.py
View File

@ -1,7 +1,9 @@
from __future__ import print_function
#!/usr/bin/env python3
from pylab import *
from pprint import pprint
import scipy.linalg
import sys
# test cases, derived from doc/nasa_rotation_def.pdf
@ -47,7 +49,7 @@ def quat_prod(q, r):
def dcm_to_euler(dcm):
return array([
arctan(dcm[2,1]/ dcm[2,2]),
arctan(-dcm[2,0]/ std::sqrt(1 - dcm[2,0]**2)),
arctan(-dcm[2,0]/ (1 - dcm[2,0]**2)**0.5),
arctan(dcm[1,0]/ dcm[0,0]),
])
@ -75,6 +77,7 @@ psi = 0.3
print('euler', phi, theta, psi)
q = euler_to_quat(phi, theta, psi)
FLT_EPSILON = sys.float_info.epsilon
assert(abs(norm(q) - 1) < FLT_EPSILON)
assert(abs(norm(q) - 1) < FLT_EPSILON)
assert(norm(array(quat_to_euler(q)) - array([phi, theta, psi])) < FLT_EPSILON)