diff --git a/Makefile b/Makefile index b46b46b178..2a34d96ddb 100644 --- a/Makefile +++ b/Makefile @@ -82,6 +82,7 @@ SUBDIRS = $(PPRZCENTER) $(MISC) $(LOGALIZER) # xml files used as input for header generation # MESSAGES_XML = $(CONF)/messages.xml +ABI_XML = $(CONF)/abi.xml UBX_XML = $(CONF)/ubx.xml MTK_XML = $(CONF)/mtk.xml XSENS_XML = $(CONF)/xsens_MTi-G.xml @@ -218,7 +219,7 @@ $(DL_PROTOCOL2_H) : $(MESSAGES_XML) tools $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ -$(ABI_MESSAGES_H) : $(MESSAGES_XML) tools +$(ABI_MESSAGES_H) : $(ABI_XML) tools @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(TOOLS)/gen_abi.out $< airborne > $($@_TMP) diff --git a/conf/abi.xml b/conf/abi.xml new file mode 100644 index 0000000000..896761d545 --- /dev/null +++ b/conf/abi.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + diff --git a/conf/airframes/CDW/asctec_cdw.xml b/conf/airframes/CDW/asctec_cdw.xml index 830c176d2f..b366bcb90a 100644 --- a/conf/airframes/CDW/asctec_cdw.xml +++ b/conf/airframes/CDW/asctec_cdw.xml @@ -124,7 +124,6 @@
-
diff --git a/conf/airframes/CDW/tricopter_cdw.xml b/conf/airframes/CDW/tricopter_cdw.xml index 86946f11f3..ca48ac1ec1 100644 --- a/conf/airframes/CDW/tricopter_cdw.xml +++ b/conf/airframes/CDW/tricopter_cdw.xml @@ -121,7 +121,6 @@ LiPo/LiIo-Zellen: 2-3
-
diff --git a/conf/airframes/ENAC/fixed-wing/firestorm.xml b/conf/airframes/ENAC/fixed-wing/firestorm.xml index 222712c259..4f9abc4b4c 100644 --- a/conf/airframes/ENAC/fixed-wing/firestorm.xml +++ b/conf/airframes/ENAC/fixed-wing/firestorm.xml @@ -124,7 +124,6 @@
-
diff --git a/conf/airframes/ENAC/fixed-wing/funjet2.xml b/conf/airframes/ENAC/fixed-wing/funjet2.xml index 56455152a5..670c51f99a 100644 --- a/conf/airframes/ENAC/fixed-wing/funjet2.xml +++ b/conf/airframes/ENAC/fixed-wing/funjet2.xml @@ -15,9 +15,6 @@ - diff --git a/conf/airframes/ENAC/fixed-wing/mythe.xml b/conf/airframes/ENAC/fixed-wing/mythe.xml index f6c226b11b..58d0beaac0 100644 --- a/conf/airframes/ENAC/fixed-wing/mythe.xml +++ b/conf/airframes/ENAC/fixed-wing/mythe.xml @@ -9,7 +9,6 @@ - diff --git a/conf/airframes/ENAC/fixed-wing/weasel.xml b/conf/airframes/ENAC/fixed-wing/weasel.xml index f15a4b5c41..91d220785e 100644 --- a/conf/airframes/ENAC/fixed-wing/weasel.xml +++ b/conf/airframes/ENAC/fixed-wing/weasel.xml @@ -120,7 +120,6 @@
-
diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml index 2dce3cc671..7136bc411d 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -142,7 +142,6 @@
- diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index 834132e720..ee1b52add3 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -177,7 +177,6 @@
- diff --git a/conf/airframes/ENAC/quadrotor/hen1.xml b/conf/airframes/ENAC/quadrotor/hen1.xml index 2c0bdb49ae..16a2e39f82 100644 --- a/conf/airframes/ENAC/quadrotor/hen1.xml +++ b/conf/airframes/ENAC/quadrotor/hen1.xml @@ -127,7 +127,6 @@
-
diff --git a/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml index 7a870f3b9c..a5e455afdf 100644 --- a/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml @@ -145,7 +145,6 @@
-
diff --git a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml index 064c01ae5a..989341a4ad 100644 --- a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml @@ -147,7 +147,6 @@
-
diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml index 2ee2d7548e..4356db1884 100644 --- a/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml +++ b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml @@ -129,7 +129,6 @@
-
diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml index 4e7dda0a2e..c843d8dd41 100644 --- a/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml +++ b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml @@ -134,7 +134,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 ee9e0848ac..1766366e50 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 @@ -221,7 +221,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 06f2f97307..17a36534f7 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 @@ -221,7 +221,6 @@
-
diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index 8025779613..4378808f12 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -109,7 +109,6 @@
-
diff --git a/conf/airframes/esden/cocto_lm2a2.xml b/conf/airframes/esden/cocto_lm2a2.xml index 609e7295bd..3745d83a5b 100644 --- a/conf/airframes/esden/cocto_lm2a2.xml +++ b/conf/airframes/esden/cocto_lm2a2.xml @@ -170,7 +170,6 @@ B2L -> CW
-
diff --git a/conf/airframes/esden/gain_scheduling_example.xml b/conf/airframes/esden/gain_scheduling_example.xml index 02f574bd9f..b731662fba 100644 --- a/conf/airframes/esden/gain_scheduling_example.xml +++ b/conf/airframes/esden/gain_scheduling_example.xml @@ -153,7 +153,6 @@
-
diff --git a/conf/airframes/esden/hexy_ll11a2pwm.xml b/conf/airframes/esden/hexy_ll11a2pwm.xml index f796f42c68..f011c0e02f 100644 --- a/conf/airframes/esden/hexy_ll11a2pwm.xml +++ b/conf/airframes/esden/hexy_ll11a2pwm.xml @@ -167,7 +167,6 @@
-
diff --git a/conf/airframes/esden/hexy_lm2a2pwm.xml b/conf/airframes/esden/hexy_lm2a2pwm.xml index 73554f3209..6078dbf311 100644 --- a/conf/airframes/esden/hexy_lm2a2pwm.xml +++ b/conf/airframes/esden/hexy_lm2a2pwm.xml @@ -128,7 +128,6 @@
-
diff --git a/conf/airframes/esden/lisa2_hex.xml b/conf/airframes/esden/lisa2_hex.xml index b21dddffcb..36bed33636 100644 --- a/conf/airframes/esden/lisa2_hex.xml +++ b/conf/airframes/esden/lisa2_hex.xml @@ -135,7 +135,6 @@
-
diff --git a/conf/airframes/esden/qs_asp22.xml b/conf/airframes/esden/qs_asp22.xml index c4ef62b938..b6d90d7b42 100644 --- a/conf/airframes/esden/qs_asp22.xml +++ b/conf/airframes/esden/qs_asp22.xml @@ -161,7 +161,6 @@
-
diff --git a/conf/airframes/esden/quady_ll11a2pwm.xml b/conf/airframes/esden/quady_ll11a2pwm.xml index ca80e5a5d2..b33c992432 100644 --- a/conf/airframes/esden/quady_ll11a2pwm.xml +++ b/conf/airframes/esden/quady_ll11a2pwm.xml @@ -163,7 +163,6 @@
-
diff --git a/conf/airframes/esden/quady_lm1a1pwm.xml b/conf/airframes/esden/quady_lm1a1pwm.xml index 406734e757..82e8505f01 100644 --- a/conf/airframes/esden/quady_lm1a1pwm.xml +++ b/conf/airframes/esden/quady_lm1a1pwm.xml @@ -124,7 +124,6 @@
-
diff --git a/conf/airframes/esden/quady_lm2a2pwm.xml b/conf/airframes/esden/quady_lm2a2pwm.xml index e87c817b3e..9d6fd8ff2e 100644 --- a/conf/airframes/esden/quady_lm2a2pwm.xml +++ b/conf/airframes/esden/quady_lm2a2pwm.xml @@ -127,7 +127,6 @@
-
diff --git a/conf/airframes/esden/quady_lm2a2pwmppm.xml b/conf/airframes/esden/quady_lm2a2pwmppm.xml index 31fd65c39d..ed6df64b95 100644 --- a/conf/airframes/esden/quady_lm2a2pwmppm.xml +++ b/conf/airframes/esden/quady_lm2a2pwmppm.xml @@ -124,7 +124,6 @@
-
diff --git a/conf/airframes/esden/quady_ls01pwm.xml b/conf/airframes/esden/quady_ls01pwm.xml index 4a3a194c39..ef54cfdd47 100644 --- a/conf/airframes/esden/quady_ls01pwm.xml +++ b/conf/airframes/esden/quady_ls01pwm.xml @@ -127,7 +127,6 @@
-
diff --git a/conf/airframes/examples/Twinstar_energyadaptive.xml b/conf/airframes/examples/Twinstar_energyadaptive.xml index d58dbd1fb1..c5afbe19cf 100644 --- a/conf/airframes/examples/Twinstar_energyadaptive.xml +++ b/conf/airframes/examples/Twinstar_energyadaptive.xml @@ -12,7 +12,6 @@ twog_1.0 + aspirin + ETS baro + ETS speed - @@ -22,6 +21,9 @@ twog_1.0 + aspirin + ETS baro + ETS speed + + + @@ -50,12 +52,11 @@ twog_1.0 + aspirin + ETS baro + ETS speed - + + - - - + @@ -124,7 +125,6 @@ twog_1.0 + aspirin + ETS baro + ETS speed
-
diff --git a/conf/airframes/examples/booz2.xml b/conf/airframes/examples/booz2.xml index e93ccf9d2c..82d8b1b746 100644 --- a/conf/airframes/examples/booz2.xml +++ b/conf/airframes/examples/booz2.xml @@ -148,7 +148,6 @@
-
diff --git a/conf/airframes/examples/easystar_ets.xml b/conf/airframes/examples/easystar_ets.xml index a66406c0c7..4f23f35360 100644 --- a/conf/airframes/examples/easystar_ets.xml +++ b/conf/airframes/examples/easystar_ets.xml @@ -19,7 +19,6 @@ - @@ -37,9 +36,6 @@ - - - diff --git a/conf/airframes/examples/h_hex.xml b/conf/airframes/examples/h_hex.xml index 5faaedebe3..893b038012 100644 --- a/conf/airframes/examples/h_hex.xml +++ b/conf/airframes/examples/h_hex.xml @@ -147,7 +147,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 4bb3e01b2f..f0222aee85 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml @@ -119,7 +119,6 @@
-
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 691a2ea508..b65ecde371 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml @@ -122,7 +122,6 @@
-
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 b9fe1162dd..78d8298454 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml @@ -112,7 +112,6 @@
-
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 a8bdcd204e..94ec9b6ca3 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml @@ -108,7 +108,6 @@
-
diff --git a/conf/airframes/examples/lisa_asctec.xml b/conf/airframes/examples/lisa_asctec.xml index 5e4869cb87..d6ae4f8628 100644 --- a/conf/airframes/examples/lisa_asctec.xml +++ b/conf/airframes/examples/lisa_asctec.xml @@ -146,7 +146,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 a3d883c653..9e550b5176 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -102,7 +102,6 @@
-
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 9f7210bd3e..2d9dc65873 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 @@ -104,7 +104,6 @@
-
diff --git a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml index 4746f8f78d..654d30fc8c 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml @@ -94,7 +94,6 @@
-
diff --git a/conf/airframes/examples/quadrotor_lisa_s.xml b/conf/airframes/examples/quadrotor_lisa_s.xml index bc0ffa17be..481c08de7e 100644 --- a/conf/airframes/examples/quadrotor_lisa_s.xml +++ b/conf/airframes/examples/quadrotor_lisa_s.xml @@ -147,7 +147,6 @@
-
diff --git a/conf/airframes/examples/quadrotor_mlkf.xml b/conf/airframes/examples/quadrotor_mlkf.xml index 611868c6c7..a8e60c02e5 100644 --- a/conf/airframes/examples/quadrotor_mlkf.xml +++ b/conf/airframes/examples/quadrotor_mlkf.xml @@ -98,7 +98,6 @@
-
diff --git a/conf/airframes/examples/quadrotor_navgo.xml b/conf/airframes/examples/quadrotor_navgo.xml index 98df961abd..d36f1925c1 100644 --- a/conf/airframes/examples/quadrotor_navgo.xml +++ b/conf/airframes/examples/quadrotor_navgo.xml @@ -36,6 +36,8 @@ + +
@@ -117,7 +119,6 @@
- @@ -127,7 +128,6 @@
- diff --git a/conf/airframes/examples/quadrotor_px4fmu.xml b/conf/airframes/examples/quadrotor_px4fmu.xml index fb02773791..d4d2aceca5 100644 --- a/conf/airframes/examples/quadrotor_px4fmu.xml +++ b/conf/airframes/examples/quadrotor_px4fmu.xml @@ -98,7 +98,6 @@
-
diff --git a/conf/airframes/examples/quadshot_asp21_spektrum.xml b/conf/airframes/examples/quadshot_asp21_spektrum.xml index d5fc8439fd..69386a3d99 100644 --- a/conf/airframes/examples/quadshot_asp21_spektrum.xml +++ b/conf/airframes/examples/quadshot_asp21_spektrum.xml @@ -184,7 +184,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 a2c7336e20..782c9e6e4b 100644 --- a/conf/airframes/examples/stm32f4_discovery_test.xml +++ b/conf/airframes/examples/stm32f4_discovery_test.xml @@ -31,13 +31,11 @@ - + + - - - @@ -100,7 +98,6 @@
-
diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index 528753ac31..a99dd2f8c9 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -138,7 +138,6 @@
-
diff --git a/conf/airframes/obsolete/ENAC/g1_vision.xml b/conf/airframes/obsolete/ENAC/g1_vision.xml index d24b588c2e..3ffcf3ec2c 100644 --- a/conf/airframes/obsolete/ENAC/g1_vision.xml +++ b/conf/airframes/obsolete/ENAC/g1_vision.xml @@ -130,7 +130,6 @@
-
diff --git a/conf/airframes/obsolete/ENAC/mkk1-vision.xml b/conf/airframes/obsolete/ENAC/mkk1-vision.xml index 4d78dbd80b..eccb54ea99 100644 --- a/conf/airframes/obsolete/ENAC/mkk1-vision.xml +++ b/conf/airframes/obsolete/ENAC/mkk1-vision.xml @@ -163,7 +163,6 @@
-
diff --git a/conf/airframes/obsolete/ENAC/mkk1.xml b/conf/airframes/obsolete/ENAC/mkk1.xml index 18a9742bc7..99f0d73b7d 100644 --- a/conf/airframes/obsolete/ENAC/mkk1.xml +++ b/conf/airframes/obsolete/ENAC/mkk1.xml @@ -193,7 +193,6 @@
-
diff --git a/conf/airframes/obsolete/ENAC/nova1.xml b/conf/airframes/obsolete/ENAC/nova1.xml index 0ef106ba39..1bd3135b95 100644 --- a/conf/airframes/obsolete/ENAC/nova1.xml +++ b/conf/airframes/obsolete/ENAC/nova1.xml @@ -164,7 +164,6 @@
-
diff --git a/conf/airframes/obsolete/NoVa_L.xml b/conf/airframes/obsolete/NoVa_L.xml index aaf45e3e27..bc7e33c41c 100644 --- a/conf/airframes/obsolete/NoVa_L.xml +++ b/conf/airframes/obsolete/NoVa_L.xml @@ -189,7 +189,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 0c2cfe06c0..c8f68b580e 100644 --- a/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml +++ b/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml @@ -149,7 +149,6 @@
-
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 6d60135e55..b5b958a8ed 100644 --- a/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml +++ b/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml @@ -166,7 +166,6 @@
-
diff --git a/conf/airframes/obsolete/Poine/booz2_a1.xml b/conf/airframes/obsolete/Poine/booz2_a1.xml index 5823e29e03..6a66e8dfe2 100644 --- a/conf/airframes/obsolete/Poine/booz2_a1.xml +++ b/conf/airframes/obsolete/Poine/booz2_a1.xml @@ -142,7 +142,6 @@
-
diff --git a/conf/airframes/obsolete/Poine/booz2_a7.xml b/conf/airframes/obsolete/Poine/booz2_a7.xml index d3f23c31fd..d7fe9909c6 100644 --- a/conf/airframes/obsolete/Poine/booz2_a7.xml +++ b/conf/airframes/obsolete/Poine/booz2_a7.xml @@ -161,7 +161,6 @@
-
diff --git a/conf/airframes/obsolete/Poine/h_hex.xml b/conf/airframes/obsolete/Poine/h_hex.xml index 252cdacf33..7eb7fe8fee 100644 --- a/conf/airframes/obsolete/Poine/h_hex.xml +++ b/conf/airframes/obsolete/Poine/h_hex.xml @@ -128,7 +128,6 @@
-
diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml b/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml index 10b61e41a0..9276b95eff 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml @@ -216,7 +216,6 @@ second attempt
-
diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml b/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml index 5f3ee2a945..57642ca562 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml @@ -153,7 +153,6 @@
-
diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml b/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml index 406d6de2c0..f22da3d19a 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml @@ -171,7 +171,6 @@
-
diff --git a/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml b/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml index 3328cc4b18..c350ad5ef2 100644 --- a/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml +++ b/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml @@ -171,7 +171,6 @@
-
diff --git a/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml b/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml index 82f499852b..4202753b78 100644 --- a/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml +++ b/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml @@ -217,7 +217,6 @@ second attempt
-
diff --git a/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml b/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml index 0cecb4fdb2..01c4ed126f 100644 --- a/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml +++ b/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml @@ -132,7 +132,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_Aron.xml b/conf/airframes/obsolete/booz2_Aron.xml index a5a48f5295..754f24d400 100644 --- a/conf/airframes/obsolete/booz2_Aron.xml +++ b/conf/airframes/obsolete/booz2_Aron.xml @@ -134,7 +134,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_NoVa.xml b/conf/airframes/obsolete/booz2_NoVa.xml index aa12a6dfdd..928ac58841 100644 --- a/conf/airframes/obsolete/booz2_NoVa.xml +++ b/conf/airframes/obsolete/booz2_NoVa.xml @@ -171,7 +171,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_NoVa_001.xml b/conf/airframes/obsolete/booz2_NoVa_001.xml index 81c9021e83..088bf5a7ec 100644 --- a/conf/airframes/obsolete/booz2_NoVa_001.xml +++ b/conf/airframes/obsolete/booz2_NoVa_001.xml @@ -171,7 +171,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_NoVa_002.xml b/conf/airframes/obsolete/booz2_NoVa_002.xml index ed879fb742..1f268ba327 100644 --- a/conf/airframes/obsolete/booz2_NoVa_002.xml +++ b/conf/airframes/obsolete/booz2_NoVa_002.xml @@ -171,7 +171,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_a1p.xml b/conf/airframes/obsolete/booz2_a1p.xml index 18ff3d98f8..22d5124d79 100644 --- a/conf/airframes/obsolete/booz2_a1p.xml +++ b/conf/airframes/obsolete/booz2_a1p.xml @@ -146,7 +146,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_a2.xml b/conf/airframes/obsolete/booz2_a2.xml index d0f9361638..df65d7bb25 100644 --- a/conf/airframes/obsolete/booz2_a2.xml +++ b/conf/airframes/obsolete/booz2_a2.xml @@ -134,7 +134,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_a3.xml b/conf/airframes/obsolete/booz2_a3.xml index 0635e3ce89..086855cb34 100644 --- a/conf/airframes/obsolete/booz2_a3.xml +++ b/conf/airframes/obsolete/booz2_a3.xml @@ -121,7 +121,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_a4.xml b/conf/airframes/obsolete/booz2_a4.xml index e9cc7ede69..883dd43ef9 100644 --- a/conf/airframes/obsolete/booz2_a4.xml +++ b/conf/airframes/obsolete/booz2_a4.xml @@ -99,7 +99,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_a5.xml b/conf/airframes/obsolete/booz2_a5.xml index 6fc2fe07eb..e0c43ad6ee 100644 --- a/conf/airframes/obsolete/booz2_a5.xml +++ b/conf/airframes/obsolete/booz2_a5.xml @@ -138,7 +138,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_flixr.xml b/conf/airframes/obsolete/booz2_flixr.xml index a62ace8aee..379e112587 100644 --- a/conf/airframes/obsolete/booz2_flixr.xml +++ b/conf/airframes/obsolete/booz2_flixr.xml @@ -190,7 +190,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_ppzuav.xml b/conf/airframes/obsolete/booz2_ppzuav.xml index af532c20f2..a34b282fd0 100644 --- a/conf/airframes/obsolete/booz2_ppzuav.xml +++ b/conf/airframes/obsolete/booz2_ppzuav.xml @@ -188,7 +188,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_s1.xml b/conf/airframes/obsolete/booz2_s1.xml index 4cf6bb4bbe..c4a37b4cf7 100644 --- a/conf/airframes/obsolete/booz2_s1.xml +++ b/conf/airframes/obsolete/booz2_s1.xml @@ -130,7 +130,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_x1.xml b/conf/airframes/obsolete/booz2_x1.xml index 370920cd85..ee626f8deb 100644 --- a/conf/airframes/obsolete/booz2_x1.xml +++ b/conf/airframes/obsolete/booz2_x1.xml @@ -141,7 +141,6 @@
-
diff --git a/conf/airframes/obsolete/example_heli_lisam.xml b/conf/airframes/obsolete/example_heli_lisam.xml index 79e8d519ac..c0d13704d0 100644 --- a/conf/airframes/obsolete/example_heli_lisam.xml +++ b/conf/airframes/obsolete/example_heli_lisam.xml @@ -181,7 +181,6 @@ AP_MODE_NAV
-
diff --git a/conf/airframes/obsolete/mm/rotor/qmk1.xml b/conf/airframes/obsolete/mm/rotor/qmk1.xml index 37b3b6c07a..3d05729aa2 100644 --- a/conf/airframes/obsolete/mm/rotor/qmk1.xml +++ b/conf/airframes/obsolete/mm/rotor/qmk1.xml @@ -141,8 +141,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 5d1b3e397d..3e43cd0eda 100644 --- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml +++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml @@ -119,7 +119,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 5343031357..0bac6a6e4e 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 @@ -82,7 +82,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 2ba487a784..f2e2ee5a09 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 @@ -89,7 +89,6 @@
-
diff --git a/conf/firmwares/lisa_test_progs.makefile b/conf/firmwares/lisa_test_progs.makefile index 5a18345359..3fdac78122 100644 --- a/conf/firmwares/lisa_test_progs.makefile +++ b/conf/firmwares/lisa_test_progs.makefile @@ -190,6 +190,8 @@ test_baro.srcs += $(COMMON_TELEMETRY_SRCS) test_baro.CFLAGS += -I$(SRC_LISA) +test_baro.srcs += subsystems/air_data.c + ifeq ($(BOARD), lisa_l) test_baro.CFLAGS += -DUSE_I2C2 test_baro.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c @@ -200,32 +202,37 @@ test_baro.srcs += $(SRC_LISA)/test_baro_i2c.c else ifeq ($(BOARD), lisa_m) # defaults to i2c baro bmp085 on the board LISA_M_BARO ?= BARO_BOARD_BMP085 - ifeq ($(LISA_M_BARO), BARO_MS5611_SPI) - include $(CFG_SHARED)/spi_master.makefile - test_baro.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3 - test_baro.srcs += peripherals/ms5611.c - test_baro.srcs += peripherals/ms5611_spi.c - test_baro.srcs += subsystems/sensors/baro_ms5611_spi.c - test_baro.srcs += $(SRC_LISA)/test_baro_spi.c - else ifeq ($(LISA_M_BARO), BARO_MS5611_I2C) + ifeq ($(LISA_M_BARO), BARO_MS5611_I2C) test_baro.CFLAGS += -DUSE_I2C2 test_baro.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c + test_baro.CFLAGS += -DBB_MS5611_I2C_DEV=i2c2 test_baro.srcs += peripherals/ms5611.c test_baro.srcs += peripherals/ms5611_i2c.c - test_baro.srcs += subsystems/sensors/baro_ms5611_i2c.c + test_baro.srcs += boards/baro_board_ms5611_i2c.c test_baro.srcs += $(SRC_LISA)/test_baro_i2c.c else ifeq ($(LISA_M_BARO), BARO_BOARD_BMP085) - test_baro.CFLAGS += -DUSE_I2C2 + test_baro.CFLAGS += -DUSE_I2C2 test_baro.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c test_baro.srcs += peripherals/bmp085.c test_baro.srcs += $(SRC_BOARD)/baro_board.c test_baro.srcs += $(SRC_LISA)/test_baro_i2c.c endif - test_baro.CFLAGS += -D$(LISA_M_BARO) + +# Lia baro (no bmp onboard) +else ifeq ($(BOARD), lia) +LIA_BARO ?= BARO_MS5611_SPI + ifeq ($(LIA_BARO), BARO_MS5611_I2C) + test_baro.CFLAGS += -DUSE_I2C2 + test_baro.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c + test_baro.CFLAGS += -DBB_MS5611_I2C_DEV=i2c2 + test_baro.srcs += peripherals/ms5611.c + test_baro.srcs += peripherals/ms5611_i2c.c + test_baro.srcs += boards/baro_board_ms5611_i2c.c + test_baro.srcs += $(SRC_LISA)/test_baro_i2c.c + endif + endif - - # # test_rc_spektrum : sends RADIO_CONTROL messages on telemetry # diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index 884bc9a178..4fc5034140 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/conf/firmwares/rotorcraft.makefile @@ -147,106 +147,11 @@ ap.srcs += subsystems/actuators.c # # -# BARO +# AIR DATA and BARO (if needed) # -# booz baro -ifeq ($(BOARD), booz) -ap.srcs += $(SRC_BOARD)/baro_board.c -else ifeq ($(BOARD), lisa_l) -ap.CFLAGS += -DUSE_I2C2 -ap.srcs += $(SRC_BOARD)/baro_board.c +ap.srcs += subsystems/air_data.c -# Ardrone baro -else ifeq ($(BOARD)$(BOARD_TYPE), ardroneraw) -ap.srcs += $(SRC_BOARD)/baro_board.c -else ifeq ($(BOARD)$(BOARD_TYPE), ardronesdk) -ap.srcs += $(SRC_BOARD)/baro_board_dummy.c - -# Lisa/M baro -else ifeq ($(BOARD), lisa_m) -# defaults to i2c baro bmp085 on the board -LISA_M_BARO ?= BARO_BOARD_BMP085 - ifeq ($(LISA_M_BARO), BARO_MS5611_SPI) - include $(CFG_SHARED)/spi_master.makefile - ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3 - ap.srcs += peripherals/ms5611.c - ap.srcs += peripherals/ms5611_spi.c - ap.srcs += subsystems/sensors/baro_ms5611_spi.c - else ifeq ($(LISA_M_BARO), BARO_MS5611_I2C) - ap.CFLAGS += -DUSE_I2C2 - ap.srcs += peripherals/ms5611.c - ap.srcs += peripherals/ms5611_i2c.c - ap.srcs += subsystems/sensors/baro_ms5611_i2c.c - else ifeq ($(LISA_M_BARO), BARO_BOARD_BMP085) - ap.srcs += peripherals/bmp085.c - ap.srcs += $(SRC_BOARD)/baro_board.c - ap.CFLAGS += -DUSE_I2C2 - endif - ap.CFLAGS += -D$(LISA_M_BARO) - -# Lisa/S baro -else ifeq ($(BOARD), lisa_s) -# defaults to SPI baro MS5611 on the board - include $(CFG_SHARED)/spi_master.makefile - ap.CFLAGS += -DUSE_SPI1 -DUSE_SPI_SLAVE1 - ap.CFLAGS += -DMS5611_SPI_DEV=spi1 - ap.CFLAGS += -DMS5611_SLAVE_DEV=SPI_SLAVE1 - ap.srcs += peripherals/ms5611.c - ap.srcs += peripherals/ms5611_spi.c - ap.srcs += subsystems/sensors/baro_ms5611_spi.c - ap.CFLAGS += -DBARO_MS5611_SPI - -# Lia baro (no bmp onboard) -else ifeq ($(BOARD), lia) -# fixme, reuse the baro drivers in lisa_m dir -LIA_BARO ?= BARO_MS5611_SPI - ifeq ($(LIA_BARO), BARO_MS5611_SPI) - include $(CFG_SHARED)/spi_master.makefile - ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3 - ap.srcs += peripherals/ms5611.c - ap.srcs += peripherals/ms5611_spi.c - ap.srcs += subsystems/sensors/baro_ms5611_spi.c - else ifeq ($(LIA_BARO), BARO_MS5611_I2C) - ap.CFLAGS += -DUSE_I2C2 - ap.srcs += peripherals/ms5611.c - ap.srcs += peripherals/ms5611_i2c.c - ap.srcs += subsystems/sensors/baro_ms5611_i2c.c - endif - ap.CFLAGS += -D$(LIA_BARO) - -# navgo baro -else ifeq ($(BOARD), navgo) -include $(CFG_SHARED)/spi_master.makefile -ap.CFLAGS += -DUSE_SPI_SLAVE0 -ap.CFLAGS += -DUSE_SPI1 -ap.srcs += peripherals/mcp355x.c -ap.srcs += $(SRC_BOARD)/baro_board.c - -# krooz baro -else ifeq ($(BOARD), krooz) -ap.CFLAGS += -DMS5611_I2C_DEV=i2c2 -DMS5611_SLAVE_ADDR=0xEC -ap.srcs += peripherals/ms5611.c -ap.srcs += peripherals/ms5611_i2c.c -ap.srcs += subsystems/sensors/baro_ms5611_i2c.c - -else ifeq ($(BOARD), px4fmu) -ap.CFLAGS += -DUSE_I2C2 -DMS5611_I2C_DEV=i2c2 -ap.srcs += peripherals/ms5611.c -ap.srcs += peripherals/ms5611_i2c.c -ap.srcs += subsystems/sensors/baro_ms5611_i2c.c - -# apogee baro -else ifeq ($(BOARD), apogee) -ap.CFLAGS += -DUSE_I2C1 -ap.CFLAGS += -DMPL3115_I2C_DEV=i2c1 -ap.CFLAGS += -DMPL3115_ALT_MODE=0 -ap.srcs += peripherals/mpl3115.c -ap.srcs += $(SRC_BOARD)/baro_board.c -endif - -ifneq ($(BARO_LED),none) -ap.CFLAGS += -DROTORCRAFT_BARO_LED=$(BARO_LED) -endif +include $(CFG_SHARED)/baro_board.makefile # # Analog Backend diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index 9970e344d7..9b5a640f05 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -154,17 +154,11 @@ ap_srcs += state.c ap_srcs += subsystems/settings.c ap_srcs += $(SRC_ARCH)/subsystems/settings_arch.c +# AIR DATA +ap_srcs += subsystems/air_data.c + # BARO -ifeq ($(BOARD), umarim) -ifeq ($(BOARD_VERSION), 1.0) -ap_srcs += boards/umarim/baro_board.c -ap_CFLAGS += -DUSE_I2C1 -DUSE_ADS1114_1 -ap_CFLAGS += -DADS1114_I2C_DEV=i2c1 -ap_srcs += peripherals/ads1114.c -endif -else ifeq ($(BOARD), lisa_l) -ap_CFLAGS += -DUSE_I2C2 -endif +include $(CFG_SHARED)/baro_board.makefile # ahrs frequencies if configured ifdef AHRS_PROPAGATE_FREQUENCY diff --git a/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile b/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile index f6f1c628f9..0ab3fc9cca 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile @@ -1,6 +1,14 @@ +# Hey Emacs, this is a -*- makefile -*- -ap.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c +ins_srcs += $(SRC_SUBSYSTEMS)/ins.c +ins_srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c -sim.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c -nps.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c +ap.CFLAGS += $(ins_CFLAGS) +ap.srcs += $(ins_srcs) + +sim.CFLAGS += $(ins_CFLAGS) +sim.srcs += $(ins_srcs) + +nps.CFLAGS += $(ins_CFLAGS) +nps.srcs += $(ins_srcs) diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile index 4440f92bfd..b9589d04c5 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile @@ -28,6 +28,7 @@ ap.CFLAGS += -DUSE_UART$(XSENS_UART_NR) ap.CFLAGS += -DINS_LINK=UART$(XSENS_UART_NR) ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=$(XSENS_UART_BAUD) ap.CFLAGS += -DXSENS_OUTPUT_MODE=0x1836 +ap.srcs += $(SRC_SUBSYSTEMS)/ins.c ap.srcs += $(SRC_MODULES)/ins/ins_xsens.c ap.CFLAGS += -DAHRS_TRIGGERED_ATTITUDE_LOOP @@ -56,6 +57,7 @@ $(TARGET).CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c $(TARGET).CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens700.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens700.makefile index a28dbe30dc..eb064c36a9 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_xsens700.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_xsens700.makefile @@ -50,9 +50,8 @@ else ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=B230400 endif -ap.CFLAGS += -DUSE_GPS_XSENS -ap.CFLAGS += -DGPS_NB_CHANNELS=50 ap.CFLAGS += -DXSENS_OUTPUT_MODE=0x1836 +ap.srcs += $(SRC_SUBSYSTEMS)/ins.c ap.srcs += $(SRC_MODULES)/ins/ins_xsens700.c ap.CFLAGS += -DAHRS_TRIGGERED_ATTITUDE_LOOP @@ -68,28 +67,34 @@ fbw.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_xsens.h\" endif -ifeq ($(TARGET), sim) - -sim.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\" -sim.CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR - -sim.srcs += $(SRC_SUBSYSTEMS)/ahrs.c -sim.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c - -endif - ######################################### ## GPS # ap.CFLAGS += -DGPS +ap.CFLAGS += -DUSE_GPS_XSENS +ap.CFLAGS += -DGPS_NB_CHANNELS=50 +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c + +######################################### +## Simulator +SIM_TARGETS = sim jsbsim nps + +ifneq (,$(findstring $(TARGET),$(SIM_TARGETS))) + +$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\" +$(TARGET).CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR + +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c + +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c + +$(TARGET).CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +$(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c -sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG -sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" -sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c - - - - +endif diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index 8db11612fd..5484874482 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -93,8 +93,7 @@ nps.srcs += $(SRC_FIRMWARE)/datalink.c # nps.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c math/pprz_orientation_conversion.c -nps.CFLAGS += -DROTORCRAFT_BARO_LED=2 -nps.srcs += $(SRC_BOARD)/baro_board.c +nps.srcs += subsystems/air_data.c nps.CFLAGS += -DUSE_ADC nps.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c diff --git a/conf/firmwares/subsystems/rotorcraft/ins.makefile b/conf/firmwares/subsystems/rotorcraft/ins.makefile index fd4878813c..e7c485cbe4 100644 --- a/conf/firmwares/subsystems/rotorcraft/ins.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ins.makefile @@ -8,5 +8,4 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int.c # vertical filter float version $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c -$(TARGET).CFLAGS += -DUSE_VFF diff --git a/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile b/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile index 6ce7f94af9..7f1809dd83 100644 --- a/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile @@ -2,9 +2,9 @@ # extended INS with vertical filter using sonar in a better way (flap ground) # -$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_int_extended.h\" +$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_int.h\" $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int_extended.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int.c # vertical filter float version $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_extended_float.c diff --git a/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile b/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile index fd0ca65228..f14ac11350 100644 --- a/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile @@ -8,7 +8,6 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int.c # vertical filter float version $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c -$(TARGET).CFLAGS += -DUSE_VFF # horizontal filter float version $(TARGET).CFLAGS += -DUSE_HFF diff --git a/conf/firmwares/subsystems/shared/baro_board.makefile b/conf/firmwares/subsystems/shared/baro_board.makefile new file mode 100644 index 0000000000..7bea12cf0b --- /dev/null +++ b/conf/firmwares/subsystems/shared/baro_board.makefile @@ -0,0 +1,138 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Drivers for baros already on an autopilot board + + +# check that onboard baro was not explicitly disabled with +# + +USE_BARO_BOARD ?= TRUE +ifeq ($(USE_BARO_BOARD), TRUE) + +# booz baro +ifeq ($(BOARD), booz) + ap.srcs += $(SRC_BOARD)/baro_board.c + +# Lisa/L +else ifeq ($(BOARD), lisa_l) + ap.CFLAGS += -DUSE_I2C2 + ap.srcs += $(SRC_BOARD)/baro_board.c + +# Ardrone baro +else ifeq ($(BOARD)$(BOARD_TYPE), ardroneraw) + ap.srcs += $(SRC_BOARD)/baro_board.c + +# Lisa/M baro +else ifeq ($(BOARD), lisa_m) +# defaults to i2c baro bmp085 on the board +LISA_M_BARO ?= BARO_BOARD_BMP085 + ifeq ($(LISA_M_BARO), BARO_MS5611_SPI) + include $(CFG_SHARED)/spi_master.makefile + ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3 + ap.CFLAGS += -DBB_MS5611_SPI_DEV=spi2 + ap.CFLAGS += -DBB_MS5611_SLAVE_IDX=SPI_SLAVE3 + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_spi.c + ap.srcs += boards/baro_board_ms5611_spi.c + else ifeq ($(LISA_M_BARO), BARO_MS5611_I2C) + ap.CFLAGS += -DUSE_I2C2 + ap.CFLAGS += -DBB_MS5611_I2C_DEV=i2c2 + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_i2c.c + ap.srcs += boards/baro_board_ms5611_i2c.c + else ifeq ($(LISA_M_BARO), BARO_BOARD_BMP085) + ap.CFLAGS += -DUSE_I2C2 + ap.srcs += peripherals/bmp085.c + ap.srcs += $(SRC_BOARD)/baro_board.c + endif + +# Lisa/S baro +else ifeq ($(BOARD), lisa_s) +# defaults to SPI baro MS5611 on the board + include $(CFG_SHARED)/spi_master.makefile + ap.CFLAGS += -DUSE_SPI1 -DUSE_SPI_SLAVE1 + ap.CFLAGS += -DBB_MS5611_SPI_DEV=spi1 + ap.CFLAGS += -DBB_MS5611_SLAVE_IDX=SPI_SLAVE1 + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_spi.c + ap.srcs += boards/baro_board_ms5611_spi.c + +# Lia baro (no bmp onboard) +else ifeq ($(BOARD), lia) +# fixme, reuse the baro drivers in lisa_m dir +LIA_BARO ?= BARO_MS5611_SPI + ifeq ($(LIA_BARO), BARO_MS5611_SPI) + include $(CFG_SHARED)/spi_master.makefile + ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3 + ap.CFLAGS += -DBB_MS5611_SPI_DEV=spi2 + ap.CFLAGS += -DBB_MS5611_SLAVE_IDX=SPI_SLAVE3 + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_spi.c + ap.srcs += boards/baro_board_ms5611_spi.c + else ifeq ($(LIA_BARO), BARO_MS5611_I2C) + ap.CFLAGS += -DUSE_I2C2 + ap.CFLAGS += -DBB_MS5611_I2C_DEV=i2c2 + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_i2c.c + ap.srcs += boards/baro_board_ms5611_i2c.c + endif + +# navgo baro +else ifeq ($(BOARD), navgo) + include $(CFG_SHARED)/spi_master.makefile + ap.CFLAGS += -DUSE_SPI_SLAVE0 + ap.CFLAGS += -DUSE_SPI1 + ap.srcs += peripherals/mcp355x.c + ap.srcs += $(SRC_BOARD)/baro_board.c + +# krooz baro +else ifeq ($(BOARD), krooz) + ap.CFLAGS += -DBB_MS5611_I2C_DEV=i2c2 + ap.CFLAGS += -DBB_MS5611_SLAVE_ADDR=0xEC + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_i2c.c + ap.srcs += boards/baro_board_ms5611_i2c.c + +# PX4FMU +else ifeq ($(BOARD), px4fmu) + ap.CFLAGS += -DUSE_I2C2 + ap.CFLAGS += -DBB_MS5611_I2C_DEV=i2c2 + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_i2c.c + ap.srcs += boards/baro_board_ms5611_i2c.c + +# apogee baro +else ifeq ($(BOARD), apogee) + ap.CFLAGS += -DUSE_I2C1 + ap.srcs += peripherals/mpl3115.c + ap.srcs += $(SRC_BOARD)/baro_board.c + +# Umarim +else ifeq ($(BOARD), umarim) + ifeq ($(BOARD_VERSION), 1.0) + ap.CFLAGS += -DUSE_I2C1 -DUSE_ADS1114_1 + ap.CFLAGS += -DADS1114_I2C_DEV=i2c1 + ap.srcs += peripherals/ads1114.c + ap.srcs += boards/umarim/baro_board.c + endif + +endif # check board + +ifneq ($(BARO_LED),none) +ap.CFLAGS += -DBARO_LED=$(BARO_LED) +endif + +# +# add it for simulators +# +# only NPS for now... +#sim.srcs += $(SRC_BOARD)/baro_board.c +#jsbsim.srcs += $(SRC_BOARD)/baro_board.c +nps.srcs += $(SRC_BOARD)/baro_board.c + + +else # USE_BARO_BOARD is not TRUE, was explicitly disabled + +$(TARGET).CFLAGS += -DUSE_BARO_BOARD=FALSE + +endif # check USE_BARO_BOARD diff --git a/conf/messages.xml b/conf/messages.xml index 92cca695ce..3b09735149 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1345,7 +1345,7 @@ - + @@ -1891,8 +1891,8 @@ - - + + @@ -2718,15 +2718,6 @@ - - - - - - - - - diff --git a/conf/modules/baro_amsys.xml b/conf/modules/baro_amsys.xml index 2b94bb226c..7b7f72f3d4 100644 --- a/conf/modules/baro_amsys.xml +++ b/conf/modules/baro_amsys.xml @@ -20,7 +20,7 @@ - + diff --git a/conf/modules/baro_board.xml b/conf/modules/baro_board.xml deleted file mode 100644 index b5c56db26f..0000000000 --- a/conf/modules/baro_board.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - Baro board wrapper. - Allows to use baro interface on fixedwing with external barometers. - - -
- -
- - - - - - diff --git a/conf/modules/baro_ets.xml b/conf/modules/baro_ets.xml index 0b59c2a45b..ec53f74a56 100644 --- a/conf/modules/baro_ets.xml +++ b/conf/modules/baro_ets.xml @@ -19,7 +19,9 @@ - + + + @@ -31,7 +33,7 @@ - + diff --git a/conf/modules/baro_hca.xml b/conf/modules/baro_hca.xml index d874c6388c..3a368f48d8 100644 --- a/conf/modules/baro_hca.xml +++ b/conf/modules/baro_hca.xml @@ -13,7 +13,7 @@ - + diff --git a/conf/modules/baro_mpl3115.xml b/conf/modules/baro_mpl3115.xml index 81151291fb..13225e56c0 100644 --- a/conf/modules/baro_mpl3115.xml +++ b/conf/modules/baro_mpl3115.xml @@ -3,7 +3,10 @@ Baro MPL3115A2 (I2C) - + + + +
@@ -13,7 +16,7 @@ - + diff --git a/conf/modules/baro_ms5611_spi.xml b/conf/modules/baro_ms5611_spi.xml index 2ec153e554..281bc6f94f 100644 --- a/conf/modules/baro_ms5611_spi.xml +++ b/conf/modules/baro_ms5611_spi.xml @@ -7,7 +7,7 @@ Measurement Specialties MS5611-01BA pressure sensor (SPI) - +
diff --git a/conf/modules/baro_sim.xml b/conf/modules/baro_sim.xml new file mode 100644 index 0000000000..ca694bb582 --- /dev/null +++ b/conf/modules/baro_sim.xml @@ -0,0 +1,22 @@ + + + + + + Simulated barometer. + Sends the BARO_ABS ABI message with gps.hmsl converted to absolute pressure. + + + +
+ +
+ + + + + + + + + diff --git a/conf/settings/nps.xml b/conf/settings/nps.xml index ba8aa8f8c6..fd58df0a73 100644 --- a/conf/settings/nps.xml +++ b/conf/settings/nps.xml @@ -4,8 +4,8 @@ - - + + diff --git a/conf/simulator/nps/nps_sensors_params_aspirin_1.5.h b/conf/simulator/nps/nps_sensors_params_aspirin_1.5.h index 678a33833a..d9ae3d501f 100644 --- a/conf/simulator/nps/nps_sensors_params_aspirin_1.5.h +++ b/conf/simulator/nps/nps_sensors_params_aspirin_1.5.h @@ -131,14 +131,11 @@ /* - * Barometer + * Barometer (pressure and std dev in Pascal) */ -/* m */ -/* aka 2^8/INS_BARO_SENS */ -#define NPS_BARO_QNH 900. -#define NPS_BARO_SENSITIVITY 17.066667 -#define NPS_BARO_DT (1./100.) -#define NPS_BARO_NOISE_STD_DEV 5.e-2 +#define NPS_BARO_DT (1./50.) +#define NPS_BARO_NOISE_STD_DEV 2 + /* * GPS diff --git a/conf/simulator/nps/nps_sensors_params_booz2_a1.h b/conf/simulator/nps/nps_sensors_params_booz2_a1.h index 3464326aa8..f815d77033 100644 --- a/conf/simulator/nps/nps_sensors_params_booz2_a1.h +++ b/conf/simulator/nps/nps_sensors_params_booz2_a1.h @@ -130,14 +130,11 @@ /* - * Barometer + * Barometer (pressure and std dev in Pascal) */ -/* m */ -/* aka 2^8/INS_BARO_SENS */ -#define NPS_BARO_QNH 900. -#define NPS_BARO_SENSITIVITY 17.066667 -#define NPS_BARO_DT (1./100.) -#define NPS_BARO_NOISE_STD_DEV 5.e-2 +#define NPS_BARO_DT (1./50.) +#define NPS_BARO_NOISE_STD_DEV 2 + /* * GPS diff --git a/conf/simulator/nps/nps_sensors_params_booz2_a1p.h b/conf/simulator/nps/nps_sensors_params_booz2_a1p.h index 3edb61d98f..7e4e88fa38 100644 --- a/conf/simulator/nps/nps_sensors_params_booz2_a1p.h +++ b/conf/simulator/nps/nps_sensors_params_booz2_a1p.h @@ -130,14 +130,11 @@ /* - * Barometer + * Barometer (pressure and std dev in Pascal) */ -/* m */ -/* aka 2^8/INS_BARO_SENS */ -#define NPS_BARO_QNH 900. -#define NPS_BARO_SENSITIVITY 17.066667 -#define NPS_BARO_DT (1./100.) -#define NPS_BARO_NOISE_STD_DEV 5.e-2 +#define NPS_BARO_DT (1./50.) +#define NPS_BARO_NOISE_STD_DEV 2 + /* * GPS diff --git a/conf/simulator/nps/nps_sensors_params_default.h b/conf/simulator/nps/nps_sensors_params_default.h index 1874378859..d0be213c4e 100644 --- a/conf/simulator/nps/nps_sensors_params_default.h +++ b/conf/simulator/nps/nps_sensors_params_default.h @@ -126,14 +126,10 @@ /* - * Barometer + * Barometer (pressure and std dev in Pascal) */ -/* m */ -/* aka 2^8/INS_BARO_SENS */ -#define NPS_BARO_QNH 900. -#define NPS_BARO_SENSITIVITY 17.066667 -#define NPS_BARO_DT (1./100.) -#define NPS_BARO_NOISE_STD_DEV 5.e-2 +#define NPS_BARO_DT (1./50.) +#define NPS_BARO_NOISE_STD_DEV 2 /* * GPS diff --git a/sw/airborne/arch/sim/sim_baro.c b/sw/airborne/arch/sim/sim_baro.c deleted file mode 100644 index cf6221bb29..0000000000 --- a/sw/airborne/arch/sim/sim_baro.c +++ /dev/null @@ -1,37 +0,0 @@ -#include -#include "state.h" -#include "subsystems/nav.h" -#include "subsystems/gps.h" -#include "baro_MS5534A.h" - -bool_t alt_baro_enabled; -bool_t spi_message_received; -bool_t baro_MS5534A_available; -uint32_t baro_MS5534A_pressure; -uint32_t baro_MS5534A_ground_pressure; -uint16_t baro_MS5534A_temp; -float baro_MS5534A_r; -float baro_MS5534A_sigma2; -float baro_MS5534A_z; - -void baro_MS5534A_init(void) { - baro_MS5534A_ground_pressure = 100000; - - baro_MS5534A_r = 10.; - baro_MS5534A_sigma2 = 1; -} - -// void baro_MS5534A_reset(void); - -void baro_MS5534A_send(void) { - static bool_t even = FALSE; - even = !even; - - spi_message_received = even; -} - -void baro_MS5534A_event_task( void ) { - baro_MS5534A_pressure = baro_MS5534A_ground_pressure - (gps.hmsl/1000.-ground_alt) / 0.08 + ((10.*random()) / RAND_MAX); - baro_MS5534A_temp = 10 + stateGetPositionUtm_f()->alt; - baro_MS5534A_available = TRUE; -} diff --git a/sw/airborne/boards/apogee/baro_board.c b/sw/airborne/boards/apogee/baro_board.c index cfabf3b2a5..27aabe4bfe 100644 --- a/sw/airborne/boards/apogee/baro_board.c +++ b/sw/airborne/boards/apogee/baro_board.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2010 ENAC + * Copyright (C) 2013 Gautier Hattenberger (ENAC) * * This file is part of paparazzi. * @@ -26,40 +26,59 @@ * integrated barometer for Apogee boards (mpl3115) */ +#include "std.h" #include "subsystems/sensors/baro.h" +#include "peripherals/mpl3115.h" // to get MPU status #include "boards/apogee/imu_apogee.h" - -/* Common Baro struct */ -struct Baro baro; +#include "subsystems/abi.h" +#include "led.h" /** Counter to init ads1114 at startup */ #define BARO_STARTUP_COUNTER 200 uint16_t startup_cnt; +struct Mpl3115 apogee_baro; + void baro_init( void ) { - mpl3115_init(); - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 1; /* not handled on this board, use extra module */ + mpl3115_init(&apogee_baro, &i2c1, MPL3115_I2C_ADDR); +#ifdef BARO_LED + LED_OFF(BARO_LED); +#endif startup_cnt = BARO_STARTUP_COUNTER; } void baro_periodic( void ) { - if (baro.status == BS_UNINITIALIZED && mpl3115_data_available) { - // Run some loops to get correct readings from the adc - --startup_cnt; - mpl3115_data_available = FALSE; - if (startup_cnt == 0) { - baro.status = BS_RUNNING; - } - } - // Baro is slave of the MPU, only start reading it after MPU is configured - if (imu_apogee.mpu.config.initialized) - Mpl3115Periodic(); + if (imu_apogee.mpu.config.initialized) { + + if (startup_cnt > 0 && apogee_baro.data_available) { + // Run some loops to get correct readings from the adc + --startup_cnt; + apogee_baro.data_available = FALSE; +#ifdef BARO_LED + LED_TOGGLE(BARO_LED); + if (startup_cnt == 0) { + LED_ON(BARO_LED); + } +#endif + } + // Read the sensor + mpl3115_periodic(&apogee_baro); + } +} + +void apogee_baro_event(void) { + mpl3115_event(&apogee_baro); + if (apogee_baro.data_available) { + if (startup_cnt == 0) { + float pressure = ((float)apogee_baro.pressure/(1<<2)); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + } + apogee_baro.data_available = FALSE; + } } diff --git a/sw/airborne/boards/apogee/baro_board.h b/sw/airborne/boards/apogee/baro_board.h index c820fd1cb1..595a8a90cb 100644 --- a/sw/airborne/boards/apogee/baro_board.h +++ b/sw/airborne/boards/apogee/baro_board.h @@ -29,32 +29,7 @@ #ifndef BOARDS_APOGEE_BARO_H #define BOARDS_APOGEE_BARO_H -#include "std.h" -#include "peripherals/mpl3115.h" - -/* There is no differential pressure on the board but - * it can be available from an external sensor - * */ - -#define BaroAbs(_handler) { \ - mpl3115_event(); \ - if (mpl3115_data_available) { \ - baro.absolute = mpl3115_pressure; \ - if (baro.status == BS_RUNNING) { \ - _handler(); \ - mpl3115_data_available = FALSE; \ - } \ - } \ -} - -// TODO handle baro diff -#ifndef BaroDiff -#define BaroDiff(_h) {} -#endif - -#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - BaroAbs(_b_abs_handler); \ - BaroDiff(_b_diff_handler); \ -} +extern void apogee_baro_event(void); +#define BaroEvent apogee_baro_event #endif // BOARDS_APOGEE_BARO_H diff --git a/sw/airborne/boards/apogee_0.99.h b/sw/airborne/boards/apogee_0.99.h index 8a79bdc24b..6e22c73192 100644 --- a/sw/airborne/boards/apogee_0.99.h +++ b/sw/airborne/boards/apogee_0.99.h @@ -183,8 +183,10 @@ //#define SPI_SELECT_SLAVE5_PORT GPIOC //#define SPI_SELECT_SLAVE5_PIN GPIO4 -/* Activate onboard baro */ -#define BOARD_HAS_BARO 1 +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD +#define USE_BARO_BOARD 1 +#endif /* PWM */ #define PWM_USE_TIM2 1 diff --git a/sw/airborne/boards/apogee_1.0.h b/sw/airborne/boards/apogee_1.0.h index 343c514eca..7fc32b63d0 100644 --- a/sw/airborne/boards/apogee_1.0.h +++ b/sw/airborne/boards/apogee_1.0.h @@ -253,8 +253,11 @@ #define I2C2_GPIO_SDA GPIO11 -/* Activate onboard baro */ -#define BOARD_HAS_BARO 1 +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD +#define USE_BARO_BOARD 1 +#endif + /* PWM */ #define PWM_USE_TIM2 1 diff --git a/sw/airborne/boards/ardrone/baro_board.c b/sw/airborne/boards/ardrone/baro_board.c index 9b05dad93a..73c33868f3 100644 --- a/sw/airborne/boards/ardrone/baro_board.c +++ b/sw/airborne/boards/ardrone/baro_board.c @@ -27,18 +27,16 @@ */ #include "subsystems/sensors/baro.h" +#include "subsystems/abi.h" #include "baro_board.h" #include "navdata.h" -struct Baro baro; #define BMP180_OSS 0 // Parrot ARDrone uses no oversampling -void baro_init(void) { - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; -} +void baro_init(void) {} + +void baro_periodic(void) {} static inline int32_t baro_apply_calibration(int32_t raw) { @@ -69,20 +67,16 @@ static inline int32_t baro_apply_calibration_temp(int32_t tmp_raw) return (baro_calibration.b5 + 8) >> 4; } -void baro_periodic(void) +void ardrone_baro_event(void) { -} - -void process_ardrone_baro(void) -{ - if(baro.status == BS_RUNNING) { - // first read temperature because pressure calibration depends on temperature - baro.differential = baro_apply_calibration_temp(navdata.temperature_pressure); // We store the temperature in Baro-Diff - baro.absolute = baro_apply_calibration(navdata.pressure); - } - else { - if (baro_calibrated == TRUE) { - baro.status = BS_RUNNING; + if (navdata_baro_available) { + if (baro_calibrated) { + // first read temperature because pressure calibration depends on temperature + // TODO send Temperature message + baro_apply_calibration_temp(navdata.temperature_pressure); + float pressure = (float)baro_apply_calibration(navdata.pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); } + navdata_baro_available = FALSE; } } diff --git a/sw/airborne/boards/ardrone/baro_board.h b/sw/airborne/boards/ardrone/baro_board.h index d98a771108..443e94b476 100644 --- a/sw/airborne/boards/ardrone/baro_board.h +++ b/sw/airborne/boards/ardrone/baro_board.h @@ -30,26 +30,7 @@ #ifndef BOARDS_ARDRONE2_BARO_H #define BOARDS_ARDRONE2_BARO_H -#if BOARD_HAS_BARO -#include "navdata.h" - -void process_ardrone_baro(void); - - -static inline void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)){ - if (navdata_baro_available) - { - navdata_baro_available = 0; - process_ardrone_baro(); - b_abs_handler(); - } -} - -#define BaroEvent(_b_abs_handler, _b_diff_handler) {\ - baro_event(_b_abs_handler,_b_diff_handler);\ -} -#else -#define BaroEvent(_b_abs_handler, _b_diff_handler) {} -#endif +extern void ardrone_baro_event(void); +#define BaroEvent ardrone_baro_event #endif /* BOARDS_ARDRONE2_BARO_H */ diff --git a/sw/airborne/boards/ardrone/baro_board_dummy.c b/sw/airborne/boards/ardrone/baro_board_dummy.c deleted file mode 100644 index 8487675af7..0000000000 --- a/sw/airborne/boards/ardrone/baro_board_dummy.c +++ /dev/null @@ -1,40 +0,0 @@ -/* - * Copyright (C) 2013 Dino Hensen - * - * 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 boards/ardrone/baro_board_dummy.c - * Dummy Baro Board. - * - * These functions are mostly empty because this is a dummy. - */ - -#include "subsystems/sensors/baro.h" - -struct Baro baro; - -void baro_init(void) { - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; -} - -void baro_periodic(void) { -} diff --git a/sw/airborne/boards/ardrone2_raw.h b/sw/airborne/boards/ardrone2_raw.h index 6200042285..b60b41efc4 100644 --- a/sw/airborne/boards/ardrone2_raw.h +++ b/sw/airborne/boards/ardrone2_raw.h @@ -9,6 +9,10 @@ #define ActuatorsDefaultInit() ActuatorsArdroneInit() #define ActuatorsDefaultCommit() ActuatorsArdroneCommit() -#define BOARD_HAS_BARO 1 + +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD +#define USE_BARO_BOARD 1 +#endif #endif /* CONFIG_ARDRONE2_RAW */ diff --git a/sw/airborne/boards/ardrone2_sdk.h b/sw/airborne/boards/ardrone2_sdk.h index 01bb97452d..923cc22037 100644 --- a/sw/airborne/boards/ardrone2_sdk.h +++ b/sw/airborne/boards/ardrone2_sdk.h @@ -15,6 +15,6 @@ #define ActuatorsDefaultInit() {} #define ActuatorsDefaultCommit() {} -#define BOARD_HAS_BARO 0 +#define USE_BARO_BOARD 0 #endif /* CONFIG_ARDRONE2_SDK */ diff --git a/sw/airborne/boards/baro_board_ms5611_i2c.c b/sw/airborne/boards/baro_board_ms5611_i2c.c new file mode 100644 index 0000000000..3cd631019b --- /dev/null +++ b/sw/airborne/boards/baro_board_ms5611_i2c.c @@ -0,0 +1,111 @@ +/* + * Copyright (C) 2011-2013 The Paparazzi Team + * + * 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 boards/baro_board_ms5611_i2c.c + * + * Driver for onboard MS5611 baro via I2C. + * + */ + +#include "subsystems/sensors/baro.h" +#include "peripherals/ms5611_i2c.h" + +#include "mcu_periph/sys_time.h" +#include "led.h" +#include "std.h" +#include "subsystems/abi.h" + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif +#include "mcu_periph/uart.h" +#include "messages.h" +#include "subsystems/datalink/downlink.h" + + +/* default i2c address + * when CSB is set to GND addr is 0xEE + * when CSB is set to VCC addr is 0xEC + * + * Note: Aspirin 2.1 has CSB bound to GND. + */ +#ifndef BB_MS5611_SLAVE_ADDR +#define BB_MS5611_SLAVE_ADDR 0xEE +#endif + + +struct Ms5611_I2c bb_ms5611; + + +void baro_init(void) { + ms5611_i2c_init(&bb_ms5611, &BB_MS5611_I2C_DEV, BB_MS5611_SLAVE_ADDR); + +#ifdef BARO_LED + LED_OFF(BARO_LED); +#endif +} + +void baro_periodic(void) { + if (sys_time.nb_sec > 1) { + + /* call the convenience periodic that initializes the sensor and starts reading*/ + ms5611_i2c_periodic(&bb_ms5611); + +#if DEBUG + if (bb_ms5611.initialized) + RunOnceEvery((50*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, + &bb_ms5611.data.c[0], + &bb_ms5611.data.c[1], + &bb_ms5611.data.c[2], + &bb_ms5611.data.c[3], + &bb_ms5611.data.c[4], + &bb_ms5611.data.c[5], + &bb_ms5611.data.c[6], + &bb_ms5611.data.c[7]); +#endif + } +} + +void baro_event(void){ + if (sys_time.nb_sec > 1) { + ms5611_i2c_event(&bb_ms5611); + + if (bb_ms5611.data_available) { + float pressure = (float)bb_ms5611.data.pressure; + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + bb_ms5611.data_available = FALSE; + +#ifdef BARO_LED + RunOnceEvery(10,LED_TOGGLE(BARO_LED)); +#endif + +#if DEBUG + float ftempms = bb_ms5611.data.temperature / 100.; + float fbaroms = bb_ms5611.data.pressure / 100.; + DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice, + &bb_ms5611.data.d1, &bb_ms5611.data.d2, + &fbaroms, &ftempms); +#endif + } + } +} diff --git a/sw/airborne/boards/baro_board_ms5611_spi.c b/sw/airborne/boards/baro_board_ms5611_spi.c new file mode 100644 index 0000000000..61b0396d78 --- /dev/null +++ b/sw/airborne/boards/baro_board_ms5611_spi.c @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2011-2013 The Paparazzi Team + * + * 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 boards/baro_board_ms5611_spi.c + * + * Driver for onboard MS5611 baro via SPI. + * + */ + +#include "subsystems/sensors/baro.h" +#include "peripherals/ms5611_spi.h" + +#include "mcu_periph/sys_time.h" +#include "led.h" +#include "std.h" +#include "subsystems/abi.h" + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif +#include "mcu_periph/uart.h" +#include "messages.h" +#include "subsystems/datalink/downlink.h" + + +struct Ms5611_Spi bb_ms5611; + + +void baro_init(void) { + ms5611_spi_init(&bb_ms5611, &BB_MS5611_SPI_DEV, BB_MS5611_SLAVE_IDX); + +#ifdef BARO_LED + LED_OFF(BARO_LED); +#endif +} + +void baro_periodic(void) { + if (sys_time.nb_sec > 1) { + + /* call the convenience periodic that initializes the sensor and starts reading*/ + ms5611_spi_periodic(&bb_ms5611); + +#if DEBUG + if (bb_ms5611.initialized) + RunOnceEvery((50*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, + &bb_ms5611.data.c[0], + &bb_ms5611.data.c[1], + &bb_ms5611.data.c[2], + &bb_ms5611.data.c[3], + &bb_ms5611.data.c[4], + &bb_ms5611.data.c[5], + &bb_ms5611.data.c[6], + &bb_ms5611.data.c[7]); +#endif + } +} + +void baro_event(){ + if (sys_time.nb_sec > 1) { + ms5611_spi_event(&bb_ms5611); + + if (bb_ms5611.data_available) { + float pressure = (float)bb_ms5611.data.pressure; + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + bb_ms5611.data_available = FALSE; + +#ifdef BARO_LED + RunOnceEvery(10,LED_TOGGLE(BARO_LED)); +#endif + +#if DEBUG + float ftempms = bb_ms5611.data.temperature / 100.; + float fbaroms = bb_ms5611.data.pressure / 100.; + DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice, + &bb_ms5611.data.d1, &bb_ms5611.data.d2, + &fbaroms, &ftempms); +#endif + } + } +} diff --git a/sw/airborne/boards/booz/baro_board.c b/sw/airborne/boards/booz/baro_board.c index cfcbc8b5b2..e7d955ecdb 100644 --- a/sw/airborne/boards/booz/baro_board.c +++ b/sw/airborne/boards/booz/baro_board.c @@ -1,5 +1,4 @@ -/* $Id$ - * +/* * Copyright (C) 2010 The Paparazzi Team * * This file is part of paparazzi. @@ -20,48 +19,74 @@ * Boston, MA 02111-1307, USA. */ +/** + * @file boards/booz/baro_board.c + * + */ + #include "subsystems/sensors/baro.h" #include "generated/airframe.h" +#include "subsystems/abi.h" #include "led.h" -/* threshold >0 && <1023 */ + +/** threshold >0 && <1023 */ #ifndef BOOZ_ANALOG_BARO_THRESHOLD #define BOOZ_ANALOG_BARO_THRESHOLD 850 #endif -struct Baro baro; -struct BaroBoard baro_board; +/** scale factor to convert raw ADC measurement to pressure in Pascal. + * + * Sensor Sensitivity -> SS = 0.045 mv / Pa + * Sensor Gain -> G = 94.25 + * Sensitivity -> S = SS*G = 4.24125 mV / Pa + * 10 bit ADC -> A = 3.3 V / 1024 = 3.223 mV / LSB + * Total Sensitivity SENS = A / S = 0.759837 + * + * For the real pressure you also need to take into account the (variable) offset + * + * supply voltage Vs = 5V + * real sensor sensitivity Vout = Vs * (0.009 P - 0.095) + * voltage variable offset Voff(DAC) = Vs / 69.23 + (DAC * 3.3 / 1024) / 21.77 + * ADC voltage at init Vadc = 3.3*BARO_THRESHOLD/1024 = Vout - Voff + * + * => Inverting these formulas can give the 'real' pressure + * + * since we don't care that much in this case, we can take a fixed offset of 101325 Pa + */ +#ifndef BOOZ_BARO_SENS +#define BOOZ_BARO_SENS 0.759837 +#endif +struct BaroBoard baro_board; void baro_init( void ) { adc_buf_channel(ADC_CHANNEL_BARO, &baro_board.buf, DEFAULT_AV_NB_SAMPLE); - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; /* not handled on this board */ - + baro_board.status = BB_UNINITIALIZED; + baro_board.absolute = 0; baro_board.offset = 1023; DACSet(baro_board.offset); - baro_board.value_filtered = 0; - baro_board.data_available = FALSE; -#ifdef ROTORCRAFT_BARO_LED - LED_OFF(ROTORCRAFT_BARO_LED); +#ifdef BARO_LED + LED_OFF(BARO_LED); #endif } void baro_periodic(void) { - baro.absolute = baro_board.buf.sum/baro_board.buf.av_nb_sample; - baro_board.value_filtered = (3*baro_board.value_filtered + baro.absolute)/4; - if (baro.status == BS_UNINITIALIZED) { + baro_board.absolute = baro_board.buf.sum/baro_board.buf.av_nb_sample; + baro_board.value_filtered = (3*baro_board.value_filtered + baro_board.absolute)/4; + if (baro_board.status == BB_UNINITIALIZED) { RunOnceEvery(10, { baro_board_calibrate();}); } - /* else */ - baro_board.data_available = TRUE; + else { + float pressure = 101325.0 - BOOZ_BARO_SENS*(BOOZ_ANALOG_BARO_THRESHOLD - baro_board.absolute); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + } } /* decrement offset until adc reading is over a threshold */ @@ -72,18 +97,15 @@ void baro_board_calibrate(void) { else baro_board.offset--; DACSet(baro_board.offset); -#ifdef ROTORCRAFT_BARO_LED - LED_TOGGLE(ROTORCRAFT_BARO_LED); +#ifdef BARO_LED + LED_TOGGLE(BARO_LED); #endif } else { - baro.status = BS_RUNNING; -#ifdef ROTORCRAFT_BARO_LED - LED_ON(ROTORCRAFT_BARO_LED); + baro_board.status = BB_RUNNING; +#ifdef BARO_LED + LED_ON(BARO_LED); #endif } } - - - diff --git a/sw/airborne/boards/booz/baro_board.h b/sw/airborne/boards/booz/baro_board.h index 902aef2db1..f47abe700b 100644 --- a/sw/airborne/boards/booz/baro_board.h +++ b/sw/airborne/boards/booz/baro_board.h @@ -1,3 +1,30 @@ + /* + * Copyright (C) 2010-2013 Antoine Drouin, Gautier Hattenberger + * + * 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 boards/booz/baro_board.h + * + */ + #ifndef BOARDS_BOOZ_BARO_H #define BOARDS_BOOZ_BARO_H @@ -7,24 +34,23 @@ #include "mcu_periph/adc.h" #include "mcu_periph/dac.h" +enum BaroBoardStatus { + BB_UNINITIALIZED, + BB_RUNNING +}; struct BaroBoard { + enum BaroBoardStatus status; + int32_t absolute; uint16_t offset; uint16_t value_filtered; - bool_t data_available; struct adc_buf buf; }; extern struct BaroBoard baro_board; extern void baro_board_calibrate(void); - -#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - if (baro_board.data_available) { \ - _b_abs_handler(); \ - baro_board.data_available = FALSE; \ - } \ - } +#define BaroEvent() {} static inline void baro_board_SetOffset(uint16_t _o) { baro_board.offset = _o; diff --git a/sw/airborne/boards/booz_1.0.h b/sw/airborne/boards/booz_1.0.h index 1bb2007977..70fc889bc3 100644 --- a/sw/airborne/boards/booz_1.0.h +++ b/sw/airborne/boards/booz_1.0.h @@ -182,6 +182,9 @@ #endif -#define BOARD_HAS_BARO 1 +/* by default enable onboard baro */ +#ifndef USE_BARO_BOARD +#define USE_BARO_BOARD 1 +#endif #endif /* CONFIG_BOOZ2_V1_0_H */ diff --git a/sw/airborne/boards/krooz/baro_board.h b/sw/airborne/boards/krooz/baro_board.h index 5f47ad650e..c12e63138c 100644 --- a/sw/airborne/boards/krooz/baro_board.h +++ b/sw/airborne/boards/krooz/baro_board.h @@ -8,7 +8,7 @@ #ifndef BOARDS_KROOZ_BARO_H #define BOARDS_KROOZ_BARO_H -extern void baro_event(void (*b_abs_handler)(void)); -#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler) +extern void baro_event(void); +#define BaroEvent baro_event #endif /* BOARDS_KROOZ_SD_BARO_H */ diff --git a/sw/airborne/boards/krooz_sd.h b/sw/airborne/boards/krooz_sd.h index 1ecd270753..5c12ecef7e 100644 --- a/sw/airborne/boards/krooz_sd.h +++ b/sw/airborne/boards/krooz_sd.h @@ -224,8 +224,12 @@ } #endif // USE_AD1 -/* Activate onboard baro */ -#define BOARD_HAS_BARO 1 + +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD +#define USE_BARO_BOARD 1 +#endif + /* PWM */ #define PWM_USE_TIM3 1 diff --git a/sw/airborne/boards/lia/baro_board.h b/sw/airborne/boards/lia/baro_board.h deleted file mode 100644 index 59782132c8..0000000000 --- a/sw/airborne/boards/lia/baro_board.h +++ /dev/null @@ -1,16 +0,0 @@ - -/* - * FIXME, fake baro board header for BaroEvent - * - */ - -#ifndef BOARDS_LIA_BARO_H -#define BOARDS_LIA_BARO_H - -#include "std.h" - -extern void baro_event(void (*b_abs_handler)(void)); - -#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler) - -#endif /* BOARDS_LIA_BARO_H */ diff --git a/sw/airborne/boards/lia_1.1.h b/sw/airborne/boards/lia_1.1.h index 3b9c1bf419..31260f054d 100644 --- a/sw/airborne/boards/lia_1.1.h +++ b/sw/airborne/boards/lia_1.1.h @@ -166,7 +166,7 @@ // FIXME, using baro_board right now to include the appropriate header -#define BOARD_HAS_BARO 0 +#define USE_BARO_BOARD 0 #endif /* CONFIG_LIA_1_1_H */ diff --git a/sw/airborne/boards/lisa_l/baro_board.c b/sw/airborne/boards/lisa_l/baro_board.c index 9e0d825c5e..a7a6a7834b 100644 --- a/sw/airborne/boards/lisa_l/baro_board.c +++ b/sw/airborne/boards/lisa_l/baro_board.c @@ -1,10 +1,57 @@ +/* + * Copyright (C) 2010-2013 The Paparazzi Team + * + * 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. + * + */ +#include "std.h" #include "subsystems/sensors/baro.h" +#include "mcu_periph/i2c.h" +#include "subsystems/abi.h" +#include "led.h" + +enum LisaBaroStatus { + LBS_UNINITIALIZED, + LBS_RESETED, + LBS_INITIALIZING_ABS, + LBS_INITIALIZING_ABS_1, + LBS_INITIALIZING_DIFF, + LBS_INITIALIZING_DIFF_1, + LBS_IDLE, + LBS_READING_ABS, + LBS_READ_ABS, + LBS_READING_DIFF, + LBS_READ_DIFF +}; + +struct BaroBoard { + enum LisaBaroStatus status; + bool_t running; +}; + -struct Baro baro; struct BaroBoard baro_board; struct i2c_transaction baro_trans; +static inline void baro_board_send_reset(void); +static inline void baro_board_send_config_abs(void); +static inline void baro_board_send_config_diff(void); static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb); static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_addr); static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr); @@ -15,11 +62,21 @@ static inline void baro_board_read_from_current_register(uint8_t baro_addr); // differential #define BARO_DIFF_ADDR 0x92 +// FIXME +#ifndef LISA_L_BARO_SENS +#define LISA_L_BARO_SENS 1.0 +#endif + +#ifndef LISA_L_DIFF_SENS +#define LISA_L_DIFF_SENS 1.0 +#endif + void baro_init(void) { - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; +#ifdef BARO_LED + LED_OFF(BARO_LED); +#endif baro_board.status = LBS_UNINITIALIZED; + baro_board.running = FALSE; } @@ -50,7 +107,7 @@ void baro_periodic(void) { // baro_board.status = LBS_UNINITIALIZED; break; case LBS_INITIALIZING_DIFF_1: - baro.status = BS_RUNNING; + baro_board.running = TRUE; case LBS_READ_DIFF: baro_board_read_from_current_register(BARO_ABS_ADDR); baro_board.status = LBS_READING_ABS; @@ -63,10 +120,38 @@ void baro_periodic(void) { break; } +#ifdef BARO_LED + if (baro_board.running == TRUE) { + LED_ON(BARO_LED); + } + else { + LED_TOGGLE(BARO_LED); + } +#endif } +void lisa_l_baro_event(void) { + if (baro_board.status == LBS_READING_ABS && + baro_trans.status != I2CTransPending) { + baro_board.status = LBS_READ_ABS; + if (baro_trans.status == I2CTransSuccess) { + int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; + float pressure = LISA_L_BARO_SENS*(float)tmp; + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + } + } + else if (baro_board.status == LBS_READING_DIFF && + baro_trans.status != I2CTransPending) { + baro_board.status = LBS_READ_DIFF; + if (baro_trans.status == I2CTransSuccess) { + int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; + float diff = LISA_L_DIFF_SENS*(float)tmp; + AbiSendMsgBARO_DIFF(BARO_BOARD_SENDER_ID, &diff); + } + } +} -void baro_board_send_config_abs(void) { +static inline void baro_board_send_config_abs(void) { #ifndef BARO_LOW_GAIN INFO("Using High LisaL Baro Gain: Do not use below 1000hPa") baro_board_write_to_register(BARO_ABS_ADDR, 0x01, 0x86, 0x83); @@ -77,11 +162,11 @@ INFO("Using Low LisaL Baro Gain, capable of measuring below 1000hPa or more") #endif } -void baro_board_send_config_diff(void) { +static inline void baro_board_send_config_diff(void) { baro_board_write_to_register(BARO_DIFF_ADDR, 0x01, 0x84, 0x83); } -void baro_board_send_reset(void) { +static inline void baro_board_send_reset(void) { baro_trans.type = I2CTransTx; baro_trans.slave_addr = 0x00; baro_trans.len_w = 1; diff --git a/sw/airborne/boards/lisa_l/baro_board.h b/sw/airborne/boards/lisa_l/baro_board.h index 4eec5cfdee..5a39dc8ef8 100644 --- a/sw/airborne/boards/lisa_l/baro_board.h +++ b/sw/airborne/boards/lisa_l/baro_board.h @@ -1,5 +1,29 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger (ENAC) + * + * 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 boards/lisa_l/baro_board.h + * * board specific fonctions for the lisa_l board * */ @@ -7,56 +31,7 @@ #ifndef BOARDS_LISA_L_BARO_H #define BOARDS_LISA_L_BARO_H -#include "std.h" -#include "mcu_periph/i2c.h" - -enum LisaBaroStatus { - LBS_UNINITIALIZED, - LBS_RESETED, - LBS_INITIALIZING_ABS, - LBS_INITIALIZING_ABS_1, - LBS_INITIALIZING_DIFF, - LBS_INITIALIZING_DIFF_1, - LBS_IDLE, - LBS_READING_ABS, - LBS_READ_ABS, - LBS_READING_DIFF, - LBS_READ_DIFF -}; - -struct BaroBoard { - enum LisaBaroStatus status; -}; - -extern struct BaroBoard baro_board; -extern struct i2c_transaction baro_trans; - -extern void baro_board_send_reset(void); -extern void baro_board_send_config_abs(void); -extern void baro_board_send_config_diff(void); - -#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - if (baro_board.status == LBS_READING_ABS && \ - baro_trans.status != I2CTransPending) { \ - baro_board.status = LBS_READ_ABS; \ - if (baro_trans.status == I2CTransSuccess) { \ - int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; \ - baro.absolute = tmp; \ - _b_abs_handler(); \ - } \ - } \ - else if (baro_board.status == LBS_READING_DIFF && \ - baro_trans.status != I2CTransPending) { \ - baro_board.status = LBS_READ_DIFF; \ - if (baro_trans.status == I2CTransSuccess) { \ - int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; \ - baro.differential = tmp; \ - _b_diff_handler(); \ - } \ - } \ - } - - - +extern void lisa_l_baro_event(void); +#define BaroEvent lisa_l_baro_event #endif /* BOARDS_LISA_L_BARO_H */ diff --git a/sw/airborne/boards/lisa_l_1.0.h b/sw/airborne/boards/lisa_l_1.0.h index f18b7d34eb..bb21346971 100644 --- a/sw/airborne/boards/lisa_l_1.0.h +++ b/sw/airborne/boards/lisa_l_1.0.h @@ -156,7 +156,11 @@ #define BOARD_ADC_CHANNEL_3 0 #define BOARD_ADC_CHANNEL_4 15 -#define BOARD_HAS_BARO 1 + +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD +#define USE_BARO_BOARD 1 +#endif /* Default actuators driver */ diff --git a/sw/airborne/boards/lisa_m/baro_board.c b/sw/airborne/boards/lisa_m/baro_board.c index 9c09b97d90..5a6aa9a489 100644 --- a/sw/airborne/boards/lisa_m/baro_board.c +++ b/sw/airborne/boards/lisa_m/baro_board.c @@ -23,13 +23,17 @@ * Baro board interface for Bosch BMP085 on LisaM I2C2 with EOC check. */ +#include "std.h" + #include "subsystems/sensors/baro.h" +#include "peripherals/bmp085.h" #include "peripherals/bmp085_regs.h" #include +#include "subsystems/abi.h" #include "led.h" -struct Baro baro; + struct Bmp085 baro_bmp085; static bool_t baro_eoc(void) @@ -38,10 +42,6 @@ static bool_t baro_eoc(void) } void baro_init(void) { - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; - bmp085_init(&baro_bmp085, &i2c2, BMP085_SLAVE_ADDR); /* setup eoc check function */ @@ -50,32 +50,34 @@ void baro_init(void) { gpio_clear(GPIOB, GPIO0); gpio_set_mode(GPIOB, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN, GPIO0); + +#ifdef BARO_LED + LED_OFF(BARO_LED); +#endif } void baro_periodic(void) { - if (baro_bmp085.initialized) { - baro.status = BS_RUNNING; bmp085_periodic(&baro_bmp085); } - else + else { bmp085_read_eeprom_calib(&baro_bmp085); + } } -void baro_event(void (*b_abs_handler)(void)) +void baro_event(void) { bmp085_event(&baro_bmp085); if (baro_bmp085.data_available) { - baro.absolute = baro_bmp085.pressure; - b_abs_handler(); + float pressure = (float)baro_bmp085.pressure; + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); baro_bmp085.data_available = FALSE; - -#ifdef ROTORCRAFT_BARO_LED - RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED)); +#ifdef BARO_LED + RunOnceEvery(10,LED_TOGGLE(BARO_LED)); #endif } } diff --git a/sw/airborne/boards/lisa_m/baro_board.h b/sw/airborne/boards/lisa_m/baro_board.h index b84a11a7da..bb080094ab 100644 --- a/sw/airborne/boards/lisa_m/baro_board.h +++ b/sw/airborne/boards/lisa_m/baro_board.h @@ -7,20 +7,9 @@ #ifndef BOARDS_LISA_M_BARO_H #define BOARDS_LISA_M_BARO_H -#include "std.h" - // for right now we abuse this file for the ms5611 baro on aspirin as well -#if !BARO_MS5611_I2C && !BARO_MS5611 -#include "peripherals/bmp085.h" - -extern struct Bmp085 baro_bmp085; - - -#endif // !BARO_MS5611_xx - -extern void baro_event(void (*b_abs_handler)(void)); - -#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler) +extern void baro_event(void); +#define BaroEvent baro_event #endif /* BOARDS_LISA_M_BARO_H */ diff --git a/sw/airborne/boards/lisa_m_1.0.h b/sw/airborne/boards/lisa_m_1.0.h index a2b1b2e88d..e4f48c3ab2 100644 --- a/sw/airborne/boards/lisa_m_1.0.h +++ b/sw/airborne/boards/lisa_m_1.0.h @@ -111,7 +111,7 @@ #endif /* GPIO mapping for ADC1 pins, overwrites the default in arch/stm32/mcu_periph/adc_arch.c */ -// FIXME, this is not very nice, is also stm lib specific +// FIXME, this is not very nice, is also libopencm3 lib specific #ifdef USE_AD1 #define ADC1_GPIO_INIT(gpio) { \ gpio_set_mode(GPIOC, GPIO_MODE_INPUT, \ @@ -120,6 +120,10 @@ } #endif // USE_AD1 -#define BOARD_HAS_BARO 1 + +/* by default enable onboard baro */ +#ifndef USE_BARO_BOARD +#define USE_BARO_BOARD 1 +#endif #endif /* CONFIG_LISA_M_1_0_H */ diff --git a/sw/airborne/boards/lisa_m_2.0.h b/sw/airborne/boards/lisa_m_2.0.h index 3f09b51a8d..de0664a950 100644 --- a/sw/airborne/boards/lisa_m_2.0.h +++ b/sw/airborne/boards/lisa_m_2.0.h @@ -165,6 +165,9 @@ #endif // USE_AD1 -#define BOARD_HAS_BARO 1 +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD +#define USE_BARO_BOARD 1 +#endif #endif /* CONFIG_LISA_M_2_0_H */ diff --git a/sw/airborne/boards/lisa_s/baro_board.h b/sw/airborne/boards/lisa_s/baro_board.h index 7581ef4de3..8f41c8e3ca 100644 --- a/sw/airborne/boards/lisa_s/baro_board.h +++ b/sw/airborne/boards/lisa_s/baro_board.h @@ -4,13 +4,13 @@ * */ -#ifndef BOARDS_LISA_M_BARO_H -#define BOARDS_LISA_M_BARO_H +#ifndef BOARDS_LISA_S_BARO_H +#define BOARDS_LISA_S_BARO_H #include "std.h" -extern void baro_event(void (*b_abs_handler)(void)); +extern void baro_event(void); -#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler) +#define BaroEvent baro_event -#endif /* BOARDS_LISA_M_BARO_H */ +#endif /* BOARDS_LISA_S_BARO_H */ diff --git a/sw/airborne/boards/navgo/baro_board.c b/sw/airborne/boards/navgo/baro_board.c index 898878070e..fd99a5c6ca 100644 --- a/sw/airborne/boards/navgo/baro_board.c +++ b/sw/airborne/boards/navgo/baro_board.c @@ -25,12 +25,30 @@ * Navarro & Gorraz & Hattenberger */ +#include "std.h" +#include "baro_board.h" #include "subsystems/sensors/baro.h" +#include "peripherals/mcp355x.h" +#include "subsystems/abi.h" #include "led.h" -#include "mcu_periph/spi.h" -/* Common Baro struct */ -struct Baro baro; +/** scale factor to convert raw ADC measurement to pressure in Pascal. + * + * supply voltage Vs = 5V + * real sensor sensitivity Vout = Vs * (0.009 P - 0.095) with P in kPa + * 22 bit signed ADC -> only 21 useful bits ADC = 5/2^21 = 2.384e-6 V / LSB + * + * offset = 5*0.095/2.384e-6 = 199229 (LSB) + * sensitivity = (1000/0.009)*2.384e-6/5 = 0.05298 Pa/LSB + * + */ +#ifndef NAVGO_BARO_SENS +#define NAVGO_BARO_SENS 0.05298 +#endif + +#ifndef NAVGO_BARO_OFFSET +#define NAVGO_BARO_OFFSET 199229 +#endif /* Counter to init mcp355x at startup */ #define BARO_STARTUP_COUNTER 200 @@ -38,31 +56,35 @@ uint16_t startup_cnt; void baro_init( void ) { mcp355x_init(); - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; /* not handled on this board */ -#ifdef ROTORCRAFT_BARO_LED - LED_OFF(ROTORCRAFT_BARO_LED); +#ifdef BARO_LED + LED_OFF(BARO_LED); #endif startup_cnt = BARO_STARTUP_COUNTER; } void baro_periodic( void ) { - - if (baro.status == BS_UNINITIALIZED) { - // Run some loops to get correct readings from the adc + // Run some loops to get correct readings from the adc + if (startup_cnt > 0) { --startup_cnt; -#ifdef ROTORCRAFT_BARO_LED - LED_TOGGLE(ROTORCRAFT_BARO_LED); -#endif +#ifdef BARO_LED + LED_TOGGLE(BARO_LED); if (startup_cnt == 0) { - baro.status = BS_RUNNING; -#ifdef ROTORCRAFT_BARO_LED - LED_ON(ROTORCRAFT_BARO_LED); -#endif + LED_ON(BARO_LED); } +#endif } // Read the ADC (at 50/4 Hz, conversion time is 68 ms) RunOnceEvery(4,mcp355x_read()); } +void navgo_baro_event(void) { + mcp355x_event(); + if (mcp355x_data_available) { + if (startup_cnt == 0) { + // Send data when init phase is done + float pressure = NAVGO_BARO_SENS*(mcp355x_data+NAVGO_BARO_OFFSET); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + } + mcp355x_data_available = FALSE; + } +} diff --git a/sw/airborne/boards/navgo/baro_board.h b/sw/airborne/boards/navgo/baro_board.h index 56137a6806..bbd56ba3f7 100644 --- a/sw/airborne/boards/navgo/baro_board.h +++ b/sw/airborne/boards/navgo/baro_board.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2011-2012 Gautier Hattenberger + * Copyright (C) 2011-2013 Gautier Hattenberger * * This file is part of paparazzi. * @@ -29,17 +29,9 @@ #ifndef BOARDS_NAVGO_BARO_H #define BOARDS_NAVGO_BARO_H +extern void navgo_baro_event(void); -#include "std.h" -#include "peripherals/mcp355x.h" - -#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - mcp355x_event(); \ - if (mcp355x_data_available) { \ - baro.absolute = mcp355x_data; \ - _b_abs_handler(); \ - mcp355x_data_available = FALSE; \ - } \ -} +// define BaroEvent macro +#define BaroEvent navgo_baro_event #endif // BOARDS_NAVGO_BARO_H diff --git a/sw/airborne/boards/navgo_1.0.h b/sw/airborne/boards/navgo_1.0.h index 015b51d428..8cdfd75591 100644 --- a/sw/airborne/boards/navgo_1.0.h +++ b/sw/airborne/boards/navgo_1.0.h @@ -105,6 +105,10 @@ #define SERVO_REG_1 PWMMR5 #endif -#define BOARD_HAS_BARO 1 + +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD +#define USE_BARO_BOARD 1 +#endif #endif /* CONFIG_NAVGO_V1_0_H */ diff --git a/sw/airborne/boards/pc/baro_board.c b/sw/airborne/boards/pc/baro_board.c index 52f3cb19fe..d65be929b8 100644 --- a/sw/airborne/boards/pc/baro_board.c +++ b/sw/airborne/boards/pc/baro_board.c @@ -1,17 +1,41 @@ +/* + * Copyright (C) 2010-2013 Paparazzi Team + * + * 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 boards/pc/baro_board.h + * + * board specific fonction for the PC board ( simulator ) + * + */ #include "subsystems/sensors/baro.h" +#include "subsystems/abi.h" -struct Baro baro; +void baro_init(void) {} -bool_t baro_pc_available; - -void baro_init(void) {baro_pc_available=FALSE;} - -void baro_periodic(void) {baro.status = BS_RUNNING;} +void baro_periodic(void) {} void baro_feed_value(double value) { - baro.absolute = (int32_t) value; - baro_pc_available = TRUE; + float pressure = (float) value; + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); } diff --git a/sw/airborne/boards/pc/baro_board.h b/sw/airborne/boards/pc/baro_board.h index 54d902730b..e9ede28b60 100644 --- a/sw/airborne/boards/pc/baro_board.h +++ b/sw/airborne/boards/pc/baro_board.h @@ -1,4 +1,28 @@ /* + * Copyright (C) 2010-2013 Paparazzi Team + * + * 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 boards/pc/baro_board.c + * * board specific fonction for the PC board ( simulator ) * */ @@ -6,15 +30,7 @@ #ifndef BOARDS_PC_BARO_H #define BOARDS_PC_BARO_H -#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - if (baro_pc_available) { \ - _b_abs_handler(); \ - baro_pc_available = FALSE; \ - } \ - } - - -extern bool_t baro_pc_available; +#define BaroEvent() {} extern void baro_feed_value(double value); diff --git a/sw/airborne/boards/pc_sim.h b/sw/airborne/boards/pc_sim.h index 2b80d2f632..f2ac35ef1f 100644 --- a/sw/airborne/boards/pc_sim.h +++ b/sw/airborne/boards/pc_sim.h @@ -11,7 +11,12 @@ #define DefaultVoltageOfAdc(adc) (1.0*adc) - -#define BOARD_HAS_BARO 1 +#ifndef USE_BARO_BOARD +#if USE_NPS +#define USE_BARO_BOARD 1 +#else +#define USE_BARO_BOARD 0 +#endif +#endif #endif /* CONFIG_PC_SIM_H */ diff --git a/sw/airborne/boards/px4fmu/baro_board.h b/sw/airborne/boards/px4fmu/baro_board.h index 4aff1cc1c7..8ef2dacaea 100644 --- a/sw/airborne/boards/px4fmu/baro_board.h +++ b/sw/airborne/boards/px4fmu/baro_board.h @@ -7,8 +7,8 @@ #ifndef BOARDS_PX4FMU_BARO_H #define BOARDS_PX4FMU_BARO_H -extern void baro_event(void (*b_abs_handler)(void)); +extern void baro_event(void); -#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler) +#define BaroEvent baro_event #endif /* BOARDS_PX4FMU_BARO_H */ diff --git a/sw/airborne/boards/umarim/baro_board.c b/sw/airborne/boards/umarim/baro_board.c index c83675d17b..69f5866403 100644 --- a/sw/airborne/boards/umarim/baro_board.c +++ b/sw/airborne/boards/umarim/baro_board.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2010 ENAC + * Copyright (C) 2010-2013 Gautier Hattenberger * * This file is part of paparazzi. * @@ -25,11 +25,22 @@ * Navarro & Gorraz & Hattenberger */ +#include "std.h" +#include "baro_board.h" #include "subsystems/sensors/baro.h" +#include "peripherals/ads1114.h" +#include "subsystems/abi.h" +#include "led.h" +// ADC for absolute pressure +#define BARO_ABS_ADS +#define BARO_ABS_ADS ads1114_1 +#endif -/* Common Baro struct */ -struct Baro baro; +// FIXME +#ifndef UMARIM_BARO_SENS +#define UMARIM_BARO_SENS 0.0274181 +#endif /* Counter to init ads1114 at startup */ #define BARO_STARTUP_COUNTER 200 @@ -37,23 +48,36 @@ uint16_t startup_cnt; void baro_init( void ) { ads1114_init(); - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; /* not handled on this board, use extra module (ex: airspeed_ads1114) */ +#ifdef BARO_LED + LED_OFF(BARO_LED); +#endif startup_cnt = BARO_STARTUP_COUNTER; } void baro_periodic( void ) { - if (baro.status == BS_UNINITIALIZED && BARO_ABS_ADS.data_available) { - // Run some loops to get correct readings from the adc + // Run some loops to get correct readings from the adc + if (startup_cnt > 0) { --startup_cnt; - BARO_ABS_ADS.data_available = FALSE; +#ifdef BARO_LED + LED_TOGGLE(BARO_LED); if (startup_cnt == 0) { - baro.status = BS_RUNNING; + LED_ON(BARO_LED); } +#endif } // Read the ADC ads1114_read(&BARO_ABS_ADS); } +void umarim_baro_event(void) { + Ads1114Event(); + if (BARO_ABS_ADS.data_available) { + if (startup_cnt == 0) { + float pressure = UMARIM_BARO_SENS*Ads1114GetValue(BARO_ABS_ADS); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + } + BARO_ABS_ADS.data_available = FALSE; + } +} + diff --git a/sw/airborne/boards/umarim/baro_board.h b/sw/airborne/boards/umarim/baro_board.h index a296e6a28c..b340417909 100644 --- a/sw/airborne/boards/umarim/baro_board.h +++ b/sw/airborne/boards/umarim/baro_board.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2010-2012 Gautier Hattenberger + * Copyright (C) 2010-2013 Gautier Hattenberger * * This file is part of paparazzi. * @@ -29,53 +29,7 @@ #ifndef BOARDS_UMARIM_BARO_H #define BOARDS_UMARIM_BARO_H - -#include "std.h" -#include "peripherals/ads1114.h" - -/* There is no differential pressure on the board but - * it can be available from an external sensor - * */ - -#define BARO_ABS_ADS ads1114_1 - -#define BaroAbs(_ads, _handler) { \ - if (_ads.data_available) { \ - baro.absolute = Ads1114GetValue(_ads); \ - if (baro.status == BS_RUNNING) { \ - _handler(); \ - _ads.data_available = FALSE; \ - } \ - } \ -} - -#ifndef BaroDiff // Allow custom redefinition ? - -#if USE_BARO_DIFF - -#ifndef BARO_DIFF_ADS -#define BARO_DIFF_ADS ads1114_2 -#endif -#define BaroDiff(_ads, _handler) { \ - if (_ads.data_available) { \ - baro.differential = Ads1114GetValue(_ads); \ - if (baro.status == BS_RUNNING) { \ - _handler(); \ - _ads.data_available = FALSE; \ - } \ - } \ -} - -#else // Not using differential with ADS1114 -#define BaroDiff(_a, _h) {} -#endif - -#endif // ifndef BaroDiff - -#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - Ads1114Event(); \ - BaroAbs(BARO_ABS_ADS,_b_abs_handler); \ - BaroDiff(BARO_DIFF_ADS,_b_diff_handler); \ -} +extern void umarim_baro_event(void); +#define BaroEvent umarim_baro_event #endif // BOARDS_UMARIM_BARO_H diff --git a/sw/airborne/boards/umarim_1.0.h b/sw/airborne/boards/umarim_1.0.h index d34ca18317..b3859e2897 100644 --- a/sw/airborne/boards/umarim_1.0.h +++ b/sw/airborne/boards/umarim_1.0.h @@ -114,6 +114,10 @@ #define SPI1_DRDY_EINT 0 #define SPI1_DRDY_VIC_IT VIC_EINT0 -#define BOARD_HAS_BARO 1 + +/* by default enable onboard baro */ +#ifndef USE_BARO_BOARD +#define USE_BARO_BOARD 1 +#endif #endif /* CONFIG_UMARIM_V1_0_H */ diff --git a/sw/airborne/firmwares/fixedwing/ap_downlink.h b/sw/airborne/firmwares/fixedwing/ap_downlink.h index dc7ede333a..3b1d7fd279 100644 --- a/sw/airborne/firmwares/fixedwing/ap_downlink.h +++ b/sw/airborne/firmwares/fixedwing/ap_downlink.h @@ -298,16 +298,12 @@ #define PERIODIC_SEND_SCP_STATUS(_trans, _dev) {} #endif -#if USE_BAROMETER -#include "subsystems/sensors/baro.h" -#define PERIODIC_SEND_BARO_RAW(_trans, _dev) { \ - DOWNLINK_SEND_BARO_RAW(_trans, _dev, \ - &baro.absolute, \ - &baro.differential); \ +#include "subsystems/air_data.h" +#define PERIODIC_SEND_BARO_RAW(_trans, _dev) { \ + DOWNLINK_SEND_BARO_RAW(_trans, _dev, \ + &air_data.pressure, \ + &air_data.differential); \ } -#else -#define PERIODIC_SEND_BARO_RAW(_trans, _dev) {} -#endif #ifdef MEASURE_AIRSPEED #define PERIODIC_SEND_AIRSPEED(_trans, _dev) { \ diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 3ad0007e58..afb639af83 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -30,6 +30,8 @@ #define MODULES_C +#define ABI_C + #include #include "firmwares/fixedwing/main_ap.h" @@ -51,7 +53,8 @@ #if USE_AHRS_ALIGNER #include "subsystems/ahrs/ahrs_aligner.h" #endif -#if USE_BAROMETER +#include "subsystems/air_data.h" +#if USE_BARO_BOARD #include "subsystems/sensors/baro.h" #endif #include "subsystems/ins.h" @@ -81,11 +84,12 @@ #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1 #include "rc_settings.h" #endif +#include "subsystems/abi.h" #include "led.h" #ifdef USE_NPS -#include "nps_autopilot_fixedwing.h" +#include "nps_autopilot.h" #endif /* Default trim commands for roll, pitch and yaw */ @@ -139,11 +143,6 @@ volatile uint8_t ahrs_timeout_counter = 0; static inline void on_gps_solution( void ); #endif -#if USE_BAROMETER -static inline void on_baro_abs_event( void ); -static inline void on_baro_dif_event( void ); -#endif - // what version is this ???? static const uint16_t version = 1; @@ -196,7 +195,8 @@ void init_ap( void ) { ahrs_init(); #endif -#if USE_BAROMETER + air_data_init(); +#if USE_BARO_BOARD baro_init(); #endif @@ -583,7 +583,7 @@ void sensors_task( void ) { ahrs_propagate(); #endif -#if USE_BAROMETER +#if USE_BARO_BOARD baro_periodic(); #endif @@ -653,8 +653,8 @@ void event_task_ap( void ) { GpsEvent(on_gps_solution); #endif /* USE_GPS */ -#if USE_BAROMETER - BaroEvent(on_baro_abs_event, on_baro_dif_event); +#if USE_BARO_BOARD + BaroEvent(); #endif DatalinkEvent(); @@ -785,14 +785,3 @@ static inline void on_mag_event(void) #endif // USE_AHRS -#if USE_BAROMETER - -static inline void on_baro_abs_event( void ) { - ins_update_baro(); -} - -static inline void on_baro_dif_event( void ) { - -} - -#endif // USE_BAROMETER diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c index 701a644b82..ddee0741bf 100644 --- a/sw/airborne/firmwares/fixedwing/main_fbw.c +++ b/sw/airborne/firmwares/fixedwing/main_fbw.c @@ -58,7 +58,7 @@ uint8_t fbw_mode; #include "inter_mcu.h" #ifdef USE_NPS -#include "nps_autopilot_fixedwing.h" +#include "nps_autopilot.h" #endif /** Trim commands for roll, pitch and yaw. diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c index c463b3218d..96001f4198 100644 --- a/sw/airborne/firmwares/rotorcraft/datalink.c +++ b/sw/airborne/firmwares/rotorcraft/datalink.c @@ -49,7 +49,7 @@ #include "firmwares/rotorcraft/navigation.h" #include "math/pprz_geodetic_int.h" -#include "subsystems/ins.h" +#include "state.h" #define IdOfMsg(x) (x[1]) @@ -97,19 +97,18 @@ void dl_parse_msg(void) { { uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer); if (ac_id != AC_ID) break; - uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer); - struct LlaCoor_i lla; - struct EnuCoor_i enu; - lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer)); - lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer)); - /* WP_alt is in cm, lla.alt in mm */ - lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt; - enu_of_lla_point_i(&enu,&ins_ltp_def,&lla); - enu.x = POS_BFP_OF_REAL(enu.x)/100; - enu.y = POS_BFP_OF_REAL(enu.y)/100; - enu.z = POS_BFP_OF_REAL(enu.z)/100; - VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z); - DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z); + if (stateIsLocalCoordinateValid()) { + uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer); + struct LlaCoor_i lla; + lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer)); + lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer)); + /* WP_alt from message is alt above MSL in cm + * lla.alt is above ellipsoid in mm + */ + lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - state.ned_origin_i.hmsl + + state.ned_origin_i.lla.alt; + nav_move_waypoint_lla(wp_id, &lla); + } } break; #endif /* USE_NAVIGATION */ diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index cb04be854d..d77ab21995 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -28,6 +28,8 @@ #define MODULES_C +#define ABI_C + #include #include "mcu.h" #include "mcu_periph/sys_time.h" @@ -50,8 +52,11 @@ #include "subsystems/imu.h" #include "subsystems/gps.h" +#include "subsystems/air_data.h" +#if USE_BARO_BOARD #include "subsystems/sensors/baro.h" +#endif #include "subsystems/electrical.h" @@ -68,10 +73,11 @@ #include "firmwares/rotorcraft/main.h" #ifdef SITL -#include "nps_autopilot_rotorcraft.h" +#include "nps_autopilot.h" #endif #include "generated/modules.h" +#include "subsystems/abi.h" /* if PRINT_CONFIG is defined, print some config options */ @@ -95,8 +101,6 @@ PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY) static inline void on_gyro_event( void ); static inline void on_accel_event( void ); -static inline void on_baro_abs_event( void ); -static inline void on_baro_dif_event( void ); static inline void on_gps_event( void ); static inline void on_mag_event( void ); @@ -106,8 +110,10 @@ tid_t modules_tid; ///< id for modules_periodic_task() timer tid_t failsafe_tid; ///< id for failsafe_check() timer tid_t radio_control_tid; ///< id for radio_control_periodic_task() timer tid_t electrical_tid; ///< id for electrical_periodic() timer -tid_t baro_tid; ///< id for baro_periodic() timer tid_t telemetry_tid; ///< id for telemetry_periodic() timer +#if USE_BARO_BOARD +tid_t baro_tid; ///< id for baro_periodic() timer +#endif #ifndef SITL int main( void ) { @@ -136,7 +142,10 @@ STATIC_INLINE void main_init( void ) { radio_control_init(); + air_data_init(); +#if USE_BARO_BOARD baro_init(); +#endif imu_init(); #if USE_IMU_FLOAT imu_float_init(); @@ -172,8 +181,10 @@ STATIC_INLINE void main_init( void ) { radio_control_tid = sys_time_register_timer((1./60.), NULL); failsafe_tid = sys_time_register_timer(0.05, NULL); electrical_tid = sys_time_register_timer(0.1, NULL); - baro_tid = sys_time_register_timer(1./BARO_PERIODIC_FREQUENCY, NULL); telemetry_tid = sys_time_register_timer((1./TELEMETRY_FREQUENCY), NULL); +#if USE_BARO_BOARD + baro_tid = sys_time_register_timer(1./BARO_PERIODIC_FREQUENCY, NULL); +#endif } STATIC_INLINE void handle_periodic_tasks( void ) { @@ -187,10 +198,12 @@ STATIC_INLINE void handle_periodic_tasks( void ) { failsafe_check(); if (sys_time_check_and_ack_timer(electrical_tid)) electrical_periodic(); - if (sys_time_check_and_ack_timer(baro_tid)) - baro_periodic(); if (sys_time_check_and_ack_timer(telemetry_tid)) telemetry_periodic(); +#if USE_BARO_BOARD + if (sys_time_check_and_ack_timer(baro_tid)) + baro_periodic(); +#endif } STATIC_INLINE void main_periodic( void ) { @@ -257,7 +270,9 @@ STATIC_INLINE void main_event( void ) { ImuEvent(on_gyro_event, on_accel_event, on_mag_event); - BaroEvent(on_baro_abs_event, on_baro_dif_event); +#if USE_BARO_BOARD + BaroEvent(); +#endif #if USE_GPS GpsEvent(on_gps_event); @@ -294,26 +309,12 @@ static inline void on_gyro_event( void ) { if (nps_bypass_ahrs) sim_overwrite_ahrs(); #endif ins_propagate(); -#ifdef SITL - if (nps_bypass_ins) sim_overwrite_ins(); -#endif } #ifdef USE_VEHICLE_INTERFACE vi_notify_imu_available(); #endif } -static inline void on_baro_abs_event( void ) { - ins_update_baro(); -#ifdef USE_VEHICLE_INTERFACE - vi_notify_baro_abs_available(); -#endif -} - -static inline void on_baro_dif_event( void ) { - -} - static inline void on_gps_event(void) { ahrs_update_gps(); ins_update_gps(); diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 366ff9d010..e18811e500 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -44,6 +44,10 @@ #include "math/pprz_algebra_int.h" +#include "subsystems/datalink/downlink.h" +#include "messages.h" +#include "mcu_periph/uart.h" + const uint8_t nb_waypoint = NB_WAYPOINT; struct EnuCoor_i waypoints[NB_WAYPOINT]; @@ -267,7 +271,7 @@ static inline void nav_set_altitude( void ) { /** Reset the geographic reference to the current GPS fix */ unit_t nav_reset_reference( void ) { - ins_ltp_initialised = FALSE; + ins_impl.ltp_initialized = FALSE; ins.hf_realign = TRUE; ins.vf_realign = TRUE; return 0; @@ -277,9 +281,9 @@ unit_t nav_reset_alt( void ) { ins.vf_realign = TRUE; #if USE_GPS - ins_ltp_def.lla.alt = gps.lla_pos.alt; - ins_ltp_def.hmsl = gps.hmsl; - stateSetLocalOrigin_i(&ins_ltp_def); + ins_impl.ltp_def.lla.alt = gps.lla_pos.alt; + ins_impl.ltp_def.hmsl = gps.hmsl; + stateSetLocalOrigin_i(&ins_impl.ltp_def); #endif return 0; @@ -302,16 +306,25 @@ void nav_periodic_task() { /* run carrot loop */ nav_run(); - ground_alt = POS_BFP_OF_REAL((float)ins_ltp_def.hmsl / 1000.); + ground_alt = POS_BFP_OF_REAL((float)ins_impl.ltp_def.hmsl / 1000.); +} + +void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i* new_lla_pos) { + if (stateIsLocalCoordinateValid()) { + struct EnuCoor_i enu; + enu_of_lla_point_i(&enu, &state.ned_origin_i, new_lla_pos); + enu.x = POS_BFP_OF_REAL(enu.x)/100; + enu.y = POS_BFP_OF_REAL(enu.y)/100; + enu.z = POS_BFP_OF_REAL(enu.z)/100; + nav_move_waypoint(wp_id, &enu); + } } -#include "subsystems/datalink/downlink.h" -#include "messages.h" -#include "mcu_periph/uart.h" void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos) { if (wp_id < nb_waypoint) { INT32_VECT3_COPY(waypoints[wp_id],(*new_pos)); - DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x), &(new_pos->y), &(new_pos->z)); + DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x), + &(new_pos->y), &(new_pos->z)); } } diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index b8df4ee553..120e574de0 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -79,6 +79,7 @@ void compute_dist2_to_home(void); unit_t nav_reset_reference( void ) __attribute__ ((unused)); unit_t nav_reset_alt( void ) __attribute__ ((unused)); void nav_periodic_task(void); +void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i* new_lla_pos); void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos); bool_t nav_detect_ground(void); bool_t nav_is_in_flight(void); diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 22a4f8bbcd..281974a432 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -212,11 +212,11 @@ &electrical.current); \ } -#include "subsystems/sensors/baro.h" -#define PERIODIC_SEND_BARO_RAW(_trans, _dev) { \ - DOWNLINK_SEND_BARO_RAW(_trans, _dev, \ - &baro.absolute, \ - &baro.differential); \ +#include "subsystems/air_data.h" +#define PERIODIC_SEND_BARO_RAW(_trans, _dev) { \ + DOWNLINK_SEND_BARO_RAW(_trans, _dev, \ + &air_data.pressure, \ + &air_data.differential); \ } @@ -584,67 +584,71 @@ #define PERIODIC_SEND_HFF_GPS(_trans, _dev) {} #endif -#define PERIODIC_SEND_GUIDANCE_H_INT(_trans, _dev) { \ - DOWNLINK_SEND_GUIDANCE_H_INT(_trans, _dev, \ - &guidance_h_pos_sp.x, \ - &guidance_h_pos_sp.y, \ - &guidance_h_pos_ref.x, \ - &guidance_h_pos_ref.y, \ - &ins_ltp_pos.x, \ - &ins_ltp_pos.y); \ +#define PERIODIC_SEND_GUIDANCE_H_INT(_trans, _dev) { \ + DOWNLINK_SEND_GUIDANCE_H_INT(_trans, _dev, \ + &guidance_h_pos_sp.x, \ + &guidance_h_pos_sp.y, \ + &guidance_h_pos_ref.x, \ + &guidance_h_pos_ref.y, \ + &(stateGetPositionNed_i()->x), \ + &(stateGetPositionNed_i()->y)); \ } #define PERIODIC_SEND_INS_Z(_trans, _dev) { \ - DOWNLINK_SEND_INS_Z(_trans, _dev, \ - &ins_baro_alt, \ - &ins_ltp_pos.z, \ - &ins_ltp_speed.z, \ - &ins_ltp_accel.z); \ + DOWNLINK_SEND_INS_Z(_trans, _dev, \ + &ins_baro_alt, \ + &(stateGetPositionNed_i()->z), \ + &(stateGetSpeedNed_i()->z), \ + &(stateGetAccelNed_i()->z)); \ } -#define PERIODIC_SEND_INS(_trans, _dev) { \ - DOWNLINK_SEND_INS(_trans, _dev, \ - &ins_ltp_pos.x, \ - &ins_ltp_pos.y, \ - &ins_ltp_pos.z, \ - &ins_ltp_speed.x, \ - &ins_ltp_speed.y, \ - &ins_ltp_speed.z, \ - &ins_ltp_accel.x, \ - &ins_ltp_accel.y, \ - &ins_ltp_accel.z); \ +#define PERIODIC_SEND_INS(_trans, _dev) { \ + struct NedCoor_i* ltp_pos = stateGetPositionNed_i(); \ + struct NedCoor_i* ltp_speed = stateGetSpeedNed_i(); \ + struct NedCoor_i* ltp_accel = stateGetAccelNed_i(); \ + DOWNLINK_SEND_INS(_trans, _dev, \ + &(ltp_pos->x), \ + &(ltp_pos->y), \ + &(ltp_pos->z), \ + &(ltp_speed->x), \ + &(ltp_speed->y), \ + &(ltp_speed->z), \ + &(ltp_accel->x), \ + &(ltp_accel->y), \ + &(ltp_accel->z)); \ } -#define PERIODIC_SEND_INS_REF(_trans, _dev) { \ - if (ins_ltp_initialised) \ - DOWNLINK_SEND_INS_REF(_trans, _dev, \ - &ins_ltp_def.ecef.x, \ - &ins_ltp_def.ecef.y, \ - &ins_ltp_def.ecef.z, \ - &ins_ltp_def.lla.lat, \ - &ins_ltp_def.lla.lon, \ - &ins_ltp_def.lla.alt, \ - &ins_ltp_def.hmsl, \ - &ins_qfe); \ +#define PERIODIC_SEND_INS_REF(_trans, _dev) { \ + if (state.ned_initialized_i) { \ + DOWNLINK_SEND_INS_REF(_trans, _dev, \ + &state.ned_origin_i.ecef.x, \ + &state.ned_origin_i.ecef.y, \ + &state.ned_origin_i.ecef.z, \ + &state.ned_origin_i.lla.lat, \ + &state.ned_origin_i.lla.lon, \ + &state.ned_origin_i.lla.alt, \ + &state.ned_origin_i.hmsl, \ + &ins_impl.qfe); \ + } \ } #define PERIODIC_SEND_VERT_LOOP(_trans, _dev) { \ - DOWNLINK_SEND_VERT_LOOP(_trans, _dev, \ - &guidance_v_z_sp, \ - &guidance_v_zd_sp, \ - &ins_ltp_pos.z, \ - &ins_ltp_speed.z, \ - &ins_ltp_accel.z, \ - &guidance_v_z_ref, \ - &guidance_v_zd_ref, \ - &guidance_v_zdd_ref, \ - &gv_adapt_X, \ - &gv_adapt_P, \ - &gv_adapt_Xmeas, \ - &guidance_v_z_sum_err, \ - &guidance_v_ff_cmd, \ - &guidance_v_fb_cmd, \ - &guidance_v_delta_t); \ + DOWNLINK_SEND_VERT_LOOP(_trans, _dev, \ + &guidance_v_z_sp, \ + &guidance_v_zd_sp, \ + &(stateGetPositionNed_i()->z), \ + &(stateGetSpeedNed_i()->z), \ + &(stateGetAccelNed_i()->z), \ + &guidance_v_z_ref, \ + &guidance_v_zd_ref, \ + &guidance_v_zdd_ref, \ + &gv_adapt_X, \ + &gv_adapt_P, \ + &gv_adapt_Xmeas, \ + &guidance_v_z_sum_err, \ + &guidance_v_ff_cmd, \ + &guidance_v_fb_cmd, \ + &guidance_v_delta_t); \ } #define PERIODIC_SEND_TUNE_VERT(_trans, _dev) { \ @@ -656,28 +660,28 @@ } #define PERIODIC_SEND_HOVER_LOOP(_trans, _dev) { \ - DOWNLINK_SEND_HOVER_LOOP(_trans, _dev, \ - &guidance_h_pos_sp.x, \ - &guidance_h_pos_sp.y, \ - &ins_ltp_pos.x, \ - &ins_ltp_pos.y, \ - &ins_ltp_speed.x, \ - &ins_ltp_speed.y, \ - &ins_ltp_accel.x, \ - &ins_ltp_accel.y, \ - &guidance_h_pos_err.x, \ - &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_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); \ + DOWNLINK_SEND_HOVER_LOOP(_trans, _dev, \ + &guidance_h_pos_sp.x, \ + &guidance_h_pos_sp.y, \ + &(stateGetPositionNed_i()->x), \ + &(stateGetPositionNed_i()->y), \ + &(stateGetSpeedNed_i()->x), \ + &(stateGetSpeedNed_i()->y), \ + &(stateGetAccelNed_i()->x), \ + &(stateGetAccelNed_i()->y), \ + &guidance_h_pos_err.x, \ + &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_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); \ } #define PERIODIC_SEND_GUIDANCE_H_REF(_trans, _dev) { \ diff --git a/sw/airborne/lisa/test_baro_i2c.c b/sw/airborne/lisa/test_baro_i2c.c index 250bf89c33..1bcff1dde1 100644 --- a/sw/airborne/lisa/test_baro_i2c.c +++ b/sw/airborne/lisa/test_baro_i2c.c @@ -36,8 +36,12 @@ #include "subsystems/datalink/downlink.h" #include "subsystems/sensors/baro.h" +#include "subsystems/air_data.h" //#include "my_debug_servo.h" +#define ABI_C +#include "subsystems/abi.h" + static inline void main_init( void ); static inline void main_periodic_task( void ); static inline void main_event_task( void ); @@ -107,7 +111,7 @@ static inline void main_periodic_task( void ) { static inline void main_event_task( void ) { - BaroEvent(main_on_baro_abs, main_on_baro_diff); + BaroEvent(); } @@ -117,5 +121,5 @@ static inline void main_on_baro_diff(void) { } static inline void main_on_baro_abs(void) { - RunOnceEvery(5,{DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, &baro.absolute, &baro.differential);}); + RunOnceEvery(5,{DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, &air_data.pressure, &air_data.differential);}); } diff --git a/sw/airborne/math/pprz_isa.h b/sw/airborne/math/pprz_isa.h new file mode 100644 index 0000000000..28c397975b --- /dev/null +++ b/sw/airborne/math/pprz_isa.h @@ -0,0 +1,102 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * 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 math/pprz_isa.h + * @brief Paparazzi atmospheric pressure convertion utilities + * + * Conversion functions are use to approximate altitude + * from atmospheric pressure based on the standard model + * and the International Standard Atmosphere (ISA) + * + * http://en.wikipedia.org/wiki/Atmospheric_pressure + * http://en.wikipedia.org/wiki/International_Standard_Atmosphere + * + */ + +#ifndef PPRZ_ISA_H +#define PPRZ_ISA_H + +#include "std.h" +#include + +// Standard Atmosphere constants +#define PPRZ_ISA_SEA_LEVEL_PRESSURE 101325.0 +#define PPRZ_ISA_SEA_LEVEL_TEMP 288.15 +#define PPRZ_ISA_TEMP_LAPS_RATE 0.0065 +#define PPRZ_ISA_GRAVITY 9.80665 +#define PPRZ_ISA_AIR_GAS_CONSTANT (8.31447/0.0289644) + +static const float PPRZ_ISA_M_OF_P_CONST = (PPRZ_ISA_AIR_GAS_CONSTANT*PPRZ_ISA_SEA_LEVEL_TEMP/PPRZ_ISA_GRAVITY); + +/** + * Get absolute altitude from pressure (using simplified equation). + * Referrence pressure is standard pressure at sea level + * + * @param pressure current pressure in Pascal (Pa) + * @return altitude in pressure in ISA conditions + */ +static inline float pprz_isa_altitude_of_pressure(float pressure) { + if (pressure > 0.) { + return (PPRZ_ISA_M_OF_P_CONST*logf(PPRZ_ISA_SEA_LEVEL_PRESSURE/pressure)); + } else { + return 0.; + } +} + +/** + * Get relative altitude from pressure (using simplified equation). + * + * @param pressure current pressure in Pascal (Pa) + * @param ref reference pressure (QFE) when height = 0 + * @return altitude in pressure in ISA conditions + */ +static inline float pprz_isa_height_of_pressure(float pressure, float ref) { + if (pressure > 0. && ref > 0.) { + return (PPRZ_ISA_M_OF_P_CONST*logf(ref/pressure)); + } else { + return 0.; + } +} + +/** + * Get pressure in Pa from absolute altitude (using simplified equation). + * + * @param altitude current absolute altitude in meters + * @return static pressure in Pa in ISA conditions + */ +static inline float pprz_isa_pressure_of_altitude(float altitude) { + return (PPRZ_ISA_SEA_LEVEL_PRESSURE*expf((-1./PPRZ_ISA_M_OF_P_CONST)*altitude)); +} + +/** + * Get pressure in Pa from height (using simplified equation). + * + * @param altitude current relative altitude in meters + * @param ref reference pressure (QFE) when height = 0 + * @return static pressure in Pa in ISA conditions + */ +static inline float pprz_isa_pressure_of_height(float altitude, float ref) { + return (ref*expf((-1./PPRZ_ISA_M_OF_P_CONST)*altitude)); +} + +#endif /* PPRZ_ISA_H */ diff --git a/sw/airborne/modules/cam_control/cam_track.c b/sw/airborne/modules/cam_control/cam_track.c index ae7a29abda..e933f6f62c 100644 --- a/sw/airborne/modules/cam_control/cam_track.c +++ b/sw/airborne/modules/cam_control/cam_track.c @@ -55,7 +55,7 @@ uint8_t cam_status; uint8_t cam_data_len; void track_init(void) { - ins_ltp_initialised = TRUE; // ltp is initialized and centered on the target + ins_impl.ltp_initialized = TRUE; // ltp is initialized and centered on the target ins_update_on_agl = TRUE; // use sonar to update agl (assume flat ground) cam_status = UNINIT; @@ -119,8 +119,8 @@ void track_periodic_task(void) { } void track_event(void) { - if (!ins_ltp_initialised) { - ins_ltp_initialised = TRUE; + if (!ins_impl.ltp_initialized) { + ins_impl.ltp_initialized = TRUE; ins.hf_realign = TRUE; } @@ -134,24 +134,24 @@ void track_event(void) { } const stuct FlotVect2 measuremet_noise = { 10.0, 10.0 }; b2_hff_update_pos(-target_pos_ned, measurement_noise); - ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot); - ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot); - ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot); - ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot); - ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x); - ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y); + ins_impl.ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot); + ins_impl.ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot); + ins_impl.ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot); + ins_impl.ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot); + ins_impl.ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x); + ins_impl.ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y); INS_NED_TO_STATE(); #else // store pos in ins - ins_ltp_pos.x = -(POS_BFP_OF_REAL(target_pos_ned.x)); - ins_ltp_pos.y = -(POS_BFP_OF_REAL(target_pos_ned.y)); + ins_impl.ltp_pos.x = -(POS_BFP_OF_REAL(target_pos_ned.x)); + ins_impl.ltp_pos.y = -(POS_BFP_OF_REAL(target_pos_ned.y)); // compute speed from last pos // TODO get delta T // store last pos VECT3_COPY(last_pos_ned, target_pos_ned); - stateSetPositionNed_i(&ins_ltp_pos); + stateSetPositionNed_i(&ins_impl.ltp_pos); #endif b2_hff_lost_counter = 0; diff --git a/sw/airborne/modules/sensors/baro_MS5534A.c b/sw/airborne/modules/sensors/baro_MS5534A.c index 6b605df2d3..8442e22e56 100644 --- a/sw/airborne/modules/sensors/baro_MS5534A.c +++ b/sw/airborne/modules/sensors/baro_MS5534A.c @@ -33,9 +33,11 @@ #ifndef BARO_NO_DOWNLINK #include "ap_downlink.h" #endif +#include "subsystems/abi.h" #include "subsystems/nav.h" #include "state.h" + bool_t baro_MS5534A_do_reset; uint32_t baro_MS5534A_pressure; uint16_t baro_MS5534A_temp; @@ -257,11 +259,10 @@ void baro_MS5534A_event( void ) { spi_message_received = FALSE; baro_MS5534A_event_task(); if (baro_MS5534A_available) { - // baro_MS5534A_available = FALSE; // Checked by INS + baro_MS5534A_available = FALSE; baro_MS5534A_z = ground_alt +((float)baro_MS5534A_ground_pressure - baro_MS5534A_pressure)*0.084; - // if (alt_baro_enabled) { - // EstimatorSetAlt(baro_MS5534A_z); // Updated by INS - // } + float pressure = (float)baro_MS5534A_pressure; + AbiSendMsgBARO_ABS(BARO_MS5534A_SENDER_ID, &pressure); } } } diff --git a/sw/airborne/modules/sensors/baro_MS5534A.h b/sw/airborne/modules/sensors/baro_MS5534A.h index 7e98fa1c34..0b7b0f6373 100644 --- a/sw/airborne/modules/sensors/baro_MS5534A.h +++ b/sw/airborne/modules/sensors/baro_MS5534A.h @@ -54,8 +54,6 @@ void baro_MS5534A_event_task( void ); void baro_MS5534A_event( void ); -#define BaroMS5534AUpdate(_b, _h) { if (baro_MS5534A_available) { _b = baro_MS5534A_pressure; _h(); baro_MS5534A_available = FALSE; } } - #endif // USE_BARO_MS5534A #endif // BARO_MS5534A_H diff --git a/sw/airborne/modules/sensors/baro_amsys.c b/sw/airborne/modules/sensors/baro_amsys.c index faa014ff94..66bdab29c5 100644 --- a/sw/airborne/modules/sensors/baro_amsys.c +++ b/sw/airborne/modules/sensors/baro_amsys.c @@ -25,15 +25,11 @@ #include "sensors/baro_amsys.h" #include "mcu_periph/i2c.h" +#include "subsystems/abi.h" #include "state.h" #include #include "generated/flight_plan.h" // for ground alt -#ifdef SITL -#include "subsystems/gps.h" -#include "subsystems/navigation/common_nav.h" -#endif - //Messages #include "mcu_periph/uart.h" #include "messages.h" @@ -43,10 +39,6 @@ #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif -#ifdef SITL -#include "subsystems/gps.h" -#endif - #define BARO_AMSYS_ADDR 0xE4 #define BARO_AMSYS_REG 0x07 #ifndef BARO_AMSYS_SCALE @@ -77,6 +69,7 @@ #define BARO_AMSYS_I2C_DEV i2c0 #endif + // Global variables uint16_t pBaroRaw; uint16_t tBaroRaw; @@ -126,7 +119,6 @@ void baro_amsys_init( void ) { void baro_amsys_read_periodic( void ) { // Initiate next read -#ifndef SITL if (baro_amsys_i2c_trans.status == I2CTransDone){ #ifndef MEASURE_AMSYS_TEMPERATURE i2c_receive(&BARO_AMSYS_I2C_DEV, &baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 2); @@ -134,17 +126,6 @@ void baro_amsys_read_periodic( void ) { i2c_receive(&BARO_AMSYS_I2C_DEV, &baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 4); #endif } -#else // SITL - /* fake an offset so sim works for under hmsl as well */ - if (!baro_amsys_offset_init) { - baro_amsys_offset = BARO_AMSYS_OFFSET_MAX; - baro_amsys_offset_init = TRUE; - } - pBaroRaw = 0; - baro_amsys_altitude = gps.hmsl / 1000.0; - baro_amsys_adc = baro_amsys_offset - ((baro_amsys_altitude - ground_alt) / INS_BARO_SENS); - baro_amsys_valid = TRUE; -#endif #ifdef BARO_AMSYS_SYNC_SEND DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp); @@ -180,6 +161,9 @@ void baro_amsys_read_event( void ) { //Convert to pressure baro_amsys_p = (float)(pBaroRaw-BARO_AMSYS_OFFSET_MIN)*BARO_AMSYS_MAX_PRESSURE/(float)(BARO_AMSYS_OFFSET_MAX-BARO_AMSYS_OFFSET_MIN); + // Send pressure over ABI + AbiSendMsgBARO_ABS(BARO_AMSYS_SENDER_ID, &baro_amsys_p); + // compute altitude localy if (!baro_amsys_offset_init) { --baro_amsys_cnt; // Check if averaging completed diff --git a/sw/airborne/modules/sensors/baro_amsys.h b/sw/airborne/modules/sensors/baro_amsys.h index a6852dab58..5fda78c154 100644 --- a/sw/airborne/modules/sensors/baro_amsys.h +++ b/sw/airborne/modules/sensors/baro_amsys.h @@ -48,6 +48,4 @@ extern void baro_amsys_read_event( void ); #define BaroAmsysEvent() { if (baro_amsys_i2c_trans.status == I2CTransSuccess) baro_amsys_read_event(); } -#define BaroAmsysUpdate(_b, _h) { if (baro_amsys_valid) { _b = baro_amsys_adc; _h(); baro_amsys_valid = FALSE; } } - #endif // BARO_AMSYS_H diff --git a/sw/airborne/modules/sensors/baro_bmp.c b/sw/airborne/modules/sensors/baro_bmp.c index 5011b7d075..a0dbade0af 100644 --- a/sw/airborne/modules/sensors/baro_bmp.c +++ b/sw/airborne/modules/sensors/baro_bmp.c @@ -36,6 +36,7 @@ #include "mcu_periph/i2c.h" #include "led.h" #include "mcu_periph/uart.h" +#include "subsystems/abi.h" #include "messages.h" #include "subsystems/datalink/downlink.h" @@ -51,7 +52,6 @@ #define BMP_I2C_DEV i2c0 #endif - #define BARO_BMP_R 0.5 #define BARO_BMP_SIGMA2 0.1 @@ -92,6 +92,9 @@ void baro_bmp_event(void) { tmp = pow(tmp, 0.190295); baro_bmp_alt = 44330 * (1.0 - tmp); + float pressure = (float)baro_bmp.pressure; + AbiSendMsgBARO_ABS(BARO_BMP_SENDER_ID, &pressure); + #ifdef SENSOR_SYNC_SEND DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &baro_bmp.up, &baro_bmp.ut, &baro_bmp.pressure, diff --git a/sw/airborne/modules/sensors/baro_bmp.h b/sw/airborne/modules/sensors/baro_bmp.h index db1039e817..aa56e10266 100644 --- a/sw/airborne/modules/sensors/baro_bmp.h +++ b/sw/airborne/modules/sensors/baro_bmp.h @@ -36,11 +36,7 @@ extern struct Bmp085 baro_bmp; /// new measurement every 3rd baro_bmp_periodic -#ifndef SITL #define BARO_BMP_DT (BARO_BMP_PERIODIC_PERIOD / 3) -#else -#define BARO_BMP_DT BARO_BMP_PERIODIC_PERIOID -#endif extern bool_t baro_bmp_enabled; extern float baro_bmp_r; @@ -51,6 +47,4 @@ void baro_bmp_init(void); void baro_bmp_periodic(void); void baro_bmp_event(void); -#define BaroBmpUpdate(_b, _h) { if (baro_bmp.data_available) { _b = baro_bmp.pressure; _h(); baro_bmp.data_available = FALSE; } } - #endif diff --git a/sw/airborne/modules/sensors/baro_board_module.c b/sw/airborne/modules/sensors/baro_board_module.c deleted file mode 100644 index f5271f0df9..0000000000 --- a/sw/airborne/modules/sensors/baro_board_module.c +++ /dev/null @@ -1,59 +0,0 @@ -/* - * Copyright (C) 2012 Gautier Hattenberger - * - * 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. - * - */ - -/* - * Wrapper for the board specific barometer - * Allows to use external baro sensor to feed the general baro interface - */ - -#include "modules/sensors/baro_board_module.h" - -/* Common Baro struct */ -struct Baro baro; - -/* Counter to init custom baro at startup */ -#define BARO_STARTUP_COUNTER 200 -uint16_t startup_cnt; - -/** Implementation of the generic baro interface initialization. - * No need to call this functions from the modules, already done by main. - */ -void baro_init( void ) { - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; - startup_cnt = BARO_STARTUP_COUNTER; -} - -/** Implementation of the generic baro interface periodic task. - * No need to call this functions from the modules, already done by main. - */ -void baro_periodic( void ) { - if (baro.status == BS_UNINITIALIZED) { - // Run some loops to get correct readings from the adc - --startup_cnt; - if (startup_cnt == 0) { - baro.status = BS_RUNNING; - } - } -} - diff --git a/sw/airborne/modules/sensors/baro_board_module.h b/sw/airborne/modules/sensors/baro_board_module.h deleted file mode 100644 index bc5f48d23b..0000000000 --- a/sw/airborne/modules/sensors/baro_board_module.h +++ /dev/null @@ -1,70 +0,0 @@ -/* - * Copyright (C) 2011 Gautier Hattenberger - * - * 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 modules/sensors/baro_board_module.h - * - * Wrapper for the board specific barometer. - */ - -#ifndef BARO_BOARD_MODULE_H -#define BARO_BOARD_MODULE_H - -#include "subsystems/sensors/baro.h" - -/** Absolute baro macro mapping. - * Select the baro module you want to use to feed the common baro interface - * in your airframe file when configuring baro_board module - * ex: - * for module baro_ets - * @verbatim - * - * @endverbatim - */ -#ifndef BARO_ABS_EVENT -#define BARO_ABS_EVENT NoBaro -#endif - -/** Differential baro macro mapping. - * TODO - */ -#ifndef BARO_DIFF_EVENT -#define BARO_DIFF_EVENT NoBaro -#endif - -#define NoBaro(_b, _h) {} - -/** BaroEvent macro. - * Need to be maped to one the external baro running has a module - * - * Undef if necessary (already defined in a baro_board.h file) - */ -#ifdef BaroEvent -#undef BaroEvent -#endif - -#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - BARO_ABS_EVENT(baro.absolute, _b_abs_handler); \ - BARO_DIFF_EVENT(baro.differential, _b_diff_handler); \ -} - - -#endif diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c index e545e6d2a2..f6f0f80a02 100644 --- a/sw/airborne/modules/sensors/baro_ets.c +++ b/sw/airborne/modules/sensors/baro_ets.c @@ -42,15 +42,12 @@ #include "sensors/baro_ets.h" #include "mcu_periph/i2c.h" #include "state.h" +#include "subsystems/abi.h" #include #include "mcu_periph/sys_time.h" #include "subsystems/nav.h" -#ifdef SITL -#include "subsystems/gps.h" -#endif - #ifdef BARO_ETS_SYNC_SEND #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE @@ -63,9 +60,6 @@ #define BARO_ETS_ADDR 0xE8 #define BARO_ETS_REG 0x07 -#ifndef BARO_ETS_SCALE -#define BARO_ETS_SCALE 0.32 -#endif #define BARO_ETS_OFFSET_MAX 30000 #define BARO_ETS_OFFSET_MIN 10 #define BARO_ETS_OFFSET_NBSAMPLES_INIT 20 @@ -73,11 +67,35 @@ #define BARO_ETS_R 0.5 #define BARO_ETS_SIGMA2 0.1 +/** scale factor to convert raw ADC measurement to pressure in Pascal. + * @todo check value + * At low altitudes pressure change is ~1.2 kPa for every 100 meters. + * So with a scale ADC->meters of 0.32 we get: + * 12 Pascal = 0.32 * ADC + * -> SCALE = ~ 12 / 0.32 = 37.5 + */ +#ifndef BARO_ETS_SCALE +#define BARO_ETS_SCALE 37.5 +#endif + +/** scale factor to convert raw ADC measurement to altitude change in meters */ +#ifndef BARO_ETS_ALT_SCALE +#define BARO_ETS_ALT_SCALE 0.32 +#endif + +/** Pressure offset in Pascal when converting raw adc to real pressure (@todo find real value) */ +#ifndef BARO_ETS_PRESSURE_OFFSET +#define BARO_ETS_PRESSURE_OFFSET 101325.0 +#endif + #ifndef BARO_ETS_I2C_DEV #define BARO_ETS_I2C_DEV i2c0 #endif PRINT_CONFIG_VAR(BARO_ETS_I2C_DEV) + +PRINT_CONFIG_VAR(BARO_ETS_SENDER_ID) + /** delay in seconds until sensor is read after startup */ #ifndef BARO_ETS_START_DELAY #define BARO_ETS_START_DELAY 0.2 @@ -122,27 +140,13 @@ void baro_ets_init( void ) { void baro_ets_read_periodic( void ) { // Initiate next read -#ifndef SITL if (!baro_ets_delay_done) { if (SysTimeTimer(baro_ets_delay_time) < USEC_OF_SEC(BARO_ETS_START_DELAY)) return; else baro_ets_delay_done = TRUE; } - if (baro_ets_i2c_trans.status == I2CTransDone) + if (baro_ets_i2c_trans.status == I2CTransDone) { i2c_receive(&BARO_ETS_I2C_DEV, &baro_ets_i2c_trans, BARO_ETS_ADDR, 2); -#else // SITL - /* fake an offset so sim works as well */ - if (!baro_ets_offset_init) { - baro_ets_offset = 12400; - baro_ets_offset_init = TRUE; } - baro_ets_altitude = gps.hmsl / 1000.0; - baro_ets_adc = baro_ets_offset - ((baro_ets_altitude - ground_alt) / BARO_ETS_SCALE); - baro_ets_valid = TRUE; -#endif - -#ifdef BARO_ETS_SYNC_SEND - DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude); -#endif } void baro_ets_read_event( void ) { @@ -176,8 +180,10 @@ void baro_ets_read_event( void ) { } // Convert raw to m/s if (baro_ets_offset_init) { - baro_ets_altitude = ground_alt + BARO_ETS_SCALE * (float)(baro_ets_offset-baro_ets_adc); + baro_ets_altitude = ground_alt + BARO_ETS_ALT_SCALE * (float)(baro_ets_offset-baro_ets_adc); // New value available + float pressure = BARO_ETS_SCALE * (float) baro_ets_adc + BARO_ETS_PRESSURE_OFFSET; + AbiSendMsgBARO_ABS(BARO_ETS_SENDER_ID, &pressure); #ifdef BARO_ETS_SYNC_SEND DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude); #endif diff --git a/sw/airborne/modules/sensors/baro_ets.h b/sw/airborne/modules/sensors/baro_ets.h index 33f7bbd592..90d02ab4c6 100644 --- a/sw/airborne/modules/sensors/baro_ets.h +++ b/sw/airborne/modules/sensors/baro_ets.h @@ -64,6 +64,4 @@ extern void baro_ets_read_event( void ); #define BaroEtsEvent() { if (baro_ets_i2c_trans.status == I2CTransSuccess) baro_ets_read_event(); } -#define BaroEtsUpdate(_b, _h) { if (baro_ets_valid) { _b = baro_ets_adc; _h(); baro_ets_valid = FALSE; } } - #endif // BARO_ETS_H diff --git a/sw/airborne/modules/sensors/baro_hca.c b/sw/airborne/modules/sensors/baro_hca.c index b8c257c848..d1b15cfd9e 100644 --- a/sw/airborne/modules/sensors/baro_hca.c +++ b/sw/airborne/modules/sensors/baro_hca.c @@ -22,6 +22,7 @@ #include "sensors/baro_hca.h" #include "mcu_periph/i2c.h" +#include "subsystems/abi.h" #include //Messages @@ -39,6 +40,16 @@ #define BARO_HCA_MAX_OUT 27852 //dec #define BARO_HCA_MIN_OUT 1638 //dec +// FIXME +#ifndef BARO_HCA_SCALE +#define BARO_HCA_SCALE 1.0 +#endif + +// FIXME +#ifndef BARO_HCA_PRESSURE_OFFSET +#define BARO_HCA_PRESSURE_OFFSET 101325.0 +#endif + #ifndef BARO_HCA_I2C_DEV #define BARO_HCA_I2C_DEV i2c0 #endif @@ -83,6 +94,8 @@ void baro_hca_read_event( void ) { if (pBaroRaw > BARO_HCA_MAX_OUT) pBaroRaw = BARO_HCA_MAX_OUT; + float pressure = BARO_HCA_SCALE*(float)pBaroRaw + BARO_HCA_PRESSURE_OFFSET; + AbiSendMsgBARO_ABS(BARO_HCA_SENDER_ID, &pressure); } baro_hca_i2c_trans.status = I2CTransDone; diff --git a/sw/airborne/modules/sensors/baro_mpl3115.c b/sw/airborne/modules/sensors/baro_mpl3115.c index f44e248c64..af58bc2cd7 100644 --- a/sw/airborne/modules/sensors/baro_mpl3115.c +++ b/sw/airborne/modules/sensors/baro_mpl3115.c @@ -17,10 +17,18 @@ * 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 modules/sensors/baro_mpl3155.c + * + * Module for the baro MPL3115A2 from Freescale (i2c) * */ #include "modules/sensors/baro_mpl3115.h" +#include "peripherals/mpl3115.h" +#include "subsystems/abi.h" //Messages #include "mcu_periph/uart.h" @@ -31,23 +39,37 @@ #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif +#ifndef BARO_MPL3115_I2C_DEV +#define BARO_MPL3115_I2C_DEV i2c0 +#endif + +#ifndef BARO_MPL3115_I2C_SLAVE_ADDR +#define BARO_MPL3115_I2C_SLAVE_ADDR MPL3115_I2C_ADDR +#endif + + +struct Mpl3115 baro_mpl; + void baro_mpl3115_init( void ) { - mpl3115_init(); + mpl3115_init(&baro_mpl, &BARO_MPL3115_I2C_DEV, BARO_MPL3115_I2C_SLAVE_ADDR); } void baro_mpl3115_read_periodic( void ) { -#ifdef SENSOR_SYNC_SEND - if (mpl3115_data_available) { - DOWNLINK_SEND_MPL3115_BARO(DefaultChannel, DefaultDevice, &mpl3115_pressure, &mpl3115_temperature, &mpl3115_alt); - } -#endif - Mpl3115Periodic(); + mpl3115_periodic(&baro_mpl); } void baro_mpl3115_read_event( void ) { - mpl3115_event(); + mpl3115_event(&baro_mpl); + if (baro_mpl.data_available) { + float pressure = (float)baro_mpl.pressure/(1<<2); + AbiSendMsgBARO_ABS(BARO_MPL3115_SENDER_ID, &pressure); +#ifdef SENSOR_SYNC_SEND + DOWNLINK_SEND_MPL3115_BARO(DefaultChannel, DefaultDevice, &baro_mpl.pressure, &baro_mpl.temperature, &baro_mpl.alt); +#endif + baro_mpl.data_available = FALSE; + } } diff --git a/sw/airborne/modules/sensors/baro_mpl3115.h b/sw/airborne/modules/sensors/baro_mpl3115.h index 099e86bdee..de6568a356 100644 --- a/sw/airborne/modules/sensors/baro_mpl3115.h +++ b/sw/airborne/modules/sensors/baro_mpl3115.h @@ -17,14 +17,18 @@ * 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 modules/sensors/baro_mpl3155.h + * + * Module for the baro MPL3115A2 from Freescale (i2c) * */ #ifndef BARO_MPL3115_H #define BARO_MPL3115_H -#include "peripherals/mpl3115.h" - extern void baro_mpl3115_init( void ); extern void baro_mpl3115_read_periodic( void ); extern void baro_mpl3115_read_event( void ); diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c index e44e3044f8..5ee80bf457 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.c +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c @@ -29,7 +29,9 @@ #include "modules/sensors/baro_ms5611_i2c.h" +#include "math/pprz_isa.h" #include "mcu_periph/sys_time.h" +#include "subsystems/abi.h" #include "mcu_periph/uart.h" #include "messages.h" #include "subsystems/datalink/downlink.h" @@ -72,9 +74,9 @@ void baro_ms5611_periodic_check( void ) { ms5611_i2c_periodic_check(&baro_ms5611); -#if BARO_MS5611_SEND_COEFF - // send coeff every 5s - RunOnceEvery((5*BARO_MS5611_PERIODIC_CHECK_FREQUENCY), baro_ms5611_send_coeff()); +#if SENSOR_SYNC_SEND + // send coeff every 30s + RunOnceEvery((30*BARO_MS5611_PERIODIC_CHECK_FREQ), baro_ms5611_send_coeff()); #endif } @@ -90,16 +92,19 @@ void baro_ms5611_event( void ) { ms5611_i2c_event(&baro_ms5611); if (baro_ms5611.data_available) { - float tmp_float = baro_ms5611.data.pressure / 101325.0; //pressure at sea level - tmp_float = pow(tmp_float, 0.190295); - baro_ms5611_alt = 44330 * (1.0 - tmp_float); //altitude above MSL + float pressure = (float)baro_ms5611.data.pressure; + AbiSendMsgBARO_ABS(BARO_MS5611_SENDER_ID, &pressure); + baro_ms5611.data_available = FALSE; + + baro_ms5611_alt = pprz_isa_altitude_of_pressure(pressure); baro_ms5611_alt_valid = TRUE; #ifdef SENSOR_SYNC_SEND ftempms = baro_ms5611.data.temperature / 100.; fbaroms = baro_ms5611.data.pressure / 100.; DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice, - &baro_ms5611.data.d1, &baro_ms5611.data.d2, &fbaroms, &ftempms); + &baro_ms5611.data.d1, &baro_ms5611.data.d2, + &fbaroms, &ftempms); #endif } } diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.h b/sw/airborne/modules/sensors/baro_ms5611_i2c.h index 549b6bf731..ac57cf2d8d 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.h +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.h @@ -23,8 +23,4 @@ extern void baro_ms5611_periodic_check(void); extern void baro_ms5611_event(void); extern void baro_ms5611_send_coeff(void); -#define BaroMs5611UpdatePressure(_b, _h) { if (baro_ms5611.data_available) { _b = baro_ms5611.data.pressure; _h(); baro_ms5611.data_available = FALSE; } } - -#define BaroMs5611UpdateAlt(_b, _h) { if (baro_ms5611.data_available) { _b = baro_ms5611_alt; _h(); baro_ms5611.data_available = FALSE; } } - #endif diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.c b/sw/airborne/modules/sensors/baro_ms5611_spi.c index 7da2575f41..9a457b78e5 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_spi.c +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.c @@ -29,7 +29,9 @@ #include "modules/sensors/baro_ms5611_spi.h" +#include "math/pprz_isa.h" #include "mcu_periph/sys_time.h" +#include "subsystems/abi.h" #include "mcu_periph/uart.h" #include "messages.h" #include "subsystems/datalink/downlink.h" @@ -42,8 +44,8 @@ #define MS5611_SPI_DEV spi1 #endif -#ifndef MS5611_SLAVE_DEV -#define MS5611_SLAVE_DEV SPI_SLAVE0 +#ifndef MS5611_SLAVE_IDX +#define MS5611_SLAVE_IDX SPI_SLAVE0 #endif @@ -59,7 +61,7 @@ float baro_ms5611_sigma2; void baro_ms5611_init(void) { - ms5611_spi_init(&baro_ms5611, &MS5611_SPI_DEV, MS5611_SLAVE_DEV); + ms5611_spi_init(&baro_ms5611, &MS5611_SPI_DEV, MS5611_SLAVE_IDX); baro_ms5611_enabled = TRUE; baro_ms5611_alt_valid = FALSE; @@ -72,9 +74,9 @@ void baro_ms5611_periodic_check( void ) { ms5611_spi_periodic_check(&baro_ms5611); -#if BARO_MS5611_SEND_COEFF - // send coeff every 5s - RunOnceEvery((5*BARO_MS5611_PERIODIC_CHECK_FREQUENCY), baro_ms5611_send_coeff()); +#if SENSOR_SYNC_SEND + // send coeff every 30s + RunOnceEvery((5*BARO_MS5611_PERIODIC_CHECK_FREQ), baro_ms5611_send_coeff()); #endif } @@ -91,16 +93,19 @@ void baro_ms5611_event( void ) { ms5611_spi_event(&baro_ms5611); if (baro_ms5611.data_available) { - float tmp_float = baro_ms5611.data.pressure / 101325.0; //pressure at sea level - tmp_float = pow(tmp_float, 0.190295); - baro_ms5611_alt = 44330 * (1.0 - tmp_float); //altitude above MSL + float pressure = (float)baro_ms5611.data.pressure; + AbiSendMsgBARO_ABS(BARO_MS5611_SENDER_ID, &pressure); + baro_ms5611.data_available = FALSE; + + baro_ms5611_alt = pprz_isa_altitude_of_pressure(pressure); baro_ms5611_alt_valid = TRUE; #ifdef SENSOR_SYNC_SEND ftempms = baro_ms5611.data.temperature / 100.; fbaroms = baro_ms5611.data.pressure / 100.; DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice, - &baro_ms5611.data.d1, &baro_ms5611.data.d2, &fbaroms, &ftempms); + &baro_ms5611.data.d1, &baro_ms5611.data.d2, + &fbaroms, &ftempms); #endif } } diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.h b/sw/airborne/modules/sensors/baro_ms5611_spi.h index 7be9f9c2e5..43eb997e02 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_spi.h +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.h @@ -51,8 +51,4 @@ extern void baro_ms5611_periodic_check(void); extern void baro_ms5611_event(void); extern void baro_ms5611_send_coeff(void); -#define BaroMs5611UpdatePressure(_b, _h) { if (baro_ms5611.data_available) { _b = baro_ms5611.data.pressure; _h(); baro_ms5611.data_available = FALSE; } } - -#define BaroMs5611UpdateAlt(_b, _h) { if (baro_ms5611.data_available) { _b = baro_ms5611_alt; _h(); baro_ms5611.data_available = FALSE; } } - #endif diff --git a/sw/airborne/modules/sensors/baro_scp.c b/sw/airborne/modules/sensors/baro_scp.c index 94a2f67807..278fe3e4a1 100644 --- a/sw/airborne/modules/sensors/baro_scp.c +++ b/sw/airborne/modules/sensors/baro_scp.c @@ -2,6 +2,7 @@ #include "mcu_periph/sys_time.h" #include "led.h" #include "mcu.h" +#include "subsystems/abi.h" #include "mcu_periph/uart.h" #include "messages.h" @@ -184,6 +185,8 @@ static void baro_scp_read(void) { void baro_scp_event( void ) { if (baro_scp_available == TRUE) { + float pressure = (float)baro_scp_pressure; + AbiSendMsgBARO_ABS(BARO_SCP_SENDER_ID, &pressure); #ifdef SENSOR_SYNC_SEND DOWNLINK_SEND_SCP_STATUS(DefaultChannel, DefaultDevice, &baro_scp_pressure, &baro_scp_temperature); #endif diff --git a/sw/airborne/modules/sensors/baro_scp.h b/sw/airborne/modules/sensors/baro_scp.h index 77aeb504d1..c69534ec0f 100644 --- a/sw/airborne/modules/sensors/baro_scp.h +++ b/sw/airborne/modules/sensors/baro_scp.h @@ -20,6 +20,4 @@ void baro_scp_init(void); void baro_scp_periodic(void); void baro_scp_event(void); -#define BaroScpUpdate(_b, _h) { if (baro_scp_available) { _b = baro_scp_pressure; _h(); baro_scp_available = FALSE; } } - #endif diff --git a/sw/airborne/modules/sensors/baro_scp_i2c.c b/sw/airborne/modules/sensors/baro_scp_i2c.c index a3c1f24d16..eb9ff82b96 100644 --- a/sw/airborne/modules/sensors/baro_scp_i2c.c +++ b/sw/airborne/modules/sensors/baro_scp_i2c.c @@ -9,6 +9,7 @@ #include "mcu_periph/sys_time.h" #include "mcu_periph/i2c.h" +#include "subsystems/abi.h" #include "led.h" #include "mcu_periph/uart.h" @@ -99,6 +100,8 @@ void baro_scp_event( void ) { baro_scp_pressure |= scp_trans.buf[1]; baro_scp_pressure *= 25; + float pressure = (float) baro_scp_pressure; + AbiSendMsgBARO_ABS(BARO_SCP_SENDER_ID, &pressure); #ifdef SENSOR_SYNC_SEND DOWNLINK_SEND_SCP_STATUS(DefaultChannel, DefaultDevice, &baro_scp_pressure, &baro_scp_temperature); #endif diff --git a/sw/airborne/modules/sensors/baro_sim.c b/sw/airborne/modules/sensors/baro_sim.c new file mode 100644 index 0000000000..b78e6995c4 --- /dev/null +++ b/sw/airborne/modules/sensors/baro_sim.c @@ -0,0 +1,41 @@ +/* + * 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 baro_sim.c + * + * Simulate barometer pressure measurement using gps.hmsl + */ + +#include "math/pprz_isa.h" +#include "subsystems/gps.h" +#include "subsystems/abi.h" + +PRINT_CONFIG_VAR(BARO_SIM_SENDER_ID) + +void baro_sim_init(void) { + +} + +void baro_sim_periodic(void) { + float pressure = pprz_isa_pressure_of_altitude(gps.hmsl / 1000.0); + AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, &pressure); +} diff --git a/sw/airborne/modules/sensors/baro_sim.h b/sw/airborne/modules/sensors/baro_sim.h new file mode 100644 index 0000000000..a78d3218fa --- /dev/null +++ b/sw/airborne/modules/sensors/baro_sim.h @@ -0,0 +1,37 @@ +/* + * 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 baro_sim.h + * + * Simulate barometer pressure measurement using gps.hmsl + */ + +#ifndef BARO_SIM_H +#define BARO_SIM_H + +/// new measurement every baro_sim_periodic +#define BARO_SIM_DT BARO_SIM_PERIODIC_PERIOD + +extern void baro_sim_init(void); +extern void baro_sim_periodic(void); + +#endif diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.c b/sw/airborne/modules/sensors/pressure_board_navarro.c index ebe33e997a..d832fad9a0 100644 --- a/sw/airborne/modules/sensors/pressure_board_navarro.c +++ b/sw/airborne/modules/sensors/pressure_board_navarro.c @@ -27,6 +27,7 @@ #include "pressure_board_navarro.h" #include "state.h" +#include "subsystems/abi.h" /* Default I2C device on tiny is i2c0 */ @@ -56,6 +57,11 @@ #define PBN_ALTITUDE_SCALE 0.32 #endif +#ifndef PBN_PRESSURE_OFFSET +#define PBN_PRESSURE_OFFSET 101325.0 +#endif + + // Global variables uint16_t altitude_adc; uint16_t airspeed_adc; @@ -132,6 +138,9 @@ void pbn_read_event( void ) { --offset_cnt; } else { + // Compute pressure + float pressure = PBN_ALTITUDE_SCALE * (float) altitude_adc + PBN_PRESSURE_OFFSET; + AbiSendMsgBARO_ABS(BARO_PBN_SENDER_ID, &pressure); // Compute airspeed and altitude //pbn_airspeed = (-4.45 + sqrtf(19.84-0.57*(float)(airspeed_offset-airspeed_adc)))/0.28; uint16_t diff = Max(airspeed_adc-airspeed_offset, 0); diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.h b/sw/airborne/modules/sensors/pressure_board_navarro.h index 3ba8b2d208..f5e78f2a2f 100644 --- a/sw/airborne/modules/sensors/pressure_board_navarro.h +++ b/sw/airborne/modules/sensors/pressure_board_navarro.h @@ -54,8 +54,6 @@ extern void pbn_read_event( void ); #define PbnEvent() { if (pbn_trans.status == I2CTransSuccess) pbn_read_event(); } -#define BaroPbnUpdate(_b) { if (data_valid) { _b = altitude_adc; data_valid = FALSE; } } - #define PERIODIC_SEND_PBN(_chan) DOWNLINK_SEND_PBN(DefaultChannel, DefaultDevice,&airspeed_adc,&altitude_adc,&pbn_airspeed,&pbn_altitude,&airspeed_offset,&altitude_offset); #endif // PRESSURE_BOARD_NAVARRO_H diff --git a/sw/airborne/modules/vehicle_interface/vi_test_signal.c b/sw/airborne/modules/vehicle_interface/vi_test_signal.c index 35edb58220..de29c7d25e 100644 --- a/sw/airborne/modules/vehicle_interface/vi_test_signal.c +++ b/sw/airborne/modules/vehicle_interface/vi_test_signal.c @@ -67,7 +67,7 @@ void booz_fms_impl_periodic(void) { #if 0 case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: { if (guidance_v_mode < GUIDANCE_V_MODE_HOVER) - booz_fms_test_signal_start_z = ins_ltp_pos.z; + booz_fms_test_signal_start_z = ins_impl.ltp_pos.z; else { booz_fms_input.v_sp.height = (booz_fms_test_signal_counter < booz_fms_test_signal_period) ? booz_fms_test_signal_start_z : diff --git a/sw/airborne/peripherals/mpl3115.c b/sw/airborne/peripherals/mpl3115.c index 24a3bab2a1..bfac2abad1 100644 --- a/sw/airborne/peripherals/mpl3115.c +++ b/sw/airborne/peripherals/mpl3115.c @@ -27,57 +27,50 @@ #include "peripherals/mpl3115.h" #include "std.h" -#define MPL_CONF_UNINIT 0 -#define MPL_CONF_PT_DATA 1 -#define MPL_CONF_CTRL1 2 -#define MPL_CONF_DONE 3 - -// Data ready flag -volatile bool_t mpl3115_data_available; -// Data -uint32_t mpl3115_pressure; -int16_t mpl3115_temperature; -float mpl3115_alt; -// I2C transaction for reading and configuring -struct i2c_transaction mpl3115_trans; -// I2C transaction for conversion request -struct i2c_transaction mpl3115_req_trans; -// Init flag -bool_t mpl3115_initialized; -uint8_t mpl3115_init_status; - -void mpl3115_init(void) +void mpl3115_init(struct Mpl3115 *mpl, struct i2c_periph *i2c_p, uint8_t addr) { - mpl3115_trans.status = I2CTransDone; - mpl3115_req_trans.status = I2CTransDone; - mpl3115_initialized = FALSE; - mpl3115_init_status = MPL_CONF_UNINIT; - mpl3115_pressure = 0; - mpl3115_temperature = 0; - mpl3115_alt = 0.; + /* set i2c_peripheral */ + mpl->i2c_p = i2c_p; + + /* slave address */ + mpl->trans.slave_addr = addr; + + mpl->trans.status = I2CTransDone; + mpl->req_trans.status = I2CTransDone; + mpl->initialized = FALSE; + mpl->init_status = MPL_CONF_UNINIT; + + /* by default disable raw mode and set pressure mode */ + mpl->raw_mode = FALSE; + mpl->alt_mode = FALSE; + + mpl->pressure = 0; + mpl->temperature = 0; + mpl->altitude = 0.; } // Configuration function called once before normal use -static void mpl3115_send_config(void) +static void mpl3115_send_config(struct Mpl3115 *mpl) { - switch (mpl3115_init_status) { + switch (mpl->init_status) { case MPL_CONF_PT_DATA: - mpl3115_trans.buf[0] = MPL3115_REG_PT_DATA_CFG; - mpl3115_trans.buf[1] = MPL3115_PT_DATA_CFG; - i2c_transmit(&MPL3115_I2C_DEV, &mpl3115_trans, MPL3115_I2C_ADDR, 2); - mpl3115_init_status++; + mpl->trans.buf[0] = MPL3115_REG_PT_DATA_CFG; + mpl->trans.buf[1] = MPL3115_PT_DATA_CFG; + i2c_transmit(mpl->i2c_p, &mpl->trans, mpl->trans.slave_addr, 2); + mpl->init_status++; break; case MPL_CONF_CTRL1: - mpl3115_trans.buf[0] = MPL3115_REG_CTRL_REG1; - mpl3115_trans.buf[1] = MPL3115_CTRL_REG1; - i2c_transmit(&MPL3115_I2C_DEV, &mpl3115_trans, MPL3115_I2C_ADDR, 2); - mpl3115_init_status++; + mpl->trans.buf[0] = MPL3115_REG_CTRL_REG1; + mpl->trans.buf[1] = ((MPL3115_OVERSAMPLING<<3) | (mpl->raw_mode<<6) | + (mpl->alt_mode<<7)); + i2c_transmit(mpl->i2c_p, &mpl->trans, mpl->trans.slave_addr, 2); + mpl->init_status++; break; case MPL_CONF_DONE: - mpl3115_initialized = TRUE; - mpl3115_trans.status = I2CTransDone; + mpl->initialized = TRUE; + mpl->trans.status = I2CTransDone; break; default: break; @@ -85,77 +78,85 @@ static void mpl3115_send_config(void) } // Configure -void mpl3115_configure(void) +void mpl3115_configure(struct Mpl3115 *mpl) { - if (mpl3115_init_status == MPL_CONF_UNINIT) { - mpl3115_init_status++; - if (mpl3115_trans.status == I2CTransSuccess || mpl3115_trans.status == I2CTransDone) { - mpl3115_send_config(); + if (mpl->init_status == MPL_CONF_UNINIT) { + mpl->init_status++; + if (mpl->trans.status == I2CTransSuccess || mpl->trans.status == I2CTransDone) { + mpl3115_send_config(mpl); } } } // Normal reading -void mpl3115_read(void) +void mpl3115_read(struct Mpl3115 *mpl) { // ask for a reading and then prepare next conversion - if (mpl3115_initialized && mpl3115_trans.status == I2CTransDone) { - mpl3115_trans.buf[0] = MPL3115_REG_STATUS; - i2c_transceive(&MPL3115_I2C_DEV, &mpl3115_trans, MPL3115_I2C_ADDR, 1, 6); - if (mpl3115_req_trans.status == I2CTransDone) { - mpl3115_req_trans.buf[0] = MPL3115_REG_CTRL_REG1; - mpl3115_req_trans.buf[1] = MPL3115_CTRL_REG1 | MPL3115_OST_BIT; - i2c_transmit(&MPL3115_I2C_DEV, &mpl3115_req_trans, MPL3115_I2C_ADDR, 2); + if (mpl->initialized && mpl->trans.status == I2CTransDone) { + mpl->trans.buf[0] = MPL3115_REG_STATUS; + i2c_transceive(mpl->i2c_p, &mpl->trans, mpl->trans.slave_addr, 1, 6); + if (mpl->req_trans.status == I2CTransDone) { + mpl->req_trans.buf[0] = MPL3115_REG_CTRL_REG1; + mpl->req_trans.buf[1] = ((MPL3115_OVERSAMPLING<<3) | (mpl->raw_mode<<6) | + (mpl->alt_mode<<7) | MPL3115_OST_BIT); + i2c_transmit(mpl->i2c_p, &mpl->trans, mpl->trans.slave_addr, 2); } } } -void mpl3115_event(void) +void mpl3115_event(struct Mpl3115 *mpl) { - if (mpl3115_initialized) { - if (mpl3115_trans.status == I2CTransFailed) { - mpl3115_trans.status = I2CTransDone; + if (mpl->initialized) { + if (mpl->trans.status == I2CTransFailed) { + mpl->trans.status = I2CTransDone; } - else if (mpl3115_trans.status == I2CTransSuccess) { + else if (mpl->trans.status == I2CTransSuccess) { // Successfull reading and new pressure data available - if (mpl3115_trans.buf[0] & (1<<2)) { -#if MPL3115_RAW_OUTPUT - // New data available - mpl3115_pressure = ((uint32_t)mpl3115_trans.buf[1]<<16)|((uint16_t)mpl3115_trans.buf[2]<<8)|mpl3115_trans.buf[3]; - mpl3115_temperature = ((int16_t)mpl3115_trans.buf[4]<<8)|mpl3115_trans.buf[5]; - mpl3115_data_available = TRUE; -#else // Not in raw mode -#if MPL3115_ALT_MODE - uint32_t tmp = ((uint32_t)mpl3115_trans.buf[1]<<16)|((uint16_t)mpl3115_trans.buf[2]<<8)|mpl3115_trans.buf[3]; - mpl3115_alt = (float)(tmp>>4)/(1<<4); - tmp = ((int16_t)mpl3115_trans.buf[4]<<8)|mpl3115_trans.buf[5]; - mpl3115_temperature = (tmp>>4); - mpl3115_data_available = TRUE; -#else // Pressure mode - uint32_t tmp = ((uint32_t)mpl3115_trans.buf[1]<<16)|((uint16_t)mpl3115_trans.buf[2]<<8)|mpl3115_trans.buf[3]; - mpl3115_pressure = (tmp>>4); - tmp = ((int16_t)mpl3115_trans.buf[4]<<8)|mpl3115_trans.buf[5]; - mpl3115_temperature = (tmp>>4); - mpl3115_data_available = TRUE; -#endif // end alt mode -#endif // end raw mode + if (mpl->trans.buf[0] & (1<<2)) { + if (mpl->raw_mode) { + // New data available + mpl->pressure = (((uint32_t)mpl->trans.buf[1]<<16) | + ((uint16_t)mpl->trans.buf[2]<<8) | + mpl->trans.buf[3]); + mpl->temperature = ((int16_t)mpl->trans.buf[4]<<8) | mpl->trans.buf[5]; + } + else { // not in raw mode + uint32_t tmp = (((uint32_t)mpl->trans.buf[1]<<16) | + ((uint16_t)mpl->trans.buf[2]<<8) | + mpl->trans.buf[3]); + if (mpl->alt_mode) { + mpl->altitude = (float)(tmp>>4) / (1<<4); + } + else { // Pressure mode + mpl->pressure = (tmp>>4); + } + tmp = ((int16_t)mpl->trans.buf[4]<<8) | mpl->trans.buf[5]; + mpl->temperature = (tmp>>4); + } + mpl->data_available = TRUE; } - mpl3115_trans.status = I2CTransDone; + mpl->trans.status = I2CTransDone; } } - else if (!mpl3115_initialized && mpl3115_init_status != MPL_CONF_UNINIT) { // Configuring - if (mpl3115_trans.status == I2CTransSuccess || mpl3115_trans.status == I2CTransDone) { - mpl3115_trans.status = I2CTransDone; - mpl3115_send_config(); + else if (!mpl->initialized && mpl->init_status != MPL_CONF_UNINIT) { // Configuring + if (mpl->trans.status == I2CTransSuccess || mpl->trans.status == I2CTransDone) { + mpl->trans.status = I2CTransDone; + mpl3115_send_config(mpl); } - if (mpl3115_trans.status == I2CTransFailed) { - mpl3115_init_status--; - mpl3115_trans.status = I2CTransDone; - mpl3115_send_config(); // Retry config (TODO max retry) + if (mpl->trans.status == I2CTransFailed) { + mpl->init_status--; + mpl->trans.status = I2CTransDone; + mpl3115_send_config(mpl); // Retry config (TODO max retry) } } - if (mpl3115_req_trans.status == I2CTransSuccess || mpl3115_req_trans.status == I2CTransFailed) { - mpl3115_req_trans.status = I2CTransDone; + if (mpl->req_trans.status == I2CTransSuccess || mpl->req_trans.status == I2CTransFailed) { + mpl->req_trans.status = I2CTransDone; } } +void mpl3115_periodic(struct Mpl3115 *mpl) { + if (mpl->initialized) + mpl3115_read(mpl); + else + mpl3115_configure(mpl); +} diff --git a/sw/airborne/peripherals/mpl3115.h b/sw/airborne/peripherals/mpl3115.h index 5c723ed379..096f91c4ce 100644 --- a/sw/airborne/peripherals/mpl3115.h +++ b/sw/airborne/peripherals/mpl3115.h @@ -34,11 +34,6 @@ /* Device address (8 bits) */ #define MPL3115_I2C_ADDR 0xC0 -/* Default I2C device */ -#ifndef MPL3115_I2C_DEV -#define MPL3115_I2C_DEV i2c0 -#endif - /* Registers */ #define MPL3115_REG_STATUS 0x00 #define MPL3115_REG_OUT_P_MSB 0x01 @@ -63,36 +58,34 @@ #ifndef MPL3115_OVERSAMPLING #define MPL3115_OVERSAMPLING 0x5 // Oversample ratio (0x5: 130ms between data sample) #endif -#ifndef MPL3115_RAW_OUTPUT -#define MPL3115_RAW_OUTPUT 0x0 // Raw output (disable alt mode if true) -#endif -#ifndef MPL3115_ALT_MODE -#define MPL3115_ALT_MODE 0x1 // 0: baro, 1: alti (disable by raw mode) -#endif -#define MPL3115_CTRL_REG1 ((MPL3115_OVERSAMPLING<<3)|(MPL3115_RAW_OUTPUT<<6)|(MPL3115_ALT_MODE<<7)) -// Config done flag -extern bool_t mpl3115_initialized; -// Data ready flag -extern volatile bool_t mpl3115_data_available; -// Data -extern uint32_t mpl3115_pressure; -extern int16_t mpl3115_temperature; -extern float mpl3115_alt; -// I2C transaction structure -//extern struct i2c_transaction mpl3115_trans; +enum Mpl3115Status { + MPL_CONF_UNINIT, + MPL_CONF_PT_DATA, + MPL_CONF_CTRL1, + MPL_CONF_DONE +}; + +struct Mpl3115 { + struct i2c_periph *i2c_p; + struct i2c_transaction trans; ///< I2C transaction for reading and configuring + struct i2c_transaction req_trans; ///< I2C transaction for conversion request + enum Mpl3115Status init_status; + bool_t initialized; ///< config done flag + volatile bool_t data_available; ///< data ready flag + bool_t raw_mode; ///< set to TRUE to enable raw output + bool_t alt_mode; ///< set to TRUE to enable altitude output (otherwise pressure) + int16_t temperature; ///< temperature in 1/16 degrees Celcius + uint32_t pressure; ///< pressure in 1/4 Pascal + float altitude; ///< altitude in meters +}; // Functions -extern void mpl3115_init(void); -extern void mpl3115_configure(void); -extern void mpl3115_read(void); -extern void mpl3115_event(void); - -// Macro for using MPL3115 in periodic function -#define Mpl3115Periodic() { \ - if (mpl3115_initialized) mpl3115_read(); \ - else mpl3115_configure(); \ -} +extern void mpl3115_init(struct Mpl3115 *mpl, struct i2c_periph *i2c_p, uint8_t addr); +extern void mpl3115_configure(struct Mpl3115 *mpl); +extern void mpl3115_read(struct Mpl3115 *mpl); +extern void mpl3115_event(struct Mpl3115 *mpl); +extern void mpl3115_periodic(struct Mpl3115 *mpl); #endif // MPL3115_H diff --git a/sw/airborne/subsystems/abi.h b/sw/airborne/subsystems/abi.h index 1e950e4e9f..d0289b13b4 100644 --- a/sw/airborne/subsystems/abi.h +++ b/sw/airborne/subsystems/abi.h @@ -19,11 +19,16 @@ * Boston, MA 02111-1307, USA. */ +/** + * @file subsystems/abi.h + * + * Main include for ABI (AirBorneInterface). + * @todo explain how to use ABI + */ + #ifndef ABI_H #define ABI_H -/* TODO Explain how to use ABI */ - #include "abi_messages.h" #endif /* ABI_H */ diff --git a/sw/airborne/subsystems/abi_common.h b/sw/airborne/subsystems/abi_common.h index 1e668b2e9c..f4eac21514 100644 --- a/sw/airborne/subsystems/abi_common.h +++ b/sw/airborne/subsystems/abi_common.h @@ -19,7 +19,11 @@ * Boston, MA 02111-1307, USA. */ -/* Common tools for ABI middelware */ +/** + * @file subsystems/abi_common.h + * + * Common tools for ABI middelware. + */ #ifndef ABI_COMMON_H #define ABI_COMMON_H @@ -32,20 +36,21 @@ * Ex: '#include "subsystems/gps.h"' in order to use the GpsState structure */ +#include "subsystems/abi_sender_ids.h" /* Some magic to avoid to compile C code, only headers */ #ifdef ABI_C -#define EXTERN +#define ABI_EXTERN #else -#define EXTERN extern +#define ABI_EXTERN extern #endif /** Generic callback definition */ typedef void (*abi_callback)(void); /** Broadcast address. - * When passing broadcast address has a sender id, - * messages from all sender are received + * When passing broadcast address as a sender id, + * messages from all senders are received */ #define ABI_BROADCAST 0 diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h new file mode 100644 index 0000000000..f87b5298b8 --- /dev/null +++ b/sw/airborne/subsystems/abi_sender_ids.h @@ -0,0 +1,79 @@ +/* + * 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 subsystems/abi_sender_ids.h + * + * Convenience defines for ABI sender IDs. + */ + +#ifndef ABI_SENDER_IDS_H +#define ABI_SENDER_IDS_H + +/** default onboard baro */ +#ifndef BARO_BOARD_SENDER_ID +#define BARO_BOARD_SENDER_ID 1 +#endif + +/* + * IDs of baro modules that can be loaded + */ +#ifndef BARO_MS5611_SENDER_ID +#define BARO_MS5611_SENDER_ID 10 +#endif + +#ifndef BARO_AMSYS_SENDER_ID +#define BARO_AMSYS_SENDER_ID 11 +#endif + +#ifndef BARO_BMP_SENDER_ID +#define BARO_BMP_SENDER_ID 12 +#endif + +#ifndef BARO_ETS_SENDER_ID +#define BARO_ETS_SENDER_ID 13 +#endif + +#ifndef BARO_MS5534A_SENDER_ID +#define BARO_MS5534A_SENDER_ID 14 +#endif + +#ifndef BARO_HCA_SENDER_ID +#define BARO_HCA_SENDER_ID 15 +#endif + +#ifndef BARO_MPL3115_SENDER_ID +#define BARO_MPL3115_SENDER_ID 16 +#endif + +#ifndef BARO_SCP_SENDER_ID +#define BARO_SCP_SENDER_ID 17 +#endif + +#ifndef BARO_PBN_SENDER_ID +#define BARO_PBN_SENDER_ID 18 +#endif + +#ifndef BARO_SIM_SENDER_ID +#define BARO_SIM_SENDER_ID 19 +#endif + +#endif /* ABI_SENDER_IDS_H */ diff --git a/sw/airborne/subsystems/air_data.c b/sw/airborne/subsystems/air_data.c new file mode 100644 index 0000000000..e7cd6b068f --- /dev/null +++ b/sw/airborne/subsystems/air_data.c @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * 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 subsystems/air_data.c + * Air Data interface + * - pressures + * - airspeed + * - angle of attack and sideslip + * - wind + */ + +#include "subsystems/air_data.h" +#include "subsystems/abi.h" + +/** global AirData state + */ +struct AirData air_data; + +/** ABI bindings + */ +#ifndef AIR_DATA_BARO_ABS_ID +#define AIR_DATA_BARO_ABS_ID ABI_BROADCAST +#endif +static abi_event pressure_abs_ev; + +static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, const float * pressure) { + air_data.pressure = *pressure; +} + +/** AirData initialization. Called at startup. + * Bind ABI messages + */ +void air_data_init( void ) { + AbiBindMsgBARO_ABS(AIR_DATA_BARO_ABS_ID, &pressure_abs_ev, pressure_abs_cb); +} + diff --git a/sw/airborne/subsystems/air_data.h b/sw/airborne/subsystems/air_data.h new file mode 100644 index 0000000000..b4963c4832 --- /dev/null +++ b/sw/airborne/subsystems/air_data.h @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * 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 subsystems/air_data.h + * Air Data interface + * - pressures + * - airspeed + * - angle of attack and sideslip + * - wind + */ + +#ifndef AIR_DATA_H +#define AIR_DATA_H + +#include "std.h" + +/** Air Data strucute */ +struct AirData { + float pressure; ///< Static atmospheric pressure (Pa) + float differential; ///< Differential pressure (dynamic - static pressure) (Pa) + float airspeed; ///< Conventional Air Speed (m/s) + float aoa; ///< angle of attack (rad) + float sideslip; ///< sideslip angle (rad) + float wind_speed; ///< wind speed (m/s) + float wind_dir; ///< wind direction (rad, 0 north, >0 clockwise) +}; + +/** global AirData state + */ +extern struct AirData air_data; + +/** AirData initialization. Called at startup. + */ +extern void air_data_init( void ); + +#endif /* AIR_DATA_H */ + diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c index 0af2ea4569..4190cfe069 100644 --- a/sw/airborne/subsystems/ins/hf_float.c +++ b/sw/airborne/subsystems/ins/hf_float.c @@ -28,7 +28,6 @@ */ #include "subsystems/ins/hf_float.h" -#include "subsystems/ins.h" #include "subsystems/imu.h" #include "state.h" #include "subsystems/gps.h" @@ -459,14 +458,6 @@ void b2_hff_propagate(void) { b2_hff_propagate_x(&b2_hff_state); b2_hff_propagate_y(&b2_hff_state); - /* update ins state from horizontal filter */ - ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot); - ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot); - ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot); - ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot); - ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x); - ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y); - #ifdef GPS_LAG /* increase lag counter on last saved state */ if (b2_hff_rb_n > 0) @@ -490,7 +481,7 @@ void b2_hff_propagate(void) { -void b2_hff_update_gps(void) { +void b2_hff_update_gps(struct FloatVect2* pos_ned, struct FloatVect2* speed_ned) { b2_hff_lost_counter = 0; #if USE_GPS_ACC4R @@ -508,20 +499,13 @@ void b2_hff_update_gps(void) { #endif /* update filter state with measurement */ - b2_hff_update_x(&b2_hff_state, ins_gps_pos_m_ned.x, Rgps_pos); - b2_hff_update_y(&b2_hff_state, ins_gps_pos_m_ned.y, Rgps_pos); + b2_hff_update_x(&b2_hff_state, pos_ned->x, Rgps_pos); + b2_hff_update_y(&b2_hff_state, pos_ned->y, Rgps_pos); #ifdef HFF_UPDATE_SPEED - b2_hff_update_xdot(&b2_hff_state, ins_gps_speed_m_s_ned.x, Rgps_vel); - b2_hff_update_ydot(&b2_hff_state, ins_gps_speed_m_s_ned.y, Rgps_vel); + b2_hff_update_xdot(&b2_hff_state, speed_ned->x, Rgps_vel); + b2_hff_update_ydot(&b2_hff_state, speed_ned->y, Rgps_vel); #endif - /* update ins state */ - ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot); - ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot); - ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot); - ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot); - ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x); - ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y); #ifdef GPS_LAG } else if (b2_hff_rb_n > 0) { @@ -530,11 +514,11 @@ void b2_hff_update_gps(void) { PRINT_DBG(2, ("update. rb_n: %d lag_counter: %d lag_cnt_err: %d\n", b2_hff_rb_n, b2_hff_rb_last->lag_counter, lag_counter_err)); if (abs(lag_counter_err) <= GPS_LAG_TOL_N) { b2_hff_rb_last->rollback = TRUE; - b2_hff_update_x(b2_hff_rb_last, ins_gps_pos_m_ned.x, Rgps_pos); - b2_hff_update_y(b2_hff_rb_last, ins_gps_pos_m_ned.y, Rgps_pos); + b2_hff_update_x(b2_hff_rb_last, pos_ned->x, Rgps_pos); + b2_hff_update_y(b2_hff_rb_last, pos_ned->y, Rgps_pos); #ifdef HFF_UPDATE_SPEED - b2_hff_update_xdot(b2_hff_rb_last, ins_gps_speed_m_s_ned.x, Rgps_vel); - b2_hff_update_ydot(b2_hff_rb_last, ins_gps_speed_m_s_ned.y, Rgps_vel); + b2_hff_update_xdot(b2_hff_rb_last, speed_ned->x, Rgps_vel); + b2_hff_update_ydot(b2_hff_rb_last, speed_ned->y, Rgps_vel); #endif past_save_counter = GPS_DT_N-1;// + lag_counter_err; PRINT_DBG(2, ("gps updated. past_save_counter: %d\n", past_save_counter)); diff --git a/sw/airborne/subsystems/ins/hf_float.h b/sw/airborne/subsystems/ins/hf_float.h index 2d1c08dbc9..c53ddb6b8b 100644 --- a/sw/airborne/subsystems/ins/hf_float.h +++ b/sw/airborne/subsystems/ins/hf_float.h @@ -81,7 +81,7 @@ extern float b2_hff_ydd_meas; extern void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot); extern void b2_hff_propagate(void); -extern void b2_hff_update_gps(void); +extern void b2_hff_update_gps(struct FloatVect2* pos_ned, struct FloatVect2* speed_ned); extern void b2_hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos); extern void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel); extern void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel); diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index 8f0004ffd1..791a20fc7d 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -24,7 +24,9 @@ * Filters altitude and climb rate for fixedwings. */ -#include "subsystems/ins.h" +#include "subsystems/ins/ins_alt_float.h" + +#include "subsystems/abi.h" #include #include @@ -46,9 +48,18 @@ float ins_alt_dot; #if USE_BAROMETER #include "subsystems/sensors/baro.h" -int32_t ins_qfe; -bool_t ins_baro_initialised; +#include "math/pprz_isa.h" +float ins_qfe; +bool_t ins_baro_initialized; float ins_baro_alt; + +// Baro event on ABI +#ifndef INS_BARO_ID +#define INS_BARO_ID BARO_BOARD_SENDER_ID +#endif +abi_event baro_ev; +static void baro_cb(uint8_t sender_id, const float *pressure); + #endif void ins_init() { @@ -62,13 +73,16 @@ void ins_init() { #if USE_BAROMETER ins_qfe = 0;; - ins_baro_initialised = FALSE; + ins_baro_initialized = FALSE; ins_baro_alt = 0.; + // Bind to BARO_ABS message + AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); #endif ins.vf_realign = FALSE; EstimatorSetAlt(0.); + ins.status = INS_RUNNING; } void ins_periodic( void ) { @@ -83,36 +97,37 @@ void ins_realign_v(float z __attribute__ ((unused))) { void ins_propagate() { } -void ins_update_baro() { +void ins_update_baro() {} + #if USE_BAROMETER - // TODO update kalman filter with baro struct - if (baro.status == BS_RUNNING) { - if (!ins_baro_initialised) { - ins_qfe = baro.absolute; - ins_baro_initialised = TRUE; - } - if (ins.vf_realign) { - ins.vf_realign = FALSE; - ins_qfe = baro.absolute; - } - else { /* not realigning, so normal update with baro measurement */ - /* altitude decreases with increasing baro.absolute pressure */ - ins_baro_alt = ground_alt - (baro.absolute - ins_qfe) * INS_BARO_SENS; - /* run the filter */ - EstimatorSetAlt(ins_baro_alt); - /* set new altitude, just copy old horizontal position */ - struct UtmCoor_f utm; - UTM_COPY(utm, *stateGetPositionUtm_f()); - utm.alt = ins_alt; - stateSetPositionUtm_f(&utm); - struct NedCoor_f ned_vel; - memcpy(&ned_vel, stateGetSpeedNed_f(), sizeof(struct NedCoor_f)); - ned_vel.z = -ins_alt_dot; - stateSetSpeedNed_f(&ned_vel); - } +static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) { + if (!ins_baro_initialized) { + ins_qfe = *pressure; + ins_baro_initialized = TRUE; + } + if (ins.vf_realign) { + ins.vf_realign = FALSE; + ins_alt = ground_alt; + ins_alt_dot = 0.; + ins_qfe = *pressure; + alt_kalman_reset(); + } + else { /* not realigning, so normal update with baro measurement */ + ins_baro_alt = ground_alt + pprz_isa_height_of_pressure(*pressure, ins_qfe); + /* run the filter */ + EstimatorSetAlt(ins_baro_alt); + /* set new altitude, just copy old horizontal position */ + struct UtmCoor_f utm; + UTM_COPY(utm, *stateGetPositionUtm_f()); + utm.alt = ins_alt; + stateSetPositionUtm_f(&utm); + struct NedCoor_f ned_vel; + memcpy(&ned_vel, stateGetSpeedNed_f(), sizeof(struct NedCoor_f)); + ned_vel.z = -ins_alt_dot; + stateSetSpeedNed_f(&ned_vel); } -#endif } +#endif void ins_update_gps(void) { @@ -178,7 +193,11 @@ void alt_kalman(float z_meas) { float SIGMA2; #if USE_BAROMETER -#if USE_BARO_MS5534A +#ifdef SITL + DT = BARO_SIM_DT; + R = 0.5; + SIGMA2 = 0.1; +#elif USE_BARO_MS5534A if (alt_baro_enabled) { DT = BARO_DT; R = baro_MS5534A_r; diff --git a/sw/airborne/subsystems/ins/ins_alt_float.h b/sw/airborne/subsystems/ins/ins_alt_float.h index 613ebc5281..837ca01c35 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.h +++ b/sw/airborne/subsystems/ins/ins_alt_float.h @@ -56,9 +56,9 @@ #include "modules/sensors/baro_amsys.h" #endif -extern int32_t ins_qfe; -extern float ins_baro_alt; -extern bool_t ins_baro_initialised; +extern float ins_qfe; +extern float ins_baro_alt; +extern bool_t ins_baro_initialized; #endif //USE_BAROMETER extern float ins_alt; ///< estimated altitude above MSL in meters diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c index a50f0ed908..cdce78e65d 100644 --- a/sw/airborne/subsystems/ins/ins_ardrone2.c +++ b/sw/airborne/subsystems/ins/ins_ardrone2.c @@ -41,53 +41,39 @@ PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") #endif - -/* TODO: implement in state */ -int32_t ins_qfe; -int32_t ins_baro_alt; - -//Keep track of gps pos and the init pos -struct NedCoor_i ins_ltp_pos; -struct LtpDef_i ins_ltp_def; - -// Keep track of INS LTP accel and speed -struct NedCoor_f ins_ltp_accel; -struct NedCoor_f ins_ltp_speed; - -bool_t ins_ltp_initialised; +struct InsArdrone2 ins_impl; void ins_init() { #if USE_INS_NAV_INIT - struct LlaCoor_i llh_nav; + struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ + llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0); + llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0); + /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ + llh_nav0.alt = NAV_ALT0 + NAV_MSL0; - /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */ - llh_nav.lat = INT32_RAD_OF_DEG(NAV_LAT0); - llh_nav.lon = INT32_RAD_OF_DEG(NAV_LON0); - llh_nav.alt = NAV_ALT0 + NAV_MSL0; + struct EcefCoor_i ecef_nav0; + ecef_of_lla_i(&ecef_nav0, &llh_nav0); - //Convert ltp - ltp_def_from_lla_i(&ins_ltp_def, &llh_nav); - ins_ltp_def.hmsl = NAV_ALT0; + ltp_def_from_ecef_i(&ins_impl.ltp_def, &ecef_nav0); + ins_impl.ltp_def.hmsl = NAV_ALT0; + stateSetLocalOrigin_i(&ins_impl.ltp_def); - //Set the ltp - stateSetLocalOrigin_i(&ins_ltp_def); - - ins_ltp_initialised = TRUE; + ins_impl.ltp_initialized = TRUE; #else - ins_ltp_initialised = FALSE; + ins_impl.ltp_initialized = FALSE; #endif ins.vf_realign = FALSE; ins.hf_realign = FALSE; - INT32_VECT3_ZERO(ins_ltp_pos); - - // TODO correct init - ins.status = INS_RUNNING; + INT32_VECT3_ZERO(ins_impl.ltp_pos); + INT32_VECT3_ZERO(ins_impl.ltp_speed); + INT32_VECT3_ZERO(ins_impl.ltp_accel); } void ins_periodic( void ) { - + if (ins_impl.ltp_initialized) + ins.status = INS_RUNNING; } void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) { @@ -100,21 +86,21 @@ void ins_realign_v(float z __attribute__ ((unused))) { void ins_propagate() { /* untilt accels and speeds */ - FLOAT_RMAT_VECT3_TRANSP_MUL(ins_ltp_accel, (*stateGetNedToBodyRMat_f()), ahrs_impl.accel); - FLOAT_RMAT_VECT3_TRANSP_MUL(ins_ltp_speed, (*stateGetNedToBodyRMat_f()), ahrs_impl.speed); + FLOAT_RMAT_VECT3_TRANSP_MUL(ins_impl.ltp_accel, (*stateGetNedToBodyRMat_f()), ahrs_impl.accel); + FLOAT_RMAT_VECT3_TRANSP_MUL(ins_impl.ltp_speed, (*stateGetNedToBodyRMat_f()), ahrs_impl.speed); //Add g to the accelerations - ins_ltp_accel.z += 9.81; + ins_impl.ltp_accel.z += 9.81; //Save the accelerations and speeds - stateSetAccelNed_f(&ins_ltp_accel); - stateSetSpeedNed_f(&ins_ltp_speed); + stateSetAccelNed_f(&ins_impl.ltp_accel); + stateSetSpeedNed_f(&ins_impl.ltp_speed); //Don't set the height if we use the one from the gps #if !USE_GPS_HEIGHT //Set the height and save the position - ins_ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; - stateSetPositionNed_i(&ins_ltp_pos); + ins_impl.ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; + stateSetPositionNed_i(&ins_impl.ltp_pos); #endif } @@ -128,27 +114,27 @@ void ins_update_gps(void) { //Check for GPS fix if (gps.fix == GPS_FIX_3D) { //Set the initial coordinates - if(!ins_ltp_initialised) { - ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos); - ins_ltp_def.lla.alt = gps.lla_pos.alt; - ins_ltp_def.hmsl = gps.hmsl; - ins_ltp_initialised = TRUE; - stateSetLocalOrigin_i(&ins_ltp_def); + if(!ins_impl.ltp_initialized) { + ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos); + ins_impl.ltp_def.lla.alt = gps.lla_pos.alt; + ins_impl.ltp_def.hmsl = gps.hmsl; + ins_impl.ltp_initialized = TRUE; + stateSetLocalOrigin_i(&ins_impl.ltp_def); } //Set the x and y and maybe z position in ltp and save struct NedCoor_i ins_gps_pos_cm_ned; - ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos); + ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos); //When we don't want to use the height of the navdata we can use the gps height #if USE_GPS_HEIGHT - INT32_VECT3_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); + INT32_VECT3_SCALE_2(ins_impl.ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); #else - INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); + INT32_VECT2_SCALE_2(ins_impl.ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); #endif //Set the local origin - stateSetPositionNed_i(&ins_ltp_pos); + stateSetPositionNed_i(&ins_impl.ltp_pos); } #endif /* USE_GPS */ } diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.h b/sw/airborne/subsystems/ins/ins_ardrone2.h index c14e362eea..617c2845f3 100644 --- a/sw/airborne/subsystems/ins/ins_ardrone2.h +++ b/sw/airborne/subsystems/ins/ins_ardrone2.h @@ -24,22 +24,26 @@ * INS implementation for ardrone2-sdk. */ -#ifndef INS_INT_H -#define INS_INT_H +#ifndef INS_ARDRONE2_SDK_H +#define INS_ARDRONE2_SDK_H #include "subsystems/ins.h" #include "std.h" #include "math/pprz_geodetic_int.h" #include "math/pprz_algebra_float.h" -//TODO: implement in state -extern int32_t ins_qfe; -extern int32_t ins_baro_alt; +struct InsArdrone2 { + struct LtpDef_i ltp_def; + bool_t ltp_initialized; -extern struct NedCoor_i ins_ltp_pos; -extern struct LtpDef_i ins_ltp_def; -extern struct NedCoor_f ins_ltp_speed; -extern struct NedCoor_f ins_ltp_accel; -extern bool_t ins_ltp_initialised; + float qfe; ///< not used, only dummy for INS_REF message -#endif /* INS_INT_H */ + /* output LTP NED */ + struct NedCoor_i ltp_pos; + struct NedCoor_i ltp_speed; + struct NedCoor_i ltp_accel; +}; + +extern struct InsArdrone2 ins_impl; + +#endif /* INS_ARDRONE2_SDK_H */ diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c index c274b32024..ab1d5356f1 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c @@ -38,6 +38,8 @@ void ins_init() { struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); stateSetPositionUtm_f(&utm0); + + ins.status = INS_RUNNING; } void ins_periodic( void ) { diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 950397b7fa..b11a8d85c9 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -28,13 +28,16 @@ #include "subsystems/ins/ins_int.h" +#include "subsystems/abi.h" + #include "subsystems/imu.h" -#include "subsystems/sensors/baro.h" #include "subsystems/gps.h" #include "generated/airframe.h" -#if USE_VFF +#if USE_VFF_EXTENDED +#include "subsystems/ins/vf_extended_float.h" +#else #include "subsystems/ins/vf_float.h" #endif @@ -42,96 +45,114 @@ #include "subsystems/ins/hf_float.h" #endif -#ifdef SITL -#include "nps_fdm.h" +#if defined SITL && USE_NPS +//#include "nps_fdm.h" +#include "nps_autopilot.h" #include #endif - #include "math/pprz_geodetic_int.h" +#include "math/pprz_isa.h" #include "generated/flight_plan.h" + +#if USE_SONAR + +#if !USE_VFF_EXTENDED +#error USE_SONAR needs USE_VFF_EXTENDED +#endif + +#ifdef INS_SONAR_THROTTLE_THRESHOLD +#include "firmwares/rotorcraft/stabilization.h" +#endif + +#ifndef INS_SONAR_OFFSET +#define INS_SONAR_OFFSET 0 +#endif +#define VFF_R_SONAR_0 0.1 +#define VFF_R_SONAR_OF_M 0.2 + +#endif // USE_SONAR + #ifndef USE_INS_NAV_INIT #define USE_INS_NAV_INIT TRUE PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") #endif -/* gps transformed to LTP-NED */ -struct LtpDef_i ins_ltp_def; - bool_t ins_ltp_initialised; -struct NedCoor_i ins_gps_pos_cm_ned; -struct NedCoor_i ins_gps_speed_cm_s_ned; + +/** default barometer to use in INS */ +#ifndef INS_BARO_ID +#define INS_BARO_ID BARO_BOARD_SENDER_ID +#endif +abi_event baro_ev; +static void baro_cb(uint8_t sender_id, const float *pressure); + +struct InsInt ins_impl; + + +static void ins_init_origin_from_flightplan(void); +static void ins_ned_to_state(void); #if USE_HFF -/* horizontal gps transformed to NED in meters as float */ -struct FloatVect2 ins_gps_pos_m_ned; -struct FloatVect2 ins_gps_speed_m_s_ned; +static void ins_update_from_hff(void); #endif -/* barometer */ -#if USE_VFF -int32_t ins_qfe; -bool_t ins_baro_initialised; -int32_t ins_baro_alt; -#endif -/* output */ -struct NedCoor_i ins_ltp_pos; -struct NedCoor_i ins_ltp_speed; -struct NedCoor_i ins_ltp_accel; +void ins_init(void) { - -void ins_init() { #if USE_INS_NAV_INIT - ins_ltp_initialised = TRUE; - - /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */ - struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ - llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0); - llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0); - /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ - llh_nav0.alt = NAV_ALT0 + NAV_MSL0; - - struct EcefCoor_i ecef_nav0; - ecef_of_lla_i(&ecef_nav0, &llh_nav0); - - ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0); - ins_ltp_def.hmsl = NAV_ALT0; - stateSetLocalOrigin_i(&ins_ltp_def); + ins_init_origin_from_flightplan(); + ins_impl.ltp_initialized = TRUE; #else - ins_ltp_initialised = FALSE; + ins_impl.ltp_initialized = FALSE; #endif -#if USE_VFF - ins_baro_initialised = FALSE; - vff_init(0., 0., 0.); + + // Bind to BARO_ABS message + AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); + ins_impl.baro_initialized = FALSE; + +#if USE_SONAR + ins_impl.update_on_agl = FALSE; + init_median_filter(&ins_impl.sonar_median); + ins_impl.sonar_offset = INS_SONAR_OFFSET; #endif + ins.vf_realign = FALSE; ins.hf_realign = FALSE; + + /* init vertical and horizontal filters */ +#if USE_VFF_EXTENDED + vff_init(0., 0., 0., 0.); +#else + vff_init(0., 0., 0.); +#endif #if USE_HFF b2_hff_init(0., 0., 0., 0.); #endif - INT32_VECT3_ZERO(ins_ltp_pos); - INT32_VECT3_ZERO(ins_ltp_speed); - INT32_VECT3_ZERO(ins_ltp_accel); - // TODO correct init - ins.status = INS_RUNNING; + INT32_VECT3_ZERO(ins_impl.ltp_pos); + INT32_VECT3_ZERO(ins_impl.ltp_speed); + INT32_VECT3_ZERO(ins_impl.ltp_accel); } -void ins_periodic( void ) { +void ins_periodic(void) { + if (ins_impl.ltp_initialized) + ins.status = INS_RUNNING; } -void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) { #if USE_HFF +void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) { b2_hff_realign(pos, speed); -#endif /* USE_HFF */ } +#else +void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), + struct FloatVect2 speed __attribute__ ((unused))) {} +#endif /* USE_HFF */ -void ins_realign_v(float z __attribute__ ((unused))) { -#if USE_VFF + +void ins_realign_v(float z) { vff_realign(z); -#endif } void ins_propagate() { @@ -141,116 +162,218 @@ void ins_propagate() { struct Int32Vect3 accel_meas_ltp; INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, (*stateGetNedToBodyRMat_i()), accel_meas_body); -#if USE_VFF float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z); - if (baro.status == BS_RUNNING && ins_baro_initialised) { + if (ins_impl.baro_initialized) { vff_propagate(z_accel_meas_float); - ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); - ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); - ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); + ins_impl.ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); + ins_impl.ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); + ins_impl.ltp_pos.z = POS_BFP_OF_REAL(vff_z); } else { // feed accel from the sensors - // subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp) - ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); + // subtract -9.81m/s2 (acceleration measured due to gravity, + // but vehicle not accelerating in ltp) + ins_impl.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); } -#else - ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); -#endif /* USE_VFF */ #if USE_HFF /* propagate horizontal filter */ b2_hff_propagate(); + /* convert and copy result to ins_impl */ + ins_update_from_hff(); #else - ins_ltp_accel.x = accel_meas_ltp.x; - ins_ltp_accel.y = accel_meas_ltp.y; + ins_impl.ltp_accel.x = accel_meas_ltp.x; + ins_impl.ltp_accel.y = accel_meas_ltp.y; #endif /* USE_HFF */ - INS_NED_TO_STATE(); + ins_ned_to_state(); } -void ins_update_baro() { -#if USE_VFF - if (baro.status == BS_RUNNING) { - if (!ins_baro_initialised) { - ins_qfe = baro.absolute; - ins_baro_initialised = TRUE; - } - if (ins.vf_realign) { - ins.vf_realign = FALSE; - ins_qfe = baro.absolute; - vff_realign(0.); - ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); - ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); - ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); - } - else { /* not realigning, so normal update with baro measurement */ - ins_baro_alt = ((baro.absolute - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN; - float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt); - vff_update(alt_float); - } +static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) { + if (!ins_impl.baro_initialized) { + ins_impl.qfe = *pressure; + ins_impl.baro_initialized = TRUE; } - INS_NED_TO_STATE(); + if (ins.vf_realign) { + ins.vf_realign = FALSE; + ins_impl.qfe = *pressure; + vff_realign(0.); + ins_impl.ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); + ins_impl.ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); + ins_impl.ltp_pos.z = POS_BFP_OF_REAL(vff_z); + } + else { + ins_impl.baro_z = -pprz_isa_height_of_pressure(*pressure, ins_impl.qfe); +#if USE_VFF_EXTENDED + vff_update_baro(ins_impl.baro_z); +#else + vff_update(ins_impl.baro_z); #endif + } + ins_ned_to_state(); +} + + +void ins_update_baro() { } void ins_update_gps(void) { #if USE_GPS if (gps.fix == GPS_FIX_3D) { - if (!ins_ltp_initialised) { - ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos); - ins_ltp_def.lla.alt = gps.lla_pos.alt; - ins_ltp_def.hmsl = gps.hmsl; - ins_ltp_initialised = TRUE; - stateSetLocalOrigin_i(&ins_ltp_def); + if (!ins_impl.ltp_initialized) { + ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos); + ins_impl.ltp_def.lla.alt = gps.lla_pos.alt; + ins_impl.ltp_def.hmsl = gps.hmsl; + ins_impl.ltp_initialized = TRUE; + stateSetLocalOrigin_i(&ins_impl.ltp_def); } - ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos); - ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel); + + struct NedCoor_i gps_pos_cm_ned; + ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos); + /// @todo maybe use gps.ned_vel directly?? + struct NedCoor_i gps_speed_cm_s_ned; + ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_impl.ltp_def, &gps.ecef_vel); + #if USE_HFF - VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y); - VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.); - VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y); - VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.); + /* horizontal gps transformed to NED in meters as float */ + struct FloatVect2 gps_pos_m_ned; + VECT2_ASSIGN(gps_pos_m_ned, gps_pos_cm_ned.x, gps_pos_cm_ned.y); + VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.); + + struct FloatVect2 gps_speed_m_s_ned; + VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y); + VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.); + if (ins.hf_realign) { ins.hf_realign = FALSE; -#ifdef SITL - struct FloatVect2 true_pos, true_speed; - VECT2_COPY(true_pos, fdm.ltpprz_pos); - VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel); - b2_hff_realign(true_pos, true_speed); -#else const struct FloatVect2 zero = {0.0, 0.0}; - b2_hff_realign(ins_gps_pos_m_ned, zero); -#endif + b2_hff_realign(gps_pos_m_ned, zero); } - b2_hff_update_gps(); -#if !USE_VFF /* vff not used */ - ins_ltp_pos.z = (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; - ins_ltp_speed.z = (ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN; -#endif /* vff not used */ -#endif /* hff used */ + // run horizontal filter + b2_hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned); + // convert and copy result to ins_impl + ins_update_from_hff(); +#else /* hff not used */ + /* simply copy horizontal pos/speed from gps */ + INT32_VECT2_SCALE_2(ins_impl.ltp_pos, gps_pos_cm_ned, + INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); + INT32_VECT2_SCALE_2(ins_impl.ltp_speed, gps_speed_cm_s_ned, + INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); +#endif /* USE_HFF */ -#if !USE_HFF /* hff not used */ -#if !USE_VFF /* neither hf nor vf used */ - INT32_VECT3_SCALE_3(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); - INT32_VECT3_SCALE_3(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); -#else /* only vff used */ - INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); - INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); -#endif - -#if USE_GPS_LAG_HACK - VECT2_COPY(d_pos, ins_ltp_speed); - INT32_VECT2_RSHIFT(d_pos, d_pos, 11); - VECT2_ADD(ins_ltp_pos, d_pos); -#endif -#endif /* hff not used */ - - INS_NED_TO_STATE(); + ins_ned_to_state(); } #endif /* USE_GPS */ } + +//#define INS_SONAR_VARIANCE_THRESHOLD 0.01 + +#ifdef INS_SONAR_VARIANCE_THRESHOLD + +#include "messages.h" +#include "mcu_periph/uart.h" +#include "subsystems/datalink/downlink.h" + +#include "math/pprz_stat.h" +#define VAR_ERR_MAX 10 +float var_err[VAR_ERR_MAX]; +uint8_t var_idx = 0; +#endif + + void ins_update_sonar() { +#if USE_SONAR + static float last_offset = 0.; + // new value filtered with median_filter + ins_impl.sonar_alt = update_median_filter(&ins_impl.sonar_median, sonar_meas); + float sonar = (ins_impl.sonar_alt - ins_impl.sonar_offset) * INS_SONAR_SENS; + +#ifdef INS_SONAR_VARIANCE_THRESHOLD + /* compute variance of error between sonar and baro alt */ + float err = sonar + ins_impl.baro_z; // sonar positive up, baro positive down !!!! + var_err[var_idx] = err; + var_idx = (var_idx + 1) % VAR_ERR_MAX; + float var = variance_float(var_err, VAR_ERR_MAX); + DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&err, &sonar, &var); + //DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&ins_impl.sonar_alt, &sonar, &var); +#endif + + /* update filter assuming a flat ground */ + if (sonar < INS_SONAR_MAX_RANGE +#ifdef INS_SONAR_MIN_RANGE + && sonar > INS_SONAR_MIN_RANGE +#endif +#ifdef INS_SONAR_THROTTLE_THRESHOLD + && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD +#endif +#ifdef INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_ROLL] < INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_ROLL] > -INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_PITCH] < INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_PITCH] > -INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_YAW] < INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_YAW] > -INS_SONAR_STAB_THRESHOLD +#endif +#ifdef INS_SONAR_BARO_THRESHOLD + && ins_impl.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */ +#endif +#ifdef INS_SONAR_VARIANCE_THRESHOLD + && var < INS_SONAR_VARIANCE_THRESHOLD +#endif + && ins_impl.update_on_agl + && ins_impl.baro_initialized) { + vff_update_alt_conf(-sonar, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabs(sonar)); + last_offset = vff_offset; + } + else { + /* update offset with last value to avoid divergence */ + vff_update_offset(last_offset); + } +#endif // USE_SONAR } + + +/** initialize the local origin (ltp_def) from flight plan position */ +static void ins_init_origin_from_flightplan(void) { + + struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ + llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0); + llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0); + /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ + llh_nav0.alt = NAV_ALT0 + NAV_MSL0; + + struct EcefCoor_i ecef_nav0; + ecef_of_lla_i(&ecef_nav0, &llh_nav0); + + ltp_def_from_ecef_i(&ins_impl.ltp_def, &ecef_nav0); + ins_impl.ltp_def.hmsl = NAV_ALT0; + stateSetLocalOrigin_i(&ins_impl.ltp_def); + +} + +/** copy position and speed to state interface */ +static void ins_ned_to_state(void) { + stateSetPositionNed_i(&ins_impl.ltp_pos); + stateSetSpeedNed_i(&ins_impl.ltp_speed); + stateSetAccelNed_i(&ins_impl.ltp_accel); + +#if defined SITL && USE_NPS + if (nps_bypass_ins) + sim_overwrite_ins(); +#endif +} + + +#if USE_HFF +/** update ins state from horizontal filter */ +static void ins_update_from_hff(void) { + ins_impl.ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot); + ins_impl.ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot); + ins_impl.ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot); + ins_impl.ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot); + ins_impl.ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x); + ins_impl.ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y); +} +#endif diff --git a/sw/airborne/subsystems/ins/ins_int.h b/sw/airborne/subsystems/ins/ins_int.h index c72c805c52..71171c5fbf 100644 --- a/sw/airborne/subsystems/ins/ins_int.h +++ b/sw/airborne/subsystems/ins/ins_int.h @@ -34,42 +34,34 @@ #include "math/pprz_geodetic_int.h" #include "math/pprz_algebra_float.h" -// TODO integrate all internal state to the structure -///** Ins implementation state (fixed point) */ -//struct InsInt { -//}; -// -///** global INS state */ -//extern struct InsInt ins_impl; - -/* gps transformed to LTP-NED */ -extern struct LtpDef_i ins_ltp_def; -extern bool_t ins_ltp_initialised; -extern struct NedCoor_i ins_gps_pos_cm_ned; -extern struct NedCoor_i ins_gps_speed_cm_s_ned; - -/* barometer */ -#if USE_VFF -extern int32_t ins_baro_alt; ///< altitude calculated from baro in meters with #INT32_POS_FRAC -extern int32_t ins_qfe; -extern bool_t ins_baro_initialised; +#if USE_SONAR +#include "filters/median_filter.h" #endif -/* output LTP NED */ -extern struct NedCoor_i ins_ltp_pos; -extern struct NedCoor_i ins_ltp_speed; -extern struct NedCoor_i ins_ltp_accel; -#if USE_HFF -/* horizontal gps transformed to NED in meters as float */ -extern struct FloatVect2 ins_gps_pos_m_ned; -extern struct FloatVect2 ins_gps_speed_m_s_ned; -#endif +/** Ins implementation state (fixed point) */ +struct InsInt { + struct LtpDef_i ltp_def; + bool_t ltp_initialized; -/* copy position and speed to state interface */ -#define INS_NED_TO_STATE() { \ - stateSetPositionNed_i(&ins_ltp_pos); \ - stateSetSpeedNed_i(&ins_ltp_speed); \ - stateSetAccelNed_i(&ins_ltp_accel); \ -} + /* output LTP NED */ + struct NedCoor_i ltp_pos; + struct NedCoor_i ltp_speed; + struct NedCoor_i ltp_accel; + + /* baro */ + float baro_z; ///< z-position calculated from baro in meters (z-down) + float qfe; + bool_t baro_initialized; + +#if USE_SONAR + bool_t update_on_agl; /* use sonar to update agl if available */ + int32_t sonar_alt; + int32_t sonar_offset; + struct MedianFilterInt sonar_median; +#endif +}; + +/** global INS state */ +extern struct InsInt ins_impl; #endif /* INS_INT_H */ diff --git a/sw/airborne/subsystems/ins/ins_int_extended.c b/sw/airborne/subsystems/ins/ins_int_extended.c deleted file mode 100644 index aefa07f10f..0000000000 --- a/sw/airborne/subsystems/ins/ins_int_extended.c +++ /dev/null @@ -1,323 +0,0 @@ -/* - * Copyright (C) 2008-2010 The Paparazzi Team - * Copyright (C) 2012 Gautier Hattenberger - * - * 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 subsystems/ins/ins_int_extended.c - * - * INS for rotorcrafts combining vertical and horizontal filters. - * - */ - -#include "subsystems/ins/ins_int_extended.h" - -#include "subsystems/imu.h" -#include "subsystems/sensors/baro.h" -#include "subsystems/gps.h" - -#include "generated/airframe.h" -#include "math/pprz_algebra_int.h" -#include "math/pprz_algebra_float.h" - -#include "state.h" - -#include "subsystems/ins/vf_extended_float.h" - -#if USE_HFF -#include "subsystems/ins/hf_float.h" -#endif - -#ifdef SITL -#include "nps_fdm.h" -#include -#endif - -#ifdef INS_SONAR_THROTTLE_THRESHOLD -#include "firmwares/rotorcraft/stabilization.h" -#endif - -#include "math/pprz_geodetic_int.h" - -#include "generated/flight_plan.h" - -#ifndef USE_INS_NAV_INIT -#define USE_INS_NAV_INIT TRUE -PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") -#endif - - -/* gps transformed to LTP-NED */ -struct LtpDef_i ins_ltp_def; - bool_t ins_ltp_initialised; -struct NedCoor_i ins_gps_pos_cm_ned; -struct NedCoor_i ins_gps_speed_cm_s_ned; -#if USE_HFF -/* horizontal gps transformed to NED in meters as float */ -struct FloatVect2 ins_gps_pos_m_ned; -struct FloatVect2 ins_gps_speed_m_s_ned; -#endif - -/* barometer */ -int32_t ins_qfe; -bool_t ins_baro_initialised; -int32_t ins_baro_alt; -#include "filters/median_filter.h" -struct MedianFilterInt baro_median; - -#if USE_SONAR -/* sonar */ -bool_t ins_update_on_agl; -int32_t ins_sonar_alt; -int32_t ins_sonar_offset; -struct MedianFilterInt sonar_median; -#ifndef INS_SONAR_OFFSET -#define INS_SONAR_OFFSET 0 -#endif -#define VFF_R_SONAR_0 0.1 -#define VFF_R_SONAR_OF_M 0.2 -#endif // USE_SONAR - -/* output */ -struct NedCoor_i ins_ltp_pos; -struct NedCoor_i ins_ltp_speed; -struct NedCoor_i ins_ltp_accel; - - -void ins_init() { -#if USE_INS_NAV_INIT - ins_ltp_initialised = TRUE; - - /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */ - struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ - llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0); - llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0); - /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ - llh_nav0.alt = NAV_ALT0 + NAV_MSL0; - - struct EcefCoor_i ecef_nav0; - ecef_of_lla_i(&ecef_nav0, &llh_nav0); - - ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0); - ins_ltp_def.hmsl = NAV_ALT0; - stateSetLocalOrigin_i(&ins_ltp_def); -#else - ins_ltp_initialised = FALSE; -#endif - - ins_baro_initialised = FALSE; - init_median_filter(&baro_median); - -#if USE_SONAR - ins_update_on_agl = FALSE; - init_median_filter(&sonar_median); - ins_sonar_offset = INS_SONAR_OFFSET; -#endif - - vff_init(0., 0., 0., 0.); - ins.vf_realign = FALSE; - ins.hf_realign = FALSE; -#if USE_HFF - b2_hff_init(0., 0., 0., 0.); -#endif - INT32_VECT3_ZERO(ins_ltp_pos); - INT32_VECT3_ZERO(ins_ltp_speed); - INT32_VECT3_ZERO(ins_ltp_accel); -} - -void ins_periodic( void ) { -} - -#if USE_HFF -void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) { - b2_hff_realign(pos, speed); -} -#else -void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {} -#endif /* USE_HFF */ - - -void ins_realign_v(float z) { - vff_realign(z); -} - -void ins_propagate() { - /* untilt accels */ - struct Int32Vect3 accel_meas_body; - INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel); - struct Int32Vect3 accel_meas_ltp; - INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, *stateGetNedToBodyRMat_i(), accel_meas_body); - - float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z); - if (baro.status == BS_RUNNING && ins_baro_initialised) { - vff_propagate(z_accel_meas_float); - ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); - ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); - ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); - } - else { // feed accel from the sensors - // subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp) - ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); - } - -#if USE_HFF - /* propagate horizontal filter */ - b2_hff_propagate(); -#else - ins_ltp_accel.x = accel_meas_ltp.x; - ins_ltp_accel.y = accel_meas_ltp.y; -#endif /* USE_HFF */ - - INS_NED_TO_STATE(); -} - -void ins_update_baro() { - int32_t baro_pressure = update_median_filter(&baro_median, baro.absolute); - if (baro.status == BS_RUNNING) { - if (!ins_baro_initialised) { - ins_qfe = baro_pressure; - ins_baro_initialised = TRUE; - } - if (ins.vf_realign) { - ins.vf_realign = FALSE; - ins_qfe = baro_pressure; - vff_realign(0.); - ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); - ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); - ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); - } - else { /* not realigning, so normal update with baro measurement */ - ins_baro_alt = ((baro_pressure - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN; - float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt); - vff_update_baro(alt_float); - } - } - INS_NED_TO_STATE(); -} - - -void ins_update_gps(void) { -#if USE_GPS - if (gps.fix == GPS_FIX_3D) { - if (!ins_ltp_initialised) { - ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos); - ins_ltp_def.lla.alt = gps.lla_pos.alt; - ins_ltp_def.hmsl = gps.hmsl; - ins_ltp_initialised = TRUE; - stateSetLocalOrigin_i(&ins_ltp_def); - } - ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos); - ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel); -#if USE_HFF - VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y); - VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.); - VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y); - VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.); - if (ins.hf_realign) { - ins.hf_realign = FALSE; -#ifdef SITL - struct FloatVect2 true_pos, true_speed; - VECT2_COPY(true_pos, fdm.ltpprz_pos); - VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel); - b2_hff_realign(true_pos, true_speed); -#else - const struct FloatVect2 zero = {0.0, 0.0}; - b2_hff_realign(ins_gps_pos_m_ned, zero); -#endif - } - b2_hff_update_gps(); -#endif /* hff used */ - -#if !USE_HFF /* hff not used */ - INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); - INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); -#endif /* hff not used */ - - } - INS_NED_TO_STATE(); -#endif /* USE_GPS */ -} - -//#define INS_SONAR_VARIANCE_THRESHOLD 0.01 - -#ifdef INS_SONAR_VARIANCE_THRESHOLD - -#include "messages.h" -#include "mcu_periph/uart.h" -#include "subsystems/datalink/downlink.h" - -#include "math/pprz_stat.h" -#define VAR_ERR_MAX 10 -float var_err[VAR_ERR_MAX]; -uint8_t var_idx = 0; -#endif - - -void ins_update_sonar() { -#if USE_SONAR - static float last_offset = 0.; - // new value filtered with median_filter - ins_sonar_alt = update_median_filter(&sonar_median, sonar_meas); - float sonar = (ins_sonar_alt - ins_sonar_offset) * INS_SONAR_SENS; - -#ifdef INS_SONAR_VARIANCE_THRESHOLD - /* compute variance of error between sonar and baro alt */ - int32_t err = POS_BFP_OF_REAL(sonar) + ins_baro_alt; // sonar positive up, baro positive down !!!! - var_err[var_idx] = POS_FLOAT_OF_BFP(err); - var_idx = (var_idx + 1) % VAR_ERR_MAX; - float var = variance_float(var_err, VAR_ERR_MAX); - DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&err, &sonar, &var); - //DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&ins_sonar_alt, &sonar, &var); -#endif - - /* update filter assuming a flat ground */ - if (sonar < INS_SONAR_MAX_RANGE -#ifdef INS_SONAR_MIN_RANGE - && sonar > INS_SONAR_MIN_RANGE -#endif -#ifdef INS_SONAR_THROTTLE_THRESHOLD - && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD -#endif -#ifdef INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_ROLL] < INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_ROLL] > -INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_PITCH] < INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_PITCH] > -INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_YAW] < INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_YAW] > -INS_SONAR_STAB_THRESHOLD -#endif -#ifdef INS_SONAR_BARO_THRESHOLD - && ins_baro_alt > -POS_BFP_OF_REAL(INS_SONAR_BARO_THRESHOLD) /* z down */ -#endif -#ifdef INS_SONAR_VARIANCE_THRESHOLD - && var < INS_SONAR_VARIANCE_THRESHOLD -#endif - && ins_update_on_agl - && baro.status == BS_RUNNING) { - vff_update_alt_conf(-sonar, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabs(sonar)); - last_offset = vff_offset; - } - else { - /* update offset with last value to avoid divergence */ - vff_update_offset(last_offset); - } -#endif // USE_SONAR -} - diff --git a/sw/airborne/subsystems/ins/ins_int_extended.h b/sw/airborne/subsystems/ins/ins_int_extended.h deleted file mode 100644 index 6d0ae9b2ec..0000000000 --- a/sw/airborne/subsystems/ins/ins_int_extended.h +++ /dev/null @@ -1,82 +0,0 @@ -/* - * Copyright (C) 2008-2012 The Paparazzi Team - * - * 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 subsystems/ins/ins_int_extended.h - * - * INS for rotorcrafts combining vertical and horizontal filters. - * This filter includes estimation of the baro drift based on sonar. - * - * Vertical filter is always enabled - * - * TODO: use GPS alt if good enough, especially when sonar readings are not available - * - */ - -#ifndef INS_INT_EXTENDED_H -#define INS_INT_EXTENDED_H - -#include "subsystems/ins.h" -#include "std.h" -#include "math/pprz_geodetic_int.h" -#include "math/pprz_algebra_float.h" - -// TODO integrate all internal state to the structure -///** Ins extended implementation state (fixed point) */ -//struct InsIntExtended { -//}; -// -///** global INS state */ -//extern struct InsInt ins_impl; - -/* gps transformed to LTP-NED */ -extern struct LtpDef_i ins_ltp_def; -extern bool_t ins_ltp_initialised; -extern struct NedCoor_i ins_gps_pos_cm_ned; -extern struct NedCoor_i ins_gps_speed_cm_s_ned; - -/* barometer */ -extern int32_t ins_baro_alt; ///< altitude calculated from baro in meters with #INT32_POS_FRAC -extern int32_t ins_qfe; -extern bool_t ins_baro_initialised; -#if USE_SONAR -extern bool_t ins_update_on_agl; /* use sonar to update agl if available */ -extern int32_t ins_sonar_offset; -#endif - -/* output LTP NED */ -extern struct NedCoor_i ins_ltp_pos; -extern struct NedCoor_i ins_ltp_speed; -extern struct NedCoor_i ins_ltp_accel; -#if USE_HFF -/* horizontal gps transformed to NED in meters as float */ -extern struct FloatVect2 ins_gps_pos_m_ned; -extern struct FloatVect2 ins_gps_speed_m_s_ned; -#endif - -/* copy position and speed to state interface */ -#define INS_NED_TO_STATE() { \ - stateSetPositionNed_i(&ins_ltp_pos); \ - stateSetSpeedNed_i(&ins_ltp_speed); \ - stateSetAccelNed_i(&ins_ltp_accel); \ -} - -#endif /* INS_INT_EXTENDED_H */ diff --git a/sw/airborne/subsystems/navigation/common_nav.c b/sw/airborne/subsystems/navigation/common_nav.c index 7e641c05d6..9e2623e3fe 100644 --- a/sw/airborne/subsystems/navigation/common_nav.c +++ b/sw/airborne/subsystems/navigation/common_nav.c @@ -26,6 +26,7 @@ #include "subsystems/navigation/common_nav.h" #include "generated/flight_plan.h" +#include "subsystems/ins.h" #include "subsystems/gps.h" #include "math/pprz_geodetic_float.h" @@ -116,6 +117,10 @@ unit_t nav_reset_reference( void ) { struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); + // realign INS if needed + ins.hf_realign = TRUE; + ins.vf_realign = TRUE; + previous_ground_alt = ground_alt; ground_alt = gps.hmsl/1000.; return 0; diff --git a/sw/airborne/subsystems/sensors/baro.h b/sw/airborne/subsystems/sensors/baro.h index 76dd521dbc..643da7bbef 100644 --- a/sw/airborne/subsystems/sensors/baro.h +++ b/sw/airborne/subsystems/sensors/baro.h @@ -23,31 +23,16 @@ * @file subsystems/sensors/baro.h * * Common barometric sensor implementation. + * Used with baro integrated to the autopilot board. + * Implementation is in boards//baro_board.[ch] * */ #ifndef SUBSYSTEMS_SENSORS_BARO_H #define SUBSYSTEMS_SENSORS_BARO_H -#include - -enum BaroStatus { - BS_UNINITIALIZED, - BS_RUNNING -}; - -/* pressure in which units ? */ -struct Baro { - int32_t absolute; - int32_t differential; - enum BaroStatus status; -}; - -extern struct Baro baro; - - #include BOARD_CONFIG -#if defined BOARD_HAS_BARO +#if USE_BARO_BOARD #include "baro_board.h" #endif diff --git a/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c b/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c deleted file mode 100644 index a3c446e4bd..0000000000 --- a/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c +++ /dev/null @@ -1,120 +0,0 @@ -/* - * Copyright (C) 2011-2013 The Paparazzi Team - * - * 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 subsystems/sensors/baro_ms5611_i2c.c - * - * Driver for MS5611 baro via I2C. - * - */ - -#include "subsystems/sensors/baro.h" -#include "peripherals/ms5611_i2c.h" - -#include "mcu_periph/sys_time.h" -#include "led.h" -#include "std.h" - -#ifndef MS5611_I2C_DEV -#define MS5611_I2C_DEV i2c2 -#endif - -/* default i2c address - * when CSB is set to GND addr is 0xEE - * when CSB is set to VCC addr is 0xEC - * - * Note: Aspirin 2.1 has CSB bound to GND. - */ -#ifndef MS5611_SLAVE_ADDR -#define MS5611_SLAVE_ADDR 0xEE -#endif - -#ifdef DEBUG - -#ifndef DOWNLINK_DEVICE -#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE -#endif -#include "mcu_periph/uart.h" -#include "messages.h" -#include "subsystems/datalink/downlink.h" - -float fbaroms, ftempms; -#endif - -struct Baro baro; -struct Ms5611_I2c baro_ms5611; - - -void baro_init(void) { - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; - - ms5611_i2c_init(&baro_ms5611, &MS5611_I2C_DEV, MS5611_SLAVE_ADDR); -} - -void baro_periodic(void) { - if (sys_time.nb_sec > 1) { - - /* call the convenience periodic that initializes the sensor and starts reading*/ - ms5611_i2c_periodic(&baro_ms5611); - - if (baro_ms5611.initialized) { - baro.status = BS_RUNNING; -#if DEBUG - RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, - &baro_ms5611.data.c[0], - &baro_ms5611.data.c[1], - &baro_ms5611.data.c[2], - &baro_ms5611.data.c[3], - &baro_ms5611.data.c[4], - &baro_ms5611.data.c[5], - &baro_ms5611.data.c[6], - &baro_ms5611.data.c[7])); -#endif - } - } -} - -void baro_event(void (*b_abs_handler)(void)){ - if (sys_time.nb_sec > 1) { - ms5611_i2c_event(&baro_ms5611); - - if (baro_ms5611.data_available) { - baro.absolute = baro_ms5611.data.pressure; - b_abs_handler(); - baro_ms5611.data_available = FALSE; - -#ifdef ROTORCRAFT_BARO_LED - RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED)); -#endif - -#if DEBUG - ftempms = baro_ms5611.data.temperature / 100.; - fbaroms = baro_ms5611.data.pressure / 100.; - DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice, - &baro_ms5611.data.d1, &baro_ms5611.data.d2, - &fbaroms, &ftempms); -#endif - } - } -} diff --git a/sw/airborne/subsystems/sensors/baro_ms5611_spi.c b/sw/airborne/subsystems/sensors/baro_ms5611_spi.c deleted file mode 100644 index bd8603ea43..0000000000 --- a/sw/airborne/subsystems/sensors/baro_ms5611_spi.c +++ /dev/null @@ -1,118 +0,0 @@ -/* - * Copyright (C) 2011-2013 The Paparazzi Team - * - * 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 subsystems/sensors/baro_ms5611_spi.c - * - * Driver for MS5611 baro on LisaM/Aspirin2.2 via SPI. - * - */ - -#include "subsystems/sensors/baro.h" -#include "peripherals/ms5611_spi.h" - -#include "mcu_periph/sys_time.h" -#include "led.h" -#include "std.h" - -#ifndef MS5611_SPI_DEV -#define MS5611_SPI_DEV spi2 -#endif - -/* SPI SLAVE3 is on pin PC13 - * Aspirin 2.2 has ms5611 on SPI bus - */ -#ifndef MS5611_SLAVE_DEV -#define MS5611_SLAVE_DEV SPI_SLAVE3 -#endif - - -#ifdef DEBUG - -#ifndef DOWNLINK_DEVICE -#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE -#endif -#include "mcu_periph/uart.h" -#include "messages.h" -#include "subsystems/datalink/downlink.h" - -float fbaroms, ftempms; -#endif - -struct Baro baro; -struct Ms5611_Spi baro_ms5611; - - -void baro_init(void) { - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; - - ms5611_spi_init(&baro_ms5611, &MS5611_SPI_DEV, MS5611_SLAVE_DEV); -} - -void baro_periodic(void) { - if (sys_time.nb_sec > 1) { - - /* call the convenience periodic that initializes the sensor and starts reading*/ - ms5611_spi_periodic(&baro_ms5611); - - if (baro_ms5611.initialized) { - baro.status = BS_RUNNING; -#if DEBUG - RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, - &baro_ms5611.data.c[0], - &baro_ms5611.data.c[1], - &baro_ms5611.data.c[2], - &baro_ms5611.data.c[3], - &baro_ms5611.data.c[4], - &baro_ms5611.data.c[5], - &baro_ms5611.data.c[6], - &baro_ms5611.data.c[7])); -#endif - } - } -} - -void baro_event(void (*b_abs_handler)(void)){ - if (sys_time.nb_sec > 1) { - ms5611_spi_event(&baro_ms5611); - - if (baro_ms5611.data_available) { - baro.absolute = baro_ms5611.data.pressure; - b_abs_handler(); - baro_ms5611.data_available = FALSE; - -#ifdef ROTORCRAFT_BARO_LED - RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED)); -#endif - -#if DEBUG - ftempms = baro_ms5611.data.temperature / 100.; - fbaroms = baro_ms5611.data.pressure / 100.; - DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice, - &baro_ms5611.data.d1, &baro_ms5611.data.d2, - &fbaroms, &ftempms); -#endif - } - } -} diff --git a/sw/simulator/nps/nps_autopilot.h b/sw/simulator/nps/nps_autopilot.h index 730c46cce0..04d8304d0b 100644 --- a/sw/simulator/nps/nps_autopilot.h +++ b/sw/simulator/nps/nps_autopilot.h @@ -11,10 +11,15 @@ struct NpsAutopilot { extern struct NpsAutopilot autopilot; +extern bool_t nps_bypass_ahrs; +extern bool_t nps_bypass_ins; +extern void sim_overwrite_ahrs(void); +extern void sim_overwrite_ins(void); + extern void nps_autopilot_init(enum NpsRadioControlType type, int num_script, char* js_dev); extern void nps_autopilot_run_step(double time); extern void nps_autopilot_run_systime_step(void); + #endif /* NPS_AUTOPILOT_H */ - diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c index 27826df4e3..409a0c3a43 100644 --- a/sw/simulator/nps/nps_autopilot_fixedwing.c +++ b/sw/simulator/nps/nps_autopilot_fixedwing.c @@ -20,7 +20,7 @@ * Boston, MA 02111-1307, USA. */ -#include "nps_autopilot_fixedwing.h" +#include "nps_autopilot.h" #ifdef FBW #include "firmwares/fixedwing/main_fbw.h" @@ -50,14 +50,19 @@ #include "subsystems/commands.h" - struct NpsAutopilot autopilot; bool_t nps_bypass_ahrs; +bool_t nps_bypass_ins; #ifndef NPS_BYPASS_AHRS #define NPS_BYPASS_AHRS FALSE #endif +#ifndef NPS_BYPASS_INS +#define NPS_BYPASS_INS FALSE +#endif + + #if !defined (FBW) || !defined (AP) #error NPS does not currently support dual processor simulation for FBW and AP on fixedwing! #endif @@ -68,6 +73,7 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha nps_electrical_init(); nps_bypass_ahrs = NPS_BYPASS_AHRS; + nps_bypass_ins = NPS_BYPASS_INS; Fbw(init); Ap(init); @@ -105,8 +111,7 @@ void nps_autopilot_run_step(double time) { } if (nps_sensors_baro_available()) { - /** @todo feed baro values */ - //baro_feed_value(sensors.baro.value); + baro_feed_value(sensors.baro.value); Fbw(event_task); Ap(event_task); } @@ -121,6 +126,10 @@ void nps_autopilot_run_step(double time) { sim_overwrite_ahrs(); } + if (nps_bypass_ins) { + sim_overwrite_ins(); + } + Fbw(handle_periodic_tasks); Ap(handle_periodic_tasks); @@ -141,3 +150,19 @@ void sim_overwrite_ahrs(void) { stateSetBodyRates_f(&rates_f); } + +void sim_overwrite_ins(void) { + + struct NedCoor_f ltp_pos; + VECT3_COPY(ltp_pos, fdm.ltpprz_pos); + stateSetPositionNed_f(<p_pos); + + struct NedCoor_f ltp_speed; + VECT3_COPY(ltp_speed, fdm.ltpprz_ecef_vel); + stateSetSpeedNed_f(<p_speed); + + struct NedCoor_f ltp_accel; + VECT3_COPY(ltp_accel, fdm.ltpprz_ecef_accel); + stateSetAccelNed_f(<p_accel); + +} diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.h b/sw/simulator/nps/nps_autopilot_fixedwing.h deleted file mode 100644 index a4d05a199b..0000000000 --- a/sw/simulator/nps/nps_autopilot_fixedwing.h +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef NPS_AUTOPILOT_FIXEDWING_H -#define NPS_AUTOPILOT_FIXEDWING_H - -#include "nps_autopilot.h" - - -extern bool_t nps_bypass_ahrs; -extern void sim_overwrite_ahrs(void); - -#endif /* NPS_AUTOPILOT_FIXEDWING_H */ diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c index 6641cc5dc8..681d5bf5d8 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.c +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c @@ -19,7 +19,7 @@ * Boston, MA 02111-1307, USA. */ -#include "nps_autopilot_rotorcraft.h" +#include "nps_autopilot.h" #include "firmwares/rotorcraft/main.h" #include "nps_sensors.h" diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.h b/sw/simulator/nps/nps_autopilot_rotorcraft.h deleted file mode 100644 index b71ffd7eb8..0000000000 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.h +++ /dev/null @@ -1,12 +0,0 @@ -#ifndef NPS_AUTOPILOT_ROTORCRAFT_H -#define NPS_AUTOPILOT_ROTORCRAFT_H - -#include "nps_autopilot.h" - - -extern bool_t nps_bypass_ahrs; -extern bool_t nps_bypass_ins; -extern void sim_overwrite_ahrs(void); -extern void sim_overwrite_ins(void); - -#endif /* NPS_AUTOPILOT_ROTORCRAFT_H */ diff --git a/sw/simulator/nps/nps_ivy_rotorcraft.c b/sw/simulator/nps/nps_ivy_rotorcraft.c index bf0ef5562b..afb4764d7d 100644 --- a/sw/simulator/nps/nps_ivy_rotorcraft.c +++ b/sw/simulator/nps/nps_ivy_rotorcraft.c @@ -8,8 +8,8 @@ #include "nps_autopilot.h" #include "nps_fdm.h" #include "nps_sensors.h" -#include "subsystems/ins.h" #include "firmwares/rotorcraft/navigation.h" +#include "state.h" #ifdef RADIO_CONTROL_TYPE_DATALINK #include "subsystems/radio_control.h" @@ -31,7 +31,6 @@ void nps_ivy_init(char* ivy_bus) { } -//TODO use datalink parsing from rotorcraft instead of doing it here explicitly #include "generated/settings.h" #include "dl_protocol.h" @@ -43,16 +42,13 @@ static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), uint8_t wp_id = atoi(argv[1]); struct LlaCoor_i lla; - struct EnuCoor_i enu; lla.lat = INT32_RAD_OF_DEG(atoi(argv[3])); lla.lon = INT32_RAD_OF_DEG(atoi(argv[4])); - lla.alt = atoi(argv[5])*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt; - enu_of_lla_point_i(&enu,&ins_ltp_def,&lla); - enu.x = POS_BFP_OF_REAL(enu.x)/100; - enu.y = POS_BFP_OF_REAL(enu.y)/100; - enu.z = POS_BFP_OF_REAL(enu.z)/100; - VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z); - DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z); - printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, enu.x, enu.y, enu.z); + /* WP_alt from message is alt above MSL in cm + * lla.alt is above ellipsoid in mm + */ + lla.alt = atoi(argv[5]) *10 - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt; + nav_move_waypoint_lla(wp_id, &lla); + printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, waypoints[wp_id].x, waypoints[wp_id].y, waypoints[wp_id].z); } } diff --git a/sw/simulator/nps/nps_sensor_baro.c b/sw/simulator/nps/nps_sensor_baro.c index f0fdce2686..22ca9f9a4c 100644 --- a/sw/simulator/nps/nps_sensor_baro.c +++ b/sw/simulator/nps/nps_sensor_baro.c @@ -1,6 +1,7 @@ #include "nps_sensor_baro.h" #include "generated/airframe.h" +#include "math/pprz_isa.h" #include "std.h" #include "nps_fdm.h" @@ -21,15 +22,10 @@ void nps_sensor_baro_run_step(struct NpsSensorBaro* baro, double time) { if (time < baro->next_update) return; - /*if (time < 10.) - baro->value = rint(time*90); - else {*/ - double z = fdm.ltpprz_pos.z + get_gaussian_noise()*NPS_BARO_NOISE_STD_DEV; - double baro_reading = NPS_BARO_QNH + z * NPS_BARO_SENSITIVITY; - baro_reading = rint(baro_reading); - baro->value = baro_reading; - Bound(baro->value, 0, 1024); - //} + /* pressure in Pascal */ + baro->value = pprz_isa_pressure_of_altitude(fdm.hmsl); + /* add noise with std dev Pascal */ + baro->value += get_gaussian_noise() * NPS_BARO_NOISE_STD_DEV; baro->next_update += NPS_BARO_DT; baro->data_available = TRUE; diff --git a/sw/simulator/nps/nps_sensor_baro.h b/sw/simulator/nps/nps_sensor_baro.h index 06630b066d..0a07c4a04c 100644 --- a/sw/simulator/nps/nps_sensor_baro.h +++ b/sw/simulator/nps/nps_sensor_baro.h @@ -7,7 +7,7 @@ #include "std.h" struct NpsSensorBaro { - double value; + double value; ///< pressure in Pascal double next_update; bool_t data_available; }; diff --git a/sw/tools/gen_abi.ml b/sw/tools/gen_abi.ml index 2b2eb95a10..d8b5010a58 100644 --- a/sw/tools/gen_abi.ml +++ b/sw/tools/gen_abi.ml @@ -92,17 +92,17 @@ module Gen_onboard = struct let print_struct = fun h size -> Printf.fprintf h "\n/* Array and linked list structure */\n"; Printf.fprintf h "#define ABI_MESSAGE_NB %d\n\n" (size+1); - Printf.fprintf h "EXTERN abi_event* abi_queues[ABI_MESSAGE_NB];\n" + Printf.fprintf h "ABI_EXTERN abi_event* abi_queues[ABI_MESSAGE_NB];\n" (* Print arguments' function from fields *) - let print_args = fun h fields starter -> + let print_args = fun h fields -> let rec args = fun h l -> match l with [] -> Printf.fprintf h ")" - | [(n,t)] -> Printf.fprintf h "const %s %s)" t n - | (n,t)::l' -> Printf.fprintf h "const %s %s, " t n; args h l' + | [(n,t)] -> Printf.fprintf h ", const %s * %s)" t n + | (n,t)::l' -> Printf.fprintf h ", const %s * %s" t n; args h l' in - Printf.fprintf h "(%s" starter; + Printf.fprintf h "(uint8_t sender_id"; args h fields (* Print callbacks prototypes for all messages *) @@ -110,7 +110,7 @@ module Gen_onboard = struct Printf.fprintf h "\n/* Callbacks */\n"; List.iter (fun msg -> Printf.fprintf h "typedef void (*abi_callback%s)" (String.capitalize msg.name); - print_args h msg.fields ""; + print_args h msg.fields; Printf.fprintf h ";\n"; ) messages @@ -129,18 +129,18 @@ module Gen_onboard = struct let rec args = fun h l -> match l with [] -> Printf.fprintf h ");\n" - | [(n,_)] -> Printf.fprintf h "%s);\n" n - | (n,_)::l' -> Printf.fprintf h "%s, " n; args h l' + | [(n,_)] -> Printf.fprintf h ", %s);\n" n + | (n,_)::l' -> Printf.fprintf h ", %s" n; args h l' in let name = String.capitalize msg.name in Printf.fprintf h "\nstatic inline void AbiSendMsg%s" name; - print_args h msg.fields "uint8_t sender_id, "; + print_args h msg.fields; Printf.fprintf h " {\n"; Printf.fprintf h " abi_event* e;\n"; Printf.fprintf h " ABI_FOREACH(abi_queues[ABI_%s_ID],e) {\n" name; Printf.fprintf h " if (e->id == ABI_BROADCAST || e->id == sender_id) {\n"; Printf.fprintf h " abi_callback%s cb = (abi_callback%s)(e->cb);\n" name name; - Printf.fprintf h " cb("; + Printf.fprintf h " cb(sender_id"; args h msg.fields; Printf.fprintf h " };\n"; Printf.fprintf h " };\n";