diff --git a/conf/airframes/CDW/debug_i2c.xml b/conf/airframes/CDW/debug_i2c.xml index 4a03deaf14..3a279378ab 100644 --- a/conf/airframes/CDW/debug_i2c.xml +++ b/conf/airframes/CDW/debug_i2c.xml @@ -84,9 +84,9 @@ - - - + + + @@ -115,17 +115,17 @@ - - - + + + - - - + + + - - - + + + @@ -172,12 +172,12 @@ - - + + @@ -189,10 +189,10 @@
- - - - + + + +
diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml index e6a47257d5..44e4371d67 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -142,9 +142,9 @@ - - - + + +
@@ -173,17 +173,17 @@ - - - + + + - - - + + + - - - + + + @@ -208,8 +208,8 @@ - - + + @@ -218,10 +218,10 @@
- - - - + + + +
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index 3232d17180..2c55323d36 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -105,9 +105,9 @@ - - - + + + @@ -136,17 +136,17 @@ - - - + + + - - - + + + - - - + + + @@ -170,8 +170,8 @@ - - + + @@ -182,10 +182,10 @@
- - - - + + + +
diff --git a/conf/airframes/ENAC/quadrotor/g1_vision.xml b/conf/airframes/ENAC/quadrotor/g1_vision.xml index b346f87f90..1a9fd10b02 100644 --- a/conf/airframes/ENAC/quadrotor/g1_vision.xml +++ b/conf/airframes/ENAC/quadrotor/g1_vision.xml @@ -79,9 +79,9 @@ - - - + + + @@ -110,17 +110,17 @@ - - - + + + - - - + + + - - - + + + @@ -152,8 +152,8 @@ - - + + @@ -164,10 +164,10 @@
- - - - + + + +
diff --git a/conf/airframes/ENAC/quadrotor/mkk1-vision.xml b/conf/airframes/ENAC/quadrotor/mkk1-vision.xml index d67ba5f0be..653ae0fafe 100644 --- a/conf/airframes/ENAC/quadrotor/mkk1-vision.xml +++ b/conf/airframes/ENAC/quadrotor/mkk1-vision.xml @@ -99,9 +99,9 @@ - - - + + + @@ -111,9 +111,9 @@ - - - + + + @@ -143,17 +143,17 @@ - - - + + + - - - + + + - - - + + + @@ -186,8 +186,8 @@ - - + + @@ -197,10 +197,10 @@
- - - - + + + +
diff --git a/conf/airframes/ENAC/quadrotor/mkk1.xml b/conf/airframes/ENAC/quadrotor/mkk1.xml index 94362a4947..d9c7be6a38 100644 --- a/conf/airframes/ENAC/quadrotor/mkk1.xml +++ b/conf/airframes/ENAC/quadrotor/mkk1.xml @@ -129,9 +129,9 @@ - - - + + + @@ -141,9 +141,9 @@ - - - + + + @@ -173,17 +173,17 @@ - - - + + + - - - + + + - - - + + + @@ -206,8 +206,8 @@ - - + + @@ -217,10 +217,10 @@
- - - - + + + +
diff --git a/conf/airframes/ENAC/quadrotor/navgo.xml b/conf/airframes/ENAC/quadrotor/navgo.xml index 745662131c..5b898ba78f 100644 --- a/conf/airframes/ENAC/quadrotor/navgo.xml +++ b/conf/airframes/ENAC/quadrotor/navgo.xml @@ -158,9 +158,9 @@ - - - + + + @@ -189,17 +189,17 @@ - - - + + + - - - + + + - - - + + + @@ -219,8 +219,8 @@ - - + + @@ -229,10 +229,10 @@
- - - - + + + +
diff --git a/conf/airframes/ENAC/quadrotor/nova1.xml b/conf/airframes/ENAC/quadrotor/nova1.xml index afba70204c..2e1e0f2447 100644 --- a/conf/airframes/ENAC/quadrotor/nova1.xml +++ b/conf/airframes/ENAC/quadrotor/nova1.xml @@ -100,9 +100,9 @@ - - - + + + @@ -112,9 +112,9 @@ - - - + + + @@ -144,17 +144,17 @@ - - - + + + - - - + + + - - - + + + @@ -177,8 +177,8 @@ - - + + @@ -188,10 +188,10 @@
- - - - + + + +
diff --git a/conf/airframes/NoVa_L.xml b/conf/airframes/NoVa_L.xml index 870898f9e9..e54555d794 100644 --- a/conf/airframes/NoVa_L.xml +++ b/conf/airframes/NoVa_L.xml @@ -112,9 +112,9 @@ - - - + + + @@ -143,17 +143,17 @@ - - - + + + - - - + + + - - - + + + @@ -200,12 +200,12 @@ - - + + @@ -217,10 +217,10 @@
- - - - + + + +
diff --git a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml index 8e8f29094a..039210f933 100644 --- a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml +++ b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml @@ -76,9 +76,9 @@ - - - + + +
@@ -108,30 +108,30 @@ - - - + + + - - - + + + - - - + + + @@ -160,9 +160,9 @@ - - - + + + @@ -171,9 +171,9 @@
- - - + + +
diff --git a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml index 378aa2f8ea..9dcfcbfb62 100644 --- a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml +++ b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml @@ -76,9 +76,9 @@ - - - + + + @@ -109,31 +109,31 @@ - - - + + + - - - + + + - - - + + + @@ -178,9 +178,9 @@ - - - + + + @@ -189,9 +189,9 @@
- - - + + +
diff --git a/conf/airframes/Poine/booz2_a1.xml b/conf/airframes/Poine/booz2_a1.xml index 493d489ba4..994704b005 100644 --- a/conf/airframes/Poine/booz2_a1.xml +++ b/conf/airframes/Poine/booz2_a1.xml @@ -90,9 +90,9 @@ - - - + + + @@ -122,17 +122,17 @@ - - - + + + - - - + + + - - - + + + @@ -151,9 +151,9 @@ - - - + + + @@ -163,9 +163,9 @@
- - - + + +
diff --git a/conf/airframes/Poine/booz2_a1p.xml b/conf/airframes/Poine/booz2_a1p.xml index 097c2993de..b400829f6e 100644 --- a/conf/airframes/Poine/booz2_a1p.xml +++ b/conf/airframes/Poine/booz2_a1p.xml @@ -88,9 +88,9 @@ - - - + + +
@@ -120,17 +120,17 @@ - - - + + + - - - + + + - - - + + + @@ -155,9 +155,9 @@ - - - + + + @@ -167,9 +167,9 @@
- - - + + +
diff --git a/conf/airframes/Poine/booz2_a7.xml b/conf/airframes/Poine/booz2_a7.xml index 376c72fad3..5aa9a6707c 100644 --- a/conf/airframes/Poine/booz2_a7.xml +++ b/conf/airframes/Poine/booz2_a7.xml @@ -84,9 +84,9 @@ - - - + + +
@@ -115,17 +115,17 @@ - - - + + + - - - + + + - - - + + + @@ -172,12 +172,12 @@ - - + + @@ -189,10 +189,10 @@
- - - - + + + +
diff --git a/conf/airframes/Poine/h_hex.xml b/conf/airframes/Poine/h_hex.xml index 5cfdc5ceaa..53b3b285b6 100644 --- a/conf/airframes/Poine/h_hex.xml +++ b/conf/airframes/Poine/h_hex.xml @@ -86,9 +86,9 @@ - - - + + +
@@ -117,17 +117,17 @@ - - - + + + - - - + + + - - - + + + @@ -146,9 +146,9 @@ - - - + + + @@ -158,9 +158,9 @@
- - - + + +
diff --git a/conf/airframes/UofAdelaide/A1000_BOOZ.xml b/conf/airframes/UofAdelaide/A1000_BOOZ.xml index a1de5fec40..aab64eeeab 100644 --- a/conf/airframes/UofAdelaide/A1000_BOOZ.xml +++ b/conf/airframes/UofAdelaide/A1000_BOOZ.xml @@ -152,13 +152,13 @@ second attempt - - - + + + - - - + + + @@ -166,9 +166,9 @@ second attempt + + + -->
@@ -196,17 +196,17 @@ second attempt - - - + + + - - - + + + - - - + + + @@ -225,9 +225,9 @@ second attempt - - - + + + @@ -236,9 +236,9 @@ second attempt
- - - + + +
diff --git a/conf/airframes/UofAdelaide/A1000_LISA.xml b/conf/airframes/UofAdelaide/A1000_LISA.xml index 056593ff0e..919d3ed2ea 100644 --- a/conf/airframes/UofAdelaide/A1000_LISA.xml +++ b/conf/airframes/UofAdelaide/A1000_LISA.xml @@ -84,9 +84,9 @@ - - - + + +
@@ -115,13 +115,13 @@ + --> - - - + + + - - - + + + - - - + + + @@ -162,9 +162,9 @@ - - - + + + @@ -174,9 +174,9 @@
- - - + + +
diff --git a/conf/airframes/UofAdelaide/A1000_NOVA.xml b/conf/airframes/UofAdelaide/A1000_NOVA.xml index f6da7f4fcc..5bc4f2fbc9 100644 --- a/conf/airframes/UofAdelaide/A1000_NOVA.xml +++ b/conf/airframes/UofAdelaide/A1000_NOVA.xml @@ -79,13 +79,13 @@ - - - + + + - - - + + + @@ -119,30 +119,30 @@ - - - + + + - - - + + + - - - + + + @@ -185,8 +185,8 @@ - - + + @@ -196,11 +196,11 @@
- - - - - + + + + +
diff --git a/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml b/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml index 550ef036c3..062a08ed32 100644 --- a/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml +++ b/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml @@ -79,13 +79,13 @@ - - - + + + - - - + + + @@ -119,30 +119,30 @@ - - - + + + - - - + + + - - - + + + @@ -185,8 +185,8 @@ - - + + @@ -196,11 +196,11 @@
- - - - - + + + + +
diff --git a/conf/airframes/UofAdelaide/booz2_a1000.xml b/conf/airframes/UofAdelaide/booz2_a1000.xml index f46a9fbbe9..b42cb89a90 100755 --- a/conf/airframes/UofAdelaide/booz2_a1000.xml +++ b/conf/airframes/UofAdelaide/booz2_a1000.xml @@ -151,13 +151,13 @@ second attempt - - - + + + - - - + + + @@ -165,9 +165,9 @@ second attempt + + + -->
@@ -196,17 +196,17 @@ second attempt - - - + + + - - - + + + - - - + + + @@ -226,9 +226,9 @@ second attempt - - - + + + @@ -238,9 +238,9 @@ second attempt
- - - + + +
diff --git a/conf/airframes/UofAdelaide/lisa_a1000.xml b/conf/airframes/UofAdelaide/lisa_a1000.xml index ac45acf0c9..50be06bc31 100644 --- a/conf/airframes/UofAdelaide/lisa_a1000.xml +++ b/conf/airframes/UofAdelaide/lisa_a1000.xml @@ -81,9 +81,9 @@ - - - + + +
@@ -112,17 +112,17 @@ - - - + + + - - - + + + - - - + + + @@ -141,9 +141,9 @@ - - - + + + @@ -153,9 +153,9 @@
- - - + + +
diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index d36ddb88d5..6f381ae18c 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -125,13 +125,13 @@ - - - + + + - - - + + + @@ -165,17 +165,17 @@ - - - + + + - - - + + + - - - + + + @@ -194,9 +194,9 @@ - - - + + + @@ -205,9 +205,9 @@
- - - + + +
diff --git a/conf/airframes/booz2_ppzuav.xml b/conf/airframes/booz2_ppzuav.xml index 09f4cfc87c..602e744aa7 100644 --- a/conf/airframes/booz2_ppzuav.xml +++ b/conf/airframes/booz2_ppzuav.xml @@ -64,13 +64,13 @@ - - - + + + - - - + + + @@ -104,30 +104,30 @@ - - - + + + - - - + + + - - - + + + @@ -160,8 +160,8 @@ - - + + @@ -171,11 +171,11 @@
- - - - - + + + + +
diff --git a/conf/airframes/esden/jt_lisam.xml b/conf/airframes/esden/jt_lisam.xml index ef337afcec..047a87ce3e 100644 --- a/conf/airframes/esden/jt_lisam.xml +++ b/conf/airframes/esden/jt_lisam.xml @@ -75,9 +75,9 @@ - - - + + +
@@ -108,17 +108,17 @@ - - - + + + - - - + + + - - - + + + @@ -137,9 +137,9 @@ - - - + + + @@ -157,9 +157,9 @@
- - - + + +
diff --git a/conf/airframes/esden/lisa_asctec.xml b/conf/airframes/esden/lisa_asctec.xml index 07e4c8cbfa..b274d95895 100644 --- a/conf/airframes/esden/lisa_asctec.xml +++ b/conf/airframes/esden/lisa_asctec.xml @@ -91,9 +91,9 @@ - - - + + +
@@ -122,17 +122,17 @@ - - - + + + - - - + + + - - - + + + @@ -151,9 +151,9 @@ - - - + + + @@ -163,9 +163,9 @@
- - - + + +
diff --git a/conf/airframes/esden/lisa_asctec_aspirin.xml b/conf/airframes/esden/lisa_asctec_aspirin.xml index 30636b76ff..cc8015e2ef 100644 --- a/conf/airframes/esden/lisa_asctec_aspirin.xml +++ b/conf/airframes/esden/lisa_asctec_aspirin.xml @@ -91,9 +91,9 @@ - - - + + +
@@ -122,17 +122,17 @@ - - - + + + - - - + + + - - - + + + @@ -151,9 +151,9 @@ - - - + + + @@ -163,9 +163,9 @@
- - - + + +
diff --git a/conf/airframes/esden/lisa_m_pwm.xml b/conf/airframes/esden/lisa_m_pwm.xml index 6f75dd2b97..c56c61c399 100644 --- a/conf/airframes/esden/lisa_m_pwm.xml +++ b/conf/airframes/esden/lisa_m_pwm.xml @@ -105,9 +105,9 @@ - - - + + +
@@ -136,17 +136,17 @@ - - - + + + - - - + + + - - - + + + @@ -165,9 +165,9 @@ - - - + + + @@ -183,9 +183,9 @@
- - - + + +
diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml index 0ed7f413b5..378eab40d3 100644 --- a/conf/airframes/esden/lisa_pwm_aspirin.xml +++ b/conf/airframes/esden/lisa_pwm_aspirin.xml @@ -79,9 +79,9 @@ - - - + + +
@@ -110,17 +110,17 @@ - - - + + + - - - + + + - - - + + + @@ -146,9 +146,9 @@ - - - + + + @@ -158,9 +158,9 @@
- - - + + +
diff --git a/conf/airframes/esden/synerani_4B.xml b/conf/airframes/esden/synerani_4B.xml index 0fe02deda5..ff6b2fed00 100644 --- a/conf/airframes/esden/synerani_4B.xml +++ b/conf/airframes/esden/synerani_4B.xml @@ -82,9 +82,9 @@ - - - + + +
@@ -113,17 +113,17 @@ - - - + + + - - - + + + - - - + + + @@ -149,9 +149,9 @@ - - - + + + @@ -161,9 +161,9 @@
- - - + + +
diff --git a/conf/airframes/mm/rotor/qmk1.xml b/conf/airframes/mm/rotor/qmk1.xml index c4bacf6f87..e8562ff786 100644 --- a/conf/airframes/mm/rotor/qmk1.xml +++ b/conf/airframes/mm/rotor/qmk1.xml @@ -91,9 +91,9 @@ - - - + + +
@@ -122,17 +122,17 @@ - - - + + + - - - + + + - - - + + + @@ -152,8 +152,8 @@ - - + + @@ -164,9 +164,9 @@
- - - + + +
diff --git a/conf/airframes/obsolete/booz2_Aron.xml b/conf/airframes/obsolete/booz2_Aron.xml index 80af3ce202..8512e7990a 100644 --- a/conf/airframes/obsolete/booz2_Aron.xml +++ b/conf/airframes/obsolete/booz2_Aron.xml @@ -83,9 +83,9 @@ - - - + + +
@@ -114,17 +114,17 @@ - - - + + + - - - + + + - - - + + + @@ -145,9 +145,9 @@ - - - + + + @@ -155,9 +155,9 @@
- - - + + +
diff --git a/conf/airframes/obsolete/booz2_NoVa.xml b/conf/airframes/obsolete/booz2_NoVa.xml index a9e7ff831d..472114d6a7 100644 --- a/conf/airframes/obsolete/booz2_NoVa.xml +++ b/conf/airframes/obsolete/booz2_NoVa.xml @@ -79,13 +79,13 @@ - - - + + + - - - + + + @@ -119,30 +119,30 @@ - - - + + + - - - + + + - - - + + + @@ -185,8 +185,8 @@ - - + + @@ -196,11 +196,11 @@
- - - - - + + + + +
diff --git a/conf/airframes/obsolete/booz2_NoVa_001.xml b/conf/airframes/obsolete/booz2_NoVa_001.xml index 582d05c7b4..c6e20076fd 100644 --- a/conf/airframes/obsolete/booz2_NoVa_001.xml +++ b/conf/airframes/obsolete/booz2_NoVa_001.xml @@ -79,13 +79,13 @@ - - - + + + - - - + + + @@ -119,30 +119,30 @@ - - - + + + - - - + + + - - - + + + @@ -185,8 +185,8 @@ - - + + @@ -196,11 +196,11 @@
- - - - - + + + + +
diff --git a/conf/airframes/obsolete/booz2_NoVa_002.xml b/conf/airframes/obsolete/booz2_NoVa_002.xml index 27229199c7..c243de3ae4 100644 --- a/conf/airframes/obsolete/booz2_NoVa_002.xml +++ b/conf/airframes/obsolete/booz2_NoVa_002.xml @@ -79,13 +79,13 @@ - - - + + + - - - + + + @@ -119,30 +119,30 @@ - - - + + + - - - + + + - - - + + + @@ -185,8 +185,8 @@ - - + + @@ -196,11 +196,11 @@
- - - - - + + + + +
diff --git a/conf/airframes/obsolete/booz2_a2.xml b/conf/airframes/obsolete/booz2_a2.xml index 95bb96b694..c6dc404e3c 100644 --- a/conf/airframes/obsolete/booz2_a2.xml +++ b/conf/airframes/obsolete/booz2_a2.xml @@ -77,9 +77,9 @@ - - - + + +
@@ -108,17 +108,17 @@ - - - + + + - - - + + + - - - + + + @@ -145,9 +145,9 @@ - - - + + + @@ -155,9 +155,9 @@
- - - + + +
diff --git a/conf/airframes/obsolete/booz2_a3.xml b/conf/airframes/obsolete/booz2_a3.xml index b16d1e39ae..7e7755e84e 100644 --- a/conf/airframes/obsolete/booz2_a3.xml +++ b/conf/airframes/obsolete/booz2_a3.xml @@ -64,9 +64,9 @@ - - - + + +
@@ -95,17 +95,17 @@ - - - + + + - - - + + + - - - + + + @@ -131,8 +131,8 @@ - - + + @@ -140,9 +140,9 @@
- - - + + +
diff --git a/conf/airframes/obsolete/booz2_a4.xml b/conf/airframes/obsolete/booz2_a4.xml index 8d1a6b0822..337c76d244 100644 --- a/conf/airframes/obsolete/booz2_a4.xml +++ b/conf/airframes/obsolete/booz2_a4.xml @@ -66,9 +66,9 @@ - - - + + +
@@ -84,10 +84,10 @@ - - + + - + @@ -109,8 +109,8 @@ - - + + @@ -118,9 +118,9 @@
- - - + + +
diff --git a/conf/airframes/obsolete/booz2_a5.xml b/conf/airframes/obsolete/booz2_a5.xml index 9b4918ead3..bcdd61a563 100644 --- a/conf/airframes/obsolete/booz2_a5.xml +++ b/conf/airframes/obsolete/booz2_a5.xml @@ -87,9 +87,9 @@ - - - + + +
@@ -118,17 +118,17 @@ - - - + + + - - - + + + - - - + + + @@ -147,9 +147,9 @@ - - - + + + @@ -159,9 +159,9 @@
- - - + + +
diff --git a/conf/airframes/obsolete/booz2_s1.xml b/conf/airframes/obsolete/booz2_s1.xml index 106ad4cb11..9711a48c3f 100644 --- a/conf/airframes/obsolete/booz2_s1.xml +++ b/conf/airframes/obsolete/booz2_s1.xml @@ -78,9 +78,9 @@ - - - + + +
@@ -110,17 +110,17 @@ - - - + + + - - - + + + - - - + + + @@ -138,9 +138,9 @@ - - - + + + @@ -151,9 +151,9 @@
- - - + + +
diff --git a/conf/airframes/obsolete/booz2_x1.xml b/conf/airframes/obsolete/booz2_x1.xml index d24a301207..7ce46bd0d3 100644 --- a/conf/airframes/obsolete/booz2_x1.xml +++ b/conf/airframes/obsolete/booz2_x1.xml @@ -89,9 +89,9 @@ - - - + + +
@@ -121,17 +121,17 @@ - - - + + + - - - + + + - - - + + + @@ -150,9 +150,9 @@ - - - + + + @@ -161,9 +161,9 @@
- - - + + +
diff --git a/conf/settings/settings_UofAdelaide.xml b/conf/settings/settings_UofAdelaide.xml index 16ebab381e..0ad179f67e 100644 --- a/conf/settings/settings_UofAdelaide.xml +++ b/conf/settings/settings_UofAdelaide.xml @@ -33,9 +33,9 @@ - - - + + + @@ -55,9 +55,9 @@ - - - + + + @@ -66,11 +66,11 @@ - - - - - + + + + + diff --git a/conf/settings/settings_booz2.xml b/conf/settings/settings_booz2.xml index 97cff2e7b1..20a04ce36b 100644 --- a/conf/settings/settings_booz2.xml +++ b/conf/settings/settings_booz2.xml @@ -22,9 +22,9 @@ - - - + + + @@ -44,9 +44,9 @@ - - - + + + @@ -55,11 +55,11 @@ - - - - - + + + + + diff --git a/conf/settings/settings_booz2_jtm.xml b/conf/settings/settings_booz2_jtm.xml index 3441e55d55..22b005f7e6 100644 --- a/conf/settings/settings_booz2_jtm.xml +++ b/conf/settings/settings_booz2_jtm.xml @@ -22,9 +22,9 @@ - - - + + + @@ -44,9 +44,9 @@ - - - + + + @@ -55,11 +55,11 @@ - - - - - + + + + + diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 5bb1c2dfa8..0e45915486 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -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 ?? diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index d8745f9ce5..eba0bee30c 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -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; } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c index daf738ed93..140cc9f44a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c @@ -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 */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index 24569f0e78..1e5a845b81 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -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] = diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index 36f6dd9df7..e84c238123 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -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); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index a221633e5e..767c99f06f 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -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); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c index 44ff7ac984..62891feed3 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c @@ -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); diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h index a57e67dd7c..9bc9b54c8a 100644 --- a/sw/airborne/math/pprz_algebra.h +++ b/sw/airborne/math/pprz_algebra.h @@ -27,6 +27,7 @@ #include /* for FLT_EPSILON */ #include /* for memcpy */ +#include "std.h" /* for ABS */ #define SQUARE(_a) ((_a)*(_a)) @@ -101,6 +102,13 @@ (_a).z = (_z); \ } +/* a = {abs(x), abs(y), abs(z)} */ +#define VECT3_ASSIGN_ABS(_a, _x, _y, _z) { \ + (_a).x = ABS(_x); \ + (_a).y = ABS(_y); \ + (_a).z = ABS(_z); \ + } + /* a = b */ #define VECT3_COPY(_a, _b) { \ (_a).x = (_b).x; \ @@ -199,6 +207,13 @@ if ((_v).z > (_v_max).y) (_v).z = (_v_max).z; else if ((_v).z < (_v_min).z) (_v).z = (_v_min).z; \ } +/* */ +#define VECT3_ABS(_vo, _vi) { \ + (_vo).x = ABS((_vi).x); \ + (_vo).y = ABS((_vi).y); \ + (_vo).z = ABS((_vi).z); \ + } + #define VECT3_CROSS_PRODUCT(_vo, _v1, _v2) { \ (_vo).x = (_v1).y*(_v2).z - (_v1).z*(_v2).y; \ (_vo).y = (_v1).z*(_v2).x - (_v1).x*(_v2).z; \