Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-27 00:37:37 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Merge pull request #663 from paparazzi/ref_quat_scale

Browse Source 
stabilization attitude: reference quaternion scale

Fix scale of angle in 2nd order model, since error quaternion contains the half-angles we get 2*omega^2*err

This means that you should halve your REF_OMEGA_[PQR] parameters to get nearly the same as before.
This commit is contained in:
Felix Ruess
2014-04-10 23:45:25 +02:00
parent a2448b2268 f6ff7a6d7c
commit 2b57e3ad03
29 changed files with 290 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ardrone2_raw.xml
+3 -3
View File
@@ -145,17 +145,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="900" unit="deg/s"/>
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9"/>
<define name="REF_MAX_P" value="600." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="900" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9"/>
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="600" unit="deg/s"/>
<define name="REF_OMEGA_R" value="300" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="400." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(4000.)"/>
conf/airframes/ardrone2_raw_optitrack.xml
+3 -3
View File
@@ -147,17 +147,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="900" unit="deg/s"/>
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9"/>
<define name="REF_MAX_P" value="600." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="900" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9"/>
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="600" unit="deg/s"/>
<define name="REF_OMEGA_R" value="300" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="400." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(4000.)"/>
conf/airframes/examples/booz2.xml
+3 -3
View File
@@ -112,17 +112,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9"/>
<define name="REF_MAX_P" value="300." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9"/>
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
conf/airframes/examples/h_hex.xml
+3 -3
View File
@@ -111,17 +111,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="300." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
conf/airframes/examples/krooz_sd/krooz_sd_bre_hexa_mkk.xml
+3 -3
View File
@@ -153,17 +153,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_OMEGA_P" value="225" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="180." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(400.)"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="225" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="180." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(400.)"/>
<define name="REF_OMEGA_R" value="400" unit="deg/s"/>
<define name="REF_OMEGA_R" value="200" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="70." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(360.)"/>
conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml
+3 -3
View File
@@ -153,17 +153,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_OMEGA_P" value="225" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="180." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(400.)"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="225" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="180." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(400.)"/>
<define name="REF_OMEGA_R" value="400" unit="deg/s"/>
<define name="REF_OMEGA_R" value="200" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="70." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(360.)"/>
conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml
+3 -3
View File
@@ -158,17 +158,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_OMEGA_P" value="225" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="180." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(400.)"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="225" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="180." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(400.)"/>
<define name="REF_OMEGA_R" value="400" unit="deg/s"/>
<define name="REF_OMEGA_R" value="200" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="70." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(360.)"/>
conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml
+3 -3
View File
@@ -154,17 +154,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_OMEGA_P" value="225" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.87"/>
<define name="REF_MAX_P" value="180." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(400.)"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="225" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.87"/>
<define name="REF_MAX_Q" value="180." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(400.)"/>
<define name="REF_OMEGA_R" value="400" unit="deg/s"/>
<define name="REF_OMEGA_R" value="150" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="70." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(360.)"/>
conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml
+3 -3
View File
@@ -146,17 +146,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="90." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(180.)"/>
conf/airframes/examples/lisa_asctec.xml
+3 -3
View File
@@ -110,17 +110,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="300." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
+3 -3
View File
@@ -140,17 +140,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml
+3 -3
View File
@@ -143,17 +143,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
conf/airframes/examples/quadrotor_lisa_m_mkk.xml
+3 -3
View File
@@ -132,17 +132,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
conf/airframes/examples/quadrotor_lisa_mx.xml
+3 -3
View File
@@ -151,17 +151,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
conf/airframes/examples/quadrotor_lisa_s.xml
+3 -3
View File
@@ -108,17 +108,17 @@
<define name="DEADBAND_A" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="1500" unit="deg/s"/>
<define name="REF_OMEGA_P" value="750" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="300." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="1500" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="750" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1000.)"/>
conf/airframes/examples/quadrotor_mlkf.xml
+3 -3
View File
@@ -134,17 +134,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="{RadOfDeg(800)}"/>
<define name="REF_OMEGA_P" value="{RadOfDeg(400)}"/>
<define name="REF_ZETA_P" value="{0.85}"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="{RadOfDeg(800)}"/>
<define name="REF_OMEGA_Q" value="{RadOfDeg(400)}"/>
<define name="REF_ZETA_Q" value="{0.85}"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="{RadOfDeg(500)}"/>
<define name="REF_OMEGA_R" value="{RadOfDeg(250)}"/>
<define name="REF_ZETA_R" value="{0.85}"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
conf/airframes/examples/quadrotor_navgo.xml
+3 -3
View File
@@ -155,17 +155,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="300." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_OMEGA_R" value="150" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="90." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
conf/airframes/examples/quadrotor_px4fmu.xml
+3 -3
View File
@@ -136,17 +136,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
conf/airframes/examples/quadshot_asp21_spektrum.xml
+3 -3
View File
@@ -149,17 +149,17 @@ More information on the Quadshot can be found at transition-robotics.com -->
<define name="SP_PSI_DELTA_LIMIT" value="90" unit="deg"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="3000" unit="deg/s"/>
<define name="REF_OMEGA_P" value="1500" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="300." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_Q" value="3000" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="1500" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="3000" unit="deg/s"/>
<define name="REF_OMEGA_R" value="1500" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="90." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
conf/airframes/examples/stm32f4_discovery_test.xml
+3 -3
View File
@@ -142,17 +142,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
conf/airframes/fraser_lisa_m_rotorcraft.xml
+3 -3
View File
@@ -173,17 +173,17 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
conf/settings/control/stabilization_att_float_quat.xml
+7
View File
@@ -21,6 +21,13 @@
<dl_setting var="stabilization_gains[0].d.z" min="1" step="1" max="4000" module="stabilization/stabilization_attitude" shortname="dgain r" param="STABILIZATION_ATTITUDE_PSI_DGAIN"/>
<dl_setting var="stabilization_gains[0].rates_d.z" min="0" step="1" max="500" module="stabilization/stabilization_attitude" shortname="dgaind r" param="STABILIZATION_ATTITUDE_PHI_DGAIN_D"/>
<dl_setting var="stabilization_gains[0].dd.z" min="0" step="1" max="1000" module="stabilization/stabilization_attitude" shortname="ddgain r" param="STABILIZATION_ATTITUDE_PSI_DDGAIN"/>
<dl_setting var="stab_att_ref_model[0].omega.p" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_ref_quat_float" shortname="omega p" param="STABILIZATION_ATTITUDE_REF_OMEGA_P" handler="SetOmegaP"/>
<dl_setting var="stab_att_ref_model[0].zeta.p" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_ref_quat_float" shortname="zeta p" param="STABILIZATION_ATTITUDE_REF_ZETA_P"/>
<dl_setting var="stab_att_ref_model[0].omega.q" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_ref_quat_float" shortname="omega q" param="STABILIZATION_ATTITUDE_REF_OMEGA_Q" handler="SetOmegaQ"/>
<dl_setting var="stab_att_ref_model[0].zeta.q" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_ref_quat_float" shortname="zeta q" param="STABILIZATION_ATTITUDE_REF_ZETA_Q"/>
<dl_setting var="stab_att_ref_model[0].omega.r" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_ref_quat_float" shortname="omega r" param="STABILIZATION_ATTITUDE_REF_OMEGA_P" handler="SetOmegaR"/>
<dl_setting var="stab_att_ref_model[0].zeta.r" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_ref_quat_float" shortname="zeta r" param="STABILIZATION_ATTITUDE_REF_ZETA_R"/>
</dl_settings>
</dl_settings>
conf/settings/control/stabilization_att_int_quat.xml
+29
View File
@@ -0,0 +1,29 @@
<!DOCTYPE settings SYSTEM "../settings.dtd">
<settings>
<dl_settings>
<dl_settings NAME="Att Loop">
<dl_setting var="stabilization_gains.p.x" min="1" step="1" max="8000" module="stabilization/stabilization_attitude_common_int" shortname="pgain phi" param="STABILIZATION_ATTITUDE_PHI_PGAIN"/>
<dl_setting var="stabilization_gains.d.x" min="1" step="1" max="4000" module="stabilization/stabilization_attitude_common_int" shortname="dgain p" param="STABILIZATION_ATTITUDE_PHI_DGAIN"/>
<dl_setting var="stabilization_gains.i.x" min="0" step="1" max="800" module="stabilization/stabilization_attitude_common_int" shortname="igain phi" handler="SetKiPhi"param="STABILIZATION_ATTITUDE_PHI_IGAIN" />
<dl_setting var="stabilization_gains.dd.x" min="0" step="1" max="1000" module="stabilization/stabilization_attitude_common_int" shortname="ddgain p" param="STABILIZATION_ATTITUDE_PHI_DDGAIN"/>
<dl_setting var="stabilization_gains.p.y" min="1" step="1" max="8000" module="stabilization/stabilization_attitude_common_int" shortname="pgain theta" param="STABILIZATION_ATTITUDE_THETA_PGAIN"/>
<dl_setting var="stabilization_gains.d.y" min="1" step="1" max="4000" module="stabilization/stabilization_attitude_common_int" shortname="dgain q" param="STABILIZATION_ATTITUDE_THETA_DGAIN"/>
<dl_setting var="stabilization_gains.i.y" min="0" step="1" max="800" module="stabilization/stabilization_attitude_common_int" shortname="igain theta" param="STABILIZATION_ATTITUDE_THETA_IGAIN"/>
<dl_setting var="stabilization_gains.dd.y" min="0" step="1" max="1000" module="stabilization/stabilization_attitude_common_int" shortname="ddgain q" param="STABILIZATION_ATTITUDE_THETA_DDGAIN"/>
<dl_setting var="stabilization_gains.p.z" min="1" step="1" max="4000" module="stabilization/stabilization_attitude_common_int" shortname="pgain psi" param="STABILIZATION_ATTITUDE_PSI_PGAIN"/>
<dl_setting var="stabilization_gains.d.z" min="1" step="1" max="4000" module="stabilization/stabilization_attitude_common_int" shortname="dgain r" param="STABILIZATION_ATTITUDE_PSI_DGAIN"/>
<dl_setting var="stabilization_gains.i.z" min="0" step="1" max="400" module="stabilization/stabilization_attitude_common_int" shortname="igain psi" param="STABILIZATION_ATTITUDE_PSI_IGAIN"/>
<dl_setting var="stabilization_gains.dd.z" min="0" step="1" max="1000" module="stabilization/stabilization_attitude_common_int" shortname="ddgain r" param="STABILIZATION_ATTITUDE_PSI_DDGAIN"/>
<dl_setting var="stab_att_ref_model.omega.p" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_ref_quat_int" shortname="omega p" param="STABILIZATION_ATTITUDE_REF_OMEGA_P" handler="SetOmegaP"/>
<dl_setting var="stab_att_ref_model.zeta.p" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_ref_quat_int" shortname="zeta p" param="STABILIZATION_ATTITUDE_REF_ZETA_P" handler="SetZetaP"/>
<dl_setting var="stab_att_ref_model.omega.q" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_ref_quat_int" shortname="omega q" param="STABILIZATION_ATTITUDE_REF_OMEGA_Q" handler="SetOmegaQ"/>
<dl_setting var="stab_att_ref_model.zeta.q" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_ref_quat_int" shortname="zeta q" param="STABILIZATION_ATTITUDE_REF_ZETA_Q" handler="SetZetaQ"/>
<dl_setting var="stab_att_ref_model.omega.r" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_ref_quat_int" shortname="omega r" param="STABILIZATION_ATTITUDE_REF_OMEGA_P" handler="SetOmegaR"/>
<dl_setting var="stab_att_ref_model.zeta.r" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_ref_quat_int" shortname="zeta r" param="STABILIZATION_ATTITUDE_REF_ZETA_R" handler="SetZetaR"/>
</dl_settings>
</dl_settings>
</settings>
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
+4 -11
View File
@@ -29,22 +29,15 @@
#include "math/pprz_algebra_int.h"
#define REF_ACCEL_FRAC 12
#define REF_RATE_FRAC 16
#define REF_ANGLE_FRAC 20
extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
extern struct Int32Eulers stab_att_ref_euler; ///< with #REF_ANGLE_FRAC
extern struct Int32Rates stab_att_ref_rate; ///< with #REF_RATE_FRAC
extern struct Int32Rates stab_att_ref_accel; ///< with #REF_ACCEL_FRAC
struct Int32RefModel {
struct Int32Rates omega;
struct Int32Rates zeta;
};
extern struct Int32RefModel stab_att_ref_model;
#define REF_ACCEL_FRAC 12
#define REF_RATE_FRAC 16
#define REF_ANGLE_FRAC 20
extern void stabilization_attitude_ref_init(void);
extern void stabilization_attitude_ref_update(void);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
+36 -4
View File
@@ -58,6 +58,8 @@ static const float zeta_q[] = STABILIZATION_ATTITUDE_REF_ZETA_Q;
static const float omega_r[] = STABILIZATION_ATTITUDE_REF_OMEGA_R;
static const float zeta_r[] = STABILIZATION_ATTITUDE_REF_ZETA_R;
struct FloatRates two_omega_squared[STABILIZATION_ATTITUDE_GAIN_NB];
static inline void reset_psi_ref_from_body(void) {
//sp has been set from body using stabilization_attitude_get_yaw_f, use that value
stab_att_ref_euler.psi = stab_att_sp_euler.psi;
@@ -72,6 +74,34 @@ static inline void update_ref_quat_from_eulers(void) {
FLOAT_QUAT_WRAP_SHORTEST(stab_att_ref_quat);
}
void stabilization_attitude_ref_idx_set_omega_p(uint8_t idx, float omega) {
stab_att_ref_model[i].omega.p = omega;
two_omega_squared[i].p = 2 * omega * omega;
}
void stabilization_attitude_ref_idx_set_omega_q(uint8_t idx, float omega) {
stab_att_ref_model[i].omega.q = omega;
two_omega_squared[i].q = 2 * omega * omega;
}
void stabilization_attitude_ref_idx_set_omega_r(uint8_t idx, float omega) {
stab_att_ref_model[i].omega.r = omega;
two_omega_squared[i].r = 2 * omega * omega;
}
void stabilization_attitude_ref_set_omega_p(float omega) {
stabilization_attitude_ref_idx_set_omega_p(ref_idx, omega);
}
void stabilization_attitude_ref_set_omega_q(float omega) {
stabilization_attitude_ref_idx_set_omega_q(ref_idx, omega);
}
void stabilization_attitude_ref_set_omega_r(float omega) {
stabilization_attitude_ref_idx_set_omega_r(ref_idx, omega);
}
void stabilization_attitude_ref_init(void) {
FLOAT_EULERS_ZERO(stab_att_sp_euler);
@@ -84,6 +114,7 @@ void stabilization_attitude_ref_init(void) {
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
RATES_ASSIGN(stab_att_ref_model[i].omega, omega_p[i], omega_q[i], omega_r[i]);
RATES_ASSIGN(stab_att_ref_model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]);
RATES_ASSIGN(two_omega_squared[i], 2*omega_p[i]*omega_p[i], 2*omega_q[i]*omega_q[i], 2*omega_r[i]*omega_r[i]);
}
}
@@ -136,13 +167,14 @@ void stabilization_attitude_ref_update(void) {
FLOAT_QUAT_INV_COMP(err, stab_att_sp_quat, stab_att_ref_quat);
/* wrap it in the shortest direction */
FLOAT_QUAT_WRAP_SHORTEST(err);
/* propagate the 2nd order linear model */
/* propagate the 2nd order linear model: xdotdot = -2*zeta*omega*xdot - omega^2*x */
/* since error quaternion contains the half-angles we get 2*omega^2*err */
stab_att_ref_accel.p = -2.*stab_att_ref_model[ref_idx].zeta.p*stab_att_ref_model[ref_idx].omega.p*stab_att_ref_rate.p
- stab_att_ref_model[ref_idx].omega.p*stab_att_ref_model[ref_idx].omega.p*err.qx;
- two_omega_squared[ref_idx].p * err.qx;
stab_att_ref_accel.q = -2.*stab_att_ref_model[ref_idx].zeta.q*stab_att_ref_model[ref_idx].omega.q*stab_att_ref_rate.q
- stab_att_ref_model[ref_idx].omega.q*stab_att_ref_model[ref_idx].omega.q*err.qy;
- two_omega_squared[ref_idx].q * err.qy;
stab_att_ref_accel.r = -2.*stab_att_ref_model[ref_idx].zeta.r*stab_att_ref_model[ref_idx].omega.r*stab_att_ref_rate.r
- stab_att_ref_model[ref_idx].omega.r*stab_att_ref_model[ref_idx].omega.r*err.qz;
- two_omega_squared[ref_idx].r * err.qz;
/* saturate acceleration */
const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h
+17
View File
@@ -43,4 +43,21 @@
void stabilization_attitude_ref_enter(void);
void stabilization_attitude_ref_schedule(uint8_t idx);
extern void stabilization_attitude_ref_idx_set_omega_p(uint8_t idx, float omega);
extern void stabilization_attitude_ref_idx_set_omega_q(uint8_t idx, float omega);
extern void stabilization_attitude_ref_idx_set_omega_r(uint8_t idx, float omega);
extern void stabilization_attitude_ref_set_omega_p(float omega);
extern void stabilization_attitude_ref_set_omega_q(float omega);
extern void stabilization_attitude_ref_set_omega_r(float omega);
#define stabilization_attitude_ref_quat_float_SetOmegaP(_val) { \
stabilization_attitude_ref_set_omega_p(_val); \
}
#define stabilization_attitude_ref_quat_float_SetOmegaQ(_val) { \
stabilization_attitude_ref_set_omega_q(_val); \
}
#define stabilization_attitude_ref_quat_float_SetOmegaR(_val) { \
stabilization_attitude_ref_set_omega_r(_val); \
}
#endif /* STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_FLOAT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
+81 -35
View File
@@ -39,27 +39,6 @@
#define REF_RATE_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_Q, REF_RATE_FRAC)
#define REF_RATE_MAX_R BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_R, REF_RATE_FRAC)
#define OMEGA_P STABILIZATION_ATTITUDE_REF_OMEGA_P
#define ZETA_P STABILIZATION_ATTITUDE_REF_ZETA_P
#define ZETA_OMEGA_P_RES 10
#define ZETA_OMEGA_P BFP_OF_REAL((ZETA_P*OMEGA_P), ZETA_OMEGA_P_RES)
#define OMEGA_2_P_RES 7
#define OMEGA_2_P BFP_OF_REAL((OMEGA_P*OMEGA_P), OMEGA_2_P_RES)
#define OMEGA_Q STABILIZATION_ATTITUDE_REF_OMEGA_Q
#define ZETA_Q STABILIZATION_ATTITUDE_REF_ZETA_Q
#define ZETA_OMEGA_Q_RES 10
#define ZETA_OMEGA_Q BFP_OF_REAL((ZETA_Q*OMEGA_Q), ZETA_OMEGA_Q_RES)
#define OMEGA_2_Q_RES 7
#define OMEGA_2_Q BFP_OF_REAL((OMEGA_Q*OMEGA_Q), OMEGA_2_Q_RES)
#define OMEGA_R STABILIZATION_ATTITUDE_REF_OMEGA_R
#define ZETA_R STABILIZATION_ATTITUDE_REF_ZETA_R
#define ZETA_OMEGA_R_RES 10
#define ZETA_OMEGA_R BFP_OF_REAL((ZETA_R*OMEGA_R), ZETA_OMEGA_R_RES)
#define OMEGA_2_R_RES 7
#define OMEGA_2_R BFP_OF_REAL((OMEGA_R*OMEGA_R), OMEGA_2_R_RES)
struct Int32Eulers stab_att_sp_euler;
struct Int32Quat stab_att_sp_quat;
@@ -68,11 +47,81 @@ struct Int32Quat stab_att_ref_quat;
struct Int32Rates stab_att_ref_rate;
struct Int32Rates stab_att_ref_accel;
struct Int32RefModel stab_att_ref_model = {
struct FloatRefModel stab_att_ref_model = {
{STABILIZATION_ATTITUDE_REF_OMEGA_P, STABILIZATION_ATTITUDE_REF_OMEGA_Q, STABILIZATION_ATTITUDE_REF_OMEGA_R},
{STABILIZATION_ATTITUDE_REF_ZETA_P, STABILIZATION_ATTITUDE_REF_ZETA_Q, STABILIZATION_ATTITUDE_REF_ZETA_R}
};
#define TWO_ZETA_OMEGA_RES 10
#define TWO_OMEGA_2_RES 7
static struct Int32Rates two_zeta_omega;
static struct Int32Rates two_omega_2;
static void update_ref_model_p(void) {
two_zeta_omega.p = BFP_OF_REAL((2*stab_att_ref_model.zeta.p * stab_att_ref_model.omega.p), TWO_ZETA_OMEGA_RES);
two_omega_2.p = BFP_OF_REAL((2*stab_att_ref_model.omega.p * stab_att_ref_model.omega.p), TWO_OMEGA_2_RES);
}
static void update_ref_model_q(void) {
two_zeta_omega.q = BFP_OF_REAL((2*stab_att_ref_model.zeta.q * stab_att_ref_model.omega.q), TWO_ZETA_OMEGA_RES);
two_omega_2.q = BFP_OF_REAL((2*stab_att_ref_model.omega.q * stab_att_ref_model.omega.q), TWO_OMEGA_2_RES);
}
static void update_ref_model_r(void) {
two_zeta_omega.r = BFP_OF_REAL((2*stab_att_ref_model.zeta.r * stab_att_ref_model.omega.r), TWO_ZETA_OMEGA_RES);
two_omega_2.r = BFP_OF_REAL((2*stab_att_ref_model.omega.r * stab_att_ref_model.omega.r), TWO_OMEGA_2_RES);
}
static void update_ref_model(void) {
update_ref_model_p();
update_ref_model_q();
update_ref_model_r();
}
void stabilization_attitude_ref_set_omega_p(float omega_p) {
stab_att_ref_model.omega.p = omega_p;
update_ref_model_p();
}
void stabilization_attitude_ref_set_omega_q(float omega_q) {
stab_att_ref_model.omega.q = omega_q;
update_ref_model_q();
}
void stabilization_attitude_ref_set_omega_r(float omega_r) {
stab_att_ref_model.omega.r = omega_r;
update_ref_model_r();
}
void stabilization_attitude_ref_set_omega(struct FloatRates *omega) {
stabilization_attitude_ref_set_omega_p(omega->p);
stabilization_attitude_ref_set_omega_q(omega->q);
stabilization_attitude_ref_set_omega_r(omega->r);
}
void stabilization_attitude_ref_set_zeta_p(float zeta_p) {
stab_att_ref_model.zeta.p = zeta_p;
update_ref_model_p();
}
void stabilization_attitude_ref_set_zeta_q(float zeta_q) {
stab_att_ref_model.zeta.q = zeta_q;
update_ref_model_q();
}
void stabilization_attitude_ref_set_zeta_r(float zeta_r) {
stab_att_ref_model.zeta.r = zeta_r;
update_ref_model_r();
}
void stabilization_attitude_ref_set_zeta(struct FloatRates *zeta) {
stabilization_attitude_ref_set_zeta_p(zeta->p);
stabilization_attitude_ref_set_zeta_q(zeta->q);
stabilization_attitude_ref_set_zeta_r(zeta->r);
}
static inline void reset_psi_ref_from_body(void) {
//sp has been set from body using stabilization_attitude_get_yaw_i, use that value
stab_att_ref_euler.psi = stab_att_sp_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
@@ -89,12 +138,7 @@ void stabilization_attitude_ref_init(void) {
INT_RATES_ZERO(stab_att_ref_rate);
INT_RATES_ZERO(stab_att_ref_accel);
/*
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
RATES_ASSIGN(stab_att_ref_model[i].omega, omega_p[i], omega_q[i], omega_r[i]);
RATES_ASSIGN(stab_att_ref_model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]);
}
*/
update_ref_model();
}
@@ -157,17 +201,19 @@ void stabilization_attitude_ref_update(void) {
INT32_QUAT_INV_COMP(err, stab_att_sp_quat, stab_att_ref_quat);
/* wrap it in the shortest direction */
INT32_QUAT_WRAP_SHORTEST(err);
/* propagate the 2nd order linear model */
/* propagate the 2nd order linear model : accel = -2*zeta*omega * rate - omega^2 * angle */
const struct Int32Rates accel_rate = {
((int32_t)(-2.*ZETA_OMEGA_P) * (stab_att_ref_rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (ZETA_OMEGA_P_RES),
((int32_t)(-2.*ZETA_OMEGA_Q) * (stab_att_ref_rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (ZETA_OMEGA_Q_RES),
((int32_t)(-2.*ZETA_OMEGA_R) * (stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (ZETA_OMEGA_R_RES) };
(-two_zeta_omega.p * (stab_att_ref_rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (TWO_ZETA_OMEGA_RES),
(-two_zeta_omega.q * (stab_att_ref_rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (TWO_ZETA_OMEGA_RES),
(-two_zeta_omega.r * (stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (TWO_ZETA_OMEGA_RES) };
/* since error quaternion contains the half-angles we get 2*omega^2*err */
const struct Int32Rates accel_angle = {
((int32_t)(-OMEGA_2_P) * (err.qx >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES),
((int32_t)(-OMEGA_2_Q) * (err.qy >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES),
((int32_t)(-OMEGA_2_R) * (err.qz >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) };
(-two_omega_2.p * (err.qx >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES),
(-two_omega_2.q * (err.qy >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES),
(-two_omega_2.r * (err.qz >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES) };
RATES_SUM(stab_att_ref_accel, accel_rate, accel_angle);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h
+43 -3
View File
@@ -27,8 +27,8 @@
*
*/
#ifndef STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H
#define STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H
#ifndef STABILIZATION_ATTITUDE_REF_QUAT_INT_H
#define STABILIZATION_ATTITUDE_REF_QUAT_INT_H
#include "stabilization_attitude_ref_int.h"
@@ -37,4 +37,44 @@ extern struct Int32Quat stab_att_ref_quat; ///< with #INT32_QUAT_FRAC
void stabilization_attitude_ref_enter(void);
#endif /* STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H */
/* ref model is in float and then used to precompute ref values in int */
#include "math/pprz_algebra_float.h"
struct FloatRefModel {
struct FloatRates omega;
struct FloatRates zeta;
};
extern struct FloatRefModel stab_att_ref_model;
extern void stabilization_attitude_ref_set_omega(struct FloatRates *omega);
extern void stabilization_attitude_ref_set_omega_p(float omega_p);
extern void stabilization_attitude_ref_set_omega_q(float omega_q);
extern void stabilization_attitude_ref_set_omega_r(float omega_r);
extern void stabilization_attitude_ref_set_zeta(struct FloatRates *zeta);
extern void stabilization_attitude_ref_set_zeta_p(float zeta_p);
extern void stabilization_attitude_ref_set_zeta_q(float zeta_q);
extern void stabilization_attitude_ref_set_zeta_r(float zeta_r);
#define stabilization_attitude_ref_quat_int_SetOmegaP(_val) { \
stabilization_attitude_ref_set_omega_p(_val); \
}
#define stabilization_attitude_ref_quat_int_SetOmegaQ(_val) { \
stabilization_attitude_ref_set_omega_q(_val); \
}
#define stabilization_attitude_ref_quat_int_SetOmegaR(_val) { \
stabilization_attitude_ref_set_omega_r(_val); \
}
#define stabilization_attitude_ref_quat_int_SetZetaP(_val) { \
stabilization_attitude_ref_set_zeta_p(_val); \
}
#define stabilization_attitude_ref_quat_int_SetZetaQ(_val) { \
stabilization_attitude_ref_set_zeta_q(_val); \
}
#define stabilization_attitude_ref_quat_int_SetZetaR(_val) { \
stabilization_attitude_ref_set_zeta_r(_val); \
}
#endif /* STABILIZATION_ATTITUDE_REF_QUAT_INT_H */
sw/airborne/test/stabilization/generated/airframe.h
+10 -10
View File
@@ -12,15 +12,15 @@
#define STABILIZATION_ATTITUDE_SP_MAX_THETA 0.7853981625
#define STABILIZATION_ATTITUDE_SP_MAX_R 1.570796325
#define STABILIZATION_ATTITUDE_REF_OMEGA_P {RadOfDeg(800)}
#define STABILIZATION_ATTITUDE_REF_OMEGA_P {RadOfDeg(400)}
#define STABILIZATION_ATTITUDE_REF_ZETA_P {0.85}
#define STABILIZATION_ATTITUDE_REF_MAX_P 6.981317
#define STABILIZATION_ATTITUDE_REF_MAX_PDOT RadOfDeg(8000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_Q {RadOfDeg(800)}
#define STABILIZATION_ATTITUDE_REF_MAX_PDOT RadOfDeg(4000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_Q {RadOfDeg(400)}
#define STABILIZATION_ATTITUDE_REF_ZETA_Q {0.85}
#define STABILIZATION_ATTITUDE_REF_MAX_Q 6.981317
#define STABILIZATION_ATTITUDE_REF_MAX_QDOT RadOfDeg(8000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_R {RadOfDeg(500)}
#define STABILIZATION_ATTITUDE_REF_MAX_QDOT RadOfDeg(4000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_R {RadOfDeg(250)}
#define STABILIZATION_ATTITUDE_REF_ZETA_R {0.85}
#define STABILIZATION_ATTITUDE_REF_MAX_R 3.14159265
#define STABILIZATION_ATTITUDE_REF_MAX_RDOT RadOfDeg(1800.)
@@ -32,15 +32,15 @@
#define STABILIZATION_ATTITUDE_SP_MAX_THETA 0.7853981625
#define STABILIZATION_ATTITUDE_SP_MAX_R 1.570796325
#define STABILIZATION_ATTITUDE_REF_OMEGA_P 13.962634
#define STABILIZATION_ATTITUDE_REF_OMEGA_P 6.981317
#define STABILIZATION_ATTITUDE_REF_ZETA_P 0.85
#define STABILIZATION_ATTITUDE_REF_MAX_P 6.981317
#define STABILIZATION_ATTITUDE_REF_MAX_PDOT RadOfDeg(8000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_Q 13.962634
#define STABILIZATION_ATTITUDE_REF_MAX_PDOT RadOfDeg(4000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_Q 6.981317
#define STABILIZATION_ATTITUDE_REF_ZETA_Q 0.85
#define STABILIZATION_ATTITUDE_REF_MAX_Q 6.981317
#define STABILIZATION_ATTITUDE_REF_MAX_QDOT RadOfDeg(8000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_R 8.72664625
#define STABILIZATION_ATTITUDE_REF_MAX_QDOT RadOfDeg(4000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_R 4.363323125
#define STABILIZATION_ATTITUDE_REF_ZETA_R 0.85
#define STABILIZATION_ATTITUDE_REF_MAX_R 3.14159265
#define STABILIZATION_ATTITUDE_REF_MAX_RDOT RadOfDeg(1800.)