diff --git a/conf/airframes/CDW/asctec_cdw.xml b/conf/airframes/CDW/asctec_cdw.xml index b366bcb90a..b5ca327b07 100644 --- a/conf/airframes/CDW/asctec_cdw.xml +++ b/conf/airframes/CDW/asctec_cdw.xml @@ -181,10 +181,6 @@ - - - - diff --git a/conf/airframes/CDW/tricopter_cdw.xml b/conf/airframes/CDW/tricopter_cdw.xml index ca48ac1ec1..3585eac907 100644 --- a/conf/airframes/CDW/tricopter_cdw.xml +++ b/conf/airframes/CDW/tricopter_cdw.xml @@ -178,10 +178,6 @@ LiPo/LiIo-Zellen: 2-3 - - - -
diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml index 7136bc411d..c1a5406c3d 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -224,9 +224,7 @@ - -
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index ee1b52add3..1d1a729e9a 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -198,9 +198,7 @@ - - diff --git a/conf/airframes/ENAC/quadrotor/hen1.xml b/conf/airframes/ENAC/quadrotor/hen1.xml index 16a2e39f82..2b5fa5b811 100644 --- a/conf/airframes/ENAC/quadrotor/hen1.xml +++ b/conf/airframes/ENAC/quadrotor/hen1.xml @@ -204,9 +204,7 @@ - - diff --git a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml index cf2739449c..0542c01425 100644 --- a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml +++ b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml @@ -174,10 +174,6 @@ - - - - diff --git a/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml index a5e455afdf..4ce0305ecf 100644 --- a/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml @@ -156,10 +156,6 @@ - - - - diff --git a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml index 989341a4ad..6e5b38afe9 100644 --- a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml @@ -158,10 +158,6 @@ - - - - diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml index 4356db1884..f28c89cdd5 100644 --- a/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml +++ b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml @@ -140,10 +140,6 @@ - - - - diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml index c843d8dd41..d06a6cbdee 100644 --- a/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml +++ b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml @@ -145,10 +145,6 @@ - - - - diff --git a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml index 1766366e50..af754f073d 100644 --- a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml +++ b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml @@ -233,10 +233,6 @@ - - - - diff --git a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml index 17a36534f7..a474774180 100644 --- a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml +++ b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml @@ -233,10 +233,6 @@ - - - - diff --git a/conf/airframes/ardrone2_sdk.xml b/conf/airframes/ardrone2_sdk.xml index 73c21af476..792c0bc74f 100644 --- a/conf/airframes/ardrone2_sdk.xml +++ b/conf/airframes/ardrone2_sdk.xml @@ -81,10 +81,6 @@ - - - - diff --git a/conf/airframes/esden/cocto_lm2a2.xml b/conf/airframes/esden/cocto_lm2a2.xml index 3745d83a5b..692ba0ecb6 100644 --- a/conf/airframes/esden/cocto_lm2a2.xml +++ b/conf/airframes/esden/cocto_lm2a2.xml @@ -181,10 +181,6 @@ B2L -> CW - - - - diff --git a/conf/airframes/esden/gain_scheduling_example.xml b/conf/airframes/esden/gain_scheduling_example.xml index b731662fba..c169f8c314 100644 --- a/conf/airframes/esden/gain_scheduling_example.xml +++ b/conf/airframes/esden/gain_scheduling_example.xml @@ -164,8 +164,6 @@ - - diff --git a/conf/airframes/esden/hexy_ll11a2pwm.xml b/conf/airframes/esden/hexy_ll11a2pwm.xml index f011c0e02f..c7cc5ce652 100644 --- a/conf/airframes/esden/hexy_ll11a2pwm.xml +++ b/conf/airframes/esden/hexy_ll11a2pwm.xml @@ -178,10 +178,6 @@ - - - - diff --git a/conf/airframes/esden/hexy_lm2a2pwm.xml b/conf/airframes/esden/hexy_lm2a2pwm.xml index 6078dbf311..7eb3cd3420 100644 --- a/conf/airframes/esden/hexy_lm2a2pwm.xml +++ b/conf/airframes/esden/hexy_lm2a2pwm.xml @@ -139,10 +139,6 @@ - - - - diff --git a/conf/airframes/esden/lisa2_hex.xml b/conf/airframes/esden/lisa2_hex.xml index 36bed33636..a8c786cc69 100644 --- a/conf/airframes/esden/lisa2_hex.xml +++ b/conf/airframes/esden/lisa2_hex.xml @@ -146,10 +146,6 @@ - - - - diff --git a/conf/airframes/esden/qs_asp22.xml b/conf/airframes/esden/qs_asp22.xml index b6d90d7b42..120cb296c8 100644 --- a/conf/airframes/esden/qs_asp22.xml +++ b/conf/airframes/esden/qs_asp22.xml @@ -172,8 +172,6 @@ - - diff --git a/conf/airframes/esden/quady_ll11a2pwm.xml b/conf/airframes/esden/quady_ll11a2pwm.xml index b33c992432..d4337f236c 100644 --- a/conf/airframes/esden/quady_ll11a2pwm.xml +++ b/conf/airframes/esden/quady_ll11a2pwm.xml @@ -174,10 +174,6 @@ - - - - diff --git a/conf/airframes/esden/quady_lm1a1pwm.xml b/conf/airframes/esden/quady_lm1a1pwm.xml index 82e8505f01..517dad5dd2 100644 --- a/conf/airframes/esden/quady_lm1a1pwm.xml +++ b/conf/airframes/esden/quady_lm1a1pwm.xml @@ -135,10 +135,6 @@ - - - - diff --git a/conf/airframes/esden/quady_lm2a2pwm.xml b/conf/airframes/esden/quady_lm2a2pwm.xml index 9d6fd8ff2e..b23eb2c303 100644 --- a/conf/airframes/esden/quady_lm2a2pwm.xml +++ b/conf/airframes/esden/quady_lm2a2pwm.xml @@ -138,10 +138,6 @@ - - - - diff --git a/conf/airframes/esden/quady_lm2a2pwmppm.xml b/conf/airframes/esden/quady_lm2a2pwmppm.xml index ed6df64b95..bb11c56f1d 100644 --- a/conf/airframes/esden/quady_lm2a2pwmppm.xml +++ b/conf/airframes/esden/quady_lm2a2pwmppm.xml @@ -135,10 +135,6 @@ - - - - diff --git a/conf/airframes/esden/quady_ls01pwm.xml b/conf/airframes/esden/quady_ls01pwm.xml index ef54cfdd47..00b255d544 100644 --- a/conf/airframes/esden/quady_ls01pwm.xml +++ b/conf/airframes/esden/quady_ls01pwm.xml @@ -138,10 +138,6 @@ - - - - diff --git a/conf/airframes/examples/booz2.xml b/conf/airframes/examples/booz2.xml index 82d8b1b746..72a05e09bc 100644 --- a/conf/airframes/examples/booz2.xml +++ b/conf/airframes/examples/booz2.xml @@ -159,10 +159,6 @@ - - - - diff --git a/conf/airframes/examples/h_hex.xml b/conf/airframes/examples/h_hex.xml index 893b038012..8bb83004de 100644 --- a/conf/airframes/examples/h_hex.xml +++ b/conf/airframes/examples/h_hex.xml @@ -158,10 +158,6 @@ - - - - diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml index f0222aee85..6de9a1cd82 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml @@ -205,9 +205,7 @@ - -
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml index b65ecde371..a01191d274 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml @@ -208,9 +208,7 @@ - -
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml index 78d8298454..ffdede1e73 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml @@ -24,8 +24,6 @@ - - @@ -106,6 +104,7 @@
+ @@ -144,35 +143,35 @@ - + - - - - + + + + - - - - + + + + - + - - + + - - - + + + - - - + + + @@ -181,7 +180,7 @@ - +
@@ -190,16 +189,14 @@ - - - - - - + + + + + + + - - -
@@ -208,9 +205,8 @@ - - +
@@ -228,8 +224,8 @@
- - + +
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml index 94ec9b6ca3..4a3944ecb8 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml @@ -193,9 +193,7 @@ - -
diff --git a/conf/airframes/examples/lisa_asctec.xml b/conf/airframes/examples/lisa_asctec.xml index d6ae4f8628..1440ea0781 100644 --- a/conf/airframes/examples/lisa_asctec.xml +++ b/conf/airframes/examples/lisa_asctec.xml @@ -157,10 +157,6 @@ - - - -
diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml index 9e550b5176..b332a0c730 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -183,16 +183,13 @@ - - - -
+ diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml index 2d9dc65873..136a0b1d02 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml @@ -186,10 +186,6 @@ - - - -
diff --git a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml index 654d30fc8c..016537f4f4 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml @@ -175,10 +175,6 @@ - - - - diff --git a/conf/airframes/examples/quadrotor_lisa_s.xml b/conf/airframes/examples/quadrotor_lisa_s.xml index 481c08de7e..c963633c5e 100644 --- a/conf/airframes/examples/quadrotor_lisa_s.xml +++ b/conf/airframes/examples/quadrotor_lisa_s.xml @@ -158,10 +158,6 @@ - - - - diff --git a/conf/airframes/examples/quadrotor_mlkf.xml b/conf/airframes/examples/quadrotor_mlkf.xml index a8e60c02e5..5c264fc3a7 100644 --- a/conf/airframes/examples/quadrotor_mlkf.xml +++ b/conf/airframes/examples/quadrotor_mlkf.xml @@ -183,10 +183,6 @@ - - - - diff --git a/conf/airframes/examples/quadrotor_navgo.xml b/conf/airframes/examples/quadrotor_navgo.xml index d36f1925c1..3316eec5ff 100644 --- a/conf/airframes/examples/quadrotor_navgo.xml +++ b/conf/airframes/examples/quadrotor_navgo.xml @@ -208,9 +208,7 @@ - - diff --git a/conf/airframes/examples/quadrotor_px4fmu.xml b/conf/airframes/examples/quadrotor_px4fmu.xml index d4d2aceca5..b189c2c8e6 100644 --- a/conf/airframes/examples/quadrotor_px4fmu.xml +++ b/conf/airframes/examples/quadrotor_px4fmu.xml @@ -179,10 +179,6 @@ - - - - diff --git a/conf/airframes/examples/quadshot_asp21_spektrum.xml b/conf/airframes/examples/quadshot_asp21_spektrum.xml index 69386a3d99..d1bbc5525d 100644 --- a/conf/airframes/examples/quadshot_asp21_spektrum.xml +++ b/conf/airframes/examples/quadshot_asp21_spektrum.xml @@ -196,8 +196,6 @@ More information on the Quadshot can be found at transition-robotics.com --> - - diff --git a/conf/airframes/examples/stm32f4_discovery_test.xml b/conf/airframes/examples/stm32f4_discovery_test.xml index 782c9e6e4b..6735c16ec3 100644 --- a/conf/airframes/examples/stm32f4_discovery_test.xml +++ b/conf/airframes/examples/stm32f4_discovery_test.xml @@ -179,10 +179,6 @@ - - - - diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index a99dd2f8c9..9139db0e35 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -219,16 +219,13 @@ - - - -
- + + diff --git a/conf/airframes/obsolete/ENAC/g1_vision.xml b/conf/airframes/obsolete/ENAC/g1_vision.xml index 3ffcf3ec2c..988b66025f 100644 --- a/conf/airframes/obsolete/ENAC/g1_vision.xml +++ b/conf/airframes/obsolete/ENAC/g1_vision.xml @@ -155,9 +155,7 @@ - -
diff --git a/conf/airframes/obsolete/ENAC/mkk1-vision.xml b/conf/airframes/obsolete/ENAC/mkk1-vision.xml index eccb54ea99..dc44ffc4bc 100644 --- a/conf/airframes/obsolete/ENAC/mkk1-vision.xml +++ b/conf/airframes/obsolete/ENAC/mkk1-vision.xml @@ -188,10 +188,6 @@ - - - - diff --git a/conf/airframes/obsolete/ENAC/mkk1.xml b/conf/airframes/obsolete/ENAC/mkk1.xml index 99f0d73b7d..e1b71b9eb3 100644 --- a/conf/airframes/obsolete/ENAC/mkk1.xml +++ b/conf/airframes/obsolete/ENAC/mkk1.xml @@ -208,10 +208,6 @@ - - - - diff --git a/conf/airframes/obsolete/ENAC/nova1.xml b/conf/airframes/obsolete/ENAC/nova1.xml index 1bd3135b95..7824e3e3a3 100644 --- a/conf/airframes/obsolete/ENAC/nova1.xml +++ b/conf/airframes/obsolete/ENAC/nova1.xml @@ -179,10 +179,6 @@ - - - - diff --git a/conf/airframes/obsolete/NoVa_L.xml b/conf/airframes/obsolete/NoVa_L.xml index bc7e33c41c..890f626200 100644 --- a/conf/airframes/obsolete/NoVa_L.xml +++ b/conf/airframes/obsolete/NoVa_L.xml @@ -207,10 +207,6 @@ --> - - - - diff --git a/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml b/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml index c8f68b580e..3a16d424a6 100644 --- a/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml +++ b/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml @@ -163,9 +163,7 @@ - - diff --git a/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml b/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml index b5b958a8ed..6c37dde48e 100644 --- a/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml +++ b/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml @@ -181,9 +181,7 @@ - - diff --git a/conf/airframes/obsolete/Poine/booz2_a1.xml b/conf/airframes/obsolete/Poine/booz2_a1.xml index 6a66e8dfe2..c57175d1ca 100644 --- a/conf/airframes/obsolete/Poine/booz2_a1.xml +++ b/conf/airframes/obsolete/Poine/booz2_a1.xml @@ -153,10 +153,6 @@ - - - - diff --git a/conf/airframes/obsolete/Poine/booz2_a7.xml b/conf/airframes/obsolete/Poine/booz2_a7.xml index d7fe9909c6..ebaa87a22b 100644 --- a/conf/airframes/obsolete/Poine/booz2_a7.xml +++ b/conf/airframes/obsolete/Poine/booz2_a7.xml @@ -179,10 +179,6 @@ - - - - diff --git a/conf/airframes/obsolete/Poine/h_hex.xml b/conf/airframes/obsolete/Poine/h_hex.xml index 7eb7fe8fee..3551f55043 100644 --- a/conf/airframes/obsolete/Poine/h_hex.xml +++ b/conf/airframes/obsolete/Poine/h_hex.xml @@ -139,10 +139,6 @@ - - - - diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml b/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml index 9276b95eff..70be3d2a50 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml @@ -227,10 +227,6 @@ second attempt - - - - diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml b/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml index 57642ca562..ead16fa04c 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml @@ -164,10 +164,6 @@ - - - - diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml b/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml index f22da3d19a..aaae07223d 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml @@ -188,9 +188,7 @@ - - diff --git a/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml b/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml index c350ad5ef2..03d5cc45d3 100644 --- a/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml +++ b/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml @@ -188,9 +188,7 @@ - - diff --git a/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml b/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml index 4202753b78..abdc4df4dd 100644 --- a/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml +++ b/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml @@ -228,10 +228,6 @@ second attempt - - - - diff --git a/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml b/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml index 01c4ed126f..cfccc53017 100644 --- a/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml +++ b/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml @@ -143,10 +143,6 @@ - - - - diff --git a/conf/airframes/obsolete/booz2_Aron.xml b/conf/airframes/obsolete/booz2_Aron.xml index 754f24d400..426c2a1991 100644 --- a/conf/airframes/obsolete/booz2_Aron.xml +++ b/conf/airframes/obsolete/booz2_Aron.xml @@ -148,9 +148,7 @@ - -
diff --git a/conf/airframes/obsolete/booz2_NoVa.xml b/conf/airframes/obsolete/booz2_NoVa.xml index 928ac58841..11aa351569 100644 --- a/conf/airframes/obsolete/booz2_NoVa.xml +++ b/conf/airframes/obsolete/booz2_NoVa.xml @@ -188,9 +188,7 @@ - -
diff --git a/conf/airframes/obsolete/booz2_NoVa_001.xml b/conf/airframes/obsolete/booz2_NoVa_001.xml index 088bf5a7ec..8b303bb013 100644 --- a/conf/airframes/obsolete/booz2_NoVa_001.xml +++ b/conf/airframes/obsolete/booz2_NoVa_001.xml @@ -188,9 +188,7 @@ - - diff --git a/conf/airframes/obsolete/booz2_NoVa_002.xml b/conf/airframes/obsolete/booz2_NoVa_002.xml index 1f268ba327..d0e286b964 100644 --- a/conf/airframes/obsolete/booz2_NoVa_002.xml +++ b/conf/airframes/obsolete/booz2_NoVa_002.xml @@ -188,9 +188,7 @@ - - diff --git a/conf/airframes/obsolete/booz2_a1p.xml b/conf/airframes/obsolete/booz2_a1p.xml index 22d5124d79..743dd891d9 100644 --- a/conf/airframes/obsolete/booz2_a1p.xml +++ b/conf/airframes/obsolete/booz2_a1p.xml @@ -157,10 +157,6 @@ - - - - diff --git a/conf/airframes/obsolete/booz2_a2.xml b/conf/airframes/obsolete/booz2_a2.xml index df65d7bb25..88ffa59789 100644 --- a/conf/airframes/obsolete/booz2_a2.xml +++ b/conf/airframes/obsolete/booz2_a2.xml @@ -148,9 +148,7 @@ - -
diff --git a/conf/airframes/obsolete/booz2_a3.xml b/conf/airframes/obsolete/booz2_a3.xml index 086855cb34..c234c2b1a7 100644 --- a/conf/airframes/obsolete/booz2_a3.xml +++ b/conf/airframes/obsolete/booz2_a3.xml @@ -133,9 +133,7 @@ - -
diff --git a/conf/airframes/obsolete/booz2_a4.xml b/conf/airframes/obsolete/booz2_a4.xml index 883dd43ef9..776ed8f12b 100644 --- a/conf/airframes/obsolete/booz2_a4.xml +++ b/conf/airframes/obsolete/booz2_a4.xml @@ -111,9 +111,7 @@ - -
diff --git a/conf/airframes/obsolete/booz2_a5.xml b/conf/airframes/obsolete/booz2_a5.xml index e0c43ad6ee..3f79fb0e75 100644 --- a/conf/airframes/obsolete/booz2_a5.xml +++ b/conf/airframes/obsolete/booz2_a5.xml @@ -149,10 +149,6 @@ - - - -
diff --git a/conf/airframes/obsolete/booz2_flixr.xml b/conf/airframes/obsolete/booz2_flixr.xml index 379e112587..b3a8cab4f5 100644 --- a/conf/airframes/obsolete/booz2_flixr.xml +++ b/conf/airframes/obsolete/booz2_flixr.xml @@ -203,9 +203,7 @@ - - diff --git a/conf/airframes/obsolete/booz2_ppzuav.xml b/conf/airframes/obsolete/booz2_ppzuav.xml index a34b282fd0..6915b576da 100644 --- a/conf/airframes/obsolete/booz2_ppzuav.xml +++ b/conf/airframes/obsolete/booz2_ppzuav.xml @@ -164,9 +164,7 @@ - -
diff --git a/conf/airframes/obsolete/booz2_s1.xml b/conf/airframes/obsolete/booz2_s1.xml index c4a37b4cf7..54d25a357e 100644 --- a/conf/airframes/obsolete/booz2_s1.xml +++ b/conf/airframes/obsolete/booz2_s1.xml @@ -141,9 +141,7 @@ - -
diff --git a/conf/airframes/obsolete/booz2_x1.xml b/conf/airframes/obsolete/booz2_x1.xml index ee626f8deb..97fc3920ba 100644 --- a/conf/airframes/obsolete/booz2_x1.xml +++ b/conf/airframes/obsolete/booz2_x1.xml @@ -153,9 +153,7 @@ - - diff --git a/conf/airframes/obsolete/example_heli_lisam.xml b/conf/airframes/obsolete/example_heli_lisam.xml index c0d13704d0..33e8073432 100644 --- a/conf/airframes/obsolete/example_heli_lisam.xml +++ b/conf/airframes/obsolete/example_heli_lisam.xml @@ -192,10 +192,6 @@ AP_MODE_NAV - - - - diff --git a/conf/airframes/obsolete/mm/rotor/qmk1.xml b/conf/airframes/obsolete/mm/rotor/qmk1.xml index 3d05729aa2..a593c6aec1 100644 --- a/conf/airframes/obsolete/mm/rotor/qmk1.xml +++ b/conf/airframes/obsolete/mm/rotor/qmk1.xml @@ -152,10 +152,6 @@ - - - - diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml index 3e43cd0eda..154bce55bf 100644 --- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml +++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml @@ -200,10 +200,6 @@ - - - - diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml index 0bac6a6e4e..5fd31a6e33 100644 --- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml +++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml @@ -163,10 +163,6 @@ - - - -
diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml index f2e2ee5a09..32b121c0f6 100644 --- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml +++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml @@ -170,10 +170,6 @@ - - - -
diff --git a/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile b/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile index d79598d8eb..448148fedc 100644 --- a/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile @@ -3,6 +3,7 @@ STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_QUAT STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_quat_float.h\" STAB_ATT_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_float.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_float.c +STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_transformations.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c ap.CFLAGS += $(STAB_ATT_CFLAGS) diff --git a/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile b/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile index 317172085a..50e15da7b1 100644 --- a/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile @@ -3,6 +3,7 @@ STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_QUAT STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_quat_int.h\" STAB_ATT_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c +STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_transformations.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c ap.CFLAGS += $(STAB_ATT_CFLAGS) diff --git a/conf/settings/control/rotorcraft_guidance.xml b/conf/settings/control/rotorcraft_guidance.xml index 45c73a958b..c44ecb0069 100644 --- a/conf/settings/control/rotorcraft_guidance.xml +++ b/conf/settings/control/rotorcraft_guidance.xml @@ -15,6 +15,7 @@ + diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 842f3ccf21..4f48097d9f 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -30,6 +30,9 @@ #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" #include "firmwares/rotorcraft/navigation.h" +/* for guidance_v_thrust_coeff */ +#include "firmwares/rotorcraft/guidance/guidance_v.h" + #include "state.h" #include "generated/airframe.h" @@ -55,24 +58,30 @@ PRINT_CONFIG_VAR(GUIDANCE_H_USE_REF) +#ifndef GUIDANCE_H_APPROX_FORCE_BY_THRUST +#define GUIDANCE_H_APPROX_FORCE_BY_THRUST FALSE +#endif + uint8_t guidance_h_mode; bool_t guidance_h_use_ref; +bool_t guidance_h_approx_force_by_thrust; struct Int32Vect2 guidance_h_pos_sp; struct Int32Vect2 guidance_h_pos_ref; struct Int32Vect2 guidance_h_speed_ref; struct Int32Vect2 guidance_h_accel_ref; - +#if GUIDANCE_H_USE_SPEED_REF +struct Int32Vect2 guidance_h_speed_sp; +#endif struct Int32Vect2 guidance_h_pos_err; struct Int32Vect2 guidance_h_speed_err; -struct Int32Vect2 guidance_h_pos_err_sum; +struct Int32Vect2 guidance_h_trim_att_integrator; struct Int32Vect2 guidance_h_nav_err; +struct Int32Vect2 guidance_h_cmd_earth; struct Int32Eulers guidance_h_rc_sp; -struct Int32Vect2 guidance_h_command_earth; -struct Int32Vect2 guidance_h_stick_earth_sp; -struct Int32Eulers guidance_h_command_body; +int32_t guidance_h_heading_sp; int32_t guidance_h_pgain; int32_t guidance_h_dgain; @@ -88,17 +97,19 @@ static void guidance_h_traj_run(bool_t in_flight); static void guidance_h_hover_enter(void); static void guidance_h_nav_enter(void); static inline void transition_run(void); +static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flight); void guidance_h_init(void) { guidance_h_mode = GUIDANCE_H_MODE_KILL; guidance_h_use_ref = GUIDANCE_H_USE_REF; + guidance_h_approx_force_by_thrust = GUIDANCE_H_APPROX_FORCE_BY_THRUST; INT_VECT2_ZERO(guidance_h_pos_sp); - INT_VECT2_ZERO(guidance_h_pos_err_sum); + INT_VECT2_ZERO(guidance_h_trim_att_integrator); INT_EULERS_ZERO(guidance_h_rc_sp); - INT_EULERS_ZERO(guidance_h_command_body); + guidance_h_heading_sp = 0; guidance_h_pgain = GUIDANCE_H_PGAIN; guidance_h_igain = GUIDANCE_H_IGAIN; guidance_h_dgain = GUIDANCE_H_DGAIN; @@ -114,7 +125,7 @@ static inline void reset_guidance_reference_from_current_position(void) { INT_VECT2_ZERO(guidance_h_accel_ref); gh_set_ref(guidance_h_pos_ref, guidance_h_speed_ref, guidance_h_accel_ref); - INT_VECT2_ZERO(guidance_h_pos_err_sum); + INT_VECT2_ZERO(guidance_h_trim_att_integrator); } void guidance_h_mode_changed(uint8_t new_mode) { @@ -203,6 +214,9 @@ void guidance_h_read_rc(bool_t in_flight) { case GUIDANCE_H_MODE_HOVER: stabilization_attitude_read_rc_setpoint_eulers(&guidance_h_rc_sp, in_flight); +#if GUIDANCE_H_USE_SPEED_REF + read_rc_setpoint_speed_i(&guidance_h_speed_sp, in_flight); +#endif break; case GUIDANCE_H_MODE_NAV: @@ -247,10 +261,12 @@ void guidance_h_run(bool_t in_flight) { guidance_h_update_reference(); /* set psi command */ - guidance_h_command_body.psi = guidance_h_rc_sp.psi; - /* compute roll and pitch commands and set final attitude setpoint */ + guidance_h_heading_sp = guidance_h_rc_sp.psi; + /* compute x,y earth commands */ guidance_h_traj_run(in_flight); - + /* set final attitude setpoint */ + stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, + guidance_h_heading_sp); stabilization_attitude_run(in_flight); break; @@ -262,9 +278,11 @@ void guidance_h_run(bool_t in_flight) { struct Int32Eulers sp_cmd_i; sp_cmd_i.phi = nav_roll; sp_cmd_i.theta = nav_pitch; - /* FIXME: heading can't be set via attitude block yet, use current heading for now */ + /** @todo: heading can't be set via attitude block yet. + * use current euler psi for now, should be real heading + */ sp_cmd_i.psi = stateGetNedToBodyEulers_i()->psi; - stabilization_attitude_set_cmd_i(&sp_cmd_i); + stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i); } else { INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot); @@ -272,10 +290,13 @@ void guidance_h_run(bool_t in_flight) { guidance_h_update_reference(); /* set psi command */ - guidance_h_command_body.psi = nav_heading; - INT32_ANGLE_NORMALIZE(guidance_h_command_body.psi); - /* compute roll and pitch commands and set final attitude setpoint */ + guidance_h_heading_sp = nav_heading; + INT32_ANGLE_NORMALIZE(guidance_h_heading_sp); + /* compute x,y earth commands */ guidance_h_traj_run(in_flight); + /* set final attitude setpoint */ + stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, + guidance_h_heading_sp); } stabilization_attitude_run(in_flight); break; @@ -289,6 +310,11 @@ void guidance_h_run(bool_t in_flight) { static void guidance_h_update_reference(void) { /* compute reference even if usage temporarily disabled via guidance_h_use_ref */ #if GUIDANCE_H_USE_REF +#if GUIDANCE_H_USE_SPEED_REF + if(guidance_h_mode == GUIDANCE_H_MODE_HOVER) + gh_update_ref_from_speed_sp(guidance_h_speed_sp); + else +#endif gh_update_ref_from_pos_sp(guidance_h_pos_sp); #endif @@ -303,21 +329,31 @@ static void guidance_h_update_reference(void) { INT_VECT2_ZERO(guidance_h_speed_ref); INT_VECT2_ZERO(guidance_h_accel_ref); } + +#if GUIDANCE_H_USE_SPEED_REF + if(guidance_h_mode == GUIDANCE_H_MODE_HOVER) { + VECT2_COPY(guidance_h_pos_sp, guidance_h_pos_ref); // for display only + } +#endif } #define MAX_POS_ERR POS_BFP_OF_REAL(16.) #define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.) -#define MAX_POS_ERR_SUM ((int32_t)(MAX_POS_ERR)<< 12) + +#ifndef GUIDANCE_H_THRUST_CMD_FILTER +#define GUIDANCE_H_THRUST_CMD_FILTER 10 +#endif /* with a pgain of 100 and a scale of 2, * you get an angle of 5.6 degrees for 1m pos error */ #define GH_GAIN_SCALE 2 -/** maximum bank angle: default 20 deg */ -#define TRAJ_MAX_BANK BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC) - static void guidance_h_traj_run(bool_t in_flight) { + /* maximum bank angle: default 20 deg, max 40 deg*/ + static const int32_t traj_max_bank = Max(BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC), + BFP_OF_REAL(RadOfDeg(40), INT32_ANGLE_FRAC)); + static const int32_t total_max_bank = BFP_OF_REAL(RadOfDeg(45), INT32_ANGLE_FRAC); /* compute position error */ VECT2_DIFF(guidance_h_pos_err, guidance_h_pos_ref, *stateGetPositionNed_i()); @@ -329,48 +365,50 @@ static void guidance_h_traj_run(bool_t in_flight) { /* saturate it */ VECT2_STRIM(guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR); - /* update pos error integral, zero it if not in_flight */ + /* run PID */ + int32_t pd_x = + ((guidance_h_pgain * guidance_h_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + + ((guidance_h_dgain * (guidance_h_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)); + int32_t pd_y = + ((guidance_h_pgain * guidance_h_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + + ((guidance_h_dgain * (guidance_h_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)); + guidance_h_cmd_earth.x = + pd_x + + ((guidance_h_again * guidance_h_accel_ref.x) >> 8); /* acceleration feedforward gain */ + guidance_h_cmd_earth.y = + pd_y + + ((guidance_h_again * guidance_h_accel_ref.y) >> 8); /* acceleration feedforward gain */ + + /* trim max bank angle from PD */ + VECT2_STRIM(guidance_h_cmd_earth, -traj_max_bank, traj_max_bank); + + /* Update pos & speed error integral, zero it if not in_flight. + * Integrate twice as fast when not only POS but also SPEED are wrong, + * but do not integrate POS errors when the SPEED is already catching up. + */ if (in_flight) { - VECT2_ADD(guidance_h_pos_err_sum, guidance_h_pos_err); - /* saturate it */ - VECT2_STRIM(guidance_h_pos_err_sum, -MAX_POS_ERR_SUM, MAX_POS_ERR_SUM); + /* ANGLE_FRAC (12) * GAIN (8) * LOOP_FREQ (9) -> ANGLE_FRAX (12) */ + guidance_h_trim_att_integrator.x += ((guidance_h_igain * pd_x) >> (8)); + guidance_h_trim_att_integrator.y += ((guidance_h_igain * pd_y) >> (8)); + /* saturate it */ + VECT2_STRIM(guidance_h_trim_att_integrator, -(traj_max_bank << 12) , (traj_max_bank << 12)); + /* add it to the command */ + guidance_h_cmd_earth.x += guidance_h_trim_att_integrator.x >> 12; + guidance_h_cmd_earth.y += guidance_h_trim_att_integrator.y >> 12; } else { - INT_VECT2_ZERO(guidance_h_pos_err_sum); + INT_VECT2_ZERO(guidance_h_trim_att_integrator); } - /* run PID */ - guidance_h_command_earth.x = - ((guidance_h_pgain * guidance_h_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + - ((guidance_h_dgain * (guidance_h_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)) + - ((guidance_h_igain * (guidance_h_pos_err_sum.x >> 12)) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + - ((guidance_h_again * guidance_h_accel_ref.x) >> 8); /* feedforward gain */ - guidance_h_command_earth.y = - ((guidance_h_pgain * guidance_h_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + - ((guidance_h_dgain * (guidance_h_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)) + - ((guidance_h_igain * (guidance_h_pos_err_sum.y >> 12)) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + - ((guidance_h_again * guidance_h_accel_ref.y) >> 8); /* feedforward gain */ + /* compute a better approximation of force commands by taking thrust into account */ + if (guidance_h_approx_force_by_thrust && in_flight) { + static int32_t thrust_cmd_filt; + int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC; + thrust_cmd_filt = (thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + vertical_thrust) / (GUIDANCE_H_THRUST_CMD_FILTER + 1); + guidance_h_cmd_earth.x = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.x * MAX_PPRZ / INT32_ANGLE_PI_2), thrust_cmd_filt)); + guidance_h_cmd_earth.y = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.y * MAX_PPRZ / INT32_ANGLE_PI_2), thrust_cmd_filt)); + } - VECT2_STRIM(guidance_h_command_earth, -TRAJ_MAX_BANK, TRAJ_MAX_BANK); - - /* Rotate to body frame */ - int32_t s_psi, c_psi; - int32_t psi = stateGetNedToBodyEulers_i()->psi; - PPRZ_ITRIG_SIN(s_psi, psi); - PPRZ_ITRIG_COS(c_psi, psi); - - // Restore angle ref resolution after rotation - guidance_h_command_body.phi = - ( - s_psi * guidance_h_command_earth.x + c_psi * guidance_h_command_earth.y) >> INT32_TRIG_FRAC; - guidance_h_command_body.theta = - - ( c_psi * guidance_h_command_earth.x + s_psi * guidance_h_command_earth.y) >> INT32_TRIG_FRAC; - - - /* Add RC roll and pitch setpoints for emergency corrections */ - guidance_h_command_body.phi += guidance_h_rc_sp.phi; - guidance_h_command_body.theta += guidance_h_rc_sp.theta; - - /* Set attitude setpoint from pseudo-euler commands */ - stabilization_attitude_set_cmd_i(&guidance_h_command_body); + VECT2_STRIM(guidance_h_cmd_earth, -total_max_bank, total_max_bank); } static void guidance_h_hover_enter(void) { @@ -402,3 +440,34 @@ static inline void transition_run(void) { transition_theta_offset = INT_MULT_RSHIFT((transition_percentage<<(INT32_ANGLE_FRAC-INT32_PERCENTAGE_FRAC))/100, max_offset, INT32_ANGLE_FRAC); #endif } + +/// read speed setpoint from RC +static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flight) { + if(in_flight) { + // negative pitch is forward + int64_t rc_x = -radio_control.values[RADIO_PITCH]; + int64_t rc_y = radio_control.values[RADIO_ROLL]; + DeadBand(rc_x, MAX_PPRZ/20); + DeadBand(rc_y, MAX_PPRZ/20); + + // convert input from MAX_PPRZ range to SPEED_BFP + int32_t max_speed = SPEED_BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED); + /// @todo calc proper scale while making sure a division by zero can't occur + //int32_t rc_norm = sqrtf(rc_x * rc_x + rc_y * rc_y); + //int32_t max_pprz = rc_norm * MAX_PPRZ / Max(abs(rc_x), abs(rc_y); + rc_x = rc_x * max_speed / MAX_PPRZ; + rc_y = rc_y * max_speed / MAX_PPRZ; + + /* Rotate from body to NED frame by negative psi angle */ + int32_t psi = -stateGetNedToBodyEulers_i()->psi; + int32_t s_psi, c_psi; + PPRZ_ITRIG_SIN(s_psi, psi); + PPRZ_ITRIG_COS(c_psi, psi); + speed_sp->x = (int32_t)(((int64_t)c_psi * rc_x - (int64_t)s_psi * rc_y) >> INT32_TRIG_FRAC); + speed_sp->y = (int32_t)(((int64_t)s_psi * rc_x + (int64_t)c_psi * rc_y) >> INT32_TRIG_FRAC); + } + else { + speed_sp->x = 0; + speed_sp->y = 0; + } +} diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index 5a136317d5..f8a934ce43 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -53,6 +53,7 @@ extern uint8_t guidance_h_mode; extern bool_t guidance_h_use_ref; +extern bool_t guidance_h_approx_force_by_thrust; /** horizontal position setpoint in NED. * fixed point representation: Q23.8 @@ -66,12 +67,17 @@ extern struct Int32Vect2 guidance_h_accel_ref; ///< with #INT32_ACCEL_FRAC extern struct Int32Vect2 guidance_h_pos_err; extern struct Int32Vect2 guidance_h_speed_err; -extern struct Int32Vect2 guidance_h_pos_err_sum; +extern struct Int32Vect2 guidance_h_trim_att_integrator; extern struct Int32Vect2 guidance_h_nav_err; + +/** horizontal guidance command. + * In north/east with #INT32_ANGLE_FRAC + * @todo convert to real force command + */ +extern struct Int32Vect2 guidance_h_cmd_earth; extern struct Int32Eulers guidance_h_rc_sp; ///< with #INT32_ANGLE_FRAC -extern struct Int32Vect2 guidance_h_command_earth; -extern struct Int32Eulers guidance_h_command_body; ///< with #INT32_ANGLE_FRAC +extern int32_t guidance_h_heading_sp; ///< with #INT32_ANGLE_FRAC extern int32_t guidance_h_pgain; extern int32_t guidance_h_dgain; @@ -89,7 +95,7 @@ extern void guidance_h_run(bool_t in_flight); #define guidance_h_SetKi(_val) { \ guidance_h_igain = _val; \ - INT_VECT2_ZERO(guidance_h_pos_err_sum); \ + INT_VECT2_ZERO(guidance_h_trim_att_integrator); \ } /* Make sure that ref can only be temporarily disabled for testing, diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c index 34b23d212c..fb8be1fa67 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c @@ -47,18 +47,9 @@ struct Int32Vect2 gh_speed_ref; */ struct Int64Vect2 gh_pos_ref; -/** Accel saturation. - * tanf(RadOfDeg(30.))*9.81 = 5.66 - */ -#ifndef GUIDANCE_H_REF_MAX_ACCEL -#define GUIDANCE_H_REF_MAX_ACCEL 5.66 -#endif + static const int32_t gh_max_accel = BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC); -/** Speed saturation */ -#ifndef GUIDANCE_H_REF_MAX_SPEED -#define GUIDANCE_H_REF_MAX_SPEED 5. -#endif /** @todo GH_MAX_SPEED must be limited to 2^14 to avoid overflow */ #define GH_MAX_SPEED_REF_FRAC 7 static const int32_t gh_max_speed = BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GH_MAX_SPEED_REF_FRAC); diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h index ab285baa11..179097db55 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h @@ -31,6 +31,18 @@ #include "math/pprz_algebra.h" #include "math/pprz_algebra_int.h" +/** Speed saturation */ +#ifndef GUIDANCE_H_REF_MAX_SPEED +#define GUIDANCE_H_REF_MAX_SPEED 5. +#endif + +/** Accel saturation. + * tanf(RadOfDeg(30.))*9.81 = 5.66 + */ +#ifndef GUIDANCE_H_REF_MAX_ACCEL +#define GUIDANCE_H_REF_MAX_ACCEL 5.66 +#endif + /** Update frequency */ #define GH_FREQ_FRAC 9 diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 23f625b442..7ec75d152d 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -62,6 +62,19 @@ PRINT_CONFIG_VAR(GUIDANCE_V_NOMINAL_HOVER_THROTTLE) PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_THROTTLE_ENABLED) +#ifndef GUIDANCE_V_CLIMB_RC_DEADBAND +#define GUIDANCE_V_CLIMB_RC_DEADBAND MAX_PPRZ/10 +#endif + +#ifndef GUIDANCE_V_MAX_RC_CLIMB_SPEED +#define GUIDANCE_V_MAX_RC_CLIMB_SPEED GUIDANCE_V_REF_MIN_ZD +#endif + +#ifndef GUIDANCE_V_MAX_RC_DESCENT_SPEED +#define GUIDANCE_V_MAX_RC_DESCENT_SPEED GUIDANCE_V_REF_MAX_ZD +#endif + + uint8_t guidance_v_mode; int32_t guidance_v_ff_cmd; int32_t guidance_v_fb_cmd; @@ -94,14 +107,14 @@ int32_t guidance_v_ki; int32_t guidance_v_z_sum_err; -static int32_t guidance_v_thrust_coeff; +int32_t guidance_v_thrust_coeff; #define GuidanceVSetRef(_pos, _speed, _accel) { \ - gv_set_ref(_pos, _speed, _accel); \ - guidance_v_z_ref = _pos; \ - guidance_v_zd_ref = _speed; \ - guidance_v_zdd_ref = _accel; \ + gv_set_ref(_pos, _speed, _accel); \ + guidance_v_z_ref = _pos; \ + guidance_v_zd_ref = _speed; \ + guidance_v_zdd_ref = _accel; \ } static int32_t get_vertical_thrust_coeff(void); @@ -131,9 +144,18 @@ void guidance_v_read_rc(void) { guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE]; /* used in RC_CLIMB */ - guidance_v_rc_zd_sp = ((MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]) * GUIDANCE_V_RC_CLIMB_COEF; - DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_RC_CLIMB_DEAD_BAND); + guidance_v_rc_zd_sp = (MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]; + DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_CLIMB_RC_DEADBAND); + static const int32_t climb_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_CLIMB_SPEED) / + (MAX_PPRZ/2 - GUIDANCE_V_CLIMB_RC_DEADBAND)); + static const int32_t descent_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_DESCENT_SPEED) / + (MAX_PPRZ/2 - GUIDANCE_V_CLIMB_RC_DEADBAND)); + + if(guidance_v_rc_zd_sp > 0) + guidance_v_rc_zd_sp *= descent_scale; + else + guidance_v_rc_zd_sp *= climb_scale; } void guidance_v_mode_changed(uint8_t new_mode) { diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h index 4a94b4cfa8..93e0aeaa4d 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h @@ -91,6 +91,9 @@ extern float guidance_v_nominal_throttle; */ extern bool_t guidance_v_adapt_throttle_enabled; + +extern int32_t guidance_v_thrust_coeff; + extern int32_t guidance_v_kp; ///< vertical control P-gain extern int32_t guidance_v_kd; ///< vertical control D-gain extern int32_t guidance_v_ki; ///< vertical control I-gain diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c index 281827bb0b..31d1e2c0e4 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c @@ -58,14 +58,7 @@ int64_t gv_z_ref; #endif #define GV_MAX_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZDD, GV_ZDD_REF_FRAC) -#ifndef GUIDANCE_V_REF_MIN_ZD -#define GUIDANCE_V_REF_MIN_ZD (-3.) -#endif #define GV_MIN_ZD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD , GV_ZD_REF_FRAC) - -#ifndef GUIDANCE_V_REF_MAX_ZD -#define GUIDANCE_V_REF_MAX_ZD ( 3.) -#endif #define GV_MAX_ZD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD , GV_ZD_REF_FRAC) /* second order model natural frequency and damping */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h index f462a07a2b..3816a9031b 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h @@ -32,6 +32,16 @@ #include "math/pprz_algebra.h" #include "math/pprz_algebra_int.h" + +#ifndef GUIDANCE_V_REF_MIN_ZD +#define GUIDANCE_V_REF_MIN_ZD (-3.) +#endif + +#ifndef GUIDANCE_V_REF_MAX_ZD +#define GUIDANCE_V_REF_MAX_ZD ( 3.) +#endif + + /** Update frequency */ #define GV_FREQ_FRAC 9 diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index e18811e500..c6964ca67c 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -145,13 +145,11 @@ static inline void nav_advance_carrot(void) { void nav_run(void) { -#if !GUIDANCE_H_USE_REF - PRINT_CONFIG_MSG("NOT using horizontal guidance reference :-(") - nav_advance_carrot(); -#else - PRINT_CONFIG_MSG("Using horizontal guidance reference :-)") - // if H_REF is used, CARROT_DIST is not used +#if GUIDANCE_H_USE_REF + // if GUIDANCE_H_USE_REF, CARROT_DIST is not used VECT2_COPY(navigation_carrot, navigation_target); +#else + nav_advance_carrot(); #endif nav_set_altitude(); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h index 94b47466e4..8917a2d93c 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h @@ -34,8 +34,9 @@ extern void stabilization_attitude_init(void); extern void stabilization_attitude_read_rc(bool_t in_flight); extern void stabilization_attitude_enter(void); extern void stabilization_attitude_set_failsafe_setpoint(void); -extern void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd); -extern void stabilization_attitude_run(bool_t in_flight); +extern void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy); +extern void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading); +extern void stabilization_attitude_run(bool_t in_flight); #endif /* STABILIZATION_ATTITUDE_H */ 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 3a5df19417..3e77aac20c 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c @@ -86,10 +86,23 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_euler.psi = stateGetNedToBodyEulers_f()->psi; } -void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { - EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd); +void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { + EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *rpy); } +void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { + struct FloatVect2 cmd_f; + cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x); + cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y); + + /* Rotate horizontal commands to body frame by psi */ + float psi = stateGetNedToBodyEulers_f()->psi; + float s_psi = sinf(psi); + float c_psi = cosf(psi); + stab_att_sp_euler.phi = -s_psi * cmd_f.x + c_psi * cmd_f.y; + stab_att_sp_euler.theta = -c_psi * cmd_f.x - s_psi * cmd_f.y; + stab_att_sp_euler.psi = ANGLE_FLOAT_OF_BFP(heading); +} #define MAX_SUM_ERR RadOfDeg(56000) 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 a86f9c4c80..27285f4403 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -102,10 +102,20 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi; } -void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { - memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers)); +void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { + memcpy(&stab_att_sp_euler, rpy, sizeof(struct Int32Eulers)); } +void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { + /* Rotate horizontal commands to body frame by psi */ + int32_t psi = stateGetNedToBodyEulers_i()->psi; + int32_t s_psi, c_psi; + PPRZ_ITRIG_SIN(s_psi, psi); + PPRZ_ITRIG_COS(c_psi, psi); + stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC; + stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC; + stab_att_sp_euler.psi = heading; +} #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b)) 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 a5764fa859..ee7f42f067 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -25,6 +25,7 @@ #include "firmwares/rotorcraft/stabilization.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h" #include #include "math/pprz_algebra_float.h" @@ -142,75 +143,21 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_quat.qz = sinf(heading2); } -void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { - EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd); +void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { + // copy euler setpoint for debugging + EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *rpy); - /* orientation vector describing simultaneous rotation of roll/pitch */ - struct FloatVect3 ov; - ov.x = stab_att_sp_euler.phi; - ov.y = stab_att_sp_euler.theta; - ov.z = 0.0; - /* quaternion from that orientation vector */ - struct FloatQuat q_rp; - FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); + quat_from_rpy_cmd_f(&stab_att_sp_quat, &stab_att_sp_euler); +} - /// @todo optimize yaw angle calculation +void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { + struct FloatVect2 cmd_f; + cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x); + cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y); + float heading_f; + heading_f = ANGLE_FLOAT_OF_BFP(heading); - /* - * Instead of using the psi setpoint angle to rotate around the body z-axis, - * calculate the real angle needed to align the projection of the body x-axis - * onto the horizontal plane with the psi setpoint. - * - * angle between two vectors a and b: - * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n)) - * where n is the thrust vector (i.e. both a and b lie in that plane) - */ - const struct FloatVect3 xaxis = {1.0, 0.0, 0.0}; - const struct FloatVect3 zaxis = {0.0, 0.0, 1.0}; - struct FloatVect3 a; - FLOAT_QUAT_VMULT(a, q_rp, xaxis); - - // desired heading vect in earth x-y plane - struct FloatVect3 psi_vect; - psi_vect.x = cosf(stab_att_sp_euler.psi); - psi_vect.y = sinf(stab_att_sp_euler.psi); - psi_vect.z = 0.0; - // normal is the direction of the thrust vector - struct FloatVect3 normal; - FLOAT_QUAT_VMULT(normal, q_rp, zaxis); - - // projection of desired heading onto body x-y plane - // b = v - dot(v,n)*n - float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal); - struct FloatVect3 dotn; - FLOAT_VECT3_SMUL(dotn, normal, dot); - - // b = v - dot(v,n)*n - struct FloatVect3 b; - FLOAT_VECT3_DIFF(b, psi_vect, dotn); - dot = FLOAT_VECT3_DOT_PRODUCT(a, b); - struct FloatVect3 cross; - VECT3_CROSS_PRODUCT(cross, a, b); - // norm of the cross product - float nc = FLOAT_VECT3_NORM(cross); - // angle = atan2(norm(cross(a,b)), dot(a,b)) - float yaw2 = atan2(nc, dot) / 2.0; - - // negative angle if needed - // sign(dot(cross(a,b), n) - float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, normal); - if (dot_cross_ab < 0) { - yaw2 = -yaw2; - } - - /* quaternion with yaw command */ - struct FloatQuat q_yaw; - QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); - - /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ - FLOAT_QUAT_COMP(stab_att_sp_quat, q_yaw, q_rp); - FLOAT_QUAT_NORMALIZE(stab_att_sp_quat); - FLOAT_QUAT_WRAP_SHORTEST(stab_att_sp_quat); + quat_from_earth_cmd_f(&stab_att_sp_quat, &cmd_f, heading_f); } #ifndef GAIN_PRESCALER_FF diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h index 9077bbd8cb..8655fc23ac 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h @@ -31,13 +31,6 @@ #include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h" -#ifndef STABILIZATION_ATTITUDE_GAIN_NB -#define STABILIZATION_ATTITUDE_GAIN_NB 1 -#endif - -#ifndef STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT -#define STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT 0 -#endif extern struct FloatAttitudeGains stabilization_gains[]; 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 f709f9f5c0..0f49833b4a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -25,6 +25,7 @@ #include "firmwares/rotorcraft/stabilization.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h" #include #include "math/pprz_algebra_float.h" @@ -92,85 +93,27 @@ void stabilization_attitude_set_failsafe_setpoint(void) { PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2); } -void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { - // copy euler setpoint for debugging - memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers)); +void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { + // stab_att_sp_euler.psi still used in ref.. + memcpy(&stab_att_sp_euler, rpy, sizeof(struct Int32Eulers)); - /// @todo calc sp_quat in fixed-point + quat_from_rpy_cmd_i(&stab_att_sp_quat, &stab_att_sp_euler); +} - /* orientation vector describing simultaneous rotation of roll/pitch */ - struct FloatVect3 ov; - ov.x = ANGLE_FLOAT_OF_BFP(sp_cmd->phi); - ov.y = ANGLE_FLOAT_OF_BFP(sp_cmd->theta); - ov.z = 0.0; - /* quaternion from that orientation vector */ - struct FloatQuat q_rp; - FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); +void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { + // stab_att_sp_euler.psi still used in ref.. + stab_att_sp_euler.psi = heading; - const float psi_sp = ANGLE_FLOAT_OF_BFP(sp_cmd->psi); + // compute sp_euler phi/theta for debugging/telemetry + /* Rotate horizontal commands to body frame by psi */ + int32_t psi = stateGetNedToBodyEulers_i()->psi; + int32_t s_psi, c_psi; + PPRZ_ITRIG_SIN(s_psi, psi); + PPRZ_ITRIG_COS(c_psi, psi); + stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC; + stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC; - /// @todo optimize yaw angle calculation - - /* - * Instead of using the psi setpoint angle to rotate around the body z-axis, - * calculate the real angle needed to align the projection of the body x-axis - * onto the horizontal plane with the psi setpoint. - * - * angle between two vectors a and b: - * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n)) - * where n is the thrust vector (i.e. both a and b lie in that plane) - */ - const struct FloatVect3 xaxis = {1.0, 0.0, 0.0}; - const struct FloatVect3 zaxis = {0.0, 0.0, 1.0}; - struct FloatVect3 a; - FLOAT_QUAT_VMULT(a, q_rp, xaxis); - - // desired heading vect in earth x-y plane - struct FloatVect3 psi_vect; - psi_vect.x = cosf(psi_sp); - psi_vect.y = sinf(psi_sp); - psi_vect.z = 0.0; - // normal is the direction of the thrust vector - struct FloatVect3 normal; - FLOAT_QUAT_VMULT(normal, q_rp, zaxis); - - // projection of desired heading onto body x-y plane - // b = v - dot(v,n)*n - float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal); - struct FloatVect3 dotn; - FLOAT_VECT3_SMUL(dotn, normal, dot); - - // b = v - dot(v,n)*n - struct FloatVect3 b; - FLOAT_VECT3_DIFF(b, psi_vect, dotn); - - dot = FLOAT_VECT3_DOT_PRODUCT(a, b); - struct FloatVect3 cross; - VECT3_CROSS_PRODUCT(cross, a, b); - // norm of the cross product - float nc = FLOAT_VECT3_NORM(cross); - // angle = atan2(norm(cross(a,b)), dot(a,b)) - float yaw2 = atan2(nc, dot) / 2.0; - - // negative angle if needed - // sign(dot(cross(a,b), n) - float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, normal); - if (dot_cross_ab < 0) { - yaw2 = -yaw2; - } - - /* quaternion with yaw command */ - struct FloatQuat q_yaw; - QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); - - /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ - struct FloatQuat q_sp; - FLOAT_QUAT_COMP(q_sp, q_yaw, q_rp); - FLOAT_QUAT_NORMALIZE(q_sp); - FLOAT_QUAT_WRAP_SHORTEST(q_sp); - - /* convert to fixed point */ - QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp); + quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading); } #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c new file mode 100644 index 0000000000..12ca369d62 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c @@ -0,0 +1,131 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file stabilization_attitude_quat_transformations.c + * Quaternion transformation functions. + */ + +#include "stabilization_attitude_quat_transformations.h" + +void quat_from_rpy_cmd_i(struct Int32Quat *quat, struct Int32Eulers *rpy) { + struct FloatEulers rpy_f; + EULERS_FLOAT_OF_BFP(rpy_f, *rpy); + struct FloatQuat quat_f; + quat_from_rpy_cmd_f(&quat_f, &rpy_f); + QUAT_BFP_OF_REAL(*quat, quat_f); +} + +void quat_from_rpy_cmd_f(struct FloatQuat *quat, struct FloatEulers *rpy) { + // only a plug for now... doesn't apply roll/pitch wrt. current yaw angle + + /* orientation vector describing simultaneous rotation of roll/pitch/yaw */ + const struct FloatVect3 ov = {rpy->phi, rpy->theta, rpy->psi}; + /* quaternion from that orientation vector */ + FLOAT_QUAT_OF_ORIENTATION_VECT(*quat, ov); + +} + +void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading) { + // use float conversion for now... + struct FloatVect2 cmd_f; + cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x); + cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y); + float heading_f = ANGLE_FLOAT_OF_BFP(heading); + + struct FloatQuat quat_f; + quat_from_earth_cmd_f(&quat_f, &cmd_f, heading_f); + + // convert back to fixed point + QUAT_BFP_OF_REAL(*quat, quat_f); +} + +void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading) { + + /* cmd_x is positive to north = negative pitch + * cmd_y is positive to east = positive roll + * + * orientation vector describing simultaneous rotation of roll/pitch + */ + const struct FloatVect3 ov = {cmd->y, -cmd->x, 0.0}; + /* quaternion from that orientation vector */ + struct FloatQuat q_rp; + FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); + + /* as rotation matrix */ + struct FloatRMat R_rp; + FLOAT_RMAT_OF_QUAT(R_rp, q_rp); + /* body x-axis (before heading command) is first column */ + struct FloatVect3 b_x; + VECT3_ASSIGN(b_x, R_rp.m[0], R_rp.m[3], R_rp.m[6]); + /* body z-axis (thrust vect) is last column */ + struct FloatVect3 thrust_vect; + VECT3_ASSIGN(thrust_vect, R_rp.m[2], R_rp.m[5], R_rp.m[8]); + + /// @todo optimize yaw angle calculation + + /* + * Instead of using the psi setpoint angle to rotate around the body z-axis, + * calculate the real angle needed to align the projection of the body x-axis + * onto the horizontal plane with the psi setpoint. + * + * angle between two vectors a and b: + * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n)) + * where the normal n is the thrust vector (i.e. both a and b lie in that plane) + */ + + // desired heading vect in earth x-y plane + const struct FloatVect3 psi_vect = {cosf(heading), sinf(heading), 0.0}; + + /* projection of desired heading onto body x-y plane + * b = v - dot(v,n)*n + */ + float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, thrust_vect); + struct FloatVect3 dotn; + FLOAT_VECT3_SMUL(dotn, thrust_vect, dot); + + // b = v - dot(v,n)*n + struct FloatVect3 b; + FLOAT_VECT3_DIFF(b, psi_vect, dotn); + dot = FLOAT_VECT3_DOT_PRODUCT(b_x, b); + struct FloatVect3 cross; + VECT3_CROSS_PRODUCT(cross, b_x, b); + // norm of the cross product + float nc = FLOAT_VECT3_NORM(cross); + // angle = atan2(norm(cross(a,b)), dot(a,b)) + float yaw2 = atan2(nc, dot) / 2.0; + + // negative angle if needed + // sign(dot(cross(a,b), n) + float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, thrust_vect); + if (dot_cross_ab < 0) { + yaw2 = -yaw2; + } + + /* quaternion with yaw command */ + struct FloatQuat q_yaw; + QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); + + /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ + FLOAT_QUAT_COMP(*quat, q_rp, q_yaw); + FLOAT_QUAT_NORMALIZE(*quat); + FLOAT_QUAT_WRAP_SHORTEST(*quat); + +} diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h new file mode 100644 index 0000000000..1c57038b3f --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file stabilization_attitude_quat_transformations.h + * Quaternion transformation functions. + */ + +#ifndef STABILIZATION_ATTITUDE_QUAT_TRANSFORMATIONS_H +#define STABILIZATION_ATTITUDE_QUAT_TRANSFORMATIONS_H + +#include "math/pprz_algebra_float.h" +#include "math/pprz_algebra_int.h" + +extern void quat_from_rpy_cmd_i(struct Int32Quat *quat, struct Int32Eulers *rpy); +extern void quat_from_rpy_cmd_f(struct FloatQuat *quat, struct FloatEulers *rpy); + +extern void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading); +extern void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading); + +#endif diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c index 7054d1824b..0affbcd21b 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c @@ -221,15 +221,14 @@ void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bo * @param[out] q quaternion representing the RC roll/pitch input */ void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat* q) { - q->qx = radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ / 2; - q->qy = radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ / 2; - q->qz = 0.0; + /* orientation vector describing simultaneous rotation of roll/pitch */ + struct FloatVect3 ov; + ov.x = radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ / 2; + ov.y = radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ / 2; + ov.z = 0.0; - /* normalize */ - float norm = sqrtf(1.0 + SQUARE(q->qx)+ SQUARE(q->qy)); - q->qi = 1.0 / norm; - q->qx /= norm; - q->qy /= norm; + /* quaternion from that orientation vector */ + FLOAT_QUAT_OF_ORIENTATION_VECT(*q, ov); } /** Read roll/pitch command from RC as quaternion. diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c index a70e780560..04e3e60b10 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c @@ -1,4 +1,5 @@ -#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h" #include "generated/airframe.h" struct FloatEulers stab_att_sp_euler; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h index 762e66ed96..ba55615872 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h @@ -22,10 +22,7 @@ #ifndef STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H #define STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H -#include "math/pprz_algebra_float.h" - #include "stabilization_attitude_ref_float.h" -#include "stabilization_attitude_ref.h" void stabilization_attitude_ref_enter(void); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c index d7e8566a8b..14472fbb03 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c @@ -24,7 +24,8 @@ * */ -#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h" #include "generated/airframe.h" struct Int32Eulers stab_att_sp_euler; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h index bf3aacf19c..08579a412a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h @@ -28,7 +28,5 @@ #define STABILIZATION_ATTITUDE_REF_EULER_INT_H #include "stabilization_attitude_ref_int.h" -#include "stabilization_attitude_ref.h" - #endif /* STABILIZATION_ATTITUDE_REF_EULER_INT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h index 2faf9e0bc9..fb62bcc457 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h @@ -27,9 +27,7 @@ #ifndef STABILIZATION_ATTITUDE_REF_FLOAT_H #define STABILIZATION_ATTITUDE_REF_FLOAT_H -#include "generated/airframe.h" - -#include "state.h" +#include "math/pprz_algebra_float.h" extern struct FloatEulers stab_att_sp_euler; extern struct FloatQuat stab_att_sp_quat; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h index c0ef65a2b8..24175b5fc2 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h @@ -29,8 +29,6 @@ #include "math/pprz_algebra_int.h" -#include "state.h" - extern struct Int32Eulers stab_att_ref_euler; ///< with #REF_ANGLE_FRAC extern struct Int32Quat stab_att_ref_quat; ///< with #INT32_QUAT_FRAC extern struct Int32Rates stab_att_ref_rate; ///< with #REF_RATE_FRAC diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c index da50880f15..ff15cf95a8 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c @@ -28,10 +28,9 @@ */ #include "generated/airframe.h" -#include "firmwares/rotorcraft/stabilization.h" -#include "state.h" -#include "stabilization_attitude_ref_float.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h" #define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_REF_MAX_PDOT #define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_QDOT diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h index 0424fbf820..b5543ed82b 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h @@ -30,14 +30,15 @@ #ifndef STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_FLOAT_H #define STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_FLOAT_H -#include "subsystems/radio_control.h" -#include "math/pprz_algebra_float.h" - #include "stabilization_attitude_ref_float.h" -#include "stabilization_attitude_ref.h" -#define DEADBAND_EXCEEDED(VARIABLE, VALUE) ((VARIABLE > VALUE) || (VARIABLE < -VALUE)) -#define APPLY_DEADBAND(VARIABLE, VALUE) (DEADBAND_EXCEEDED(VARIABLE, VALUE) ? VARIABLE : 0.0) +#ifndef STABILIZATION_ATTITUDE_GAIN_NB +#define STABILIZATION_ATTITUDE_GAIN_NB 1 +#endif + +#ifndef STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT +#define STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT 0 +#endif void stabilization_attitude_ref_enter(void); void stabilization_attitude_ref_schedule(uint8_t idx); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c index b0202847dd..aacd33d198 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c @@ -28,9 +28,8 @@ */ #include "generated/airframe.h" -#include "firmwares/rotorcraft/stabilization.h" - -#include "stabilization_attitude_ref_int.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h" #define REF_ACCEL_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_PDOT, REF_ACCEL_FRAC) #define REF_ACCEL_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_QDOT, REF_ACCEL_FRAC) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h index 3b1cbaa3e8..4fe03f3218 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h @@ -31,7 +31,6 @@ #define STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H #include "stabilization_attitude_ref_int.h" -#include "stabilization_attitude_ref.h" void stabilization_attitude_ref_enter(void); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h similarity index 92% rename from sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h rename to sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h index 11d41dfb39..5bb4efb06d 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h @@ -19,12 +19,12 @@ * Boston, MA 02111-1307, USA. */ -/** @file firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h - * Common rotorcraft attitude reference generation include. +/** @file firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h + * Common rotorcraft attitude reference saturation include. */ -#ifndef STABILIZATION_ATTITUDE_REF_H -#define STABILIZATION_ATTITUDE_REF_H +#ifndef STABILIZATION_ATTITUDE_REF_SATURATE_H +#define STABILIZATION_ATTITUDE_REF_SATURATE_H #define SATURATE_SPEED_TRIM_ACCEL() { \ if (stab_att_ref_rate.p >= REF_RATE_MAX_P) { \ @@ -60,4 +60,4 @@ } -#endif /* STABILIZATION_ATTITUDE_REF_H */ +#endif /* STABILIZATION_ATTITUDE_REF_SATURATE_H */ diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 281974a432..33db5f41cd 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -673,15 +673,15 @@ &guidance_h_pos_err.y, \ &guidance_h_speed_err.x, \ &guidance_h_speed_err.y, \ - &guidance_h_pos_err_sum.x, \ - &guidance_h_pos_err_sum.y, \ + &guidance_h_trim_att_integrator.x, \ + &guidance_h_trim_att_integrator.y, \ &guidance_h_nav_err.x, \ &guidance_h_nav_err.y, \ - &guidance_h_command_earth.x, \ - &guidance_h_command_earth.y, \ - &guidance_h_command_body.phi, \ - &guidance_h_command_body.theta, \ - &guidance_h_command_body.psi); \ + &guidance_h_cmd_earth.x, \ + &guidance_h_cmd_earth.y, \ + &guidance_h_cmd_earth.x, \ + &guidance_h_cmd_earth.y, \ + &guidance_h_heading_sp); \ } #define PERIODIC_SEND_GUIDANCE_H_REF(_trans, _dev) { \ @@ -712,7 +712,7 @@ &guidance_h_pos_sp.y, \ &guidance_h_pos_sp.x, \ &carrot_up, \ - &guidance_h_command_body.psi, \ + &guidance_h_heading_sp, \ &stabilization_cmd[COMMAND_THRUST], \ &autopilot_flight_time); \ }