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

Merge pull request #134 from paparazzi/rotorcraft_positive_control_gains

Browse Source 
Rotorcraft positive control gains
This commit is contained in:
Felix Ruess
2012-02-14 04:36:07 -08:00
parent dd15598389 ea47766125
commit fe3062931b
51 changed files with 1031 additions and 1000 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/CDW/debug_i2c.xml
+29 -29
View File
@@ -84,9 +84,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -115,17 +115,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- gaui props
<define name="PHI_PGAIN" value="-900"/>
<define name="PHI_DGAIN" value="-200"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="900"/>
<define name="PHI_DGAIN" value="200"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-900"/>
<define name="THETA_DGAIN" value="-200"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="900"/>
<define name="THETA_DGAIN" value="200"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="-900"/>
<define name="PSI_DGAIN" value="-200"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="900"/>
<define name="PSI_DGAIN" value="200"/>
<define name="PSI_IGAIN" value="10"/>
<define name="PHI_DDGAIN" value=" 200"/>
<define name="THETA_DDGAIN" value=" 200"/>
@@ -133,17 +133,17 @@
-->
<!-- feedback -->
<define name="PHI_PGAIN" value="-2000"/>
<define name="PHI_DGAIN" value="-400"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="2000"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-2000"/>
<define name="THETA_DGAIN" value="-400"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="2000"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="-2000"/>
<define name="PSI_DGAIN" value="-400"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="2000"/>
<define name="PSI_DGAIN" value="400"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -172,12 +172,12 @@
<define name="MAX_SUM_ERR" value="2000000"/>
<!--
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="200"/>
<define name="HOVER_KI" value="0"/>
-->
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="0"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
@@ -189,10 +189,10 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-15"/>
<define name="NGAIN" value="-0"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="15"/>
<define name="NGAIN" value="0"/>
</section>
<section name="MISC">
conf/airframes/ENAC/quadrotor/blender.xml
+18 -18
View File
@@ -142,9 +142,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -173,17 +173,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-160"/>
<define name="PHI_IGAIN" value="-40"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="160"/>
<define name="PHI_IGAIN" value="40"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-160"/>
<define name="THETA_IGAIN" value="-40"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="160"/>
<define name="THETA_IGAIN" value="40"/>
<define name="PSI_PGAIN" value="-1000"/>
<define name="PSI_DGAIN" value="-350"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="350"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -208,8 +208,8 @@
<define name="REF_MAX_ZDD" value=" 0.8*9.81"/>
<define name="REF_MIN_ZD" value="-1.5"/>
<define name="REF_MAX_ZD" value=" 1.5"/>
<define name="HOVER_KP" value="-400"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KP" value="400"/>
<define name="HOVER_KD" value="200"/>
<define name="HOVER_KI" value="0"/>
<!-- SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
@@ -218,10 +218,10 @@
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-140"/>
<define name="DGAIN" value="-90"/>
<define name="IGAIN" value="-30"/>
<define name="NGAIN" value="-0"/>
<define name="PGAIN" value="140"/>
<define name="DGAIN" value="90"/>
<define name="IGAIN" value="30"/>
<define name="NGAIN" value="0"/>
<!-- feedforward -->
<define name="AGAIN" value="100"/>
</section>
conf/airframes/ENAC/quadrotor/booz2_g1.xml
+18 -18
View File
@@ -105,9 +105,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -136,17 +136,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-2000"/>
<define name="PHI_DGAIN" value="-400"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="2000"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-2000"/>
<define name="THETA_DGAIN" value="-400"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="2000"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="-2000"/>
<define name="PSI_DGAIN" value="-400"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="2000"/>
<define name="PSI_DGAIN" value="400"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -170,8 +170,8 @@
<define name="REF_MAX_ZDD" value=" 0.5*9.81"/>
<define name="REF_MIN_ZD" value="-1.5"/>
<define name="REF_MAX_ZD" value=" 1.5"/>
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="0"/>
<!-- 1.5m/s for full stick : SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
@@ -182,10 +182,10 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-15"/>
<define name="NGAIN" value="-0"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="15"/>
<define name="NGAIN" value="0"/>
<!-- feedforward -->
<define name="AGAIN" value="100"/>
</section>
conf/airframes/ENAC/quadrotor/g1_vision.xml
+18 -18
View File
@@ -79,9 +79,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -110,17 +110,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-2000"/>
<define name="PHI_DGAIN" value="-400"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="2000"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-2000"/>
<define name="THETA_DGAIN" value="-400"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="2000"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="-1000"/>
<define name="PSI_DGAIN" value="-350"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="350"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -152,8 +152,8 @@
<define name="REF_MAX_ZDD" value=" 0.5*9.81"/>
<define name="REF_MIN_ZD" value="-1.5"/>
<define name="REF_MAX_ZD" value=" 1.5"/>
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="0"/>
<!-- 1.5m/s for full stick : SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
@@ -164,10 +164,10 @@
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
<define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-60"/>
<define name="IGAIN" value="-0"/>
<define name="NGAIN" value="-0"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="60"/>
<define name="IGAIN" value="0"/>
<define name="NGAIN" value="0"/>
<!-- feedforward -->
<define name="AGAIN" value="50"/>
</section>
conf/airframes/ENAC/quadrotor/mkk1-vision.xml
+21 -21
View File
@@ -99,9 +99,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -111,9 +111,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -143,17 +143,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-650"/>
<define name="PHI_DGAIN" value="-200"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="650"/>
<define name="PHI_DGAIN" value="200"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-650"/>
<define name="THETA_DGAIN" value="-200"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="650"/>
<define name="THETA_DGAIN" value="200"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-800"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-20"/>
<define name="PSI_PGAIN" value="800"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="20"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -186,8 +186,8 @@
<define name="REF_MAX_ZDD" value=" 0.5*9.81"/>
<define name="REF_MIN_ZD" value="-1.5"/>
<define name="REF_MAX_ZD" value=" 1.5"/>
<define name="HOVER_KP" value="-200"/>
<define name="HOVER_KD" value="-100"/>
<define name="HOVER_KP" value="200"/>
<define name="HOVER_KD" value="100"/>
<define name="HOVER_KI" value="0"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
@@ -197,10 +197,10 @@
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
<define name="PGAIN" value="-250"/>
<define name="DGAIN" value="-150"/>
<define name="IGAIN" value="-35"/>
<define name="NGAIN" value="-0"/>
<define name="PGAIN" value="250"/>
<define name="DGAIN" value="150"/>
<define name="IGAIN" value="35"/>
<define name="NGAIN" value="0"/>
<!-- feedforward -->
<define name="AGAIN" value="100"/>
</section>
conf/airframes/ENAC/quadrotor/mkk1.xml
+21 -21
View File
@@ -129,9 +129,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -141,9 +141,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -173,17 +173,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-650"/>
<define name="PHI_DGAIN" value="-200"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="650"/>
<define name="PHI_DGAIN" value="200"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-650"/>
<define name="THETA_DGAIN" value="-200"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="650"/>
<define name="THETA_DGAIN" value="200"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-800"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-20"/>
<define name="PSI_PGAIN" value="800"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="20"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -206,8 +206,8 @@
<define name="REF_MAX_ZDD" value=" 0.5*9.81"/>
<define name="REF_MIN_ZD" value="-1.5"/>
<define name="REF_MAX_ZD" value=" 1.5"/>
<define name="HOVER_KP" value="-200"/>
<define name="HOVER_KD" value="-100"/>
<define name="HOVER_KP" value="200"/>
<define name="HOVER_KD" value="100"/>
<define name="HOVER_KI" value="0"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
@@ -217,10 +217,10 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-60"/>
<define name="DGAIN" value="-120"/>
<define name="IGAIN" value="-10"/>
<define name="NGAIN" value="-0"/>
<define name="PGAIN" value="60"/>
<define name="DGAIN" value="120"/>
<define name="IGAIN" value="10"/>
<define name="NGAIN" value="0"/>
<!-- feedforward -->
<define name="AGAIN" value="100"/>
</section>
conf/airframes/ENAC/quadrotor/navgo.xml
+18 -18
View File
@@ -158,9 +158,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -189,17 +189,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-350"/>
<define name="PHI_DGAIN" value="-190"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="350"/>
<define name="PHI_DGAIN" value="190"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-350"/>
<define name="THETA_DGAIN" value="-190"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="350"/>
<define name="THETA_DGAIN" value="190"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-700"/>
<define name="PSI_DGAIN" value="-250"/>
<define name="PSI_IGAIN" value="-20"/>
<define name="PSI_PGAIN" value="700"/>
<define name="PSI_DGAIN" value="250"/>
<define name="PSI_IGAIN" value="20"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 70"/>
@@ -219,8 +219,8 @@
<define name="REF_MAX_ZDD" value=" 0.8*9.81"/>
<define name="REF_MIN_ZD" value="-1.5"/>
<define name="REF_MAX_ZD" value=" 1.5"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-250"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="250"/>
<define name="HOVER_KI" value="0"/>
<!-- SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
@@ -229,10 +229,10 @@
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-140"/>
<define name="DGAIN" value="-90"/>
<define name="IGAIN" value="-30"/>
<define name="NGAIN" value="-0"/>
<define name="PGAIN" value="140"/>
<define name="DGAIN" value="90"/>
<define name="IGAIN" value="30"/>
<define name="NGAIN" value="0"/>
<!-- feedforward -->
<define name="AGAIN" value="100"/>
</section>
conf/airframes/ENAC/quadrotor/nova1.xml
+21 -21
View File
@@ -100,9 +100,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -112,9 +112,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -144,17 +144,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-650"/>
<define name="PHI_DGAIN" value="-200"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="650"/>
<define name="PHI_DGAIN" value="200"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-650"/>
<define name="THETA_DGAIN" value="-200"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="650"/>
<define name="THETA_DGAIN" value="200"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-800"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-20"/>
<define name="PSI_PGAIN" value="800"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="20"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -177,8 +177,8 @@
<define name="REF_MAX_ZDD" value=" 0.5*9.81"/>
<define name="REF_MIN_ZD" value="-1.5"/>
<define name="REF_MAX_ZD" value=" 1.5"/>
<define name="HOVER_KP" value="-200"/>
<define name="HOVER_KD" value="-100"/>
<define name="HOVER_KP" value="200"/>
<define name="HOVER_KD" value="100"/>
<define name="HOVER_KI" value="0"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
@@ -188,10 +188,10 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-60"/>
<define name="DGAIN" value="-120"/>
<define name="IGAIN" value="-10"/>
<define name="NGAIN" value="-0"/>
<define name="PGAIN" value="60"/>
<define name="DGAIN" value="120"/>
<define name="IGAIN" value="10"/>
<define name="NGAIN" value="0"/>
<!-- feedforward -->
<define name="AGAIN" value="100"/>
</section>
conf/airframes/NoVa_L.xml
+29 -29
View File
@@ -112,9 +112,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -143,17 +143,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- gaui props
<define name="PHI_PGAIN" value="-900"/>
<define name="PHI_DGAIN" value="-200"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="900"/>
<define name="PHI_DGAIN" value="200"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-900"/>
<define name="THETA_DGAIN" value="-200"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="900"/>
<define name="THETA_DGAIN" value="200"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="-900"/>
<define name="PSI_DGAIN" value="-200"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="900"/>
<define name="PSI_DGAIN" value="200"/>
<define name="PSI_IGAIN" value="10"/>
<define name="PHI_DDGAIN" value=" 200"/>
<define name="THETA_DDGAIN" value=" 200"/>
@@ -161,17 +161,17 @@
-->
<!-- feedback -->
<define name="PHI_PGAIN" value="-2000"/>
<define name="PHI_DGAIN" value="-400"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="2000"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-2000"/>
<define name="THETA_DGAIN" value="-400"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="2000"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="-2000"/>
<define name="PSI_DGAIN" value="-400"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="2000"/>
<define name="PSI_DGAIN" value="400"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -200,12 +200,12 @@
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="200"/>
<define name="HOVER_KI" value="0"/>
<!--
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="0"/>
-->
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
@@ -217,10 +217,10 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-15"/>
<define name="NGAIN" value="-0"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="15"/>
<define name="NGAIN" value="0"/>
</section>
<section name="MISC">
conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml
+27 -27
View File
@@ -76,9 +76,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -108,30 +108,30 @@
<!-- feedback gains for better endurance -->
<!--
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-75"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="75"/>
-->
<!-- feedback gains for better performance -->
<define name="PHI_PGAIN" value="-2000"/>
<define name="PHI_DGAIN" value="-400"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="2000"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-2000"/>
<define name="THETA_DGAIN" value="-400"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="2000"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="-1000"/>
<define name="PSI_DGAIN" value="-350"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="350"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -160,9 +160,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-250"/>
<define name="HOVER_KI" value="-100"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="250"/>
<define name="HOVER_KI" value="100"/>
<!-- SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- SPEED_BFP_OF_REAL(1.5) * 20% -->
@@ -171,9 +171,9 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml
+27 -27
View File
@@ -76,9 +76,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -109,31 +109,31 @@
<!-- feedback gains for better endurance -->
<!--
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-75"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="75"/>
-->
<!-- feedback gains for better performance -->
<define name="PHI_PGAIN" value="-2000"/>
<define name="PHI_DGAIN" value="-400"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="2000"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-2000"/>
<define name="THETA_DGAIN" value="-400"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="2000"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="-1000"/>
<define name="PSI_DGAIN" value="-350"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="350"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -178,9 +178,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-250"/>
<define name="HOVER_KI" value="-100"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="250"/>
<define name="HOVER_KI" value="100"/>
<!-- SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- SPEED_BFP_OF_REAL(1.5) * 20% -->
@@ -189,9 +189,9 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
conf/airframes/Poine/booz2_a1.xml
+18 -18
View File
@@ -90,9 +90,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -122,17 +122,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-75"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="75"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -151,9 +151,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KI" value="-100"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="200"/>
<define name="HOVER_KI" value="100"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
@@ -163,9 +163,9 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="BAT">
conf/airframes/Poine/booz2_a1p.xml
+18 -18
View File
@@ -88,9 +88,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -120,17 +120,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-75"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="75"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -155,9 +155,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KI" value="-100"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="200"/>
<define name="HOVER_KI" value="100"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
@@ -167,9 +167,9 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="BAT">
conf/airframes/Poine/booz2_a7.xml
+29 -29
View File
@@ -84,9 +84,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -115,17 +115,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- gaui props
<define name="PHI_PGAIN" value="-900"/>
<define name="PHI_DGAIN" value="-200"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="900"/>
<define name="PHI_DGAIN" value="200"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-900"/>
<define name="THETA_DGAIN" value="-200"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="900"/>
<define name="THETA_DGAIN" value="200"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="-900"/>
<define name="PSI_DGAIN" value="-200"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="900"/>
<define name="PSI_DGAIN" value="200"/>
<define name="PSI_IGAIN" value="10"/>
<define name="PHI_DDGAIN" value=" 200"/>
<define name="THETA_DDGAIN" value=" 200"/>
@@ -133,17 +133,17 @@
-->
<!-- feedback -->
<define name="PHI_PGAIN" value="-2000"/>
<define name="PHI_DGAIN" value="-400"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="2000"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-2000"/>
<define name="THETA_DGAIN" value="-400"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="2000"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="-2000"/>
<define name="PSI_DGAIN" value="-400"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="2000"/>
<define name="PSI_DGAIN" value="400"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -172,12 +172,12 @@
<define name="MAX_SUM_ERR" value="2000000"/>
<!--
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="200"/>
<define name="HOVER_KI" value="0"/>
-->
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="0"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
@@ -189,10 +189,10 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-15"/>
<define name="NGAIN" value="-0"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="15"/>
<define name="NGAIN" value="0"/>
</section>
<section name="MISC">
conf/airframes/Poine/h_hex.xml
+18 -18
View File
@@ -86,9 +86,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -117,17 +117,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-75"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="75"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -146,9 +146,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KI" value="-100"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="200"/>
<define name="HOVER_KI" value="100"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
@@ -158,9 +158,9 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="MISC">
conf/airframes/UofAdelaide/A1000_BOOZ.xml
+24 -24
View File
@@ -152,13 +152,13 @@ second attempt
<define name="REF_TAU" value="4" />
<!-- feedback -->
<define name="GAIN_P" value="-400" />
<define name="GAIN_Q" value="-400" />
<define name="GAIN_R" value="-350" />
<define name="GAIN_P" value="400" />
<define name="GAIN_Q" value="400" />
<define name="GAIN_R" value="350" />
<define name="IGAIN_P" value="-75" />
<define name="IGAIN_Q" value="-75" />
<define name="IGAIN_R" value="-50" />
<define name="IGAIN_P" value="75" />
<define name="IGAIN_Q" value="75" />
<define name="IGAIN_R" value="50" />
<!-- feedforward -->
<define name="DDGAIN_P" value="300" />
@@ -166,9 +166,9 @@ second attempt
<define name="DDGAIN_R" value="300" />
<!-- original values
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/> -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/> -->
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
@@ -196,17 +196,17 @@ second attempt
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-75"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="75"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -225,9 +225,9 @@ second attempt
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KI" value="-100"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="200"/>
<define name="HOVER_KI" value="100"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
@@ -236,9 +236,9 @@ second attempt
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="BAT">
conf/airframes/UofAdelaide/A1000_LISA.xml
+25 -25
View File
@@ -84,9 +84,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -115,13 +115,13 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback
<define name="PHI_PGAIN" value="-5000"/>
<define name="PHI_DGAIN" value="-400"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="5000"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-5000"/>
<define name="THETA_DGAIN" value="-400"/>
<define name="THETA_IGAIN" value="-400"/>
<define name="THETA_PGAIN" value="5000"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="400"/>
<define name="PSI_PGAIN" value="2000"/>
<define name="PSI_DGAIN" value="350"/>
@@ -130,20 +130,20 @@
feedforward
<define name="PHI_DDGAIN" value=" 300"/>
<define name="THETA_DDGAIN" value=" 300"/>
<define name="PSI_DDGAIN" value=" -300"/> -->
<define name="PSI_DDGAIN" value="300"/> -->
<!-- feedback -->
<define name="PHI_PGAIN" value="-1500"/>
<define name="PHI_DGAIN" value="-30"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="1500"/>
<define name="PHI_DGAIN" value="30"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-1500"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="1500"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-1000"/>
<define name="PSI_DGAIN" value="-300"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="300"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -162,9 +162,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KI" value="-100"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="200"/>
<define name="HOVER_KI" value="100"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
@@ -174,9 +174,9 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="MISC">
conf/airframes/UofAdelaide/A1000_NOVA.xml
+31 -31
View File
@@ -79,13 +79,13 @@
<define name="REF_TAU" value="4" />
<!-- feedback -->
<define name="GAIN_P" value="-400" />
<define name="GAIN_Q" value="-400" />
<define name="GAIN_R" value="-350" />
<define name="GAIN_P" value="400" />
<define name="GAIN_Q" value="400" />
<define name="GAIN_R" value="350" />
<define name="IGAIN_P" value="-75" />
<define name="IGAIN_Q" value="-75" />
<define name="IGAIN_R" value="-50" />
<define name="IGAIN_P" value="75" />
<define name="IGAIN_Q" value="75" />
<define name="IGAIN_R" value="50" />
<!-- feedforward -->
<define name="DDGAIN_P" value="300" />
@@ -119,30 +119,30 @@
<!-- feedback -->
<!--
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-75"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="75"/>
-->
<define name="PHI_PGAIN" value="-1000"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="1000"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-1000"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="1000"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-1000"/>
<define name="PSI_DGAIN" value="-350"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="350"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -185,8 +185,8 @@
<define name="REF_MAX_ZDD" value=" 0.5*9.81"/>
<define name="REF_MIN_ZD" value="-1.5"/>
<define name="REF_MAX_ZD" value=" 1.5"/>
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="0"/>
<!-- 1.5m/s for full stick : SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
@@ -196,11 +196,11 @@
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-60"/>
<define name="IGAIN" value="-15"/>
<define name="NGAIN" value="-0"/>
<define name="AGAIN" value="-0"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="60"/>
<define name="IGAIN" value="15"/>
<define name="NGAIN" value="0"/>
<define name="AGAIN" value="0"/>
</section>
<section name="BAT">
conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
+31 -31
View File
@@ -79,13 +79,13 @@
<define name="REF_TAU" value="4" />
<!-- feedback -->
<define name="GAIN_P" value="-400" />
<define name="GAIN_Q" value="-400" />
<define name="GAIN_R" value="-350" />
<define name="GAIN_P" value="400" />
<define name="GAIN_Q" value="400" />
<define name="GAIN_R" value="350" />
<define name="IGAIN_P" value="-75" />
<define name="IGAIN_Q" value="-75" />
<define name="IGAIN_R" value="-50" />
<define name="IGAIN_P" value="75" />
<define name="IGAIN_Q" value="75" />
<define name="IGAIN_R" value="50" />
<!-- feedforward -->
<define name="DDGAIN_P" value="300" />
@@ -119,30 +119,30 @@
<!-- feedback -->
<!--
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-75"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="75"/>
-->
<define name="PHI_PGAIN" value="-1000"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="1000"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-1000"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="1000"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-1000"/>
<define name="PSI_DGAIN" value="-350"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="350"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -185,8 +185,8 @@
<define name="REF_MAX_ZDD" value=" 0.5*9.81"/>
<define name="REF_MIN_ZD" value="-1.5"/>
<define name="REF_MAX_ZD" value=" 1.5"/>
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="0"/>
<!-- 1.5m/s for full stick : SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
@@ -196,11 +196,11 @@
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-60"/>
<define name="IGAIN" value="-15"/>
<define name="NGAIN" value="-0"/>
<define name="AGAIN" value="-0"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="60"/>
<define name="IGAIN" value="15"/>
<define name="NGAIN" value="0"/>
<define name="AGAIN" value="0"/>
</section>
<section name="BAT">
conf/airframes/UofAdelaide/booz2_a1000.xml
+24 -24
View File
@@ -151,13 +151,13 @@ second attempt
<define name="REF_TAU" value="4" />
<!-- feedback -->
<define name="GAIN_P" value="-400" />
<define name="GAIN_Q" value="-400" />
<define name="GAIN_R" value="-350" />
<define name="GAIN_P" value="400" />
<define name="GAIN_Q" value="400" />
<define name="GAIN_R" value="350" />
<define name="IGAIN_P" value="-75" />
<define name="IGAIN_Q" value="-75" />
<define name="IGAIN_R" value="-50" />
<define name="IGAIN_P" value="75" />
<define name="IGAIN_Q" value="75" />
<define name="IGAIN_R" value="50" />
<!-- feedforward -->
<define name="DDGAIN_P" value="300" />
@@ -165,9 +165,9 @@ second attempt
<define name="DDGAIN_R" value="300" />
<!-- original values
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/> -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/> -->
</section>
@@ -196,17 +196,17 @@ second attempt
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-75"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="75"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -226,9 +226,9 @@ second attempt
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KI" value="-100"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="200"/>
<define name="HOVER_KI" value="100"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
@@ -238,9 +238,9 @@ second attempt
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="BAT">
conf/airframes/UofAdelaide/lisa_a1000.xml
+18 -18
View File
@@ -81,9 +81,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -112,17 +112,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-2000"/>
<define name="PHI_DGAIN" value="-400"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="2000"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-2000"/>
<define name="THETA_DGAIN" value="-400"/>
<define name="THETA_IGAIN" value="-400"/>
<define name="THETA_PGAIN" value="2000"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="400"/>
<define name="PSI_PGAIN" value="-1000"/>
<define name="PSI_DGAIN" value="-350"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="350"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -141,9 +141,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KI" value="-100"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="200"/>
<define name="HOVER_KI" value="100"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
@@ -153,9 +153,9 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="MISC">
conf/airframes/booz2_flixr.xml
+21 -21
View File
@@ -125,13 +125,13 @@
<define name="REF_TAU" value="4" />
<!-- feedback -->
<define name="GAIN_P" value="-400" />
<define name="GAIN_Q" value="-400" />
<define name="GAIN_R" value="-350" />
<define name="GAIN_P" value="400" />
<define name="GAIN_Q" value="400" />
<define name="GAIN_R" value="350" />
<define name="IGAIN_P" value="-75" />
<define name="IGAIN_Q" value="-75" />
<define name="IGAIN_R" value="-50" />
<define name="IGAIN_P" value="75" />
<define name="IGAIN_Q" value="75" />
<define name="IGAIN_R" value="50" />
<!-- feedforward -->
<define name="DDGAIN_P" value="300" />
@@ -165,17 +165,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)" />
<!-- feedback -->
<define name="PHI_PGAIN" value="-1000" />
<define name="PHI_DGAIN" value="-300" />
<define name="PHI_IGAIN" value="-100" />
<define name="PHI_PGAIN" value="1000" />
<define name="PHI_DGAIN" value="300" />
<define name="PHI_IGAIN" value="100" />
<define name="THETA_PGAIN" value="-1000" />
<define name="THETA_DGAIN" value="-300" />
<define name="THETA_IGAIN" value="-100" />
<define name="THETA_PGAIN" value="1000" />
<define name="THETA_DGAIN" value="300" />
<define name="THETA_IGAIN" value="100" />
<define name="PSI_PGAIN" value="-500" />
<define name="PSI_DGAIN" value="-320" />
<define name="PSI_IGAIN" value="-75" />
<define name="PSI_PGAIN" value="500" />
<define name="PSI_DGAIN" value="320" />
<define name="PSI_IGAIN" value="75" />
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300" />
@@ -194,9 +194,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)" />
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)" />
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-100"/>
<define name="HOVER_KD" value="-60"/>
<define name="HOVER_KI" value="-10"/>
<define name="HOVER_KP" value="100"/>
<define name="HOVER_KD" value="60"/>
<define name="HOVER_KI" value="10"/>
<!-- 1.5m/s for full stick : SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value="163" />
<!-- SPEED_BFP_OF_REAL(1.5) * 20% -->
@@ -205,9 +205,9 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-70"/>
<define name="DGAIN" value="-150"/>
<define name="IGAIN" value="-15"/>
<define name="PGAIN" value="70"/>
<define name="DGAIN" value="150"/>
<define name="IGAIN" value="15"/>
</section>
<section name="BAT">
conf/airframes/booz2_ppzuav.xml
+31 -31
View File
@@ -64,13 +64,13 @@
<define name="REF_TAU" value="4" />
<!-- feedback -->
<define name="GAIN_P" value="-400" />
<define name="GAIN_Q" value="-400" />
<define name="GAIN_R" value="-350" />
<define name="GAIN_P" value="400" />
<define name="GAIN_Q" value="400" />
<define name="GAIN_R" value="350" />
<define name="IGAIN_P" value="-75" />
<define name="IGAIN_Q" value="-75" />
<define name="IGAIN_R" value="-50" />
<define name="IGAIN_P" value="75" />
<define name="IGAIN_Q" value="75" />
<define name="IGAIN_R" value="50" />
<!-- feedforward -->
<define name="DDGAIN_P" value="300" />
@@ -104,30 +104,30 @@
<!-- feedback -->
<!--
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-75"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="75"/>
-->
<define name="PHI_PGAIN" value="-1000"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="1000"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-1000"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="1000"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-1000"/>
<define name="PSI_DGAIN" value="-350"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="350"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -160,8 +160,8 @@
<define name="REF_MAX_ZDD" value=" 0.5*9.81"/>
<define name="REF_MIN_ZD" value="-1.5"/>
<define name="REF_MAX_ZD" value=" 1.5"/>
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="0"/>
<!-- 1.5m/s for full stick : SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
@@ -171,11 +171,11 @@
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-60"/>
<define name="IGAIN" value="-15"/>
<define name="NGAIN" value="-0"/>
<define name="AGAIN" value="-0"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="60"/>
<define name="IGAIN" value="15"/>
<define name="NGAIN" value="0"/>
<define name="AGAIN" value="0"/>
</section>
<section name="BAT">
conf/airframes/esden/jt_lisam.xml
+18 -18
View File
@@ -75,9 +75,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -108,17 +108,17 @@
<define name="REF_MAX_PDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PSI_PGAIN" value="-750"/>
<define name="PSI_DGAIN" value="-370"/>
<define name="PSI_IGAIN" value="-100"/>
<define name="PSI_PGAIN" value="750"/>
<define name="PSI_DGAIN" value="370"/>
<define name="PSI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-800"/>
<define name="THETA_DGAIN" value="-240"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="800"/>
<define name="THETA_DGAIN" value="240"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PHI_PGAIN" value="-4000"/>
<define name="PHI_DGAIN" value="-600"/>
<define name="PHI_IGAIN" value="-10"/>
<define name="PHI_PGAIN" value="4000"/>
<define name="PHI_DGAIN" value="600"/>
<define name="PHI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -137,9 +137,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<define name="HOVER_KI" value="-20"/>
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="20"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
@@ -157,9 +157,9 @@
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
conf/airframes/esden/lisa_asctec.xml
+18 -18
View File
@@ -91,9 +91,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -122,17 +122,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-900"/>
<define name="PHI_DGAIN" value="-200"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="900"/>
<define name="PHI_DGAIN" value="200"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-900"/>
<define name="THETA_DGAIN" value="-200"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="900"/>
<define name="THETA_DGAIN" value="200"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="-900"/>
<define name="PSI_DGAIN" value="-200"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="900"/>
<define name="PSI_DGAIN" value="200"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 200"/>
@@ -151,9 +151,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KI" value="-100"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="200"/>
<define name="HOVER_KI" value="100"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
@@ -163,9 +163,9 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="MISC">
conf/airframes/esden/lisa_asctec_aspirin.xml
+18 -18
View File
@@ -91,9 +91,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -122,17 +122,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-900"/>
<define name="PHI_DGAIN" value="-200"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="900"/>
<define name="PHI_DGAIN" value="200"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-900"/>
<define name="THETA_DGAIN" value="-200"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="900"/>
<define name="THETA_DGAIN" value="200"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="-900"/>
<define name="PSI_DGAIN" value="-200"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="900"/>
<define name="PSI_DGAIN" value="200"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 200"/>
@@ -151,9 +151,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KI" value="-100"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="200"/>
<define name="HOVER_KI" value="100"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
@@ -163,9 +163,9 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="MISC">
conf/airframes/esden/lisa_m_pwm.xml
+18 -18
View File
@@ -105,9 +105,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -136,17 +136,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-1000"/>
<define name="PHI_DGAIN" value="-400"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="1000"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-1000"/>
<define name="THETA_DGAIN" value="-400"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="1000"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="-1000"/>
<define name="PSI_DGAIN" value="-400"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="400"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -165,9 +165,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<define name="HOVER_KI" value="-20"/>
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="20"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
@@ -183,9 +183,9 @@
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="MISC">
conf/airframes/esden/lisa_pwm_aspirin.xml
+18 -18
View File
@@ -79,9 +79,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -110,17 +110,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-1900"/>
<define name="PHI_DGAIN" value="-380"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="1900"/>
<define name="PHI_DGAIN" value="380"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-1900"/>
<define name="THETA_DGAIN" value="-380"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="1900"/>
<define name="THETA_DGAIN" value="380"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="-2000"/>
<define name="PSI_DGAIN" value="-400"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="2000"/>
<define name="PSI_DGAIN" value="400"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -146,9 +146,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<!--define name="HOVER_KI" value="-100"/-->
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<!--define name="HOVER_KI" value="100"/-->
<define name="HOVER_KI" value="0"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
@@ -158,9 +158,9 @@
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="MISC">
conf/airframes/esden/synerani_4B.xml
+18 -18
View File
@@ -82,9 +82,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -113,17 +113,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-1900"/>
<define name="PHI_DGAIN" value="-380"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="1900"/>
<define name="PHI_DGAIN" value="380"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-1900"/>
<define name="THETA_DGAIN" value="-380"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="1900"/>
<define name="THETA_DGAIN" value="380"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="-2000"/>
<define name="PSI_DGAIN" value="-400"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="2000"/>
<define name="PSI_DGAIN" value="400"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -149,9 +149,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<!--define name="HOVER_KI" value="-100"/-->
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<!--define name="HOVER_KI" value="100"/-->
<define name="HOVER_KI" value="0"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
@@ -161,9 +161,9 @@
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="MISC">
conf/airframes/mm/rotor/qmk1.xml
+17 -17
View File
@@ -91,9 +91,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -122,17 +122,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-2000"/>
<define name="PHI_DGAIN" value="-400"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="2000"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-2000"/>
<define name="THETA_DGAIN" value="-400"/>
<define name="THETA_IGAIN" value="-400"/>
<define name="THETA_PGAIN" value="2000"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="400"/>
<define name="PSI_PGAIN" value="-1000"/>
<define name="PSI_DGAIN" value="-350"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="350"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -152,8 +152,8 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="200"/>
<define name="HOVER_KI" value="0"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
@@ -164,9 +164,9 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="MISC">
conf/airframes/obsolete/booz2_Aron.xml
+18 -18
View File
@@ -83,9 +83,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -114,17 +114,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-75"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="75"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -145,9 +145,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-250"/>
<define name="HOVER_KI" value="-100"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="250"/>
<define name="HOVER_KI" value="100"/>
<!-- SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- SPEED_BFP_OF_REAL(1.5) * 20% -->
@@ -155,9 +155,9 @@
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="BAT">
conf/airframes/obsolete/booz2_NoVa.xml
+31 -31
View File
@@ -79,13 +79,13 @@
<define name="REF_TAU" value="4" />
<!-- feedback -->
<define name="GAIN_P" value="-400" />
<define name="GAIN_Q" value="-400" />
<define name="GAIN_R" value="-350" />
<define name="GAIN_P" value="400" />
<define name="GAIN_Q" value="400" />
<define name="GAIN_R" value="350" />
<define name="IGAIN_P" value="-75" />
<define name="IGAIN_Q" value="-75" />
<define name="IGAIN_R" value="-50" />
<define name="IGAIN_P" value="75" />
<define name="IGAIN_Q" value="75" />
<define name="IGAIN_R" value="50" />
<!-- feedforward -->
<define name="DDGAIN_P" value="300" />
@@ -119,30 +119,30 @@
<!-- feedback -->
<!--
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-75"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="75"/>
-->
<define name="PHI_PGAIN" value="-2000"/>
<define name="PHI_DGAIN" value="-400"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="PHI_PGAIN" value="2000"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="-2000"/>
<define name="THETA_DGAIN" value="-400"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="2000"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="-1000"/>
<define name="PSI_DGAIN" value="-350"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="350"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -185,8 +185,8 @@
<define name="REF_MAX_ZDD" value=" 0.5*9.81"/>
<define name="REF_MIN_ZD" value="-1.5"/>
<define name="REF_MAX_ZD" value=" 1.5"/>
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="0"/>
<!-- 1.5m/s for full stick : SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
@@ -196,11 +196,11 @@
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-70"/>
<define name="DGAIN" value="-150"/>
<define name="IGAIN" value="-15"/>
<define name="NGAIN" value="-0"/>
<define name="AGAIN" value="-0"/>
<define name="PGAIN" value="70"/>
<define name="DGAIN" value="150"/>
<define name="IGAIN" value="15"/>
<define name="NGAIN" value="0"/>
<define name="AGAIN" value="0"/>
</section>
<section name="BAT">
conf/airframes/obsolete/booz2_NoVa_001.xml
+31 -31
View File
@@ -79,13 +79,13 @@
<define name="REF_TAU" value="4" />
<!-- feedback -->
<define name="GAIN_P" value="-400" />
<define name="GAIN_Q" value="-400" />
<define name="GAIN_R" value="-350" />
<define name="GAIN_P" value="400" />
<define name="GAIN_Q" value="400" />
<define name="GAIN_R" value="350" />
<define name="IGAIN_P" value="-75" />
<define name="IGAIN_Q" value="-75" />
<define name="IGAIN_R" value="-50" />
<define name="IGAIN_P" value="75" />
<define name="IGAIN_Q" value="75" />
<define name="IGAIN_R" value="50" />
<!-- feedforward -->
<define name="DDGAIN_P" value="300" />
@@ -119,30 +119,30 @@
<!-- feedback -->
<!--
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-75"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="75"/>
-->
<define name="PHI_PGAIN" value="-1000"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="1000"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-1000"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="1000"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-1000"/>
<define name="PSI_DGAIN" value="-350"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="350"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -185,8 +185,8 @@
<define name="REF_MAX_ZDD" value=" 0.5*9.81"/>
<define name="REF_MIN_ZD" value="-1.5"/>
<define name="REF_MAX_ZD" value=" 1.5"/>
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="0"/>
<!-- 1.5m/s for full stick : SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
@@ -196,11 +196,11 @@
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-60"/>
<define name="IGAIN" value="-15"/>
<define name="NGAIN" value="-0"/>
<define name="AGAIN" value="-0"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="60"/>
<define name="IGAIN" value="15"/>
<define name="NGAIN" value="0"/>
<define name="AGAIN" value="0"/>
</section>
<section name="BAT">
conf/airframes/obsolete/booz2_NoVa_002.xml
+31 -31
View File
@@ -79,13 +79,13 @@
<define name="REF_TAU" value="4" />
<!-- feedback -->
<define name="GAIN_P" value="-400" />
<define name="GAIN_Q" value="-400" />
<define name="GAIN_R" value="-350" />
<define name="GAIN_P" value="400" />
<define name="GAIN_Q" value="400" />
<define name="GAIN_R" value="350" />
<define name="IGAIN_P" value="-75" />
<define name="IGAIN_Q" value="-75" />
<define name="IGAIN_R" value="-50" />
<define name="IGAIN_P" value="75" />
<define name="IGAIN_Q" value="75" />
<define name="IGAIN_R" value="50" />
<!-- feedforward -->
<define name="DDGAIN_P" value="300" />
@@ -119,30 +119,30 @@
<!-- feedback -->
<!--
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-75"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="75"/>
-->
<define name="PHI_PGAIN" value="-1000"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="1000"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-1000"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="1000"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-1000"/>
<define name="PSI_DGAIN" value="-350"/>
<define name="PSI_IGAIN" value="-10"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="350"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -185,8 +185,8 @@
<define name="REF_MAX_ZDD" value=" 0.5*9.81"/>
<define name="REF_MIN_ZD" value="-1.5"/>
<define name="REF_MAX_ZD" value=" 1.5"/>
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="0"/>
<!-- 1.5m/s for full stick : SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
@@ -196,11 +196,11 @@
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-70"/>
<define name="DGAIN" value="-150"/>
<define name="IGAIN" value="-15"/>
<define name="NGAIN" value="-0"/>
<define name="AGAIN" value="-0"/>
<define name="PGAIN" value="70"/>
<define name="DGAIN" value="150"/>
<define name="IGAIN" value="15"/>
<define name="NGAIN" value="0"/>
<define name="AGAIN" value="0"/>
</section>
<section name="BAT">
conf/airframes/obsolete/booz2_a2.xml
+18 -18
View File
@@ -77,9 +77,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -108,17 +108,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-75"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="75"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -145,9 +145,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-250"/>
<define name="HOVER_KI" value="-100"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="250"/>
<define name="HOVER_KI" value="100"/>
<!-- SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- SPEED_BFP_OF_REAL(1.5) * 20% -->
@@ -155,9 +155,9 @@
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="BAT">
conf/airframes/obsolete/booz2_a3.xml
+17 -17
View File
@@ -64,9 +64,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -95,17 +95,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-375"/>
<define name="PHI_DGAIN" value="-250"/>
<define name="PHI_IGAIN" value="-50"/>
<define name="PHI_PGAIN" value="375"/>
<define name="PHI_DGAIN" value="250"/>
<define name="PHI_IGAIN" value="50"/>
<define name="THETA_PGAIN" value="-375"/>
<define name="THETA_DGAIN" value="-250"/>
<define name="THETA_IGAIN" value="-50"/>
<define name="THETA_PGAIN" value="375"/>
<define name="THETA_DGAIN" value="250"/>
<define name="THETA_IGAIN" value="50"/>
<define name="PSI_PGAIN" value="-300"/>
<define name="PSI_DGAIN" value="-130"/>
<define name="PSI_IGAIN" value="-50"/>
<define name="PSI_PGAIN" value="300"/>
<define name="PSI_DGAIN" value="130"/>
<define name="PSI_IGAIN" value="50"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -131,8 +131,8 @@
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-250"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="250"/>
<!-- SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- SPEED_BFP_OF_REAL(1.5) * 20% -->
@@ -140,9 +140,9 @@
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="BAT">
conf/airframes/obsolete/booz2_a4.xml
+11 -11
View File
@@ -66,9 +66,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -84,10 +84,10 @@
<define name="PHI_THETA_DDGAIN" value=" 300"/>
<define name="PHI_THETA_IGAIN" value="-50"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_DDGAIN" value=" 300"/>
<define name="PSI_IGAIN" value="-50"/>
<define name="PSI_IGAIN" value="50"/>
</section>
@@ -109,8 +109,8 @@
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-250"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="250"/>
<!-- SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- SPEED_BFP_OF_REAL(1.5) * 20% -->
@@ -118,9 +118,9 @@
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="BAT">
conf/airframes/obsolete/booz2_a5.xml
+18 -18
View File
@@ -87,9 +87,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -118,17 +118,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-75"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="75"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -147,9 +147,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KI" value="-100"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="200"/>
<define name="HOVER_KI" value="100"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
@@ -159,9 +159,9 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="BAT">
conf/airframes/obsolete/booz2_s1.xml
+18 -18
View File
@@ -78,9 +78,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -110,17 +110,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-1200"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="1200"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-1200"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="1200"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-800"/>
<define name="PSI_DGAIN" value="-300"/>
<define name="PSI_IGAIN" value="-200"/>
<define name="PSI_PGAIN" value="800"/>
<define name="PSI_DGAIN" value="300"/>
<define name="PSI_IGAIN" value="200"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -138,9 +138,9 @@
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="HOVER_KP" value="-300"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KI" value="-0"/>
<define name="HOVER_KP" value="300"/>
<define name="HOVER_KD" value="200"/>
<define name="HOVER_KI" value="0"/>
<!-- 1.5m/s for full stick : SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- SPEED_BFP_OF_REAL(1.5) * 20% -->
@@ -151,9 +151,9 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="BAT">
conf/airframes/obsolete/booz2_x1.xml
+18 -18
View File
@@ -89,9 +89,9 @@
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="GAIN_P" value="-400"/>
<define name="GAIN_Q" value="-400"/>
<define name="GAIN_R" value="-350"/>
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>
@@ -121,17 +121,17 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-400"/>
<define name="PHI_DGAIN" value="-300"/>
<define name="PHI_IGAIN" value="-100"/>
<define name="PHI_PGAIN" value="400"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="-400"/>
<define name="THETA_DGAIN" value="-300"/>
<define name="THETA_IGAIN" value="-100"/>
<define name="THETA_PGAIN" value="400"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="-380"/>
<define name="PSI_DGAIN" value="-320"/>
<define name="PSI_IGAIN" value="-75"/>
<define name="PSI_PGAIN" value="380"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="75"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
@@ -150,9 +150,9 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KI" value="-100"/>
<define name="HOVER_KP" value="500"/>
<define name="HOVER_KD" value="200"/>
<define name="HOVER_KI" value="100"/>
<!-- 1.5m/s for full stick : SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- SPEED_BFP_OF_REAL(1.5) * 20% -->
@@ -161,9 +161,9 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
</section>
<section name="BAT">
conf/settings/settings_UofAdelaide.xml
+11 -11
View File
@@ -33,9 +33,9 @@
</dl_settings>
<dl_settings NAME="Rate Loop">
<dl_setting var="stabilization_rate_gain.p" min="-1000" step="1" max="-1" module="stabilization/stabilization_rate" shortname="gain p"/>
<dl_setting var="stabilization_rate_gain.q" min="-1000" step="1" max="-1" module="stabilization/stabilization_rate" shortname="gain q"/>
<dl_setting var="stabilization_rate_gain.r" min="-1000" step="1" max="-1" module="stabilization/stabilization_rate" shortname="gain r"/>
<dl_setting var="stabilization_rate_gain.p" min="1" step="1" max="1000" module="stabilization/stabilization_rate" shortname="gain p"/>
<dl_setting var="stabilization_rate_gain.q" min="1" step="1" max="1000" module="stabilization/stabilization_rate" shortname="gain q"/>
<dl_setting var="stabilization_rate_gain.r" min="1" step="1" max="1000" module="stabilization/stabilization_rate" shortname="gain r"/>
</dl_settings>
@@ -55,9 +55,9 @@
</dl_settings>
<dl_settings NAME="Vert Loop">
<dl_setting var="guidance_v_kp" min="-600" step="1" max="0" module="guidance/guidance_v" shortname="kp"/>
<dl_setting var="guidance_v_kd" min="-600" step="1" max="0" module="guidance/guidance_v" shortname="kd"/>
<dl_setting var="guidance_v_ki" min="-300" step="1" max="0" module="guidance/guidance_v" shortname="ki" handler="SetKi" />
<dl_setting var="guidance_v_kp" min="0" step="1" max="600" module="guidance/guidance_v" shortname="kp"/>
<dl_setting var="guidance_v_kd" min="0" step="1" max="600" module="guidance/guidance_v" shortname="kd"/>
<dl_setting var="guidance_v_ki" min="0" step="1" max="300" module="guidance/guidance_v" shortname="ki" handler="SetKi" />
<dl_setting var="guidance_v_z_sp" min="-5" step="0.5" max="3" module="guidance/guidance_v" shortname="sp" unit="2e-8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="ins_vf_realign" min="0" step="1" max="1" module="subsystems/ins" shortname="vf_realign" values="OFF|ON"/>
</dl_settings>
@@ -66,11 +66,11 @@
<dl_setting var="guidance_h_pos_sp.x" MIN="-10" MAX="10" STEP="1" module="guidance/guidance_h" shortname="sp_x_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="guidance_h_pos_sp.y" MIN="-10" MAX="10" STEP="1" module="guidance/guidance_h" shortname="sp_y_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="guidance_h_psi_sp" MIN="-180" MAX="180" STEP="5" module="guidance/guidance_h" shortname="sp_psi" unit="1/2^20r" alt_unit="deg" alt_unit_coef="0.000054641513360"/>
<dl_setting var="guidance_h_pgain" min="-400" step="1" max="0" module="guidance/guidance_h" shortname="kp"/>
<dl_setting var="guidance_h_dgain" min="-400" step="1" max="0" module="guidance/guidance_h" shortname="kd"/>
<dl_setting var="guidance_h_igain" min="-400" step="1" max="0" module="guidance/guidance_h" shortname="ki" handler="SetKi"/>
<dl_setting var="guidance_h_ngain" min="-400" step="1" max="0" module="guidance/guidance_h" shortname="kn"/>
<dl_setting var="guidance_h_again" min="-400" step="1" max="0" module="guidance/guidance_h" shortname="ka"/>
<dl_setting var="guidance_h_pgain" min="0" step="1" max="400" module="guidance/guidance_h" shortname="kp"/>
<dl_setting var="guidance_h_dgain" min="0" step="1" max="400" module="guidance/guidance_h" shortname="kd"/>
<dl_setting var="guidance_h_igain" min="0" step="1" max="400" module="guidance/guidance_h" shortname="ki" handler="SetKi"/>
<dl_setting var="guidance_h_ngain" min="0" step="1" max="400" module="guidance/guidance_h" shortname="kn"/>
<dl_setting var="guidance_h_again" min="0" step="1" max="400" module="guidance/guidance_h" shortname="ka"/>
<dl_setting var="ins_hf_realign" min="0" step="1" max="1" module="subsystems/ins" shortname="hf_realign" values="OFF|ON"/>
</dl_settings>
conf/settings/settings_booz2.xml
+11 -11
View File
@@ -22,9 +22,9 @@
</dl_settings>
<dl_settings NAME="Rate Loop">
<dl_setting var="stabilization_rate_gain.p" min="-1000" step="1" max="-1" module="stabilization/stabilization_rate" shortname="gain p"/>
<dl_setting var="stabilization_rate_gain.q" min="-1000" step="1" max="-1" module="stabilization/stabilization_rate" shortname="gain q"/>
<dl_setting var="stabilization_rate_gain.r" min="-1000" step="1" max="-1" module="stabilization/stabilization_rate" shortname="gain r"/>
<dl_setting var="stabilization_rate_gain.p" min="1" step="1" max="1000" module="stabilization/stabilization_rate" shortname="gain p"/>
<dl_setting var="stabilization_rate_gain.q" min="1" step="1" max="1000" module="stabilization/stabilization_rate" shortname="gain q"/>
<dl_setting var="stabilization_rate_gain.r" min="1" step="1" max="1000" module="stabilization/stabilization_rate" shortname="gain r"/>
</dl_settings>
@@ -44,9 +44,9 @@
</dl_settings>
<dl_settings NAME="Vert Loop">
<dl_setting var="guidance_v_kp" min="-600" step="1" max="0" module="guidance/guidance_v" shortname="kp"/>
<dl_setting var="guidance_v_kd" min="-600" step="1" max="0" module="guidance/guidance_v" shortname="kd"/>
<dl_setting var="guidance_v_ki" min="-300" step="1" max="0" module="guidance/guidance_v" shortname="ki" handler="SetKi" />
<dl_setting var="guidance_v_kp" min="0" step="1" max="600" module="guidance/guidance_v" shortname="kp"/>
<dl_setting var="guidance_v_kd" min="0" step="1" max="600" module="guidance/guidance_v" shortname="kd"/>
<dl_setting var="guidance_v_ki" min="0" step="1" max="300" module="guidance/guidance_v" shortname="ki" handler="SetKi" />
<dl_setting var="guidance_v_z_sp" min="-5" step="0.5" max="3" module="guidance/guidance_v" shortname="sp" unit="2e-8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="ins_vf_realign" min="0" step="1" max="1" module="subsystems/ins" shortname="vf_realign" values="OFF|ON"/>
</dl_settings>
@@ -55,11 +55,11 @@
<dl_setting var="guidance_h_pos_sp.x" MIN="-10" MAX="10" STEP="1" module="guidance/guidance_h" shortname="sp_x_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="guidance_h_pos_sp.y" MIN="-10" MAX="10" STEP="1" module="guidance/guidance_h" shortname="sp_y_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="guidance_h_psi_sp" MIN="-180" MAX="180" STEP="5" module="guidance/guidance_h" shortname="sp_psi" unit="1/2^20r" alt_unit="deg" alt_unit_coef="0.000054641513360"/>
<dl_setting var="guidance_h_pgain" min="-400" step="1" max="0" module="guidance/guidance_h" shortname="kp"/>
<dl_setting var="guidance_h_dgain" min="-400" step="1" max="0" module="guidance/guidance_h" shortname="kd"/>
<dl_setting var="guidance_h_igain" min="-400" step="1" max="0" module="guidance/guidance_h" shortname="ki" handler="SetKi"/>
<dl_setting var="guidance_h_ngain" min="-400" step="1" max="0" module="guidance/guidance_h" shortname="kn"/>
<dl_setting var="guidance_h_again" min="-400" step="1" max="0" module="guidance/guidance_h" shortname="ka"/>
<dl_setting var="guidance_h_pgain" min="0" step="1" max="400" module="guidance/guidance_h" shortname="kp"/>
<dl_setting var="guidance_h_dgain" min="0" step="1" max="400" module="guidance/guidance_h" shortname="kd"/>
<dl_setting var="guidance_h_igain" min="0" step="1" max="400" module="guidance/guidance_h" shortname="ki" handler="SetKi"/>
<dl_setting var="guidance_h_ngain" min="0" step="1" max="400" module="guidance/guidance_h" shortname="kn"/>
<dl_setting var="guidance_h_again" min="0" step="1" max="400" module="guidance/guidance_h" shortname="ka"/>
<dl_setting var="ins_hf_realign" min="0" step="1" max="1" module="subsystems/ins" shortname="hf_realign" values="OFF|ON"/>
</dl_settings>
conf/settings/settings_booz2_jtm.xml
+11 -11
View File
@@ -22,9 +22,9 @@
</dl_settings>
<dl_settings NAME="Rate Loop">
<dl_setting var="stabilization_rate_gain.p" min="-1000" step="1" max="-1" module="stabilization/stabilization_rate" shortname="gain p"/>
<dl_setting var="stabilization_rate_gain.q" min="-1000" step="1" max="-1" module="stabilization/stabilization_rate" shortname="gain q"/>
<dl_setting var="stabilization_rate_gain.r" min="-1000" step="1" max="-1" module="stabilization/stabilization_rate" shortname="gain r"/>
<dl_setting var="stabilization_rate_gain.p" min="1" step="1" max="1000" module="stabilization/stabilization_rate" shortname="gain p"/>
<dl_setting var="stabilization_rate_gain.q" min="1" step="1" max="1000" module="stabilization/stabilization_rate" shortname="gain q"/>
<dl_setting var="stabilization_rate_gain.r" min="1" step="1" max="1000" module="stabilization/stabilization_rate" shortname="gain r"/>
</dl_settings>
@@ -44,9 +44,9 @@
</dl_settings>
<dl_settings NAME="Vert Loop">
<dl_setting var="guidance_v_kp" min="-600" step="1" max="0" module="guidance/guidance_v" shortname="kp"/>
<dl_setting var="guidance_v_kd" min="-600" step="1" max="0" module="guidance/guidance_v" shortname="kd"/>
<dl_setting var="guidance_v_ki" min="-300" step="1" max="0" module="guidance/guidance_v" shortname="ki" handler="SetKi" />
<dl_setting var="guidance_v_kp" min="0" step="1" max="600" module="guidance/guidance_v" shortname="kp"/>
<dl_setting var="guidance_v_kd" min="0" step="1" max="600" module="guidance/guidance_v" shortname="kd"/>
<dl_setting var="guidance_v_ki" min="0" step="1" max="300" module="guidance/guidance_v" shortname="ki" handler="SetKi" />
<dl_setting var="guidance_v_z_sp" min="-5" step="0.5" max="3" module="guidance/guidance_v" shortname="sp" unit="2e-8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="ins_vf_realign" min="0" step="1" max="1" module="subsystems/ins" shortname="vf_realign" values="OFF|ON"/>
</dl_settings>
@@ -55,11 +55,11 @@
<dl_setting var="guidance_h_pos_sp.x" MIN="-10" MAX="10" STEP="1" module="guidance/guidance_h" shortname="sp_x_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="guidance_h_pos_sp.y" MIN="-10" MAX="10" STEP="1" module="guidance/guidance_h" shortname="sp_y_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="guidance_h_psi_sp" MIN="-180" MAX="180" STEP="5" module="guidance/guidance_h" shortname="sp_psi" unit="1/2^20r" alt_unit="deg" alt_unit_coef="0.000054641513360"/>
<dl_setting var="guidance_h_pgain" min="-400" step="1" max="0" module="guidance/guidance_h" shortname="kp"/>
<dl_setting var="guidance_h_dgain" min="-400" step="1" max="0" module="guidance/guidance_h" shortname="kd"/>
<dl_setting var="guidance_h_igain" min="-400" step="1" max="0" module="guidance/guidance_h" shortname="ki" handler="SetKi"/>
<dl_setting var="guidance_h_ngain" min="-400" step="1" max="0" module="guidance/guidance_h" shortname="kn"/>
<dl_setting var="guidance_h_again" min="-400" step="1" max="0" module="guidance/guidance_h" shortname="ka"/>
<dl_setting var="guidance_h_pgain" min="0" step="1" max="400" module="guidance/guidance_h" shortname="kp"/>
<dl_setting var="guidance_h_dgain" min="0" step="1" max="400" module="guidance/guidance_h" shortname="kd"/>
<dl_setting var="guidance_h_igain" min="0" step="1" max="400" module="guidance/guidance_h" shortname="ki" handler="SetKi"/>
<dl_setting var="guidance_h_ngain" min="0" step="1" max="400" module="guidance/guidance_h" shortname="kn"/>
<dl_setting var="guidance_h_again" min="0" step="1" max="400" module="guidance/guidance_h" shortname="ka"/>
<dl_setting var="ins_hf_realign" min="0" step="1" max="1" module="subsystems/ins" shortname="hf_realign" values="OFF|ON"/>
</dl_settings>
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+11 -11
View File
@@ -86,11 +86,11 @@ void guidance_h_init(void) {
INT_VECT2_ZERO(guidance_h_pos_err_sum);
INT_EULERS_ZERO(guidance_h_rc_sp);
INT_EULERS_ZERO(guidance_h_command_body);
guidance_h_pgain = GUIDANCE_H_PGAIN;
guidance_h_igain = GUIDANCE_H_IGAIN;
guidance_h_dgain = GUIDANCE_H_DGAIN;
guidance_h_ngain = GUIDANCE_H_NGAIN;
guidance_h_again = GUIDANCE_H_AGAIN;
guidance_h_pgain = ABS(GUIDANCE_H_PGAIN);
guidance_h_igain = ABS(GUIDANCE_H_IGAIN);
guidance_h_dgain = ABS(GUIDANCE_H_DGAIN);
guidance_h_ngain = ABS(GUIDANCE_H_NGAIN);
guidance_h_again = ABS(GUIDANCE_H_AGAIN);
}
@@ -224,12 +224,12 @@ void guidance_h_run(bool_t in_flight) {
__attribute__ ((always_inline)) static inline void guidance_h_hover_run(void) {
/* compute position error */
VECT2_DIFF(guidance_h_pos_err, ins_ltp_pos, guidance_h_pos_sp);
VECT2_DIFF(guidance_h_pos_err, guidance_h_pos_sp, ins_ltp_pos);
/* saturate it */
VECT2_STRIM(guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
/* compute speed error */
VECT2_COPY(guidance_h_speed_err, ins_ltp_speed);
VECT2_COPY(guidance_h_speed_err, -ins_ltp_speed);
/* saturate it */
VECT2_STRIM(guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
@@ -295,13 +295,13 @@ __attribute__ ((always_inline)) static inline void guidance_h_nav_run(bool_t in
#endif
/* compute position error */
VECT2_DIFF(guidance_h_pos_err, ins_ltp_pos, guidance_h_pos_ref);
VECT2_DIFF(guidance_h_pos_err, guidance_h_pos_ref, ins_ltp_pos);
/* saturate it */
VECT2_STRIM(guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
/* compute speed error */
//VECT2_COPY(guidance_h_speed_err, ins_ltp_speed);
VECT2_DIFF(guidance_h_speed_err, ins_ltp_speed, guidance_h_speed_ref);
VECT2_DIFF(guidance_h_speed_err, guidance_h_speed_ref, ins_ltp_speed);
/* saturate it */
VECT2_STRIM(guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
@@ -324,8 +324,8 @@ __attribute__ ((always_inline)) static inline void guidance_h_nav_run(bool_t in
}
// multiply by vector orthogonal to speed
VECT2_ASSIGN(guidance_h_nav_err,
vect_prod * (-ins_ltp_speed.y),
vect_prod * ins_ltp_speed.x);
vect_prod * ins_ltp_speed.y,
vect_prod * (-ins_ltp_speed.x));
// divide by 2 times dist ( >> 16 )
VECT2_SDIV(guidance_h_nav_err, guidance_h_nav_err, dist*dist);
// *2 ??
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+11 -10
View File
@@ -97,9 +97,9 @@ void guidance_v_init(void) {
guidance_v_mode = GUIDANCE_V_MODE_KILL;
guidance_v_kp = GUIDANCE_V_HOVER_KP;
guidance_v_kd = GUIDANCE_V_HOVER_KD;
guidance_v_ki = GUIDANCE_V_HOVER_KI;
guidance_v_kp = ABS(GUIDANCE_V_HOVER_KP);
guidance_v_kd = ABS(GUIDANCE_V_HOVER_KD);
guidance_v_ki = ABS(GUIDANCE_V_HOVER_KI);
guidance_v_z_sum_err = 0;
@@ -261,9 +261,9 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig
guidance_v_zd_ref = gv_zd_ref<<(INT32_SPEED_FRAC - GV_ZD_REF_FRAC);
guidance_v_zdd_ref = gv_zdd_ref<<(INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC);
/* compute the error to our reference */
int32_t err_z = ins_ltp_pos.z - guidance_v_z_ref;
int32_t err_z = guidance_v_z_ref - ins_ltp_pos.z;
Bound(err_z, GUIDANCE_V_MIN_ERR_Z, GUIDANCE_V_MAX_ERR_Z);
int32_t err_zd = ins_ltp_speed.z - guidance_v_zd_ref;
int32_t err_zd = guidance_v_zd_ref - ins_ltp_speed.z;
Bound(err_zd, GUIDANCE_V_MIN_ERR_ZD, GUIDANCE_V_MAX_ERR_ZD);
if (in_flight) {
@@ -297,12 +297,13 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig
#endif
/* our error command */
guidance_v_fb_cmd = ((-guidance_v_kp * err_z) >> 12) +
((-guidance_v_kd * err_zd) >> 21) +
((-guidance_v_ki * guidance_v_z_sum_err) >> 21);
guidance_v_fb_cmd = ((guidance_v_kp * err_z) >> 12) +
((guidance_v_kd * err_zd) >> 21) +
((guidance_v_ki * guidance_v_z_sum_err) >> 21);
guidance_v_delta_t = guidance_v_ff_cmd + guidance_v_fb_cmd;
// guidance_v_delta_t = guidance_v_fb_cmd;
// z-axis pointing down -> positive error means we need less thrust
guidance_v_delta_t = - (guidance_v_ff_cmd + guidance_v_fb_cmd);
// guidance_v_delta_t = -guidance_v_fb_cmd;
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
+14 -14
View File
@@ -42,24 +42,24 @@ void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
VECT3_ASSIGN(stabilization_gains.p,
STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN,
STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN,
STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN);
ABS(STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN),
ABS(STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN),
ABS(STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN));
VECT3_ASSIGN(stabilization_gains.d,
STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN,
STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN,
STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN);
ABS(STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN),
ABS(STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN),
ABS(STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN));
VECT3_ASSIGN(stabilization_gains.i,
STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN,
STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN,
STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN);
ABS(STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN),
ABS(STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN),
ABS(STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN));
VECT3_ASSIGN(stabilization_gains.dd,
STABILIZATION_ATTITUDE_FLOAT_PHI_DDGAIN,
STABILIZATION_ATTITUDE_FLOAT_THETA_DDGAIN,
STABILIZATION_ATTITUDE_FLOAT_PSI_DDGAIN);
ABS(STABILIZATION_ATTITUDE_FLOAT_PHI_DDGAIN),
ABS(STABILIZATION_ATTITUDE_FLOAT_THETA_DDGAIN),
ABS(STABILIZATION_ATTITUDE_FLOAT_PSI_DDGAIN));
FLOAT_EULERS_ZERO( stabilization_att_sum_err );
@@ -100,7 +100,7 @@ void stabilization_attitude_run(bool_t in_flight) {
struct FloatEulers att_float;
EULERS_FLOAT_OF_BFP(att_float, ahrs.ltp_to_body_euler);
struct FloatEulers att_err;
EULERS_DIFF(att_err, att_float, stab_att_ref_euler);
EULERS_DIFF(att_err, stab_att_ref_euler, att_float);
FLOAT_ANGLE_NORMALIZE(att_err.psi);
if (in_flight) {
@@ -116,7 +116,7 @@ void stabilization_attitude_run(bool_t in_flight) {
struct FloatRates rate_float;
RATES_FLOAT_OF_BFP(rate_float, ahrs.body_rate);
struct FloatRates rate_err;
RATES_DIFF(rate_err, rate_float, stab_att_ref_rate);
RATES_DIFF(rate_err, stab_att_ref_rate, rate_float);
/* PID */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+14 -14
View File
@@ -42,24 +42,24 @@ void stabilization_attitude_init(void) {
VECT3_ASSIGN(stabilization_gains.p,
STABILIZATION_ATTITUDE_PHI_PGAIN,
STABILIZATION_ATTITUDE_THETA_PGAIN,
STABILIZATION_ATTITUDE_PSI_PGAIN);
ABS(STABILIZATION_ATTITUDE_PHI_PGAIN),
ABS(STABILIZATION_ATTITUDE_THETA_PGAIN),
ABS(STABILIZATION_ATTITUDE_PSI_PGAIN));
VECT3_ASSIGN(stabilization_gains.d,
STABILIZATION_ATTITUDE_PHI_DGAIN,
STABILIZATION_ATTITUDE_THETA_DGAIN,
STABILIZATION_ATTITUDE_PSI_DGAIN);
ABS(STABILIZATION_ATTITUDE_PHI_DGAIN),
ABS(STABILIZATION_ATTITUDE_THETA_DGAIN),
ABS(STABILIZATION_ATTITUDE_PSI_DGAIN));
VECT3_ASSIGN(stabilization_gains.i,
STABILIZATION_ATTITUDE_PHI_IGAIN,
STABILIZATION_ATTITUDE_THETA_IGAIN,
STABILIZATION_ATTITUDE_PSI_IGAIN);
ABS(STABILIZATION_ATTITUDE_PHI_IGAIN),
ABS(STABILIZATION_ATTITUDE_THETA_IGAIN),
ABS(STABILIZATION_ATTITUDE_PSI_IGAIN));
VECT3_ASSIGN(stabilization_gains.dd,
STABILIZATION_ATTITUDE_PHI_DDGAIN,
STABILIZATION_ATTITUDE_THETA_DDGAIN,
STABILIZATION_ATTITUDE_PSI_DDGAIN);
ABS(STABILIZATION_ATTITUDE_PHI_DDGAIN),
ABS(STABILIZATION_ATTITUDE_THETA_DDGAIN),
ABS(STABILIZATION_ATTITUDE_PSI_DDGAIN));
INT_EULERS_ZERO( stabilization_att_sum_err );
@@ -108,7 +108,7 @@ void stabilization_attitude_run(bool_t in_flight) {
OFFSET_AND_ROUND(stab_att_ref_euler.theta, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND(stab_att_ref_euler.psi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)) };
struct Int32Eulers att_err;
EULERS_DIFF(att_err, ahrs.ltp_to_body_euler, att_ref_scaled);
EULERS_DIFF(att_err, att_ref_scaled, ahrs.ltp_to_body_euler);
INT32_ANGLE_NORMALIZE(att_err.psi);
if (in_flight) {
@@ -126,7 +126,7 @@ void stabilization_attitude_run(bool_t in_flight) {
OFFSET_AND_ROUND(stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC)) };
struct Int32Rates rate_err;
RATES_DIFF(rate_err, ahrs.body_rate, rate_ref_scaled);
RATES_DIFF(rate_err, rate_ref_scaled, ahrs.body_rate);
/* PID */
stabilization_att_fb_cmd[COMMAND_ROLL] =
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+17 -17
View File
@@ -86,15 +86,15 @@ void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
for (int i = 0; i < STABILIZATION_ATTITUDE_FLOAT_GAIN_NB; i++) {
VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
}
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
@@ -136,35 +136,35 @@ static void attitude_run_fb(float fb_commands[], struct FloatAttitudeGains *gain
{
/* PID feedback */
fb_commands[COMMAND_ROLL] =
GAIN_PRESCALER_P * -gains->p.x * att_err->qx +
GAIN_PRESCALER_P * gains->p.x * att_err->qx +
GAIN_PRESCALER_D * gains->d.x * rate_err->p +
GAIN_PRESCALER_D * gains->rates_d.x * rate_err_d->p +
GAIN_PRESCALER_I * gains->i.x * sum_err->qx;
fb_commands[COMMAND_PITCH] =
GAIN_PRESCALER_P * -gains->p.y * att_err->qy +
GAIN_PRESCALER_P * gains->p.y * att_err->qy +
GAIN_PRESCALER_D * gains->d.y * rate_err->q +
GAIN_PRESCALER_D * gains->rates_d.y * rate_err_d->q +
GAIN_PRESCALER_I * gains->i.y * sum_err->qy;
fb_commands[COMMAND_YAW] =
GAIN_PRESCALER_P * -gains->p.z * att_err->qz +
GAIN_PRESCALER_P * gains->p.z * att_err->qz +
GAIN_PRESCALER_D * gains->d.z * rate_err->r +
GAIN_PRESCALER_D * gains->rates_d.z * rate_err_d->r +
GAIN_PRESCALER_I * gains->i.z * sum_err->qz;
fb_commands[COMMAND_ROLL_SURFACE] =
GAIN_PRESCALER_P * -gains->surface_p.x * att_err->qx +
GAIN_PRESCALER_P * gains->surface_p.x * att_err->qx +
GAIN_PRESCALER_D * gains->surface_d.x * rate_err->p +
GAIN_PRESCALER_I * gains->surface_i.x * sum_err->qx;
fb_commands[COMMAND_PITCH_SURFACE] =
GAIN_PRESCALER_P * -gains->surface_p.y * att_err->qy +
GAIN_PRESCALER_P * gains->surface_p.y * att_err->qy +
GAIN_PRESCALER_D * gains->surface_d.y * rate_err->q +
GAIN_PRESCALER_I * gains->surface_i.y * sum_err->qy;
fb_commands[COMMAND_YAW_SURFACE] =
GAIN_PRESCALER_P * -gains->surface_p.z * att_err->qz +
GAIN_PRESCALER_P * gains->surface_p.z * att_err->qz +
GAIN_PRESCALER_D * gains->surface_d.z * rate_err->r +
GAIN_PRESCALER_I * gains->surface_i.z * sum_err->qz;
@@ -189,7 +189,7 @@ void stabilization_attitude_run(bool_t enable_integrator) {
/* rate error */
struct FloatRates rate_err;
RATES_DIFF(rate_err, ahrs_float.body_rate, stab_att_ref_rate);
RATES_DIFF(rate_err, stab_att_ref_rate, ahrs_float.body_rate);
/* integrated error */
if (enable_integrator) {
@@ -199,7 +199,7 @@ void stabilization_attitude_run(bool_t enable_integrator) {
scaled_att_err.qx = att_err.qx / IERROR_SCALE;
scaled_att_err.qy = att_err.qy / IERROR_SCALE;
scaled_att_err.qz = att_err.qz / IERROR_SCALE;
FLOAT_QUAT_COMP_INV(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
FLOAT_QUAT_NORMALIZE(new_sum_err);
FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+35 -20
View File
@@ -33,12 +33,7 @@
#include "subsystems/ahrs.h"
#include "generated/airframe.h"
struct Int32AttitudeGains stabilization_gains = {
{STABILIZATION_ATTITUDE_PHI_PGAIN, STABILIZATION_ATTITUDE_THETA_PGAIN, STABILIZATION_ATTITUDE_PSI_PGAIN },
{STABILIZATION_ATTITUDE_PHI_DGAIN, STABILIZATION_ATTITUDE_THETA_DGAIN, STABILIZATION_ATTITUDE_PSI_DGAIN },
{STABILIZATION_ATTITUDE_PHI_DDGAIN, STABILIZATION_ATTITUDE_THETA_DDGAIN, STABILIZATION_ATTITUDE_PSI_DDGAIN },
{STABILIZATION_ATTITUDE_PHI_IGAIN, STABILIZATION_ATTITUDE_THETA_IGAIN, STABILIZATION_ATTITUDE_PSI_IGAIN }
};
struct Int32AttitudeGains stabilization_gains;
struct Int32Quat stabilization_att_sum_err_quat;
struct Int32Eulers stabilization_att_sum_err;
@@ -98,18 +93,38 @@ void stabilization_attitude_init(void) {
/*
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
}
*/
VECT3_ASSIGN_ABS(stabilization_gains.p,
STABILIZATION_ATTITUDE_PHI_PGAIN,
STABILIZATION_ATTITUDE_THETA_PGAIN,
STABILIZATION_ATTITUDE_PSI_PGAIN);
VECT3_ASSIGN_ABS(stabilization_gains.d,
STABILIZATION_ATTITUDE_PHI_DGAIN,
STABILIZATION_ATTITUDE_THETA_DGAIN,
STABILIZATION_ATTITUDE_PSI_DGAIN);
VECT3_ASSIGN_ABS(stabilization_gains.i,
STABILIZATION_ATTITUDE_PHI_IGAIN,
STABILIZATION_ATTITUDE_THETA_IGAIN,
STABILIZATION_ATTITUDE_PSI_IGAIN);
VECT3_ASSIGN_ABS(stabilization_gains.dd,
STABILIZATION_ATTITUDE_PHI_DDGAIN,
STABILIZATION_ATTITUDE_THETA_DDGAIN,
STABILIZATION_ATTITUDE_PSI_DDGAIN);
INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
INT_EULERS_ZERO( stabilization_att_sum_err );
}
@@ -152,17 +167,17 @@ static void attitude_run_fb(int32_t fb_commands[], struct Int32AttitudeGains *ga
{
/* PID feedback */
fb_commands[COMMAND_ROLL] =
GAIN_PRESCALER_P * -gains->p.x * QUAT1_FLOAT_OF_BFP(att_err->qx) / 4 +
GAIN_PRESCALER_P * gains->p.x * QUAT1_FLOAT_OF_BFP(att_err->qx) / 4 +
GAIN_PRESCALER_D * gains->d.x * RATE_FLOAT_OF_BFP(rate_err->p) / 16 +
GAIN_PRESCALER_I * gains->i.x * QUAT1_FLOAT_OF_BFP(sum_err->qx) / 2;
fb_commands[COMMAND_PITCH] =
GAIN_PRESCALER_P * -gains->p.y * QUAT1_FLOAT_OF_BFP(att_err->qy) / 4 +
GAIN_PRESCALER_P * gains->p.y * QUAT1_FLOAT_OF_BFP(att_err->qy) / 4 +
GAIN_PRESCALER_D * gains->d.y * RATE_FLOAT_OF_BFP(rate_err->q) / 16 +
GAIN_PRESCALER_I * gains->i.y * QUAT1_FLOAT_OF_BFP(sum_err->qy) / 2;
fb_commands[COMMAND_YAW] =
GAIN_PRESCALER_P * -gains->p.z * QUAT1_FLOAT_OF_BFP(att_err->qz) / 4 +
GAIN_PRESCALER_P * gains->p.z * QUAT1_FLOAT_OF_BFP(att_err->qz) / 4 +
GAIN_PRESCALER_D * gains->d.z * RATE_FLOAT_OF_BFP(rate_err->r) / 16 +
GAIN_PRESCALER_I * gains->i.z * QUAT1_FLOAT_OF_BFP(sum_err->qz) / 2;
@@ -188,7 +203,7 @@ void stabilization_attitude_run(bool_t enable_integrator) {
/* rate error */
struct Int32Rates rate_err;
RATES_DIFF(rate_err, ahrs.body_rate, stab_att_ref_rate);
RATES_DIFF(rate_err, stab_att_ref_rate, ahrs.body_rate);
/* integrated error */
if (enable_integrator) {
@@ -198,7 +213,7 @@ void stabilization_attitude_run(bool_t enable_integrator) {
scaled_att_err.qx = att_err.qx / IERROR_SCALE;
scaled_att_err.qy = att_err.qy / IERROR_SCALE;
scaled_att_err.qz = att_err.qz / IERROR_SCALE;
INT32_QUAT_COMP_INV(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
INT32_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
INT32_QUAT_NORMALIZE(new_sum_err);
QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
INT32_EULERS_OF_QUAT(stabilization_att_sum_err, stabilization_att_sum_err_quat);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
+10 -10
View File
@@ -101,17 +101,17 @@ void stabilization_rate_init(void) {
INT_RATES_ZERO(stabilization_rate_sp);
RATES_ASSIGN(stabilization_rate_gain,
STABILIZATION_RATE_GAIN_P,
STABILIZATION_RATE_GAIN_Q,
STABILIZATION_RATE_GAIN_R);
ABS(STABILIZATION_RATE_GAIN_P),
ABS(STABILIZATION_RATE_GAIN_Q),
ABS(STABILIZATION_RATE_GAIN_R));
RATES_ASSIGN(stabilization_rate_igain,
STABILIZATION_RATE_IGAIN_P,
STABILIZATION_RATE_IGAIN_Q,
STABILIZATION_RATE_IGAIN_R);
ABS(STABILIZATION_RATE_IGAIN_P),
ABS(STABILIZATION_RATE_IGAIN_Q),
ABS(STABILIZATION_RATE_IGAIN_R));
RATES_ASSIGN(stabilization_rate_ddgain,
STABILIZATION_RATE_DDGAIN_P,
STABILIZATION_RATE_DDGAIN_Q,
STABILIZATION_RATE_DDGAIN_R);
ABS(STABILIZATION_RATE_DDGAIN_P),
ABS(STABILIZATION_RATE_DDGAIN_Q),
ABS(STABILIZATION_RATE_DDGAIN_R));
INT_RATES_ZERO(stabilization_rate_ref);
INT_RATES_ZERO(stabilization_rate_refdot);
@@ -169,7 +169,7 @@ void stabilization_rate_run(bool_t in_flight) {
OFFSET_AND_ROUND(stabilization_rate_ref.q, (REF_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(stabilization_rate_ref.r, (REF_FRAC - INT32_RATE_FRAC)) };
struct Int32Rates _error;
RATES_DIFF(_error, ahrs.body_rate, _ref_scaled);
RATES_DIFF(_error, _ref_scaled, ahrs.body_rate);
if (in_flight) {
/* update integrator */
RATES_ADD(stabilization_rate_sum_err, _error);

Some files were not shown because too many files have changed in this diff Show More