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/README.md b/README.md index 86ced4a95d..4cd013f77b 100644 --- a/README.md +++ b/README.md @@ -18,11 +18,12 @@ Installation is described in the wiki (http://paparazzi.enac.fr/wiki/Installatio For Ubuntu users, required packages are available in the [paparazzi-uav PPA] (https://launchpad.net/~paparazzi-uav/+archive/ppa), Debian users can use http://paparazzi.enac.fr/debian - +Debian/Ubuntu packages: - **paparazzi-dev** is the meta-package that depends on everything needed to compile and run the ground segment and the simulator. -- **paparazzi-arm-multilib** ARM cross-compiling toolchain for LPC21 and STM32 based boards. - **paparazzi-jsbsim** is needed for using JSBSim as flight dynamic model for the simulator. +Recommended cross compiling toolchain: https://launchpad.net/gcc-arm-embedded + Directories quick and dirty description: ---------------------------------------- diff --git a/conf/Makefile.omap b/conf/Makefile.omap index e567d0c314..9cc359d7cd 100644 --- a/conf/Makefile.omap +++ b/conf/Makefile.omap @@ -81,6 +81,38 @@ load upload program: $(OBJDIR)/$(TARGET).elf # Kill the application -echo "killall -9 $(TARGET).elf" | telnet $(HOST) + # Make the target dir and edit the config + -{ \ + echo "mkdir -p $(TARGET_DIR)"; \ + } | telnet $(HOST) + + # Upload the drivers and new application + { \ + echo "binary"; \ + echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/cdc-acm.ko /$(SUB_DIR)/cdc-acm.ko"; \ + echo "put $(OBJDIR)/$(TARGET).elf /$(SUB_DIR)/$(TARGET).elf"; \ + echo "quit"; \ + } | ftp -n $(HOST) + + # Upload the modules and start the application + -{ \ + echo "insmod $(TARGET_DIR)/cdc-acm.ko"; \ + echo "chmod 777 $(TARGET_DIR)/$(TARGET).elf"; \ + echo "$(TARGET_DIR)/$(TARGET).elf > /dev/null 2>&1 &"; \ + } | telnet $(HOST) + +ifeq ($(ARDRONE2_REBOOT),1) + -{ \ + echo "reboot"; \ + } | telnet $(HOST) +endif + +# Program the device and start it. +load2 upload2 program2: $(OBJDIR)/$(TARGET).elf + + # Kill the application + -echo "killall -9 $(TARGET).elf" | telnet $(HOST) + # Make the target dir and edit the config -{ \ echo "mkdir -p $(TARGET_DIR)"; \ @@ -111,7 +143,7 @@ load upload program: $(OBJDIR)/$(TARGET).elf echo "chmod 777 $(TARGET_DIR)/$(TARGET).elf"; \ echo "$(TARGET_DIR)/$(TARGET).elf > /dev/null 2>&1 &"; \ } | telnet $(HOST) - + ifeq ($(ARDRONE2_REBOOT),1) -{ \ echo "reboot"; \ @@ -119,6 +151,8 @@ ifeq ($(ARDRONE2_REBOOT),1) endif + + # Link: create ELF output file from object files. .SECONDARY : $(OBJDIR)/$(TARGET).elf .PRECIOUS : $(OBJ_C_OMAP) $(OBJ_CPP_OMAP) @@ -140,6 +174,7 @@ $(OBJDIR)/%.o : %.cpp $(OBJDIR)/../Makefile.ac $(Q)test -d $(dir $@) || mkdir -p $(dir $@) $(Q)$(CXX) -c $(CXXFLAGS) $< -o $@ + # Listing of phony targets. .PHONY : all build elf clean clean_list 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..b5ca327b07 100644 --- a/conf/airframes/CDW/asctec_cdw.xml +++ b/conf/airframes/CDW/asctec_cdw.xml @@ -124,7 +124,6 @@
-
@@ -182,10 +181,6 @@ - - - - diff --git a/conf/airframes/CDW/tricopter_cdw.xml b/conf/airframes/CDW/tricopter_cdw.xml index 86946f11f3..3585eac907 100644 --- a/conf/airframes/CDW/tricopter_cdw.xml +++ b/conf/airframes/CDW/tricopter_cdw.xml @@ -121,7 +121,6 @@ LiPo/LiIo-Zellen: 2-3
-
@@ -179,10 +178,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..c1a5406c3d 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -142,7 +142,6 @@
- @@ -225,9 +224,7 @@ - -
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index 834132e720..1d1a729e9a 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -177,7 +177,6 @@
- @@ -199,9 +198,7 @@ - -
diff --git a/conf/airframes/ENAC/quadrotor/hen1.xml b/conf/airframes/ENAC/quadrotor/hen1.xml index 2c0bdb49ae..2b5fa5b811 100644 --- a/conf/airframes/ENAC/quadrotor/hen1.xml +++ b/conf/airframes/ENAC/quadrotor/hen1.xml @@ -127,7 +127,6 @@
-
@@ -205,9 +204,7 @@ - -
diff --git a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml index cf2739449c..0542c01425 100644 --- a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml +++ b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml @@ -174,10 +174,6 @@ - - - -
diff --git a/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml index 7a870f3b9c..4ce0305ecf 100644 --- a/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml @@ -145,7 +145,6 @@
-
@@ -157,10 +156,6 @@ - - - -
diff --git a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml index 064c01ae5a..6e5b38afe9 100644 --- a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml @@ -147,7 +147,6 @@
-
@@ -159,10 +158,6 @@ - - - -
diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml index 2ee2d7548e..f28c89cdd5 100644 --- a/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml +++ b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml @@ -129,7 +129,6 @@
-
@@ -141,10 +140,6 @@ - - - -
diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml index 4e7dda0a2e..d06a6cbdee 100644 --- a/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml +++ b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml @@ -134,7 +134,6 @@
-
@@ -146,10 +145,6 @@ - - - -
diff --git a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml index ee9e0848ac..af754f073d 100644 --- a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml +++ b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml @@ -221,7 +221,6 @@
-
@@ -234,10 +233,6 @@ - - - -
diff --git a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml index 06f2f97307..a474774180 100644 --- a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml +++ b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml @@ -221,7 +221,6 @@
-
@@ -234,10 +233,6 @@ - - - -
diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index 99265a1638..4378808f12 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -5,7 +5,6 @@ - @@ -20,15 +19,14 @@ + - - @@ -39,7 +37,7 @@ - + @@ -55,11 +53,12 @@ + - - - - + + + + @@ -71,40 +70,45 @@
- - - + + + + - - - - - - - - - + + + + + + + + + + + +
- - +
- -
- +
- + + + + + +
-
@@ -132,7 +136,6 @@
-
@@ -195,10 +198,13 @@
+ + + - +
@@ -221,5 +227,4 @@
- diff --git a/conf/airframes/ardrone2_sdk.xml b/conf/airframes/ardrone2_sdk.xml index 73c21af476..792c0bc74f 100644 --- a/conf/airframes/ardrone2_sdk.xml +++ b/conf/airframes/ardrone2_sdk.xml @@ -81,10 +81,6 @@ - - - - diff --git a/conf/airframes/esden/cocto_lm2a2.xml b/conf/airframes/esden/cocto_lm2a2.xml index 609e7295bd..692ba0ecb6 100644 --- a/conf/airframes/esden/cocto_lm2a2.xml +++ b/conf/airframes/esden/cocto_lm2a2.xml @@ -170,7 +170,6 @@ B2L -> CW
-
@@ -182,10 +181,6 @@ B2L -> CW - - - -
diff --git a/conf/airframes/esden/gain_scheduling_example.xml b/conf/airframes/esden/gain_scheduling_example.xml index 02f574bd9f..c169f8c314 100644 --- a/conf/airframes/esden/gain_scheduling_example.xml +++ b/conf/airframes/esden/gain_scheduling_example.xml @@ -153,7 +153,6 @@
-
@@ -165,8 +164,6 @@ - -
diff --git a/conf/airframes/esden/hexy_ll11a2pwm.xml b/conf/airframes/esden/hexy_ll11a2pwm.xml index f796f42c68..c7cc5ce652 100644 --- a/conf/airframes/esden/hexy_ll11a2pwm.xml +++ b/conf/airframes/esden/hexy_ll11a2pwm.xml @@ -167,7 +167,6 @@
-
@@ -179,10 +178,6 @@ - - - -
diff --git a/conf/airframes/esden/hexy_lm2a2pwm.xml b/conf/airframes/esden/hexy_lm2a2pwm.xml index 73554f3209..7eb3cd3420 100644 --- a/conf/airframes/esden/hexy_lm2a2pwm.xml +++ b/conf/airframes/esden/hexy_lm2a2pwm.xml @@ -128,7 +128,6 @@
-
@@ -140,10 +139,6 @@ - - - -
diff --git a/conf/airframes/esden/lisa2_hex.xml b/conf/airframes/esden/lisa2_hex.xml index b21dddffcb..a8c786cc69 100644 --- a/conf/airframes/esden/lisa2_hex.xml +++ b/conf/airframes/esden/lisa2_hex.xml @@ -135,7 +135,6 @@
-
@@ -147,10 +146,6 @@ - - - -
diff --git a/conf/airframes/esden/qs_asp22.xml b/conf/airframes/esden/qs_asp22.xml index c4ef62b938..120cb296c8 100644 --- a/conf/airframes/esden/qs_asp22.xml +++ b/conf/airframes/esden/qs_asp22.xml @@ -161,7 +161,6 @@
-
@@ -173,8 +172,6 @@ - -
diff --git a/conf/airframes/esden/quady_ll11a2pwm.xml b/conf/airframes/esden/quady_ll11a2pwm.xml index ca80e5a5d2..d4337f236c 100644 --- a/conf/airframes/esden/quady_ll11a2pwm.xml +++ b/conf/airframes/esden/quady_ll11a2pwm.xml @@ -163,7 +163,6 @@
-
@@ -175,10 +174,6 @@ - - - -
diff --git a/conf/airframes/esden/quady_lm1a1pwm.xml b/conf/airframes/esden/quady_lm1a1pwm.xml index 406734e757..517dad5dd2 100644 --- a/conf/airframes/esden/quady_lm1a1pwm.xml +++ b/conf/airframes/esden/quady_lm1a1pwm.xml @@ -124,7 +124,6 @@
-
@@ -136,10 +135,6 @@ - - - -
diff --git a/conf/airframes/esden/quady_lm2a2pwm.xml b/conf/airframes/esden/quady_lm2a2pwm.xml index e87c817b3e..b23eb2c303 100644 --- a/conf/airframes/esden/quady_lm2a2pwm.xml +++ b/conf/airframes/esden/quady_lm2a2pwm.xml @@ -127,7 +127,6 @@
-
@@ -139,10 +138,6 @@ - - - -
diff --git a/conf/airframes/esden/quady_lm2a2pwmppm.xml b/conf/airframes/esden/quady_lm2a2pwmppm.xml index 31fd65c39d..bb11c56f1d 100644 --- a/conf/airframes/esden/quady_lm2a2pwmppm.xml +++ b/conf/airframes/esden/quady_lm2a2pwmppm.xml @@ -124,7 +124,6 @@
-
@@ -136,10 +135,6 @@ - - - -
diff --git a/conf/airframes/esden/quady_ls01pwm.xml b/conf/airframes/esden/quady_ls01pwm.xml index 4a3a194c39..00b255d544 100644 --- a/conf/airframes/esden/quady_ls01pwm.xml +++ b/conf/airframes/esden/quady_ls01pwm.xml @@ -127,7 +127,6 @@
-
@@ -139,10 +138,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..72a05e09bc 100644 --- a/conf/airframes/examples/booz2.xml +++ b/conf/airframes/examples/booz2.xml @@ -148,7 +148,6 @@
-
@@ -160,10 +159,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..8bb83004de 100644 --- a/conf/airframes/examples/h_hex.xml +++ b/conf/airframes/examples/h_hex.xml @@ -147,7 +147,6 @@
-
@@ -159,10 +158,6 @@ - - - -
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml index 44f2681826..6de9a1cd82 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml @@ -112,14 +112,13 @@
- +
-
@@ -206,9 +205,7 @@ - -
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml index 4bf55eeb9f..a01191d274 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml @@ -115,14 +115,13 @@
- +
-
@@ -209,9 +208,7 @@ - -
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml index ebdda35837..ffdede1e73 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml @@ -24,8 +24,6 @@ - - @@ -105,14 +103,14 @@
- + +
-
@@ -145,35 +143,35 @@ - + - - - - + + + + - - - - + + + + - + - - + + - - - + + + - - - + + + @@ -182,7 +180,7 @@ - +
@@ -191,16 +189,14 @@ - - - - - - + + + + + + + - - -
@@ -209,9 +205,8 @@ - - +
@@ -229,8 +224,8 @@
- - + +
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml index cf85a484c0..4a3944ecb8 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml @@ -101,14 +101,13 @@
- +
-
@@ -194,9 +193,7 @@ - -
diff --git a/conf/airframes/examples/lisa_asctec.xml b/conf/airframes/examples/lisa_asctec.xml index 5e4869cb87..1440ea0781 100644 --- a/conf/airframes/examples/lisa_asctec.xml +++ b/conf/airframes/examples/lisa_asctec.xml @@ -146,7 +146,6 @@
-
@@ -158,10 +157,6 @@ - - - -
diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml index 4d68ce1534..b332a0c730 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -35,7 +35,7 @@ - + @@ -102,7 +102,6 @@
-
@@ -184,16 +183,13 @@ - - - -
+ diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml index 9f7210bd3e..136a0b1d02 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml @@ -104,7 +104,6 @@
-
@@ -187,10 +186,6 @@ - - - -
diff --git a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml index 4746f8f78d..016537f4f4 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml @@ -94,7 +94,6 @@
-
@@ -176,10 +175,6 @@ - - - -
diff --git a/conf/airframes/examples/quadrotor_lisa_s.xml b/conf/airframes/examples/quadrotor_lisa_s.xml index bc0ffa17be..c963633c5e 100644 --- a/conf/airframes/examples/quadrotor_lisa_s.xml +++ b/conf/airframes/examples/quadrotor_lisa_s.xml @@ -147,7 +147,6 @@
-
@@ -159,10 +158,6 @@ - - - -
diff --git a/conf/airframes/examples/quadrotor_mlkf.xml b/conf/airframes/examples/quadrotor_mlkf.xml index 781cbd771a..5c264fc3a7 100644 --- a/conf/airframes/examples/quadrotor_mlkf.xml +++ b/conf/airframes/examples/quadrotor_mlkf.xml @@ -27,9 +27,7 @@ - - - + @@ -100,7 +98,6 @@
-
@@ -186,10 +183,6 @@ - - - -
diff --git a/conf/airframes/examples/quadrotor_navgo.xml b/conf/airframes/examples/quadrotor_navgo.xml index 98df961abd..3316eec5ff 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 @@
- @@ -208,9 +208,7 @@ - -
diff --git a/conf/airframes/examples/quadrotor_px4fmu.xml b/conf/airframes/examples/quadrotor_px4fmu.xml index fb02773791..b189c2c8e6 100644 --- a/conf/airframes/examples/quadrotor_px4fmu.xml +++ b/conf/airframes/examples/quadrotor_px4fmu.xml @@ -98,7 +98,6 @@
-
@@ -180,10 +179,6 @@ - - - -
diff --git a/conf/airframes/examples/quadshot_asp21_spektrum.xml b/conf/airframes/examples/quadshot_asp21_spektrum.xml index d5fc8439fd..d1bbc5525d 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 -->
-
@@ -197,8 +196,6 @@ More information on the Quadshot can be found at transition-robotics.com --> - - diff --git a/conf/airframes/examples/stm32f4_discovery_test.xml b/conf/airframes/examples/stm32f4_discovery_test.xml index ed339e5a4a..6735c16ec3 100644 --- a/conf/airframes/examples/stm32f4_discovery_test.xml +++ b/conf/airframes/examples/stm32f4_discovery_test.xml @@ -24,20 +24,18 @@ - + - + + - - - @@ -100,7 +98,6 @@
-
@@ -182,10 +179,6 @@ - - - -
diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index a347658ee8..9139db0e35 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -30,7 +30,7 @@ - + @@ -138,7 +138,6 @@
-
@@ -220,16 +219,13 @@ - - - -
- + + diff --git a/conf/airframes/obsolete/ENAC/g1_vision.xml b/conf/airframes/obsolete/ENAC/g1_vision.xml index d24b588c2e..988b66025f 100644 --- a/conf/airframes/obsolete/ENAC/g1_vision.xml +++ b/conf/airframes/obsolete/ENAC/g1_vision.xml @@ -130,7 +130,6 @@
-
@@ -156,9 +155,7 @@ - - diff --git a/conf/airframes/obsolete/ENAC/mkk1-vision.xml b/conf/airframes/obsolete/ENAC/mkk1-vision.xml index 4d78dbd80b..dc44ffc4bc 100644 --- a/conf/airframes/obsolete/ENAC/mkk1-vision.xml +++ b/conf/airframes/obsolete/ENAC/mkk1-vision.xml @@ -163,7 +163,6 @@
-
@@ -189,10 +188,6 @@ - - - - diff --git a/conf/airframes/obsolete/ENAC/mkk1.xml b/conf/airframes/obsolete/ENAC/mkk1.xml index 18a9742bc7..e1b71b9eb3 100644 --- a/conf/airframes/obsolete/ENAC/mkk1.xml +++ b/conf/airframes/obsolete/ENAC/mkk1.xml @@ -193,7 +193,6 @@
-
@@ -209,10 +208,6 @@ - - - -
diff --git a/conf/airframes/obsolete/ENAC/nova1.xml b/conf/airframes/obsolete/ENAC/nova1.xml index 0ef106ba39..7824e3e3a3 100644 --- a/conf/airframes/obsolete/ENAC/nova1.xml +++ b/conf/airframes/obsolete/ENAC/nova1.xml @@ -164,7 +164,6 @@
-
@@ -180,10 +179,6 @@ - - - -
diff --git a/conf/airframes/NoVa_L.xml b/conf/airframes/obsolete/NoVa_L.xml similarity index 97% rename from conf/airframes/NoVa_L.xml rename to conf/airframes/obsolete/NoVa_L.xml index aaf45e3e27..890f626200 100644 --- a/conf/airframes/NoVa_L.xml +++ b/conf/airframes/obsolete/NoVa_L.xml @@ -189,7 +189,6 @@
-
@@ -208,10 +207,6 @@ --> - - - -
diff --git a/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml b/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml index 0c2cfe06c0..3a16d424a6 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 @@
-
@@ -164,9 +163,7 @@ - - diff --git a/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml b/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml index 6d60135e55..6c37dde48e 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 @@
-
@@ -182,9 +181,7 @@ - - diff --git a/conf/airframes/obsolete/Poine/booz2_a1.xml b/conf/airframes/obsolete/Poine/booz2_a1.xml index 5823e29e03..c57175d1ca 100644 --- a/conf/airframes/obsolete/Poine/booz2_a1.xml +++ b/conf/airframes/obsolete/Poine/booz2_a1.xml @@ -142,7 +142,6 @@
-
@@ -154,10 +153,6 @@ - - - -
diff --git a/conf/airframes/obsolete/Poine/booz2_a7.xml b/conf/airframes/obsolete/Poine/booz2_a7.xml index d3f23c31fd..ebaa87a22b 100644 --- a/conf/airframes/obsolete/Poine/booz2_a7.xml +++ b/conf/airframes/obsolete/Poine/booz2_a7.xml @@ -161,7 +161,6 @@
-
@@ -180,10 +179,6 @@ - - - -
diff --git a/conf/airframes/obsolete/Poine/h_hex.xml b/conf/airframes/obsolete/Poine/h_hex.xml index 252cdacf33..3551f55043 100644 --- a/conf/airframes/obsolete/Poine/h_hex.xml +++ b/conf/airframes/obsolete/Poine/h_hex.xml @@ -128,7 +128,6 @@
-
@@ -140,10 +139,6 @@ - - - -
diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml b/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml index 10b61e41a0..70be3d2a50 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml @@ -216,7 +216,6 @@ second attempt
-
@@ -228,10 +227,6 @@ second attempt - - - -
diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml b/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml index 5f3ee2a945..ead16fa04c 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml @@ -153,7 +153,6 @@
-
@@ -165,10 +164,6 @@ - - - -
diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml b/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml index 406d6de2c0..aaae07223d 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml @@ -171,7 +171,6 @@
-
@@ -189,9 +188,7 @@ - - diff --git a/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml b/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml index 3328cc4b18..03d5cc45d3 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 @@
-
@@ -189,9 +188,7 @@ - - diff --git a/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml b/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml index 82f499852b..abdc4df4dd 100644 --- a/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml +++ b/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml @@ -217,7 +217,6 @@ second attempt
-
@@ -229,10 +228,6 @@ second attempt - - - -
diff --git a/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml b/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml index 0cecb4fdb2..cfccc53017 100644 --- a/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml +++ b/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml @@ -132,7 +132,6 @@
-
@@ -144,10 +143,6 @@ - - - -
diff --git a/conf/airframes/obsolete/booz2_Aron.xml b/conf/airframes/obsolete/booz2_Aron.xml index a5a48f5295..426c2a1991 100644 --- a/conf/airframes/obsolete/booz2_Aron.xml +++ b/conf/airframes/obsolete/booz2_Aron.xml @@ -134,7 +134,6 @@
-
@@ -149,9 +148,7 @@ - -
diff --git a/conf/airframes/obsolete/booz2_NoVa.xml b/conf/airframes/obsolete/booz2_NoVa.xml index aa12a6dfdd..11aa351569 100644 --- a/conf/airframes/obsolete/booz2_NoVa.xml +++ b/conf/airframes/obsolete/booz2_NoVa.xml @@ -171,7 +171,6 @@
-
@@ -189,9 +188,7 @@ - - diff --git a/conf/airframes/obsolete/booz2_NoVa_001.xml b/conf/airframes/obsolete/booz2_NoVa_001.xml index 81c9021e83..8b303bb013 100644 --- a/conf/airframes/obsolete/booz2_NoVa_001.xml +++ b/conf/airframes/obsolete/booz2_NoVa_001.xml @@ -171,7 +171,6 @@
-
@@ -189,9 +188,7 @@ - - diff --git a/conf/airframes/obsolete/booz2_NoVa_002.xml b/conf/airframes/obsolete/booz2_NoVa_002.xml index ed879fb742..d0e286b964 100644 --- a/conf/airframes/obsolete/booz2_NoVa_002.xml +++ b/conf/airframes/obsolete/booz2_NoVa_002.xml @@ -171,7 +171,6 @@
-
@@ -189,9 +188,7 @@ - - diff --git a/conf/airframes/obsolete/booz2_a1p.xml b/conf/airframes/obsolete/booz2_a1p.xml index 18ff3d98f8..743dd891d9 100644 --- a/conf/airframes/obsolete/booz2_a1p.xml +++ b/conf/airframes/obsolete/booz2_a1p.xml @@ -146,7 +146,6 @@
-
@@ -158,10 +157,6 @@ - - - -
diff --git a/conf/airframes/obsolete/booz2_a2.xml b/conf/airframes/obsolete/booz2_a2.xml index d0f9361638..88ffa59789 100644 --- a/conf/airframes/obsolete/booz2_a2.xml +++ b/conf/airframes/obsolete/booz2_a2.xml @@ -134,7 +134,6 @@
-
@@ -149,9 +148,7 @@ - -
diff --git a/conf/airframes/obsolete/booz2_a3.xml b/conf/airframes/obsolete/booz2_a3.xml index 0635e3ce89..c234c2b1a7 100644 --- a/conf/airframes/obsolete/booz2_a3.xml +++ b/conf/airframes/obsolete/booz2_a3.xml @@ -121,7 +121,6 @@
-
@@ -134,9 +133,7 @@ - -
diff --git a/conf/airframes/obsolete/booz2_a4.xml b/conf/airframes/obsolete/booz2_a4.xml index e9cc7ede69..776ed8f12b 100644 --- a/conf/airframes/obsolete/booz2_a4.xml +++ b/conf/airframes/obsolete/booz2_a4.xml @@ -99,7 +99,6 @@
-
@@ -112,9 +111,7 @@ - -
diff --git a/conf/airframes/obsolete/booz2_a5.xml b/conf/airframes/obsolete/booz2_a5.xml index 6fc2fe07eb..3f79fb0e75 100644 --- a/conf/airframes/obsolete/booz2_a5.xml +++ b/conf/airframes/obsolete/booz2_a5.xml @@ -138,7 +138,6 @@
-
@@ -150,10 +149,6 @@ - - - -
diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/obsolete/booz2_flixr.xml similarity index 98% rename from conf/airframes/booz2_flixr.xml rename to conf/airframes/obsolete/booz2_flixr.xml index a62ace8aee..b3a8cab4f5 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/obsolete/booz2_flixr.xml @@ -190,7 +190,6 @@
-
@@ -204,9 +203,7 @@ - - diff --git a/conf/airframes/booz2_ppzuav.xml b/conf/airframes/obsolete/booz2_ppzuav.xml similarity index 98% rename from conf/airframes/booz2_ppzuav.xml rename to conf/airframes/obsolete/booz2_ppzuav.xml index af532c20f2..6915b576da 100644 --- a/conf/airframes/booz2_ppzuav.xml +++ b/conf/airframes/obsolete/booz2_ppzuav.xml @@ -164,9 +164,7 @@ - -
@@ -188,7 +186,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_s1.xml b/conf/airframes/obsolete/booz2_s1.xml index 4cf6bb4bbe..54d25a357e 100644 --- a/conf/airframes/obsolete/booz2_s1.xml +++ b/conf/airframes/obsolete/booz2_s1.xml @@ -130,7 +130,6 @@
-
@@ -142,9 +141,7 @@ - -
diff --git a/conf/airframes/obsolete/booz2_x1.xml b/conf/airframes/obsolete/booz2_x1.xml index 370920cd85..97fc3920ba 100644 --- a/conf/airframes/obsolete/booz2_x1.xml +++ b/conf/airframes/obsolete/booz2_x1.xml @@ -141,7 +141,6 @@
-
@@ -154,9 +153,7 @@ - -
diff --git a/conf/airframes/obsolete/example_heli_lisam.xml b/conf/airframes/obsolete/example_heli_lisam.xml index 79e8d519ac..33e8073432 100644 --- a/conf/airframes/obsolete/example_heli_lisam.xml +++ b/conf/airframes/obsolete/example_heli_lisam.xml @@ -181,7 +181,6 @@ AP_MODE_NAV
-
@@ -193,10 +192,6 @@ AP_MODE_NAV - - - -
diff --git a/conf/airframes/mentor_tum.xml b/conf/airframes/obsolete/mentor_tum.xml similarity index 100% rename from conf/airframes/mentor_tum.xml rename to conf/airframes/obsolete/mentor_tum.xml diff --git a/conf/airframes/obsolete/mm/rotor/qmk1.xml b/conf/airframes/obsolete/mm/rotor/qmk1.xml index 37b3b6c07a..a593c6aec1 100644 --- a/conf/airframes/obsolete/mm/rotor/qmk1.xml +++ b/conf/airframes/obsolete/mm/rotor/qmk1.xml @@ -141,8 +141,6 @@
- -
@@ -154,10 +152,6 @@ - - - -
diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml index 5d1b3e397d..154bce55bf 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 @@
-
@@ -201,10 +200,6 @@ - - - -
diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml index 5343031357..5fd31a6e33 100644 --- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml +++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml @@ -82,7 +82,6 @@
-
@@ -164,10 +163,6 @@ - - - -
diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml index 2ba487a784..32b121c0f6 100644 --- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml +++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml @@ -89,7 +89,6 @@
-
@@ -171,10 +170,6 @@ - - - -
diff --git a/conf/boards/stm32f4_discovery.makefile b/conf/boards/stm32f4_discovery.makefile index f00700c783..1961c9c387 100644 --- a/conf/boards/stm32f4_discovery.makefile +++ b/conf/boards/stm32f4_discovery.makefile @@ -28,8 +28,8 @@ DFU_UTIL ?= y # RADIO_CONTROL_LED ?= 4 BARO_LED ?= none -AHRS_ALIGNER_LED ?= none -GPS_LED ?= none +AHRS_ALIGNER_LED ?= 5 +GPS_LED ?= 6 SYS_TIME_LED ?= 3 # @@ -39,7 +39,7 @@ SYS_TIME_LED ?= 3 MODEM_PORT ?= UART6 MODEM_BAUD ?= B57600 -GPS_PORT ?= UART4 +GPS_PORT ?= UART3 GPS_BAUD ?= B38400 RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART2 diff --git a/conf/conf_example.xml b/conf/conf_example.xml index 893aba9cec..e8ed823458 100644 --- a/conf/conf_example.xml +++ b/conf/conf_example.xml @@ -41,7 +41,7 @@ radio="radios/cockpitSX.xml" telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/rotorcraft_basic.xml" - settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml" + settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml settings/estimation/ahrs_int_cmpl_quat.xml" gui_color="white" /> + +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/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd index 7d1fbdeb12..fb18d5ce0e 100644 --- a/conf/flight_plans/flight_plan.dtd +++ b/conf/flight_plans/flight_plan.dtd @@ -159,7 +159,8 @@ var CDATA #REQUIRED value CDATA #REQUIRED> +fun CDATA #REQUIRED +until CDATA #IMPLIED> grid CDATA #REQUIRED orientation CDATA #IMPLIED wp1 CDATA #REQUIRED -wp2 CDATA #REQUIRED> +wp2 CDATA #REQUIRED +until CDATA #IMPLIED> + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/messages.xml b/conf/messages.xml index e3d06e6d30..d876a5d04c 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1345,7 +1345,7 @@ - + @@ -1377,6 +1377,7 @@ + @@ -1890,8 +1891,8 @@ - - + + @@ -2256,6 +2257,139 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -2589,15 +2723,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/modules/mission_fw.xml b/conf/modules/mission_fw.xml new file mode 100644 index 0000000000..096e896394 --- /dev/null +++ b/conf/modules/mission_fw.xml @@ -0,0 +1,32 @@ + + + + + + Interface for mission control of fixed wing aircraft. + This module parse datalink commands for basic navigation routines + and store them in a queue. + + +
+ +
+ + + + + + + + + + + + + + + + + + + diff --git a/conf/settings/control/rotorcraft_guidance.xml b/conf/settings/control/rotorcraft_guidance.xml index 45c73a958b..c44ecb0069 100644 --- a/conf/settings/control/rotorcraft_guidance.xml +++ b/conf/settings/control/rotorcraft_guidance.xml @@ -15,6 +15,7 @@ + diff --git a/conf/settings/estimation/ahrs_float_cmpl.xml b/conf/settings/estimation/ahrs_float_cmpl.xml new file mode 100644 index 0000000000..950b651e70 --- /dev/null +++ b/conf/settings/estimation/ahrs_float_cmpl.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/conf/settings/estimation/ahrs_int_cmpl_quat.xml b/conf/settings/estimation/ahrs_int_cmpl_quat.xml index 920f3c0dc5..600de04e88 100644 --- a/conf/settings/estimation/ahrs_int_cmpl_quat.xml +++ b/conf/settings/estimation/ahrs_int_cmpl_quat.xml @@ -4,7 +4,11 @@ - + + + + + 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/imu_apogee.c b/sw/airborne/boards/apogee/imu_apogee.c index 6617c90d8b..47f004fc29 100644 --- a/sw/airborne/boards/apogee/imu_apogee.c +++ b/sw/airborne/boards/apogee/imu_apogee.c @@ -58,6 +58,13 @@ PRINT_CONFIG_VAR(APOGEE_ACCEL_RANGE) struct ImuApogee imu_apogee; +// baro config will be done later in bypass mode +bool_t configure_baro_slave(Mpu60x0ConfigSet mpu_set, void* mpu); + +bool_t configure_baro_slave(Mpu60x0ConfigSet mpu_set __attribute__ ((unused)), void* mpu __attribute__ ((unused))) { + return TRUE; +} + void imu_impl_init(void) { ///////////////////////////////////////////////////////////////////// @@ -68,6 +75,10 @@ void imu_impl_init(void) imu_apogee.mpu.config.dlpf_cfg = APOGEE_LOWPASS_FILTER; imu_apogee.mpu.config.gyro_range = APOGEE_GYRO_RANGE; imu_apogee.mpu.config.accel_range = APOGEE_ACCEL_RANGE; + // set MPU in bypass mode for the baro + imu_apogee.mpu.config.nb_slaves = 1; + imu_apogee.mpu.config.slaves[0].configure = &configure_baro_slave; + imu_apogee.mpu.config.i2c_bypass = TRUE; imu_apogee.gyr_valid = FALSE; imu_apogee.acc_valid = FALSE; 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/stm32f4_discovery.h b/sw/airborne/boards/stm32f4_discovery.h index 288385d132..49c2fdc26f 100755 --- a/sw/airborne/boards/stm32f4_discovery.h +++ b/sw/airborne/boards/stm32f4_discovery.h @@ -1,3 +1,96 @@ +/* + * STM32F4 DISCOVERY BOARD DEFINITIONS FOR THE PAPARAZZI AUTOPILOT. + * + * DISCOVERY USED FOR THE AUTOPILOT PINS + * PA0 = PWM9 + user-wake up button switch b1 normaly open. + * PA1 = PWM8 + * PA2 = PWM7 + * PA3 = PWM6 + * PA4 = ADC_4 (ADC12 IN 4)+CS43L22 DAC out, capacitively coupled+100Kohm, should not interfere. + * PA5 = FREE + * PA6 = CANNOT BE USED + * PA7 = FREE + * PA8 = SPECTRUM BIND + * PA9 = FREE (ONLY if usb is not active during runtime, PC0 must be high or input ) + * PA10 = UART2 (Spektrum input) + * PA11 = FREE if usb is not active during runtime + * PA12 = FREE if usb is not active during runtime + * PA13 = FREE + * PA14 = FREE + * PA15 = FREE + * + * PB0 = FREE + * PB1 = ADC_1 (ADC12 IN 9) + * PB2 = FREE + * PB3 = SPI1 SCL + * PB4 = SPI1 MISO + * PB5 = SPI1 MOSI + * PB6 = UART1 TX + 4K7 Ohm pull up resistor normaly used for i2c + * PB7 = UART1 RX + * PB8 = I2C1 SCL + * PB9 = I2C1 SDA + 4K7 Ohm pull up resistor normaly used for i2c + * PB10 = I2C2 SCL + MP45DT02 MEMS MIC clock in + * PB11 = I2C2 SDA + * PB12 = FREE + * PB13 = SPI2 SCL + * PB14 = PWM 10 OR SPI2 MISO + * PB15 = PWM 11 OR SPI2 MOSI + * + * PC0 = CANNOT BE USED, pulled up to 3.3v (10Kohm), low = usb power on directly applied to PA9) + * PC1 = ADC_5 (ADC123 IN 11) + * PC2 = ADC_6 (ADC123 IN 12) + * PC3 = CANNOT BE USED (MEMS microphone output) + * PC4 = ADC_3 (ADC12 IN 14) + * PC5 = ADC_2 (ADC12 IN 15) + * PC6 = UART6 TX + * PC7 = UART6 RX + * PC8 = FREE + * PC9 = PPM INPUT FROM RC RECEIVER + * PC10 = UART4 TX OR SPI3 SCL + * PC11 = UART4 RX OR SPI3 MISO + * PC12 = UART5 TX OR SPI3 MOSI + * PC13 = FREE max 3ma sink + * PC14 = FREE max 3ma sink, short circuit the relevant PCB bridge with solder to activate + * PC15 = FREE max 3ma sink, short circuit the relevant PCB bridge with solder to activate + * + * PD0 = FREE + * PD1 = FREE + * PD2 = UART5 RX + * PD3 = FREE + * PD4 = FREE + * PD5 = UART2 TX (usb power management fault output open drain) + * PD6 = UART2 RX + * PD7 = FREE + * PD8 = UART3 TX + * PD9 = UART3 RX + * PD10 = FREE + * PD11 = FREE + * PD12 = LED_3 + * PD13 = LED_4 + * PD14 = LED_5 + * PD15 = LED_6 + * + * PE0 = CANNOT BE USED (Accel int output) + * PE1 = CANNOT BE USED (Accel int output) + * PE2 = SPI SLAVE 0 + * PE3 = FREE (Needs some testing as it is used for the accel spi/i2c select pin) + * PE4 = FREE + * PE5 = PWM4 + * PE6 = PWM5 + * PE7 = SPI SLAVE 1 + * PE8 = FREE + * PE9 = PWM0 + * PE10 = FREE + * PE11 = PWM1 + * PE12 = FREE + * PE13 = PWM2 + * PE14 = PWM3 + * PE15 = FREE + * + * PH0 = CRYSTAL - OSC IN + * PH1 = CRYSTAL - OSC OUT + */ + #ifndef CONFIG_STM32F4_DISCOVERY_H #define CONFIG_STM32F4_DISCOVERY_H @@ -17,7 +110,7 @@ #endif #define LED_3_GPIO GPIOD #define LED_3_GPIO_CLK RCC_AHB1ENR_IOPDEN -#define LED_3_GPIO_PIN GPIO13 +#define LED_3_GPIO_PIN GPIO12 #define LED_3_AFIO_REMAP ((void)0) #define LED_3_GPIO_ON gpio_set #define LED_3_GPIO_OFF gpio_clear @@ -28,7 +121,7 @@ #endif #define LED_4_GPIO GPIOD #define LED_4_GPIO_CLK RCC_AHB1ENR_IOPDEN -#define LED_4_GPIO_PIN GPIO12 +#define LED_4_GPIO_PIN GPIO13 #define LED_4_AFIO_REMAP ((void)0) #define LED_4_GPIO_ON gpio_set #define LED_4_GPIO_OFF gpio_clear @@ -56,46 +149,107 @@ #define LED_6_GPIO_OFF gpio_clear -/* UART */ + +/* +// LED 9 (Green) of the STM32F4 discovery board same as USB power (VBUS). +#ifndef USE_LED_9 +#define USE_LED_9 1 +#endif +#define LED_9_GPIO GPIOA +#define LED_9_GPIO_CLK RCC_AHB1ENR_IOPAEN +#define LED_9_GPIO_PIN GPIO9 +#define LED_9_AFIO_REMAP ((void)0) +#define LED_9_GPIO_ON gpio_set +#define LED_9_GPIO_OFF gpio_clear +*/ + + +/***************************************************************************************************/ +/************************************** UART *************************************************/ +/***************************************************************************************************/ + +// If you don't need all those uarts comment a uart block out to free pins. #define UART1_GPIO_AF GPIO_AF7 -#define UART1_GPIO_PORT_RX GPIOA -#define UART1_GPIO_RX GPIO10 #define UART1_GPIO_PORT_TX GPIOB #define UART1_GPIO_TX GPIO6 +#define UART1_GPIO_PORT_RX GPIOB +#define UART1_GPIO_RX GPIO7 +// UART2 is used for the Spectrum rx input also #define UART2_GPIO_AF GPIO_AF7 -#define UART2_GPIO_PORT_RX GPIOA -#define UART2_GPIO_RX GPIO3 -#define UART2_GPIO_PORT_TX GPIOA -#define UART2_GPIO_TX GPIO2 +#define UART2_GPIO_PORT_TX GPIOD +#define UART2_GPIO_TX GPIO5 +#define UART2_GPIO_PORT_RX GPIOD +#define UART2_GPIO_RX GPIO6 +#define UART3_GPIO_AF GPIO_AF7 +#define UART3_GPIO_PORT_TX GPIOD +#define UART3_GPIO_TX GPIO8 +#define UART3_GPIO_PORT_RX GPIOD +#define UART3_GPIO_RX GPIO9 + +// UARTS 4 AND 5 CANT BE USED IF SPI3 IS NEEDED +#if !USE_SPI3 #define UART4_GPIO_AF GPIO_AF8 -#define UART4_GPIO_PORT_RX GPIOA -#define UART4_GPIO_RX GPIO1 -#define UART4_GPIO_PORT_TX GPIOA -#define UART4_GPIO_TX GPIO0 +#define UART4_GPIO_PORT_TX GPIOC +#define UART4_GPIO_TX GPIO10 +#define UART4_GPIO_PORT_RX GPIOC +#define UART4_GPIO_RX GPIO11 + +#define UART5_GPIO_AF GPIO_AF8 +#define UART5_GPIO_PORT_TX GPIOC +#define UART5_GPIO_TX GPIO12 +#define UART5_GPIO_PORT_RX GPIOD +#define UART5_GPIO_RX GPIO2 +#endif #define UART6_GPIO_AF GPIO_AF8 -#define UART6_GPIO_PORT_RX GPIOC -#define UART6_GPIO_RX GPIO7 #define UART6_GPIO_PORT_TX GPIOC #define UART6_GPIO_TX GPIO6 +#define UART6_GPIO_PORT_RX GPIOC +#define UART6_GPIO_RX GPIO7 -/* SPI */ +/***************************************************************************************************/ +/************************************** SPI *************************************************/ +/***************************************************************************************************/ + #define SPI1_GPIO_AF GPIO_AF5 -#define SPI1_GPIO_PORT_MISO GPIOA -#define SPI1_GPIO_MISO GPIO6 -#define SPI1_GPIO_PORT_MOSI GPIOA -#define SPI1_GPIO_MOSI GPIO7 -#define SPI1_GPIO_PORT_SCK GPIOA -#define SPI1_GPIO_SCK GPIO5 - -#define SPI_SELECT_SLAVE0_PORT GPIOB -#define SPI_SELECT_SLAVE0_PIN GPIO9 +#define SPI1_GPIO_PORT_SCK GPIOB +#define SPI1_GPIO_SCK GPIO3 +#define SPI1_GPIO_PORT_MISO GPIOB +#define SPI1_GPIO_MISO GPIO4 +#define SPI1_GPIO_PORT_MOSI GPIOB +#define SPI1_GPIO_MOSI GPIO5 -/* I2C mapping */ +/* CANNOT BE USED IF PWM CHANNELS 10 & 11 ARE ACTIVE !!! */ +#define SPI2_GPIO_AF GPIO_AF5 +#define SPI2_GPIO_PORT_SCK GPIOB +#define SPI2_GPIO_SCK GPIO13 +#define SPI2_GPIO_PORT_MISO GPIOB +#define SPI2_GPIO_MISO GPIO14 +#define SPI2_GPIO_PORT_MOSI GPIOB +#define SPI2_GPIO_MOSI GPIO15 + +/* CANNOT BE USED IF UARTS 4 & 5 ARE ACTIVE !!! */ +#define SPI3_GPIO_AF GPIO_AF6 +#define SPI3_GPIO_PORT_SCK GPIOC +#define SPI3_GPIO_SCK GPIO10 +#define SPI3_GPIO_PORT_MISO GPIOC +#define SPI3_GPIO_MISO GPIO11 +#define SPI3_GPIO_PORT_MOSI GPIOC +#define SPI3_GPIO_MOSI GPIO12 + +#define SPI_SELECT_SLAVE0_PORT GPIOE +#define SPI_SELECT_SLAVE0_PIN GPIO2 +#define SPI_SELECT_SLAVE1_PORT GPIOE +#define SPI_SELECT_SLAVE1_PIN GPIO7 + + +/***************************************************************************************************/ +/************************************** I2C *************************************************/ +/***************************************************************************************************/ #define I2C1_GPIO_PORT GPIOB #define I2C1_GPIO_SCL GPIO8 #define I2C1_GPIO_SDA GPIO9 @@ -104,47 +258,16 @@ #define I2C2_GPIO_SCL GPIO10 #define I2C2_GPIO_SDA GPIO11 -#define I2C3_GPIO_PORT_SCL GPIOA -#define I2C3_GPIO_PORT_SDA GPIOC -#define I2C3_GPIO_SCL GPIO8 -#define I2C3_GPIO_SDA GPIO9 +//#define I2C3_GPIO_PORT_SCL GPIOA +//#define I2C3_GPIO_SCL GPIO8 +//#define I2C3_GPIO_PORT_SDA GPIOC +//#define I2C3_GPIO_SDA GPIO9 -/* - * PPM - */ -#define USE_PPM_TIM1 1 +/***************************************************************************************************/ +/************************************** ADC *************************************************/ +/***************************************************************************************************/ -#define PPM_CHANNEL TIM_IC1 -#define PPM_TIMER_INPUT TIM_IC_IN_TI1 -#define PPM_IRQ NVIC_TIM1_CC_IRQ -#define PPM_IRQ2 NVIC_TIM1_UP_TIM10_IRQ -// Capture/Compare InteruptEnable and InterruptFlag -#define PPM_CC_IE TIM_DIER_CC1IE -#define PPM_CC_IF TIM_SR_CC1IF -#define PPM_GPIO_PORT GPIOA -#define PPM_GPIO_PIN GPIO8 -#define PPM_GPIO_AF GPIO_AF1 - - -/* - * Spektrum - */ -/* The line that is pulled low at power up to initiate the bind process */ -#define SPEKTRUM_BIND_PIN GPIO8 -#define SPEKTRUM_BIND_PIN_PORT GPIOA - -#define SPEKTRUM_UART2_RCC_REG &RCC_APB2ENR -#define SPEKTRUM_UART2_RCC_DEV RCC_APB2ENR_USART1EN -#define SPEKTRUM_UART2_BANK GPIOA -#define SPEKTRUM_UART2_PIN GPIO10 -#define SPEKTRUM_UART2_AF GPIO_AF7 -#define SPEKTRUM_UART2_IRQ NVIC_USART1_IRQ -#define SPEKTRUM_UART2_ISR usart1_isr -#define SPEKTRUM_UART2_DEV USART1 - - -/* Onboard ADCs */ #define USE_AD_TIM4 1 #define BOARD_ADC_CHANNEL_1 9 @@ -155,10 +278,12 @@ #ifndef USE_AD1 #define USE_AD1 1 #endif + /* provide defines that can be used to access the ADC_x in the code or airframe file * these directly map to the index number of the 4 adc channels defined above * 4th (index 3) is used for bat monitoring by default */ + // AUX 1 #define ADC_1 ADC1_C1 #ifdef USE_ADC_1 @@ -206,7 +331,7 @@ #endif #define USE_AD1_4 1 -/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/ +/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file */ #ifndef ADC_CHANNEL_VSUPPLY #define ADC_CHANNEL_VSUPPLY ADC_4 #endif @@ -226,6 +351,10 @@ #define DefaultVoltageOfAdc(adc) (0.00485*adc) +/***************************************************************************************************/ +/************************************ ACTUATORS ************************************************/ +/***************************************************************************************************/ + /* Default actuators driver */ #define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h" #define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) @@ -233,4 +362,248 @@ #define ActuatorsDefaultCommit() ActuatorsPwmCommit() +/***************************************************************************************************/ +/********************************** SERVO PWM *************************************************/ +/***************************************************************************************************/ + +#define PWM_USE_TIM1 1 +//#define PWM_USE_TIM3 1 +#define PWM_USE_TIM5 1 +#define PWM_USE_TIM9 1 +#define PWM_USE_TIM12 1 + +#define USE_PWM0 1 +#define USE_PWM1 1 +#define USE_PWM2 1 +#define USE_PWM3 1 +#define USE_PWM4 1 +#define USE_PWM5 1 +#define USE_PWM6 1 +#define USE_PWM7 1 +#define USE_PWM8 1 +#define USE_PWM9 1 +#if USE_SPI2 +#define USE_PWM10 0 +#define USE_PWM11 0 +#else +#define USE_PWM10 1 +#define USE_PWM11 1 +#endif + +#define ACTUATORS_PWM_NB 12 + +// PWM_SERVO_x is the index of the servo in the actuators_pwm_values array +#if USE_PWM0 +#define PWM_SERVO_0 0 +#define PWM_SERVO_0_TIMER TIM1 +#define PWM_SERVO_0_RCC_IOP RCC_AHB1ENR_IOPEEN +#define PWM_SERVO_0_GPIO GPIOE +#define PWM_SERVO_0_PIN GPIO9 +#define PWM_SERVO_0_AF GPIO_AF1 +#define PWM_SERVO_0_OC TIM_OC1 +#define PWM_SERVO_0_OC_BIT (1<<0) +#else +#define PWM_SERVO_0_OC_BIT 0 +#endif + +#if USE_PWM1 +#define PWM_SERVO_1 1 +#define PWM_SERVO_1_TIMER TIM1 +#define PWM_SERVO_1_RCC_IOP RCC_AHB1ENR_IOPEEN +#define PWM_SERVO_1_GPIO GPIOE +#define PWM_SERVO_1_PIN GPIO11 +#define PWM_SERVO_1_AF GPIO_AF1 +#define PWM_SERVO_1_OC TIM_OC2 +#define PWM_SERVO_1_OC_BIT (1<<1) +#else +#define PWM_SERVO_1_OC_BIT 0 +#endif + +#if USE_PWM2 +#define PWM_SERVO_2 2 +#define PWM_SERVO_2_TIMER TIM1 +#define PWM_SERVO_2_RCC_IOP RCC_AHB1ENR_IOPEEN +#define PWM_SERVO_2_GPIO GPIOE +#define PWM_SERVO_2_PIN GPIO13 +#define PWM_SERVO_2_AF GPIO_AF1 +#define PWM_SERVO_2_OC TIM_OC3 +#define PWM_SERVO_2_OC_BIT (1<<2) +#else +#define PWM_SERVO_2_OC_BIT 0 +#endif + +#if USE_PWM3 +#define PWM_SERVO_3 3 +#define PWM_SERVO_3_TIMER TIM1 +#define PWM_SERVO_3_RCC_IOP RCC_AHB1ENR_IOPEEN +#define PWM_SERVO_3_GPIO GPIOE +#define PWM_SERVO_3_PIN GPIO14 +#define PWM_SERVO_3_AF GPIO_AF1 +#define PWM_SERVO_3_OC TIM_OC4 +#define PWM_SERVO_3_OC_BIT (1<<3) +#else +#define PWM_SERVO_3_OC_BIT 0 +#endif + +#if USE_PWM4 +#define PWM_SERVO_4 4 +#define PWM_SERVO_4_TIMER TIM9 +#define PWM_SERVO_4_RCC_IOP RCC_AHB1ENR_IOPEEN +#define PWM_SERVO_4_GPIO GPIOE +#define PWM_SERVO_4_PIN GPIO5 +#define PWM_SERVO_4_AF GPIO_AF3 +#define PWM_SERVO_4_OC TIM_OC1 +#define PWM_SERVO_4_OC_BIT (1<<0) +#else +#define PWM_SERVO_4_OC_BIT 0 +#endif + +#if USE_PWM5 +#define PWM_SERVO_5 5 +#define PWM_SERVO_5_TIMER TIM9 +#define PWM_SERVO_5_RCC_IOP RCC_AHB1ENR_IOPEEN +#define PWM_SERVO_5_GPIO GPIOE +#define PWM_SERVO_5_PIN GPIO6 +#define PWM_SERVO_5_AF GPIO_AF3 +#define PWM_SERVO_5_OC TIM_OC2 +#define PWM_SERVO_5_OC_BIT (1<<1) +#else +#define PWM_SERVO_5_OC_BIT 0 +#endif + + +#if USE_PWM6 +#define PWM_SERVO_6 6 +#define PWM_SERVO_6_TIMER TIM5 +#define PWM_SERVO_6_RCC_IOP RCC_AHB1ENR_IOPAEN +#define PWM_SERVO_6_GPIO GPIOA +#define PWM_SERVO_6_PIN GPIO3 +#define PWM_SERVO_6_AF GPIO_AF2 +#define PWM_SERVO_6_OC TIM_OC4 +#define PWM_SERVO_6_OC_BIT (1<<3) +#else +#define PWM_SERVO_6_OC_BIT 0 +#endif + +#if USE_PWM7 +#define PWM_SERVO_7 7 +#define PWM_SERVO_7_TIMER TIM5 +#define PWM_SERVO_7_RCC_IOP RCC_AHB1ENR_IOPAEN +#define PWM_SERVO_7_GPIO GPIOA +#define PWM_SERVO_7_PIN GPIO2 +#define PWM_SERVO_7_AF GPIO_AF2 +#define PWM_SERVO_7_OC TIM_OC3 +#define PWM_SERVO_7_OC_BIT (1<<2) +#else +#define PWM_SERVO_7_OC_BIT 0 +#endif + +#if USE_PWM8 +#define PWM_SERVO_8 8 +#define PWM_SERVO_8_TIMER TIM5 +#define PWM_SERVO_8_RCC_IOP RCC_AHB1ENR_IOPAEN +#define PWM_SERVO_8_GPIO GPIOA +#define PWM_SERVO_8_PIN GPIO1 +#define PWM_SERVO_8_AF GPIO_AF2 +#define PWM_SERVO_8_OC TIM_OC2 +#define PWM_SERVO_8_OC_BIT (1<<1) +#else +#define PWM_SERVO_8_OC_BIT 0 +#endif + +#if USE_PWM9 +#define PWM_SERVO_9 9 +#define PWM_SERVO_9_TIMER TIM5 +#define PWM_SERVO_9_RCC_IOP RCC_AHB1ENR_IOPAEN +#define PWM_SERVO_9_GPIO GPIOA +#define PWM_SERVO_9_PIN GPIO0 +#define PWM_SERVO_9_AF GPIO_AF2 +#define PWM_SERVO_9_OC TIM_OC1 +#define PWM_SERVO_9_OC_BIT (1<<0) +#else +#define PWM_SERVO_9_OC_BIT 0 +#endif + +/* PWM10 AND PWM11 cannot be used if SPI2 is active. */ +#if USE_PWM10 +#define PWM_SERVO_10 10 +#define PWM_SERVO_10_TIMER TIM12 +#define PWM_SERVO_10_RCC_IOP RCC_AHB1ENR_IOPBEN +#define PWM_SERVO_10_GPIO GPIOB +#define PWM_SERVO_10_PIN GPIO14 +#define PWM_SERVO_10_AF GPIO_AF9 +#define PWM_SERVO_10_OC TIM_OC1 +#define PWM_SERVO_10_OC_BIT (1<<0) +#else +#define PWM_SERVO_10_OC_BIT 0 +#endif + +#if USE_PWM11 +#define PWM_SERVO_11 11 +#define PWM_SERVO_11_TIMER TIM12 +#define PWM_SERVO_11_RCC_IOP RCC_AHB1ENR_IOPBEN +#define PWM_SERVO_11_GPIO GPIOB +#define PWM_SERVO_11_PIN GPIO15 +#define PWM_SERVO_11_AF GPIO_AF9 +#define PWM_SERVO_11_OC TIM_OC2 +#define PWM_SERVO_11_OC_BIT (1<<1) +#else +#define PWM_SERVO_11_OC_BIT 0 +#endif + +#define PWM_TIM1_CHAN_MASK (PWM_SERVO_0_OC_BIT|PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT) +#define PWM_TIM9_CHAN_MASK (PWM_SERVO_4_OC_BIT|PWM_SERVO_5_OC_BIT) +#define PWM_TIM5_CHAN_MASK (PWM_SERVO_6_OC_BIT|PWM_SERVO_7_OC_BIT|PWM_SERVO_8_OC_BIT|PWM_SERVO_9_OC_BIT) +#define PWM_TIM12_CHAN_MASK (PWM_SERVO_10_OC_BIT|PWM_SERVO_11_OC_BIT) + +/***************************************************************************************************/ +/*********************************** PPM INPUT *************************************************/ +/***************************************************************************************************/ + +/* +#define USE_PPM_TIM1 1 + +#define PPM_CHANNEL TIM_IC1 +#define PPM_TIMER_INPUT TIM_IC_IN_TI1 +#define PPM_IRQ NVIC_TIM1_CC_IRQ +#define PPM_IRQ2 NVIC_TIM1_UP_TIM10_IRQ +// Capture/Compare InteruptEnable and InterruptFlag +#define PPM_CC_IE TIM_DIER_CC1IE +#define PPM_CC_IF TIM_SR_CC1IF +#define PPM_GPIO_PORT GPIOA +#define PPM_GPIO_PIN GPIO8 +#define PPM_GPIO_AF GPIO_AF1 +*/ + +#define USE_PPM_TIM8 1 + +#define PPM_CHANNEL TIM_IC4 +#define PPM_TIMER_INPUT TIM_IC_IN_TI4 +#define PPM_IRQ NVIC_TIM8_CC_IRQ +#define PPM_IRQ2 NVIC_TIM8_UP_TIM13_IRQ +// Capture/Compare InteruptEnable and InterruptFlag +#define PPM_CC_IE TIM_DIER_CC4IE +#define PPM_CC_IF TIM_SR_CC4IF +#define PPM_GPIO_PORT GPIOC +#define PPM_GPIO_PIN GPIO9 +#define PPM_GPIO_AF GPIO_AF3 + +/***************************************************************************************************/ +/********************************* SPECTRUM UART *************************************************/ +/***************************************************************************************************/ + +/* The line that is pulled low at power up to initiate the bind process */ +#define SPEKTRUM_BIND_PIN GPIO8 +#define SPEKTRUM_BIND_PIN_PORT GPIOA + +#define SPEKTRUM_UART2_RCC_REG &RCC_APB2ENR +#define SPEKTRUM_UART2_RCC_DEV RCC_APB2ENR_USART1EN +#define SPEKTRUM_UART2_BANK GPIOA +#define SPEKTRUM_UART2_PIN GPIO10 +#define SPEKTRUM_UART2_AF GPIO_AF7 +#define SPEKTRUM_UART2_IRQ NVIC_USART1_IRQ +#define SPEKTRUM_UART2_ISR usart1_isr +#define SPEKTRUM_UART2_DEV USART1 + + #endif /* CONFIG_STM32F4_DISCOVERY_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/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 859b3bda08..9b7e5def6a 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 */ @@ -148,11 +152,6 @@ static void send_fliter_status(void) { 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; @@ -202,7 +201,8 @@ void init_ap( void ) { register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_fliter_status); #endif -#if USE_BAROMETER + air_data_init(); +#if USE_BARO_BOARD baro_init(); #endif @@ -593,7 +593,7 @@ void sensors_task( void ) { ahrs_propagate(); #endif -#if USE_BAROMETER +#if USE_BARO_BOARD baro_periodic(); #endif @@ -663,8 +663,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(); @@ -795,14 +795,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 2e1e1f865d..5fc0844480 100644 --- a/sw/airborne/firmwares/fixedwing/main_fbw.c +++ b/sw/airborne/firmwares/fixedwing/main_fbw.c @@ -61,7 +61,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/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 05d1757785..9ce4b59ed2 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -30,6 +30,9 @@ #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" #include "firmwares/rotorcraft/navigation.h" +/* for guidance_v_thrust_coeff */ +#include "firmwares/rotorcraft/guidance/guidance_v.h" + #include "state.h" #include "generated/airframe.h" @@ -55,24 +58,30 @@ PRINT_CONFIG_VAR(GUIDANCE_H_USE_REF) +#ifndef GUIDANCE_H_APPROX_FORCE_BY_THRUST +#define GUIDANCE_H_APPROX_FORCE_BY_THRUST FALSE +#endif + uint8_t guidance_h_mode; bool_t guidance_h_use_ref; +bool_t guidance_h_approx_force_by_thrust; struct Int32Vect2 guidance_h_pos_sp; struct Int32Vect2 guidance_h_pos_ref; struct Int32Vect2 guidance_h_speed_ref; struct Int32Vect2 guidance_h_accel_ref; - +#if GUIDANCE_H_USE_SPEED_REF +struct Int32Vect2 guidance_h_speed_sp; +#endif struct Int32Vect2 guidance_h_pos_err; struct Int32Vect2 guidance_h_speed_err; -struct Int32Vect2 guidance_h_pos_err_sum; +struct Int32Vect2 guidance_h_trim_att_integrator; struct Int32Vect2 guidance_h_nav_err; +struct Int32Vect2 guidance_h_cmd_earth; struct Int32Eulers guidance_h_rc_sp; -struct Int32Vect2 guidance_h_command_earth; -struct Int32Vect2 guidance_h_stick_earth_sp; -struct Int32Eulers guidance_h_command_body; +int32_t guidance_h_heading_sp; int32_t guidance_h_pgain; int32_t guidance_h_dgain; @@ -88,6 +97,7 @@ static void guidance_h_traj_run(bool_t in_flight); static void guidance_h_hover_enter(void); static void guidance_h_nav_enter(void); static inline void transition_run(void); +static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flight); #if DOWNLINK #include "subsystems/datalink/telemetry.h" @@ -153,11 +163,12 @@ void guidance_h_init(void) { guidance_h_mode = GUIDANCE_H_MODE_KILL; guidance_h_use_ref = GUIDANCE_H_USE_REF; + guidance_h_approx_force_by_thrust = GUIDANCE_H_APPROX_FORCE_BY_THRUST; INT_VECT2_ZERO(guidance_h_pos_sp); - INT_VECT2_ZERO(guidance_h_pos_err_sum); + INT_VECT2_ZERO(guidance_h_trim_att_integrator); INT_EULERS_ZERO(guidance_h_rc_sp); - INT_EULERS_ZERO(guidance_h_command_body); + guidance_h_heading_sp = 0; guidance_h_pgain = GUIDANCE_H_PGAIN; guidance_h_igain = GUIDANCE_H_IGAIN; guidance_h_dgain = GUIDANCE_H_DGAIN; @@ -179,7 +190,7 @@ static inline void reset_guidance_reference_from_current_position(void) { INT_VECT2_ZERO(guidance_h_accel_ref); gh_set_ref(guidance_h_pos_ref, guidance_h_speed_ref, guidance_h_accel_ref); - INT_VECT2_ZERO(guidance_h_pos_err_sum); + INT_VECT2_ZERO(guidance_h_trim_att_integrator); } void guidance_h_mode_changed(uint8_t new_mode) { @@ -268,6 +279,9 @@ void guidance_h_read_rc(bool_t in_flight) { case GUIDANCE_H_MODE_HOVER: stabilization_attitude_read_rc_setpoint_eulers(&guidance_h_rc_sp, in_flight); +#if GUIDANCE_H_USE_SPEED_REF + read_rc_setpoint_speed_i(&guidance_h_speed_sp, in_flight); +#endif break; case GUIDANCE_H_MODE_NAV: @@ -312,10 +326,12 @@ void guidance_h_run(bool_t in_flight) { guidance_h_update_reference(); /* set psi command */ - guidance_h_command_body.psi = guidance_h_rc_sp.psi; - /* compute roll and pitch commands and set final attitude setpoint */ + guidance_h_heading_sp = guidance_h_rc_sp.psi; + /* compute x,y earth commands */ guidance_h_traj_run(in_flight); - + /* set final attitude setpoint */ + stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, + guidance_h_heading_sp); stabilization_attitude_run(in_flight); break; @@ -327,9 +343,11 @@ void guidance_h_run(bool_t in_flight) { struct Int32Eulers sp_cmd_i; sp_cmd_i.phi = nav_roll; sp_cmd_i.theta = nav_pitch; - /* FIXME: heading can't be set via attitude block yet, use current heading for now */ + /** @todo: heading can't be set via attitude block yet. + * use current euler psi for now, should be real heading + */ sp_cmd_i.psi = stateGetNedToBodyEulers_i()->psi; - stabilization_attitude_set_cmd_i(&sp_cmd_i); + stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i); } else { INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot); @@ -337,10 +355,13 @@ void guidance_h_run(bool_t in_flight) { guidance_h_update_reference(); /* set psi command */ - guidance_h_command_body.psi = nav_heading; - INT32_ANGLE_NORMALIZE(guidance_h_command_body.psi); - /* compute roll and pitch commands and set final attitude setpoint */ + guidance_h_heading_sp = nav_heading; + INT32_ANGLE_NORMALIZE(guidance_h_heading_sp); + /* compute x,y earth commands */ guidance_h_traj_run(in_flight); + /* set final attitude setpoint */ + stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, + guidance_h_heading_sp); } stabilization_attitude_run(in_flight); break; @@ -354,6 +375,11 @@ void guidance_h_run(bool_t in_flight) { static void guidance_h_update_reference(void) { /* compute reference even if usage temporarily disabled via guidance_h_use_ref */ #if GUIDANCE_H_USE_REF +#if GUIDANCE_H_USE_SPEED_REF + if(guidance_h_mode == GUIDANCE_H_MODE_HOVER) + gh_update_ref_from_speed_sp(guidance_h_speed_sp); + else +#endif gh_update_ref_from_pos_sp(guidance_h_pos_sp); #endif @@ -368,21 +394,31 @@ static void guidance_h_update_reference(void) { INT_VECT2_ZERO(guidance_h_speed_ref); INT_VECT2_ZERO(guidance_h_accel_ref); } + +#if GUIDANCE_H_USE_SPEED_REF + if(guidance_h_mode == GUIDANCE_H_MODE_HOVER) { + VECT2_COPY(guidance_h_pos_sp, guidance_h_pos_ref); // for display only + } +#endif } #define MAX_POS_ERR POS_BFP_OF_REAL(16.) #define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.) -#define MAX_POS_ERR_SUM ((int32_t)(MAX_POS_ERR)<< 12) + +#ifndef GUIDANCE_H_THRUST_CMD_FILTER +#define GUIDANCE_H_THRUST_CMD_FILTER 10 +#endif /* with a pgain of 100 and a scale of 2, * you get an angle of 5.6 degrees for 1m pos error */ #define GH_GAIN_SCALE 2 -/** maximum bank angle: default 20 deg */ -#define TRAJ_MAX_BANK BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC) - static void guidance_h_traj_run(bool_t in_flight) { + /* maximum bank angle: default 20 deg, max 40 deg*/ + static const int32_t traj_max_bank = Max(BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC), + BFP_OF_REAL(RadOfDeg(40), INT32_ANGLE_FRAC)); + static const int32_t total_max_bank = BFP_OF_REAL(RadOfDeg(45), INT32_ANGLE_FRAC); /* compute position error */ VECT2_DIFF(guidance_h_pos_err, guidance_h_pos_ref, *stateGetPositionNed_i()); @@ -394,48 +430,50 @@ static void guidance_h_traj_run(bool_t in_flight) { /* saturate it */ VECT2_STRIM(guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR); - /* update pos error integral, zero it if not in_flight */ + /* run PID */ + int32_t pd_x = + ((guidance_h_pgain * guidance_h_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + + ((guidance_h_dgain * (guidance_h_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)); + int32_t pd_y = + ((guidance_h_pgain * guidance_h_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + + ((guidance_h_dgain * (guidance_h_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)); + guidance_h_cmd_earth.x = + pd_x + + ((guidance_h_again * guidance_h_accel_ref.x) >> 8); /* acceleration feedforward gain */ + guidance_h_cmd_earth.y = + pd_y + + ((guidance_h_again * guidance_h_accel_ref.y) >> 8); /* acceleration feedforward gain */ + + /* trim max bank angle from PD */ + VECT2_STRIM(guidance_h_cmd_earth, -traj_max_bank, traj_max_bank); + + /* Update pos & speed error integral, zero it if not in_flight. + * Integrate twice as fast when not only POS but also SPEED are wrong, + * but do not integrate POS errors when the SPEED is already catching up. + */ if (in_flight) { - VECT2_ADD(guidance_h_pos_err_sum, guidance_h_pos_err); - /* saturate it */ - VECT2_STRIM(guidance_h_pos_err_sum, -MAX_POS_ERR_SUM, MAX_POS_ERR_SUM); + /* ANGLE_FRAC (12) * GAIN (8) * LOOP_FREQ (9) -> ANGLE_FRAX (12) */ + guidance_h_trim_att_integrator.x += ((guidance_h_igain * pd_x) >> (8)); + guidance_h_trim_att_integrator.y += ((guidance_h_igain * pd_y) >> (8)); + /* saturate it */ + VECT2_STRIM(guidance_h_trim_att_integrator, -(traj_max_bank << 12) , (traj_max_bank << 12)); + /* add it to the command */ + guidance_h_cmd_earth.x += guidance_h_trim_att_integrator.x >> 12; + guidance_h_cmd_earth.y += guidance_h_trim_att_integrator.y >> 12; } else { - INT_VECT2_ZERO(guidance_h_pos_err_sum); + INT_VECT2_ZERO(guidance_h_trim_att_integrator); } - /* run PID */ - guidance_h_command_earth.x = - ((guidance_h_pgain * guidance_h_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + - ((guidance_h_dgain * (guidance_h_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)) + - ((guidance_h_igain * (guidance_h_pos_err_sum.x >> 12)) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + - ((guidance_h_again * guidance_h_accel_ref.x) >> 8); /* feedforward gain */ - guidance_h_command_earth.y = - ((guidance_h_pgain * guidance_h_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + - ((guidance_h_dgain * (guidance_h_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)) + - ((guidance_h_igain * (guidance_h_pos_err_sum.y >> 12)) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + - ((guidance_h_again * guidance_h_accel_ref.y) >> 8); /* feedforward gain */ + /* compute a better approximation of force commands by taking thrust into account */ + if (guidance_h_approx_force_by_thrust && in_flight) { + static int32_t thrust_cmd_filt; + int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC; + thrust_cmd_filt = (thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + vertical_thrust) / (GUIDANCE_H_THRUST_CMD_FILTER + 1); + guidance_h_cmd_earth.x = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.x * MAX_PPRZ / INT32_ANGLE_PI_2), thrust_cmd_filt)); + guidance_h_cmd_earth.y = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.y * MAX_PPRZ / INT32_ANGLE_PI_2), thrust_cmd_filt)); + } - VECT2_STRIM(guidance_h_command_earth, -TRAJ_MAX_BANK, TRAJ_MAX_BANK); - - /* Rotate to body frame */ - int32_t s_psi, c_psi; - int32_t psi = stateGetNedToBodyEulers_i()->psi; - PPRZ_ITRIG_SIN(s_psi, psi); - PPRZ_ITRIG_COS(c_psi, psi); - - // Restore angle ref resolution after rotation - guidance_h_command_body.phi = - ( - s_psi * guidance_h_command_earth.x + c_psi * guidance_h_command_earth.y) >> INT32_TRIG_FRAC; - guidance_h_command_body.theta = - - ( c_psi * guidance_h_command_earth.x + s_psi * guidance_h_command_earth.y) >> INT32_TRIG_FRAC; - - - /* Add RC roll and pitch setpoints for emergency corrections */ - guidance_h_command_body.phi += guidance_h_rc_sp.phi; - guidance_h_command_body.theta += guidance_h_rc_sp.theta; - - /* Set attitude setpoint from pseudo-euler commands */ - stabilization_attitude_set_cmd_i(&guidance_h_command_body); + VECT2_STRIM(guidance_h_cmd_earth, -total_max_bank, total_max_bank); } static void guidance_h_hover_enter(void) { @@ -467,3 +505,34 @@ static inline void transition_run(void) { transition_theta_offset = INT_MULT_RSHIFT((transition_percentage<<(INT32_ANGLE_FRAC-INT32_PERCENTAGE_FRAC))/100, max_offset, INT32_ANGLE_FRAC); #endif } + +/// read speed setpoint from RC +static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flight) { + if(in_flight) { + // negative pitch is forward + int64_t rc_x = -radio_control.values[RADIO_PITCH]; + int64_t rc_y = radio_control.values[RADIO_ROLL]; + DeadBand(rc_x, MAX_PPRZ/20); + DeadBand(rc_y, MAX_PPRZ/20); + + // convert input from MAX_PPRZ range to SPEED_BFP + int32_t max_speed = SPEED_BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED); + /// @todo calc proper scale while making sure a division by zero can't occur + //int32_t rc_norm = sqrtf(rc_x * rc_x + rc_y * rc_y); + //int32_t max_pprz = rc_norm * MAX_PPRZ / Max(abs(rc_x), abs(rc_y); + rc_x = rc_x * max_speed / MAX_PPRZ; + rc_y = rc_y * max_speed / MAX_PPRZ; + + /* Rotate from body to NED frame by negative psi angle */ + int32_t psi = -stateGetNedToBodyEulers_i()->psi; + int32_t s_psi, c_psi; + PPRZ_ITRIG_SIN(s_psi, psi); + PPRZ_ITRIG_COS(c_psi, psi); + speed_sp->x = (int32_t)(((int64_t)c_psi * rc_x - (int64_t)s_psi * rc_y) >> INT32_TRIG_FRAC); + speed_sp->y = (int32_t)(((int64_t)s_psi * rc_x + (int64_t)c_psi * rc_y) >> INT32_TRIG_FRAC); + } + else { + speed_sp->x = 0; + speed_sp->y = 0; + } +} diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index 5a136317d5..f8a934ce43 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -53,6 +53,7 @@ extern uint8_t guidance_h_mode; extern bool_t guidance_h_use_ref; +extern bool_t guidance_h_approx_force_by_thrust; /** horizontal position setpoint in NED. * fixed point representation: Q23.8 @@ -66,12 +67,17 @@ extern struct Int32Vect2 guidance_h_accel_ref; ///< with #INT32_ACCEL_FRAC extern struct Int32Vect2 guidance_h_pos_err; extern struct Int32Vect2 guidance_h_speed_err; -extern struct Int32Vect2 guidance_h_pos_err_sum; +extern struct Int32Vect2 guidance_h_trim_att_integrator; extern struct Int32Vect2 guidance_h_nav_err; + +/** horizontal guidance command. + * In north/east with #INT32_ANGLE_FRAC + * @todo convert to real force command + */ +extern struct Int32Vect2 guidance_h_cmd_earth; extern struct Int32Eulers guidance_h_rc_sp; ///< with #INT32_ANGLE_FRAC -extern struct Int32Vect2 guidance_h_command_earth; -extern struct Int32Eulers guidance_h_command_body; ///< with #INT32_ANGLE_FRAC +extern int32_t guidance_h_heading_sp; ///< with #INT32_ANGLE_FRAC extern int32_t guidance_h_pgain; extern int32_t guidance_h_dgain; @@ -89,7 +95,7 @@ extern void guidance_h_run(bool_t in_flight); #define guidance_h_SetKi(_val) { \ guidance_h_igain = _val; \ - INT_VECT2_ZERO(guidance_h_pos_err_sum); \ + INT_VECT2_ZERO(guidance_h_trim_att_integrator); \ } /* Make sure that ref can only be temporarily disabled for testing, diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c index 34b23d212c..fb8be1fa67 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c @@ -47,18 +47,9 @@ struct Int32Vect2 gh_speed_ref; */ struct Int64Vect2 gh_pos_ref; -/** Accel saturation. - * tanf(RadOfDeg(30.))*9.81 = 5.66 - */ -#ifndef GUIDANCE_H_REF_MAX_ACCEL -#define GUIDANCE_H_REF_MAX_ACCEL 5.66 -#endif + static const int32_t gh_max_accel = BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC); -/** Speed saturation */ -#ifndef GUIDANCE_H_REF_MAX_SPEED -#define GUIDANCE_H_REF_MAX_SPEED 5. -#endif /** @todo GH_MAX_SPEED must be limited to 2^14 to avoid overflow */ #define GH_MAX_SPEED_REF_FRAC 7 static const int32_t gh_max_speed = BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GH_MAX_SPEED_REF_FRAC); diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h index ab285baa11..179097db55 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h @@ -31,6 +31,18 @@ #include "math/pprz_algebra.h" #include "math/pprz_algebra_int.h" +/** Speed saturation */ +#ifndef GUIDANCE_H_REF_MAX_SPEED +#define GUIDANCE_H_REF_MAX_SPEED 5. +#endif + +/** Accel saturation. + * tanf(RadOfDeg(30.))*9.81 = 5.66 + */ +#ifndef GUIDANCE_H_REF_MAX_ACCEL +#define GUIDANCE_H_REF_MAX_ACCEL 5.66 +#endif + /** Update frequency */ #define GH_FREQ_FRAC 9 diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 82e990fe8a..7385927dba 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -62,6 +62,19 @@ PRINT_CONFIG_VAR(GUIDANCE_V_NOMINAL_HOVER_THROTTLE) PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_THROTTLE_ENABLED) +#ifndef GUIDANCE_V_CLIMB_RC_DEADBAND +#define GUIDANCE_V_CLIMB_RC_DEADBAND MAX_PPRZ/10 +#endif + +#ifndef GUIDANCE_V_MAX_RC_CLIMB_SPEED +#define GUIDANCE_V_MAX_RC_CLIMB_SPEED GUIDANCE_V_REF_MIN_ZD +#endif + +#ifndef GUIDANCE_V_MAX_RC_DESCENT_SPEED +#define GUIDANCE_V_MAX_RC_DESCENT_SPEED GUIDANCE_V_REF_MAX_ZD +#endif + + uint8_t guidance_v_mode; int32_t guidance_v_ff_cmd; int32_t guidance_v_fb_cmd; @@ -94,14 +107,14 @@ int32_t guidance_v_ki; int32_t guidance_v_z_sum_err; -static int32_t guidance_v_thrust_coeff; +int32_t guidance_v_thrust_coeff; #define GuidanceVSetRef(_pos, _speed, _accel) { \ - gv_set_ref(_pos, _speed, _accel); \ - guidance_v_z_ref = _pos; \ - guidance_v_zd_ref = _speed; \ - guidance_v_zdd_ref = _accel; \ + gv_set_ref(_pos, _speed, _accel); \ + guidance_v_z_ref = _pos; \ + guidance_v_zd_ref = _speed; \ + guidance_v_zdd_ref = _accel; \ } static int32_t get_vertical_thrust_coeff(void); @@ -164,9 +177,18 @@ void guidance_v_read_rc(void) { guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE]; /* used in RC_CLIMB */ - guidance_v_rc_zd_sp = ((MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]) * GUIDANCE_V_RC_CLIMB_COEF; - DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_RC_CLIMB_DEAD_BAND); + guidance_v_rc_zd_sp = (MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]; + DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_CLIMB_RC_DEADBAND); + static const int32_t climb_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_CLIMB_SPEED) / + (MAX_PPRZ/2 - GUIDANCE_V_CLIMB_RC_DEADBAND)); + static const int32_t descent_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_DESCENT_SPEED) / + (MAX_PPRZ/2 - GUIDANCE_V_CLIMB_RC_DEADBAND)); + + if(guidance_v_rc_zd_sp > 0) + guidance_v_rc_zd_sp *= descent_scale; + else + guidance_v_rc_zd_sp *= climb_scale; } void guidance_v_mode_changed(uint8_t new_mode) { diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h index 4a94b4cfa8..93e0aeaa4d 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h @@ -91,6 +91,9 @@ extern float guidance_v_nominal_throttle; */ extern bool_t guidance_v_adapt_throttle_enabled; + +extern int32_t guidance_v_thrust_coeff; + extern int32_t guidance_v_kp; ///< vertical control P-gain extern int32_t guidance_v_kd; ///< vertical control D-gain extern int32_t guidance_v_ki; ///< vertical control I-gain diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c index 281827bb0b..31d1e2c0e4 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c @@ -58,14 +58,7 @@ int64_t gv_z_ref; #endif #define GV_MAX_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZDD, GV_ZDD_REF_FRAC) -#ifndef GUIDANCE_V_REF_MIN_ZD -#define GUIDANCE_V_REF_MIN_ZD (-3.) -#endif #define GV_MIN_ZD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD , GV_ZD_REF_FRAC) - -#ifndef GUIDANCE_V_REF_MAX_ZD -#define GUIDANCE_V_REF_MAX_ZD ( 3.) -#endif #define GV_MAX_ZD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD , GV_ZD_REF_FRAC) /* second order model natural frequency and damping */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h index f462a07a2b..3816a9031b 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h @@ -32,6 +32,16 @@ #include "math/pprz_algebra.h" #include "math/pprz_algebra_int.h" + +#ifndef GUIDANCE_V_REF_MIN_ZD +#define GUIDANCE_V_REF_MIN_ZD (-3.) +#endif + +#ifndef GUIDANCE_V_REF_MAX_ZD +#define GUIDANCE_V_REF_MAX_ZD ( 3.) +#endif + + /** Update frequency */ #define GV_FREQ_FRAC 9 diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index cbc7bef331..76ddcd9da7 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" @@ -71,10 +76,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 */ @@ -98,8 +104,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 ); @@ -109,8 +113,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 ) { @@ -139,7 +145,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(); @@ -175,8 +184,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 ) { @@ -190,10 +201,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 ) { @@ -260,7 +273,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); @@ -297,26 +312,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 30bcb9fd6b..2e99c54a76 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]; @@ -179,13 +183,11 @@ static inline void nav_advance_carrot(void) { void nav_run(void) { -#if !GUIDANCE_H_USE_REF - PRINT_CONFIG_MSG("NOT using horizontal guidance reference :-(") - nav_advance_carrot(); -#else - PRINT_CONFIG_MSG("Using horizontal guidance reference :-)") - // if H_REF is used, CARROT_DIST is not used +#if GUIDANCE_H_USE_REF + // if GUIDANCE_H_USE_REF, CARROT_DIST is not used VECT2_COPY(navigation_carrot, navigation_target); +#else + nav_advance_carrot(); #endif nav_set_altitude(); @@ -305,7 +307,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; @@ -315,9 +317,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; @@ -340,16 +342,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/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h index 94b47466e4..8917a2d93c 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h @@ -34,8 +34,9 @@ extern void stabilization_attitude_init(void); extern void stabilization_attitude_read_rc(bool_t in_flight); extern void stabilization_attitude_enter(void); extern void stabilization_attitude_set_failsafe_setpoint(void); -extern void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd); -extern void stabilization_attitude_run(bool_t in_flight); +extern void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy); +extern void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading); +extern void stabilization_attitude_run(bool_t in_flight); #endif /* STABILIZATION_ATTITUDE_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c index f70a684093..d98e976f97 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c @@ -134,10 +134,23 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_euler.psi = stateGetNedToBodyEulers_f()->psi; } -void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { - EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd); +void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { + EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *rpy); } +void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { + struct FloatVect2 cmd_f; + cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x); + cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y); + + /* Rotate horizontal commands to body frame by psi */ + float psi = stateGetNedToBodyEulers_f()->psi; + float s_psi = sinf(psi); + float c_psi = cosf(psi); + stab_att_sp_euler.phi = -s_psi * cmd_f.x + c_psi * cmd_f.y; + stab_att_sp_euler.theta = -c_psi * cmd_f.x - s_psi * cmd_f.y; + stab_att_sp_euler.psi = ANGLE_FLOAT_OF_BFP(heading); +} #define MAX_SUM_ERR RadOfDeg(56000) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index 47260a4928..3f5fed125f 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -149,10 +149,20 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi; } -void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { - memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers)); +void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { + memcpy(&stab_att_sp_euler, rpy, sizeof(struct Int32Eulers)); } +void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { + /* Rotate horizontal commands to body frame by psi */ + int32_t psi = stateGetNedToBodyEulers_i()->psi; + int32_t s_psi, c_psi; + PPRZ_ITRIG_SIN(s_psi, psi); + PPRZ_ITRIG_COS(c_psi, psi); + stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC; + stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC; + stab_att_sp_euler.psi = heading; +} #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b)) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index a5764fa859..ee7f42f067 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -25,6 +25,7 @@ #include "firmwares/rotorcraft/stabilization.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h" #include #include "math/pprz_algebra_float.h" @@ -142,75 +143,21 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_quat.qz = sinf(heading2); } -void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { - EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd); +void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { + // copy euler setpoint for debugging + EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *rpy); - /* orientation vector describing simultaneous rotation of roll/pitch */ - struct FloatVect3 ov; - ov.x = stab_att_sp_euler.phi; - ov.y = stab_att_sp_euler.theta; - ov.z = 0.0; - /* quaternion from that orientation vector */ - struct FloatQuat q_rp; - FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); + quat_from_rpy_cmd_f(&stab_att_sp_quat, &stab_att_sp_euler); +} - /// @todo optimize yaw angle calculation +void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { + struct FloatVect2 cmd_f; + cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x); + cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y); + float heading_f; + heading_f = ANGLE_FLOAT_OF_BFP(heading); - /* - * Instead of using the psi setpoint angle to rotate around the body z-axis, - * calculate the real angle needed to align the projection of the body x-axis - * onto the horizontal plane with the psi setpoint. - * - * angle between two vectors a and b: - * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n)) - * where n is the thrust vector (i.e. both a and b lie in that plane) - */ - const struct FloatVect3 xaxis = {1.0, 0.0, 0.0}; - const struct FloatVect3 zaxis = {0.0, 0.0, 1.0}; - struct FloatVect3 a; - FLOAT_QUAT_VMULT(a, q_rp, xaxis); - - // desired heading vect in earth x-y plane - struct FloatVect3 psi_vect; - psi_vect.x = cosf(stab_att_sp_euler.psi); - psi_vect.y = sinf(stab_att_sp_euler.psi); - psi_vect.z = 0.0; - // normal is the direction of the thrust vector - struct FloatVect3 normal; - FLOAT_QUAT_VMULT(normal, q_rp, zaxis); - - // projection of desired heading onto body x-y plane - // b = v - dot(v,n)*n - float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal); - struct FloatVect3 dotn; - FLOAT_VECT3_SMUL(dotn, normal, dot); - - // b = v - dot(v,n)*n - struct FloatVect3 b; - FLOAT_VECT3_DIFF(b, psi_vect, dotn); - dot = FLOAT_VECT3_DOT_PRODUCT(a, b); - struct FloatVect3 cross; - VECT3_CROSS_PRODUCT(cross, a, b); - // norm of the cross product - float nc = FLOAT_VECT3_NORM(cross); - // angle = atan2(norm(cross(a,b)), dot(a,b)) - float yaw2 = atan2(nc, dot) / 2.0; - - // negative angle if needed - // sign(dot(cross(a,b), n) - float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, normal); - if (dot_cross_ab < 0) { - yaw2 = -yaw2; - } - - /* quaternion with yaw command */ - struct FloatQuat q_yaw; - QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); - - /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ - FLOAT_QUAT_COMP(stab_att_sp_quat, q_yaw, q_rp); - FLOAT_QUAT_NORMALIZE(stab_att_sp_quat); - FLOAT_QUAT_WRAP_SHORTEST(stab_att_sp_quat); + quat_from_earth_cmd_f(&stab_att_sp_quat, &cmd_f, heading_f); } #ifndef GAIN_PRESCALER_FF diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h index 9077bbd8cb..8655fc23ac 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h @@ -31,13 +31,6 @@ #include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h" -#ifndef STABILIZATION_ATTITUDE_GAIN_NB -#define STABILIZATION_ATTITUDE_GAIN_NB 1 -#endif - -#ifndef STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT -#define STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT 0 -#endif extern struct FloatAttitudeGains stabilization_gains[]; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index cc64b8567e..311325b724 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -25,6 +25,7 @@ #include "firmwares/rotorcraft/stabilization.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h" #include #include "math/pprz_algebra_float.h" @@ -137,85 +138,27 @@ void stabilization_attitude_set_failsafe_setpoint(void) { PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2); } -void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { - // copy euler setpoint for debugging - memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers)); +void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { + // stab_att_sp_euler.psi still used in ref.. + memcpy(&stab_att_sp_euler, rpy, sizeof(struct Int32Eulers)); - /// @todo calc sp_quat in fixed-point + quat_from_rpy_cmd_i(&stab_att_sp_quat, &stab_att_sp_euler); +} - /* orientation vector describing simultaneous rotation of roll/pitch */ - struct FloatVect3 ov; - ov.x = ANGLE_FLOAT_OF_BFP(sp_cmd->phi); - ov.y = ANGLE_FLOAT_OF_BFP(sp_cmd->theta); - ov.z = 0.0; - /* quaternion from that orientation vector */ - struct FloatQuat q_rp; - FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); +void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { + // stab_att_sp_euler.psi still used in ref.. + stab_att_sp_euler.psi = heading; - const float psi_sp = ANGLE_FLOAT_OF_BFP(sp_cmd->psi); + // compute sp_euler phi/theta for debugging/telemetry + /* Rotate horizontal commands to body frame by psi */ + int32_t psi = stateGetNedToBodyEulers_i()->psi; + int32_t s_psi, c_psi; + PPRZ_ITRIG_SIN(s_psi, psi); + PPRZ_ITRIG_COS(c_psi, psi); + stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC; + stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC; - /// @todo optimize yaw angle calculation - - /* - * Instead of using the psi setpoint angle to rotate around the body z-axis, - * calculate the real angle needed to align the projection of the body x-axis - * onto the horizontal plane with the psi setpoint. - * - * angle between two vectors a and b: - * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n)) - * where n is the thrust vector (i.e. both a and b lie in that plane) - */ - const struct FloatVect3 xaxis = {1.0, 0.0, 0.0}; - const struct FloatVect3 zaxis = {0.0, 0.0, 1.0}; - struct FloatVect3 a; - FLOAT_QUAT_VMULT(a, q_rp, xaxis); - - // desired heading vect in earth x-y plane - struct FloatVect3 psi_vect; - psi_vect.x = cosf(psi_sp); - psi_vect.y = sinf(psi_sp); - psi_vect.z = 0.0; - // normal is the direction of the thrust vector - struct FloatVect3 normal; - FLOAT_QUAT_VMULT(normal, q_rp, zaxis); - - // projection of desired heading onto body x-y plane - // b = v - dot(v,n)*n - float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal); - struct FloatVect3 dotn; - FLOAT_VECT3_SMUL(dotn, normal, dot); - - // b = v - dot(v,n)*n - struct FloatVect3 b; - FLOAT_VECT3_DIFF(b, psi_vect, dotn); - - dot = FLOAT_VECT3_DOT_PRODUCT(a, b); - struct FloatVect3 cross; - VECT3_CROSS_PRODUCT(cross, a, b); - // norm of the cross product - float nc = FLOAT_VECT3_NORM(cross); - // angle = atan2(norm(cross(a,b)), dot(a,b)) - float yaw2 = atan2(nc, dot) / 2.0; - - // negative angle if needed - // sign(dot(cross(a,b), n) - float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, normal); - if (dot_cross_ab < 0) { - yaw2 = -yaw2; - } - - /* quaternion with yaw command */ - struct FloatQuat q_yaw; - QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); - - /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ - struct FloatQuat q_sp; - FLOAT_QUAT_COMP(q_sp, q_yaw, q_rp); - FLOAT_QUAT_NORMALIZE(q_sp); - FLOAT_QUAT_WRAP_SHORTEST(q_sp); - - /* convert to fixed point */ - QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp); + quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading); } #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c new file mode 100644 index 0000000000..12ca369d62 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c @@ -0,0 +1,131 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file stabilization_attitude_quat_transformations.c + * Quaternion transformation functions. + */ + +#include "stabilization_attitude_quat_transformations.h" + +void quat_from_rpy_cmd_i(struct Int32Quat *quat, struct Int32Eulers *rpy) { + struct FloatEulers rpy_f; + EULERS_FLOAT_OF_BFP(rpy_f, *rpy); + struct FloatQuat quat_f; + quat_from_rpy_cmd_f(&quat_f, &rpy_f); + QUAT_BFP_OF_REAL(*quat, quat_f); +} + +void quat_from_rpy_cmd_f(struct FloatQuat *quat, struct FloatEulers *rpy) { + // only a plug for now... doesn't apply roll/pitch wrt. current yaw angle + + /* orientation vector describing simultaneous rotation of roll/pitch/yaw */ + const struct FloatVect3 ov = {rpy->phi, rpy->theta, rpy->psi}; + /* quaternion from that orientation vector */ + FLOAT_QUAT_OF_ORIENTATION_VECT(*quat, ov); + +} + +void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading) { + // use float conversion for now... + struct FloatVect2 cmd_f; + cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x); + cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y); + float heading_f = ANGLE_FLOAT_OF_BFP(heading); + + struct FloatQuat quat_f; + quat_from_earth_cmd_f(&quat_f, &cmd_f, heading_f); + + // convert back to fixed point + QUAT_BFP_OF_REAL(*quat, quat_f); +} + +void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading) { + + /* cmd_x is positive to north = negative pitch + * cmd_y is positive to east = positive roll + * + * orientation vector describing simultaneous rotation of roll/pitch + */ + const struct FloatVect3 ov = {cmd->y, -cmd->x, 0.0}; + /* quaternion from that orientation vector */ + struct FloatQuat q_rp; + FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); + + /* as rotation matrix */ + struct FloatRMat R_rp; + FLOAT_RMAT_OF_QUAT(R_rp, q_rp); + /* body x-axis (before heading command) is first column */ + struct FloatVect3 b_x; + VECT3_ASSIGN(b_x, R_rp.m[0], R_rp.m[3], R_rp.m[6]); + /* body z-axis (thrust vect) is last column */ + struct FloatVect3 thrust_vect; + VECT3_ASSIGN(thrust_vect, R_rp.m[2], R_rp.m[5], R_rp.m[8]); + + /// @todo optimize yaw angle calculation + + /* + * Instead of using the psi setpoint angle to rotate around the body z-axis, + * calculate the real angle needed to align the projection of the body x-axis + * onto the horizontal plane with the psi setpoint. + * + * angle between two vectors a and b: + * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n)) + * where the normal n is the thrust vector (i.e. both a and b lie in that plane) + */ + + // desired heading vect in earth x-y plane + const struct FloatVect3 psi_vect = {cosf(heading), sinf(heading), 0.0}; + + /* projection of desired heading onto body x-y plane + * b = v - dot(v,n)*n + */ + float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, thrust_vect); + struct FloatVect3 dotn; + FLOAT_VECT3_SMUL(dotn, thrust_vect, dot); + + // b = v - dot(v,n)*n + struct FloatVect3 b; + FLOAT_VECT3_DIFF(b, psi_vect, dotn); + dot = FLOAT_VECT3_DOT_PRODUCT(b_x, b); + struct FloatVect3 cross; + VECT3_CROSS_PRODUCT(cross, b_x, b); + // norm of the cross product + float nc = FLOAT_VECT3_NORM(cross); + // angle = atan2(norm(cross(a,b)), dot(a,b)) + float yaw2 = atan2(nc, dot) / 2.0; + + // negative angle if needed + // sign(dot(cross(a,b), n) + float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, thrust_vect); + if (dot_cross_ab < 0) { + yaw2 = -yaw2; + } + + /* quaternion with yaw command */ + struct FloatQuat q_yaw; + QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); + + /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ + FLOAT_QUAT_COMP(*quat, q_rp, q_yaw); + FLOAT_QUAT_NORMALIZE(*quat); + FLOAT_QUAT_WRAP_SHORTEST(*quat); + +} diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h new file mode 100644 index 0000000000..1c57038b3f --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file stabilization_attitude_quat_transformations.h + * Quaternion transformation functions. + */ + +#ifndef STABILIZATION_ATTITUDE_QUAT_TRANSFORMATIONS_H +#define STABILIZATION_ATTITUDE_QUAT_TRANSFORMATIONS_H + +#include "math/pprz_algebra_float.h" +#include "math/pprz_algebra_int.h" + +extern void quat_from_rpy_cmd_i(struct Int32Quat *quat, struct Int32Eulers *rpy); +extern void quat_from_rpy_cmd_f(struct FloatQuat *quat, struct FloatEulers *rpy); + +extern void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading); +extern void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading); + +#endif diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c index 7054d1824b..0affbcd21b 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c @@ -221,15 +221,14 @@ void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bo * @param[out] q quaternion representing the RC roll/pitch input */ void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat* q) { - q->qx = radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ / 2; - q->qy = radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ / 2; - q->qz = 0.0; + /* orientation vector describing simultaneous rotation of roll/pitch */ + struct FloatVect3 ov; + ov.x = radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ / 2; + ov.y = radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ / 2; + ov.z = 0.0; - /* normalize */ - float norm = sqrtf(1.0 + SQUARE(q->qx)+ SQUARE(q->qy)); - q->qi = 1.0 / norm; - q->qx /= norm; - q->qy /= norm; + /* quaternion from that orientation vector */ + FLOAT_QUAT_OF_ORIENTATION_VECT(*q, ov); } /** Read roll/pitch command from RC as quaternion. diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c index a70e780560..04e3e60b10 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c @@ -1,4 +1,5 @@ -#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h" #include "generated/airframe.h" struct FloatEulers stab_att_sp_euler; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h index 762e66ed96..ba55615872 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h @@ -22,10 +22,7 @@ #ifndef STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H #define STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H -#include "math/pprz_algebra_float.h" - #include "stabilization_attitude_ref_float.h" -#include "stabilization_attitude_ref.h" void stabilization_attitude_ref_enter(void); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c index d7e8566a8b..14472fbb03 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c @@ -24,7 +24,8 @@ * */ -#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h" #include "generated/airframe.h" struct Int32Eulers stab_att_sp_euler; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h index bf3aacf19c..08579a412a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h @@ -28,7 +28,5 @@ #define STABILIZATION_ATTITUDE_REF_EULER_INT_H #include "stabilization_attitude_ref_int.h" -#include "stabilization_attitude_ref.h" - #endif /* STABILIZATION_ATTITUDE_REF_EULER_INT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h index 2faf9e0bc9..fb62bcc457 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h @@ -27,9 +27,7 @@ #ifndef STABILIZATION_ATTITUDE_REF_FLOAT_H #define STABILIZATION_ATTITUDE_REF_FLOAT_H -#include "generated/airframe.h" - -#include "state.h" +#include "math/pprz_algebra_float.h" extern struct FloatEulers stab_att_sp_euler; extern struct FloatQuat stab_att_sp_quat; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h index c0ef65a2b8..24175b5fc2 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h @@ -29,8 +29,6 @@ #include "math/pprz_algebra_int.h" -#include "state.h" - extern struct Int32Eulers stab_att_ref_euler; ///< with #REF_ANGLE_FRAC extern struct Int32Quat stab_att_ref_quat; ///< with #INT32_QUAT_FRAC extern struct Int32Rates stab_att_ref_rate; ///< with #REF_RATE_FRAC diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c index da50880f15..ff15cf95a8 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c @@ -28,10 +28,9 @@ */ #include "generated/airframe.h" -#include "firmwares/rotorcraft/stabilization.h" -#include "state.h" -#include "stabilization_attitude_ref_float.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h" #define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_REF_MAX_PDOT #define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_QDOT diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h index 0424fbf820..b5543ed82b 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h @@ -30,14 +30,15 @@ #ifndef STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_FLOAT_H #define STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_FLOAT_H -#include "subsystems/radio_control.h" -#include "math/pprz_algebra_float.h" - #include "stabilization_attitude_ref_float.h" -#include "stabilization_attitude_ref.h" -#define DEADBAND_EXCEEDED(VARIABLE, VALUE) ((VARIABLE > VALUE) || (VARIABLE < -VALUE)) -#define APPLY_DEADBAND(VARIABLE, VALUE) (DEADBAND_EXCEEDED(VARIABLE, VALUE) ? VARIABLE : 0.0) +#ifndef STABILIZATION_ATTITUDE_GAIN_NB +#define STABILIZATION_ATTITUDE_GAIN_NB 1 +#endif + +#ifndef STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT +#define STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT 0 +#endif void stabilization_attitude_ref_enter(void); void stabilization_attitude_ref_schedule(uint8_t idx); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c index b0202847dd..aacd33d198 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c @@ -28,9 +28,8 @@ */ #include "generated/airframe.h" -#include "firmwares/rotorcraft/stabilization.h" - -#include "stabilization_attitude_ref_int.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h" #define REF_ACCEL_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_PDOT, REF_ACCEL_FRAC) #define REF_ACCEL_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_QDOT, REF_ACCEL_FRAC) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h index 3b1cbaa3e8..4fe03f3218 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h @@ -31,7 +31,6 @@ #define STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H #include "stabilization_attitude_ref_int.h" -#include "stabilization_attitude_ref.h" void stabilization_attitude_ref_enter(void); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h similarity index 92% rename from sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h rename to sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h index 11d41dfb39..5bb4efb06d 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h @@ -19,12 +19,12 @@ * Boston, MA 02111-1307, USA. */ -/** @file firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h - * Common rotorcraft attitude reference generation include. +/** @file firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h + * Common rotorcraft attitude reference saturation include. */ -#ifndef STABILIZATION_ATTITUDE_REF_H -#define STABILIZATION_ATTITUDE_REF_H +#ifndef STABILIZATION_ATTITUDE_REF_SATURATE_H +#define STABILIZATION_ATTITUDE_REF_SATURATE_H #define SATURATE_SPEED_TRIM_ACCEL() { \ if (stab_att_ref_rate.p >= REF_RATE_MAX_P) { \ @@ -60,4 +60,4 @@ } -#endif /* STABILIZATION_ATTITUDE_REF_H */ +#endif /* STABILIZATION_ATTITUDE_REF_SATURATE_H */ diff --git a/sw/airborne/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_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h index 50f7a1e88c..70d2ea22ae 100644 --- a/sw/airborne/math/pprz_algebra_int.h +++ b/sw/airborne/math/pprz_algebra_int.h @@ -218,6 +218,13 @@ struct Int64Vect3 { INT32_SQRT(n, n2); \ } +#define INT32_VECT2_NORMALIZE(_v,_frac) { \ + int32_t n; \ + INT32_VECT2_NORM(n, _v); \ + INT32_VECT2_SCALE_2(_v, _v, BFP_OF_REAL((1.),_frac) , n); \ + } + + #define INT32_VECT2_RSHIFT(_o, _i, _r) { \ (_o).x = ((_i).x >> (_r)); \ (_o).y = ((_i).y >> (_r)); \ diff --git a/sw/airborne/math/pprz_geodetic_int.c b/sw/airborne/math/pprz_geodetic_int.c index d66e3cfe2a..0562689d24 100644 --- a/sw/airborne/math/pprz_geodetic_int.c +++ b/sw/airborne/math/pprz_geodetic_int.c @@ -92,11 +92,40 @@ void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct Ece void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) { - struct EnuCoor_i enu; enu_of_ecef_point_i(&enu, def, ecef); ENU_OF_TO_NED(*ned, enu); +} + +/** Convert a ECEF position to local ENU. + * @param[out] enu ENU position in meter << #INT32_POS_FRAC + * @param[in] def local coordinate system definition + * @param[in] ecef ECEF position in cm + */ +void enu_of_ecef_pos_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) { + struct EnuCoor_i enu_cm; + enu_of_ecef_point_i(&enu_cm, def, ecef); + + /* enu = (enu_cm / 100) << INT32_POS_FRAC + * to loose less range: + * enu_cm = enu << (INT32_POS_FRAC-2) / 25 + * which puts max enu output Q23.8 range to 8388km / 25 = 335km + */ + INT32_VECT3_LSHIFT(*enu, enu_cm, INT32_POS_FRAC-2); + VECT3_SDIV(*enu, *enu, 25); +} + + +/** Convert a ECEF position to local NED. + * @param[out] ned NED position in meter << #INT32_POS_FRAC + * @param[in] def local coordinate system definition + * @param[in] ecef ECEF position in cm + */ +void ned_of_ecef_pos_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) { + struct EnuCoor_i enu; + enu_of_ecef_pos_i(&enu, def, ecef); + ENU_OF_TO_NED(*ned, enu); } void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) { @@ -149,11 +178,23 @@ void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct Ne ecef_of_enu_vect_i(ecef, def, &enu); } + +/** Convert a point in local ENU to ECEF. + * @param[out] ecef ECEF point in cm + * @param[in] def local coordinate system definition + * @param[in] enu ENU point in cm + */ void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) { ecef_of_enu_vect_i(ecef, def, enu); INT32_VECT3_ADD(*ecef, def->ecef); } + +/** Convert a point in local NED to ECEF. + * @param[out] ecef ECEF point in cm + * @param[in] def local coordinate system definition + * @param[in] ned NED point in cm + */ void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) { struct EnuCoor_i enu; ENU_OF_TO_NED(enu, *ned); @@ -161,6 +202,37 @@ void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct N } +/** Convert a local ENU position to ECEF. + * @param[out] ecef ECEF position in cm + * @param[in] def local coordinate system definition + * @param[in] enu ENU position in meter << #INT32_POS_FRAC + */ +void ecef_of_enu_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) { + /* enu_cm = (enu * 100) >> INT32_POS_FRAC + * to loose less range: + * enu_cm = (enu * 25) >> (INT32_POS_FRAC-2) + * which puts max enu input Q23.8 range to 8388km / 25 = 335km + */ + struct EnuCoor_i enu_cm; + VECT3_SMUL(enu_cm, *enu, 25); + INT32_VECT3_RSHIFT(enu_cm, enu_cm, INT32_POS_FRAC-2); + ecef_of_enu_vect_i(ecef, def, &enu_cm); + INT32_VECT3_ADD(*ecef, def->ecef); +} + + +/** Convert a local NED position to ECEF. + * @param[out] ecef ECEF position in cm + * @param[in] def local coordinate system definition + * @param[in] ned NED position in meter << #INT32_POS_FRAC + */ +void ecef_of_ned_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) { + struct EnuCoor_i enu; + ENU_OF_TO_NED(enu, *ned); + ecef_of_enu_pos_i(ecef, def, &enu); +} + + void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) { struct EcefCoor_i ecef; ecef_of_lla_i(&ecef,lla); diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h index f17ff2dc5a..cbeac4ac5f 100644 --- a/sw/airborne/math/pprz_geodetic_int.h +++ b/sw/airborne/math/pprz_geodetic_int.h @@ -104,6 +104,8 @@ extern void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in); extern void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in); extern void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef); extern void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef); +extern void enu_of_ecef_pos_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef); +extern void ned_of_ecef_pos_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef); extern void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef); extern void ned_of_ecef_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef); extern void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla); @@ -112,6 +114,8 @@ extern void enu_of_lla_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struc extern void ned_of_lla_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla); extern void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu); extern void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned); +extern void ecef_of_enu_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu); +extern void ecef_of_ned_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned); extern void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu); extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned); 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 a73ca98e40..d570b58d33 100644 --- a/sw/airborne/modules/cam_control/cam_track.c +++ b/sw/airborne/modules/cam_control/cam_track.c @@ -64,7 +64,7 @@ static void send_cam_track(void) { } 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; @@ -129,8 +129,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; } @@ -144,24 +144,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/mission/mission.c b/sw/airborne/modules/mission/mission.c new file mode 100644 index 0000000000..d3794bbca9 --- /dev/null +++ b/sw/airborne/modules/mission/mission.c @@ -0,0 +1,251 @@ +/* + * Copyright (C) 2013 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 modules/mission/mission.c + * @brief messages parser for mission interface + */ + +#include "modules/mission/mission.h" + +#include +#include "generated/airframe.h" +#include "subsystems/datalink/datalink.h" +#include "subsystems/datalink/downlink.h" + + +struct _mission mission; + + +void mission_init(void) { + mission.insert_idx = 0; + mission.current_idx = 0; + mission.element_time = 0.; +} + +bool_t mission_insert(enum MissionInsertMode insert, struct _mission_element * element) { + uint8_t tmp; + + switch (insert) { + case Append: + tmp = (mission.insert_idx + 1) % MISSION_ELEMENT_NB; + if (tmp == mission.current_idx) return FALSE; // no room to insert element + memcpy(&mission.elements[mission.insert_idx], element, sizeof(struct _mission_element)); // add element + mission.insert_idx = tmp; // move insert index + break; + case Prepend: + if (mission.current_idx == 0) tmp = MISSION_ELEMENT_NB-1; + else tmp = mission.current_idx - 1; + if (tmp == mission.insert_idx) return FALSE; // no room to inser element + memcpy(&mission.elements[tmp], element, sizeof(struct _mission_element)); // add element + mission.current_idx = tmp; // move current index + break; + case ReplaceCurrent: + // current element can always be modified, index are not changed + memcpy(&mission.elements[mission.current_idx], element, sizeof(struct _mission_element)); + break; + case ReplaceAll: + // reset queue and index + memcpy(&mission.elements[0], element, sizeof(struct _mission_element)); + mission.insert_idx = 0; + mission.current_idx = 0; + break; + default: + // unknown insertion mode + return FALSE; + } + return TRUE; + +} + +struct _mission_element * mission_get(void) { + if (mission.current_idx == mission.insert_idx) { + return NULL; + } + return &(mission.elements[mission.current_idx]); +} + + + +/////////////////////// +// Parsing functions // +/////////////////////// + +int mission_parse_GOTO_WP(void) { + if (DL_MISSION_GOTO_WP_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + struct _mission_element me; + me.type = MissionWP; + me.element.mission_wp.wp.x = DL_MISSION_GOTO_WP_wp_east(dl_buffer); + me.element.mission_wp.wp.y = DL_MISSION_GOTO_WP_wp_north(dl_buffer); + me.element.mission_wp.wp.z = DL_MISSION_GOTO_WP_wp_alt(dl_buffer); + me.duration = DL_MISSION_GOTO_WP_duration(dl_buffer); + + enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_GOTO_WP_insert(dl_buffer)); + + return mission_insert(insert, &me); +} + +int mission_parse_GOTO_WP_LLA(void) { + if (DL_MISSION_GOTO_WP_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + struct _mission_element me; + me.type = MissionWP; + + enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_GOTO_WP_LLA_insert(dl_buffer)); + + return mission_insert(insert, &me); +} + +int mission_parse_CIRCLE(void) { + if (DL_MISSION_CIRCLE_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + struct _mission_element me; + me.type = MissionCircle; + me.element.mission_circle.center.x = DL_MISSION_CIRCLE_center_east(dl_buffer); + me.element.mission_circle.center.y = DL_MISSION_CIRCLE_center_north(dl_buffer); + me.element.mission_circle.center.z = DL_MISSION_CIRCLE_center_alt(dl_buffer); + me.element.mission_circle.radius = DL_MISSION_CIRCLE_radius(dl_buffer); + me.duration = DL_MISSION_CIRCLE_duration(dl_buffer); + + enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_CIRCLE_insert(dl_buffer)); + + return mission_insert(insert, &me); +} + +int mission_parse_CIRCLE_LLA(void) { + if (DL_MISSION_CIRCLE_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + struct _mission_element me; + me.type = MissionCircle; + me.element.mission_circle.radius = DL_MISSION_CIRCLE_LLA_radius(dl_buffer); + me.duration = DL_MISSION_CIRCLE_LLA_duration(dl_buffer); + + enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_CIRCLE_LLA_insert(dl_buffer)); + + return mission_insert(insert, &me); +} + +int mission_parse_SEGMENT(void) { + if (DL_MISSION_SEGMENT_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + struct _mission_element me; + me.type = MissionSegment; + me.element.mission_segment.from.x = DL_MISSION_SEGMENT_segment_east_1(dl_buffer); + me.element.mission_segment.from.y = DL_MISSION_SEGMENT_segment_north_1(dl_buffer); + me.element.mission_segment.from.z = DL_MISSION_SEGMENT_segment_alt(dl_buffer); + me.element.mission_segment.to.x = DL_MISSION_SEGMENT_segment_east_2(dl_buffer); + me.element.mission_segment.to.y = DL_MISSION_SEGMENT_segment_north_2(dl_buffer); + me.element.mission_segment.to.z = DL_MISSION_SEGMENT_segment_alt(dl_buffer); + me.duration = DL_MISSION_SEGMENT_duration(dl_buffer); + + enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_SEGMENT_insert(dl_buffer)); + + return mission_insert(insert, &me); +} + +int mission_parse_SEGMENT_LLA(void) { + if (DL_MISSION_SEGMENT_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + struct _mission_element me; + me.type = MissionSegment; + me.duration = DL_MISSION_SEGMENT_LLA_duration(dl_buffer); + + enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_SEGMENT_LLA_insert(dl_buffer)); + + return mission_insert(insert, &me); +} + +int mission_parse_PATH(void) { + if (DL_MISSION_PATH_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + struct _mission_element me; + me.type = MissionPath; + me.element.mission_path.path[0].x = DL_MISSION_PATH_point_east_1(dl_buffer); + me.element.mission_path.path[0].y = DL_MISSION_PATH_point_north_1(dl_buffer); + me.element.mission_path.path[0].z = DL_MISSION_PATH_path_alt(dl_buffer); + me.element.mission_path.path[1].x = DL_MISSION_PATH_point_east_2(dl_buffer); + me.element.mission_path.path[1].y = DL_MISSION_PATH_point_north_2(dl_buffer); + me.element.mission_path.path[1].z = DL_MISSION_PATH_path_alt(dl_buffer); + me.element.mission_path.path[2].x = DL_MISSION_PATH_point_east_3(dl_buffer); + me.element.mission_path.path[2].y = DL_MISSION_PATH_point_north_3(dl_buffer); + me.element.mission_path.path[2].z = DL_MISSION_PATH_path_alt(dl_buffer); + me.element.mission_path.path[3].x = DL_MISSION_PATH_point_east_4(dl_buffer); + me.element.mission_path.path[3].y = DL_MISSION_PATH_point_north_4(dl_buffer); + me.element.mission_path.path[3].z = DL_MISSION_PATH_path_alt(dl_buffer); + me.element.mission_path.path[4].x = DL_MISSION_PATH_point_east_5(dl_buffer); + me.element.mission_path.path[4].y = DL_MISSION_PATH_point_north_5(dl_buffer); + me.element.mission_path.path[4].z = DL_MISSION_PATH_path_alt(dl_buffer); + me.element.mission_path.nb = DL_MISSION_PATH_nb(dl_buffer); + if (me.element.mission_path.nb > MISSION_PATH_NB) me.element.mission_path.nb = MISSION_PATH_NB; + me.element.mission_path.path_idx = 0; + me.duration = DL_MISSION_PATH_duration(dl_buffer); + + enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_PATH_insert(dl_buffer)); + + return mission_insert(insert, &me); +} + +int mission_parse_PATH_LLA(void) { + if (DL_MISSION_PATH_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + struct _mission_element me; + me.type = MissionPath; + me.element.mission_path.nb = DL_MISSION_PATH_LLA_nb(dl_buffer); + if (me.element.mission_path.nb > MISSION_PATH_NB) me.element.mission_path.nb = MISSION_PATH_NB; + me.element.mission_path.path_idx = 0; + me.duration = DL_MISSION_PATH_LLA_duration(dl_buffer); + + enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_PATH_LLA_insert(dl_buffer)); + + return mission_insert(insert, &me); +} + +int mission_parse_GOTO_MISSION(void) { + if (DL_GOTO_MISSION_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + uint8_t mission_id = DL_GOTO_MISSION_mission_id(dl_buffer); + if (mission_id < MISSION_ELEMENT_NB) { + mission.current_idx = mission_id; + } + else return FALSE; + + return TRUE; +} + +int mission_parse_NEXT_MISSION(void) { + if (DL_NEXT_MISSION_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + if (mission.current_idx == mission.insert_idx) return FALSE; // already at the last position + + // increment current index + mission.current_idx = (mission.current_idx + 1) % MISSION_ELEMENT_NB; + return TRUE; +} + +int mission_parse_END_MISSION(void) { + if (DL_END_MISSION_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + // set current index to insert index (last position) + mission.current_idx = mission.insert_idx; + return TRUE; +} + diff --git a/sw/airborne/modules/mission/mission.h b/sw/airborne/modules/mission/mission.h new file mode 100644 index 0000000000..d423c48c12 --- /dev/null +++ b/sw/airborne/modules/mission/mission.h @@ -0,0 +1,142 @@ +/* + * 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 modules/mission/mission.h + * @brief mission planner library + * + * Provide the generic interface for the mission control + * Handle the parsing of datalink messages + */ + +#ifndef MISSION_H +#define MISSION_H + +#include "std.h" +#include "math/pprz_geodetic_float.h" + +enum MissionType { + MissionWP, + MissionCircle, + MissionSegment, + MissionPath, + MissionSurvey, + MissionEight, + MissionOval +}; + +enum MissionInsertMode { + Append, ///< add at the last position + Prepend, ///< add before the current element + ReplaceCurrent, ///< replace current element + ReplaceAll ///< remove all elements and add the new one +}; + +struct _mission_wp { + struct EnuCoor_f wp; +}; + +struct _mission_circle { + struct EnuCoor_f center; + float radius; +}; + +struct _mission_segment { + struct EnuCoor_f from; + struct EnuCoor_f to; +}; + +#define MISSION_PATH_NB 5 +struct _mission_path { + struct EnuCoor_f path[MISSION_PATH_NB]; + uint8_t path_idx; + uint8_t nb; +}; + +struct _mission_element { + enum MissionType type; + union { + struct _mission_wp mission_wp; + struct _mission_circle mission_circle; + struct _mission_segment mission_segment; + struct _mission_path mission_path; + } element; + float duration; ///< time to spend in the element (<= 0 to disable) +}; + +#define MISSION_ELEMENT_NB 20 +struct _mission { + struct _mission_element elements[MISSION_ELEMENT_NB]; + float element_time; ///< time in second spend in the current element + uint8_t insert_idx; ///< inserstion index + uint8_t current_idx; ///< current mission element index +}; + +extern struct _mission mission; + +/** Init mission structure + */ +extern void mission_init(void); + +/** Insert a mission element according to the insertion mode + * @param insert insertion mode + * @param element mission element structure + * @return return TRUE if insertion is succesful, FALSE otherwise + */ +extern bool_t mission_insert(enum MissionInsertMode insert, struct _mission_element * element); + +/** Get current mission element + * @return return a pointer to the next mission element or NULL if no more elements + */ +extern struct _mission_element * mission_get(void); + +/** Run mission + * + * This function should be implemented into a dedicated file since + * navigation functions are different for different firmwares + * + * Currently, this function should be called from the flight plan + * + * @return return TRUE when the mission is running, FALSE when it is finished + */ +extern int mission_run(); + +/** Parsing functions called when a mission message is received + */ +extern int mission_parse_GOTO_WP(void); +extern int mission_parse_GOTO_WP_LLA(void); +extern int mission_parse_CIRCLE(void); +extern int mission_parse_CIRCLE_LLA(void); +extern int mission_parse_SEGMENT(void); +extern int mission_parse_SEGMENT_LLA(void); +extern int mission_parse_PATH(void); +extern int mission_parse_PATH_LLA(void); +extern int mission_parse_SURVEY(void); +extern int mission_parse_SURVEY_LLA(void); +extern int mission_parse_GOTO_MISSION(void); +extern int mission_parse_NEXT_MISSION(void); +extern int mission_parse_END_MISSION(void); + +/** Status report messages + * @todo + */ + +#endif // MISSION diff --git a/sw/airborne/modules/mission/mission_fw_nav.c b/sw/airborne/modules/mission/mission_fw_nav.c new file mode 100644 index 0000000000..f0b24e8094 --- /dev/null +++ b/sw/airborne/modules/mission/mission_fw_nav.c @@ -0,0 +1,146 @@ +/* + * 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 modules/mission/mission_fw_nav.c + * @brief mission navigation for fixedwing aircraft + * + * Implement specific navigation routines for the mission control + * of a fixedwing aircraft + */ + +#include +#include "modules/mission/mission.h" +#include "firmwares/fixedwing/autopilot.h" +#include "subsystems/nav.h" + +// navigation time step +const float dt_navigation = 1.0 / ((float)NAVIGATION_FREQUENCY); + +// dirty hack to comply with nav_approaching_xy function +struct EnuCoor_f last_wp = { 0., 0., 0. }; + +/** Navigation function to a single waypoint + */ +static inline bool_t mission_nav_wp(struct _mission_wp * wp) { + if (nav_approaching_xy(wp->wp.x, wp->wp.y, last_wp.x, last_wp.y, CARROT)) { + last_wp = wp->wp; // store last wp + return FALSE; // end of mission element + } + // set navigation command + fly_to_xy(wp->wp.x, wp->wp.y); + NavVerticalAutoThrottleMode(0.); + NavVerticalAltitudeMode(wp->wp.z, 0.); + return TRUE; +} + +/** Navigation function on a circle + */ +static inline bool_t mission_nav_circle(struct _mission_circle * circle) { + nav_circle_XY(circle->center.x, circle->center.y, circle->radius); + NavVerticalAutoThrottleMode(0.); + NavVerticalAltitudeMode(circle->center.z, 0.); + return TRUE; +} + +/** Navigation function along a segment + */ +static inline bool_t mission_nav_segment(struct _mission_segment * segment) { + if (nav_approaching_xy(segment->to.x, segment->to.y, segment->from.x, segment->from.y, CARROT)) { + last_wp = segment->to; + return FALSE; // end of mission element + } + nav_route_xy(segment->from.x, segment->from.y, segment->to.x, segment->to.y); + NavVerticalAutoThrottleMode(0.); + NavVerticalAltitudeMode(segment->to.z, 0.); // both altitude should be the same anyway + return TRUE; +} + +/** Navigation function along a path + */ +static inline bool_t mission_nav_path(struct _mission_path * path) { + if (path->nb == 0) { + return FALSE; // nothing to do + } + if (path->nb == 1) { + // handle as a single waypoint + struct _mission_wp wp = { path->path[0] }; + return mission_nav_wp(&wp); + } + if (path->path_idx == path->nb-1) { + last_wp = path->path[path->path_idx]; // store last wp + return FALSE; // end of path + } + // normal case + struct EnuCoor_f from = path->path[path->path_idx]; + struct EnuCoor_f to = path->path[path->path_idx+1]; + nav_route_xy(from.x, from.y, to.x, to.y); + NavVerticalAutoThrottleMode(0.); + NavVerticalAltitudeMode(to.z, 0.); // both altitude should be the same anyway + if (nav_approaching_xy(to.x, to.y, from.x, from.y, CARROT)) { + path->path_idx++; // go to next segment + } + return TRUE; +} + + +int mission_run() { + // current element + struct _mission_element * el = NULL; + if ((el = mission_get()) == NULL) { + // TODO do something special like a waiting circle before ending the mission ? + return FALSE; // end of mission + } + + bool_t el_running = FALSE; + switch (el->type){ + case MissionWP: + el_running = mission_nav_wp(&(el->element.mission_wp)); + break; + case MissionCircle: + el_running = mission_nav_circle(&(el->element.mission_circle)); + break; + case MissionSegment: + el_running = mission_nav_segment(&(el->element.mission_segment)); + break; + case MissionPath: + el_running = mission_nav_path(&(el->element.mission_path)); + break; + default: + // invalid type or pattern not yet handled + break; + } + + // increment element_time + mission.element_time += dt_navigation; + + // test if element is finished or element time is elapsed + if (!el_running || (el->duration > 0. && mission.element_time >= el->duration)) { + // reset timer + mission.element_time = 0.; + // go to next element + mission.current_idx++; + } + + return TRUE; +} + + diff --git a/sw/airborne/modules/sensors/baro_MS5534A.c b/sw/airborne/modules/sensors/baro_MS5534A.c index 3035013de0..9e5096b4cd 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 "subsystems/datalink/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,14 +259,13 @@ 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 SENSO_SYNC_SEND DOWNLINK_SEND_BARO_MS5534A(DefaultChannel, DefaultDevice, &baro_MS5534A_pressure, &baro_MS5534A_temp, &baro_MS5534A_z); #endif - // 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 60ed9919da..e45e89b246 100644 --- a/sw/airborne/modules/sensors/baro_amsys.c +++ b/sw/airborne/modules/sensors/baro_amsys.c @@ -25,25 +25,17 @@ #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" #include "subsystems/datalink/downlink.h" //#include "gps.h" -#ifdef SITL -#include "subsystems/gps.h" -#endif - #define BARO_AMSYS_ADDR 0xE4 #define BARO_AMSYS_REG 0x07 #ifndef BARO_AMSYS_SCALE @@ -74,6 +66,7 @@ #define BARO_AMSYS_I2C_DEV i2c0 #endif + // Global variables uint16_t pBaroRaw; uint16_t tBaroRaw; @@ -123,7 +116,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); @@ -131,17 +123,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); @@ -177,6 +158,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 622e8aede9..fb11910505 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" @@ -48,7 +49,6 @@ #define BMP_I2C_DEV i2c0 #endif - #define BARO_BMP_R 0.5 #define BARO_BMP_SIGMA2 0.1 @@ -89,6 +89,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 c4301495e5..46e3d787b4 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 #include "mcu_periph/uart.h" @@ -60,9 +57,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 @@ -70,11 +64,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 @@ -119,27 +137,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 ) { @@ -173,8 +177,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 944213673c..899b29dbc8 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 @@ -36,6 +37,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 @@ -80,6 +91,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 018ac5af8b..b4c96b9857 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" @@ -28,23 +36,37 @@ #include "subsystems/datalink/downlink.h" +#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 ef9f8b1124..de6568a356 100644 --- a/sw/airborne/modules/sensors/baro_mpl3115.h +++ b/sw/airborne/modules/sensors/baro_mpl3115.h @@ -17,16 +17,22 @@ * 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 ); +#define BaroMpl3115Update(_b, _h) { if (mpl3115_data_available) { _b = mpl3115_pressure; _h(); mpl3115_data_available = FALSE; } } + #endif diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c index 63be57aaa3..90096b26e5 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" @@ -69,9 +71,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 } @@ -87,16 +89,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 60de6a0842..aa6d9a785e 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" @@ -181,6 +182,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 3711301752..0b78ce7516 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" @@ -96,6 +97,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/state.c b/sw/airborne/state.c index 0b92f2fe3a..f72bc48438 100644 --- a/sw/airborne/state.c +++ b/sw/airborne/state.c @@ -69,8 +69,7 @@ void stateCalcPositionEcef_i(void) { ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f); } else if (bit_is_set(state.pos_status, POS_NED_I)) { - /// @todo check if resolution is good enough - ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); + ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); } else if (bit_is_set(state.pos_status, POS_NED_F)) { /* transform ned_f to ecef_f, set status bit, then convert to int */ @@ -114,7 +113,7 @@ void stateCalcPositionNed_i(void) { INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { - ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { /* transform ecef_f -> ned_f, set status bit, then convert to int */ @@ -206,7 +205,7 @@ void stateCalcPositionEnu_i(void) { INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { - enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { /* transform ecef_f -> enu_f, set status bit, then convert to int */ @@ -306,8 +305,7 @@ void stateCalcPositionLla_i(void) { } else if (bit_is_set(state.pos_status, POS_NED_I)) { /* transform ned_i -> ecef_i -> lla_i, set status bits */ - /// @todo check if resolution is enough - ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); + ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); SetBit(state.pos_status, POS_ECEF_I); lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); /* uses double version internally */ } @@ -360,8 +358,7 @@ void stateCalcPositionEcef_f(void) { } else if (bit_is_set(state.pos_status, POS_NED_I)) { /* transform ned_i -> ecef_i -> ecef_f, set status bits */ - /// @todo check if resolution is good enough - ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); + ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); SetBit(state.pos_status, POS_ECEF_F); ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i); } @@ -396,7 +393,7 @@ void stateCalcPositionNed_f(void) { } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { /* transform ecef_i -> ned_i -> ned_f, set status bits */ - ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_NED_I); NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); } @@ -407,7 +404,7 @@ void stateCalcPositionNed_f(void) { /* transform lla_i -> ecef_i -> ned_i -> ned_f, set status bits */ ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */ SetBit(state.pos_status, POS_ECEF_I); - ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_NED_I); NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); } @@ -481,7 +478,7 @@ void stateCalcPositionEnu_f(void) { } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { /* transform ecef_i -> enu_i -> enu_f, set status bits */ - enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_ENU_I); ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); } @@ -492,7 +489,7 @@ void stateCalcPositionEnu_f(void) { /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */ ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */ SetBit(state.pos_status, POS_ECEF_I); - enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_ENU_I); ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); } 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/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index 8965e1202c..1d85f64d22 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2010 The Paparazzi Team + * Copyright (C) 2010-2013 The Paparazzi Team * * This file is part of paparazzi. * @@ -60,6 +60,39 @@ #ifndef AHRS_PROPAGATE_FREQUENCY #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY #endif +PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) + +#ifndef AHRS_CORRECT_FREQUENCY +#define AHRS_CORRECT_FREQUENCY AHRS_PROPAGATE_FREQUENCY +#endif +PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) + +#ifndef AHRS_MAG_CORRECT_FREQUENCY +#define AHRS_MAG_CORRECT_FREQUENCY 50 +#endif +PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) + +/* + * default gains for correcting attitude and bias from accel/mag + */ +#ifndef AHRS_ACCEL_OMEGA +#define AHRS_ACCEL_OMEGA 0.063 +#endif +#ifndef AHRS_ACCEL_ZETA +#define AHRS_ACCEL_ZETA 0.9 +#endif + +#ifndef AHRS_MAG_OMEGA +#define AHRS_MAG_OMEGA 0.04 +#endif +#ifndef AHRS_MAG_ZETA +#define AHRS_MAG_ZETA 0.9 +#endif + +/** by default use the gravity heuristic to reduce gain */ +#ifndef AHRS_GRAVITY_HEURISTIC_FACTOR +#define AHRS_GRAVITY_HEURISTIC_FACTOR 30 +#endif #ifdef AHRS_UPDATE_FW_ESTIMATOR // remotely settable @@ -145,12 +178,20 @@ void ahrs_init(void) { FLOAT_RATES_ZERO(ahrs_impl.imu_rate); + /* set default filter cut-off frequency and damping */ + ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA; + ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA; + ahrs_impl.mag_omega = AHRS_MAG_OMEGA; + ahrs_impl.mag_zeta = AHRS_MAG_ZETA; + #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN ahrs_impl.correct_gravity = TRUE; #else ahrs_impl.correct_gravity = FALSE; #endif + ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR; + VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z); #if DOWNLINK @@ -180,7 +221,6 @@ void ahrs_align(void) { struct Int32Rates bias0; RATES_COPY(bias0, ahrs_aligner.lp_gyro); RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0); - ahrs.status = AHRS_RUNNING; } @@ -233,6 +273,8 @@ void ahrs_update_accel(void) { struct FloatVect3 residual; + struct FloatVect3 pseudo_gravity_measurement; + if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) { /* * centrifugal acceleration in body frame @@ -249,30 +291,51 @@ void ahrs_update_accel(void) { struct FloatVect3 acc_c_imu; FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body); - /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */ - struct FloatVect3 corrected_gravity; - VECT3_DIFF(corrected_gravity, imu_accel_float, acc_c_imu); + /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */ + VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu); - /* compute the residual of gravity vector in imu frame */ - FLOAT_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2); } else { - FLOAT_VECT3_CROSS_PRODUCT(residual, imu_accel_float, c2); + VECT3_COPY(pseudo_gravity_measurement, imu_accel_float); } -#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC - /* heuristic on acceleration norm */ - const float acc_norm = FLOAT_VECT3_NORM(imu_accel_float); - const float weight = Chop(1.-6*fabs((9.81-acc_norm)/9.81), 0., 1.); -#else - const float weight = 1.; -#endif + FLOAT_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2); - /* compute correction */ - const float gravity_rate_update_gain = -5e-2; // -5e-2 - FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain); + /* FIR filtered pseudo_gravity_measurement */ + #define FIR_FILTER_SIZE 8 + static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.}; + VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1); + VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement); + VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE); - const float gravity_bias_update_gain = 1e-5; // -5e-6 - FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain); + if (ahrs_impl.gravity_heuristic_factor) { + /* heuristic on acceleration (gravity estimate) norm */ + /* Factor how strongly to change the weight. + * e.g. for gravity_heuristic_factor 30: + * <0.66G = 0, 1G = 1.0, >1.33G = 0 + */ + + const float g_meas_norm = FLOAT_VECT3_NORM(filtered_gravity_measurement) / 9.81; + ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0; + Bound(ahrs_impl.weight, 0.15, 1.0); + } + else { + ahrs_impl.weight = 1.0; + } + + /* Complementary filter proportional gain. + * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY + */ + const float gravity_rate_update_gain = -2 * ahrs_impl.accel_zeta * ahrs_impl.accel_omega * + ahrs_impl.weight * AHRS_PROPAGATE_FREQUENCY / (AHRS_CORRECT_FREQUENCY * 9.81); + FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, gravity_rate_update_gain); + + /* Complementary filter integral gain + * Correct the gyro bias. + * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY + */ + const float gravity_bias_update_gain = ahrs_impl.accel_omega * ahrs_impl.accel_omega * + ahrs_impl.weight * ahrs_impl.weight / (AHRS_CORRECT_FREQUENCY * 9.81); + FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, gravity_bias_update_gain); /* FIXME: saturate bias */ @@ -301,37 +364,66 @@ void ahrs_update_mag_full(void) { // DISPLAY_FLOAT_VECT3("# measured", measured_imu); // DISPLAY_FLOAT_VECT3("# residual", residual); - const float mag_rate_update_gain = 2.5; + /* Complementary filter proportional gain. + * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY + */ + + const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * + AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain); - const float mag_bias_update_gain = -2.5e-3; + /* Complementary filter integral gain + * Correct the gyro bias. + * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY + */ + const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) / + AHRS_MAG_CORRECT_FREQUENCY; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain); } void ahrs_update_mag_2d(void) { + struct FloatVect2 expected_ltp; + VECT2_COPY(expected_ltp, ahrs_impl.mag_h); + // normalize expected ltp in 2D (x,y) + FLOAT_VECT2_NORMALIZE(expected_ltp); + struct FloatVect3 measured_imu; MAGS_FLOAT_OF_BFP(measured_imu, imu.mag); struct FloatVect3 measured_ltp; FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_impl.ltp_to_imu_rmat, measured_imu); + struct FloatVect2 measured_ltp_2d={measured_ltp.x, measured_ltp.y}; + + // normalize measured ltp in 2D (x,y) + FLOAT_VECT2_NORMALIZE(measured_ltp_2d); + const struct FloatVect3 residual_ltp = { 0, 0, - measured_ltp.x * ahrs_impl.mag_h.y - measured_ltp.y * ahrs_impl.mag_h.x }; + measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x }; // printf("res : %f\n", residual_ltp.z); struct FloatVect3 residual_imu; FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp); - const float mag_rate_update_gain = 2.5; + + /* Complementary filter proportional gain. + * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY + */ + const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * + AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain); - const float mag_bias_update_gain = -2.5e-3; + /* Complementary filter integral gain + * Correct the gyro bias. + * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY + */ + const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) / + AHRS_MAG_CORRECT_FREQUENCY; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain); - } @@ -349,7 +441,8 @@ void ahrs_update_mag_2d_dumb(void) { const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z; const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z; - const float res_norm = -RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,0)*me + RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,0)*mn; + const float res_norm = -RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,0) * me + + RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,0) * mn; const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,0), RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,1), RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)}; @@ -491,5 +584,3 @@ static inline void compute_body_orientation_and_rates(void) { FLOAT_RMAT_TRANSP_RATEMULT(body_rate, ahrs_impl.body_to_imu_rmat, ahrs_impl.imu_rate); stateSetBodyRates_f(&body_rate); } - - diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h index b14831fa3e..a3b83ba6b7 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h @@ -38,10 +38,21 @@ struct AhrsFloatCmpl { struct FloatRates imu_rate; struct FloatQuat ltp_to_imu_quat; struct FloatRMat ltp_to_imu_rmat; - /* for gravity correction during coordinated turns */ - float ltp_vel_norm; + + bool_t correct_gravity; ///< enable gravity correction during coordinated turns + float ltp_vel_norm; ///< velocity norm for gravity correction during coordinated turns bool_t ltp_vel_norm_valid; - bool_t correct_gravity; + + float accel_omega; ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement) + float accel_zeta; ///< filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement) + float mag_omega; ///< filter cut-off frequency for correcting the attitude (heading) from magnetometer + float mag_zeta; ///< filter damping for correcting the gyro bias from magnetometer + + /** sets how strongly the gravity heuristic reduces accel correction. + * Set to zero in order to disable gravity heuristic. + */ + uint8_t gravity_heuristic_factor; + float weight; bool_t heading_aligned; struct FloatVect3 mag_h; diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 7eafe7ebc8..5105c12b33 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2008-2012 The Paparazzi Team + * Copyright (C) 2008-2013 The Paparazzi Team * * This file is part of paparazzi. * @@ -28,6 +28,8 @@ * */ +#include "generated/airframe.h" + #include "subsystems/ahrs/ahrs_int_cmpl_quat.h" #include "subsystems/ahrs/ahrs_aligner.h" #include "subsystems/ahrs/ahrs_int_utils.h" @@ -41,26 +43,61 @@ #include "math/pprz_trig_int.h" #include "math/pprz_algebra_int.h" -#include "generated/airframe.h" - -//#include "../../test/pprz_algebra_print.h" - -static inline void ahrs_update_mag_full(void); -static inline void ahrs_update_mag_2d(void); +#ifdef AHRS_PROPAGATE_LOW_PASS_RATES +PRINT_CONFIG_MSG("LOW PASS FILTER ON GYRO RATES") +#endif #ifdef AHRS_MAG_UPDATE_YAW_ONLY -#warning "AHRS_MAG_UPDATE_YAW_ONLY is deprecated, please remove it. This is the default behaviour. Define AHRS_MAG_UPDATE_ALL_AXES to use mag for all axes and not only yaw." +#error "The define AHRS_MAG_UPDATE_YAW_ONLY doesn't exist anymore, please remove it. This is the default behaviour. Define AHRS_MAG_UPDATE_ALL_AXES to use mag for all axes and not only yaw." #endif #if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING -#warning "Using magnetometer and GPS course to update heading. Probably better to set USE_MAGNETOMETER=0 if you want to use GPS course." +#warning "Using both magnetometer and GPS course to update heading. Probably better to set USE_MAGNETOMETER=0 if you want to use GPS course." #endif + #ifndef AHRS_PROPAGATE_FREQUENCY #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY #endif +PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) + +#ifndef AHRS_CORRECT_FREQUENCY +#define AHRS_CORRECT_FREQUENCY AHRS_PROPAGATE_FREQUENCY +#endif +PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) + +#ifndef AHRS_MAG_CORRECT_FREQUENCY +#define AHRS_MAG_CORRECT_FREQUENCY 50 +#endif +PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) + +/* + * default gains for correcting attitude and bias from accel/mag + */ +#ifndef AHRS_ACCEL_OMEGA +#define AHRS_ACCEL_OMEGA 0.063 +#endif +#ifndef AHRS_ACCEL_ZETA +#define AHRS_ACCEL_ZETA 0.9 +#endif +PRINT_CONFIG_VAR(AHRS_ACCEL_OMEGA) +PRINT_CONFIG_VAR(AHRS_ACCEL_ZETA) + + +#ifndef AHRS_MAG_OMEGA +#define AHRS_MAG_OMEGA 0.04 +#endif +#ifndef AHRS_MAG_ZETA +#define AHRS_MAG_ZETA 0.9 +#endif +PRINT_CONFIG_VAR(AHRS_MAG_OMEGA) +PRINT_CONFIG_VAR(AHRS_MAG_ZETA) + +/** by default use the gravity heuristic to reduce gain */ +#ifndef AHRS_GRAVITY_HEURISTIC_FACTOR +#define AHRS_GRAVITY_HEURISTIC_FACTOR 30 +#endif -struct AhrsIntCmpl ahrs_impl; #ifdef AHRS_UPDATE_FW_ESTIMATOR // remotely settable @@ -74,7 +111,11 @@ float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; #endif +struct AhrsIntCmplQuat ahrs_impl; + static inline void set_body_state_from_quat(void); +static inline void ahrs_update_mag_full(void); +static inline void ahrs_update_mag_2d(void); #if DOWNLINK #include "subsystems/datalink/telemetry.h" @@ -125,19 +166,25 @@ void ahrs_init(void) { INT_RATES_ZERO(ahrs_impl.rate_correction); INT_RATES_ZERO(ahrs_impl.high_rez_bias); + /* set default filter cut-off frequency and damping */ + ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA; + ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA; + ahrs_set_accel_gains(); + ahrs_impl.mag_omega = AHRS_MAG_OMEGA; + ahrs_impl.mag_zeta = AHRS_MAG_ZETA; + ahrs_set_mag_gains(); + + /* set default gravity heuristic */ + ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR; + #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN ahrs_impl.correct_gravity = TRUE; #else ahrs_impl.correct_gravity = FALSE; #endif -#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC - ahrs_impl.use_gravity_heuristic = TRUE; -#else - ahrs_impl.use_gravity_heuristic = FALSE; -#endif - - VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); + VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), + MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); #if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "AHRS_QUAT_INT", send_quat); @@ -147,11 +194,13 @@ void ahrs_init(void) { } + void ahrs_align(void) { #if USE_MAGNETOMETER /* Compute an initial orientation from accel and mag directly as quaternion */ - ahrs_int_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); + ahrs_int_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, + &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); ahrs_impl.heading_aligned = TRUE; #else /* Compute an initial orientation from accel and just set heading to zero */ @@ -162,20 +211,14 @@ void ahrs_align(void) { set_body_state_from_quat(); /* Use low passed gyro value as initial bias */ - RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro); - RATES_COPY( ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro); + RATES_COPY(ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro); + RATES_COPY(ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro); INT_RATES_LSHIFT(ahrs_impl.high_rez_bias, ahrs_impl.high_rez_bias, 28); ahrs.status = AHRS_RUNNING; } - -/* - * - * - * - */ void ahrs_propagate(void) { /* unbias gyro */ @@ -191,13 +234,14 @@ void ahrs_propagate(void) { RATES_COPY(ahrs_impl.imu_rate, omega); #endif - /* add correction */ + /* add correction */ RATES_ADD(omega, ahrs_impl.rate_correction); /* and zeros it */ INT_RATES_ZERO(ahrs_impl.rate_correction); /* integrate quaternion */ - INT32_QUAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY); + INT32_QUAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_quat, ahrs_impl.high_rez_quat, + omega, AHRS_PROPAGATE_FREQUENCY); INT32_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat); set_body_state_from_quat(); @@ -205,7 +249,27 @@ void ahrs_propagate(void) { } +void ahrs_set_accel_gains(void) { + /* Complementary filter proportionnal gain + * Kp = 2 * omega * zeta * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY + * + * accel_inv_kp = 1 / (Kp * FRAC_conversion / cross_product_gain) + * accel_inv_kp = 4096 * 9.81 / Kp + */ + ahrs_impl.accel_inv_kp = 4096 * 9.81 / + (2 * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * + AHRS_PROPAGATE_FREQUENCY / AHRS_PROPAGATE_FREQUENCY); + /* Complementary filter integral gain + * Ki = omega^2 / AHRS_CORRECT_FREQUENCY + * + * accel_inv_ki = 2^5 / (Ki * FRAC_conversion / cross_product_gain) + * accel_inv_ki = 2^5 / 2^16 * 9.81 / Ki + * accel_inv_ki = 9.81 / 2048 / Ki + */ + ahrs_impl.accel_inv_ki = AHRS_CORRECT_FREQUENCY * 9.81 / 2048 / + (ahrs_impl.accel_omega * ahrs_impl.accel_omega); +} void ahrs_update_accel(void) { @@ -230,17 +294,20 @@ void ahrs_update_accel(void) { // FIXME: check overflows ! #define COMPUTATION_FRAC 16 +#define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC - const struct Int32Vect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0}; + const struct Int32Vect3 vel_tangential_body = + {ahrs_impl.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0}; struct Int32Vect3 acc_c_body; VECT3_RATES_CROSS_VECT3(acc_c_body, (*stateGetBodyRates_i()), vel_tangential_body); - INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-COMPUTATION_FRAC); + INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, ACC_FROM_CROSS_FRAC); /* convert centrifucal acceleration from body to imu frame */ struct Int32Vect3 acc_c_imu; INT32_RMAT_VMULT(acc_c_imu, imu.body_to_imu_rmat, acc_c_body); - /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */ + /* and subtract it from imu measurement to get a corrected measurement + * of the gravity vector */ INT32_VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu); } else { VECT3_COPY(pseudo_gravity_measurement, imu.accel); @@ -250,50 +317,81 @@ void ahrs_update_accel(void) { INT32_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2); - int32_t inv_weight; - if (ahrs_impl.use_gravity_heuristic) { - /* heuristic on acceleration norm */ + /* FIR filtered pseudo_gravity_measurement */ + #define FIR_FILTER_SIZE 8 + static struct Int32Vect3 filtered_gravity_measurement = {0, 0, 0}; + VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1); + VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement); + VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE); - /* FIR filtered pseudo_gravity_measurement */ - static struct Int32Vect3 filtered_gravity_measurement = {0, 0, 0}; - VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, 7); - VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement); - VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, 8); - int32_t acc_norm; - INT32_VECT3_NORM(acc_norm, filtered_gravity_measurement); - const int32_t acc_norm_d = ABS(ACCEL_BFP_OF_REAL(9.81)-acc_norm); - inv_weight = Chop(50*acc_norm_d/ACCEL_BFP_OF_REAL(9.81), 1, 50); + if (ahrs_impl.gravity_heuristic_factor) { + /* heuristic on acceleration (gravity estimate) norm */ + /* Factor how strongly to change the weight. + * e.g. for gravity_heuristic_factor 30: + * <0.66G = 0, 1G = 1.0, >1.33G = 0 + */ + + struct FloatVect3 g_meas_f; + ACCELS_FLOAT_OF_BFP(g_meas_f, filtered_gravity_measurement); + const float g_meas_norm = FLOAT_VECT3_NORM(g_meas_f)/9.81; + ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10; + Bound(ahrs_impl.weight, 0.15, 1.0); } else { - inv_weight = 1; + ahrs_impl.weight = 1.0; } - // residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 - // rate_correction FRAC = RATE_FRAC = 12 - // 2^12 / 2^24 * 5e-2 = 1/81920 - ahrs_impl.rate_correction.p += -residual.x/82000/inv_weight; - ahrs_impl.rate_correction.q += -residual.y/82000/inv_weight; - ahrs_impl.rate_correction.r += -residual.z/82000/inv_weight; + /* Complementary filter proportional gain. + * Kp = 2 * zeta * omega * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY + * + * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 + * rate_correction FRAC: RATE_FRAC = 12 + * FRAC_conversion: 2^12 / 2^24 = 1 / 4096 + * cross_product_gain : 9.81 m/s2 + * + * accel_inv_kp = 1 / (Kp * FRAC_conversion / cross_product_gain) + * accel_inv_kp = 4096 * 9.81 / Kp + * + * inv_rate_scale = 1 / (weight * Kp * FRAC_conversion / cross_product_gain) + * inv_rate_scale = inv_kp / weight + */ + int32_t inv_rate_scale = (int32_t)(ahrs_impl.accel_inv_kp / ahrs_impl.weight); + Bound(inv_rate_scale, 8192, 4194304); - // residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 - // high_rez_bias = RATE_FRAC+28 = 40 - // 2^40 / 2^24 * 5e-6 = 1/3.05 - - // ahrs_impl.high_rez_bias.p += residual.x*3; - // ahrs_impl.high_rez_bias.q += residual.y*3; - // ahrs_impl.high_rez_bias.r += residual.z*3; - - ahrs_impl.high_rez_bias.p += residual.x/(2*inv_weight); - ahrs_impl.high_rez_bias.q += residual.y/(2*inv_weight); - ahrs_impl.high_rez_bias.r += residual.z/(2*inv_weight); + ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale; + ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale; + ahrs_impl.rate_correction.r -= residual.z / inv_rate_scale; - /* */ + /* Complementary filter integral gain + * Correct the gyro bias. + * Ki = omega^2 / AHRS_CORRECT_FREQUENCY + * + * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 + * high_rez_bias = RATE_FRAC+28 = 40 + * FRAC_conversion: 2^40 / 2^24 = 2^16 + * cross_product_gain : 9.81 m/s2 + * + * accel_inv_ki = 2^5 / (Ki * FRAC_conversion / cross_product_gain) + * accel_inv_ki = 2^5 / 2^16 * 9.81 * Ki = 9.81 / 2^11 * Ki + * + * inv_bias_gain = 2^5 / (weight^2 * Ki * FRAC_conversion / cross_product_gain) + * inv_bias_gain = accel_inv_ki / weight^2 + */ + + int32_t inv_bias_gain = (int32_t)(ahrs_impl.accel_inv_ki / + (ahrs_impl.weight * ahrs_impl.weight)); + Bound(inv_bias_gain, 8, 65536) + ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5; + ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) << 5; + ahrs_impl.high_rez_bias.r += (residual.z / inv_bias_gain) << 5; + INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); } + void ahrs_update_mag(void) { #if AHRS_MAG_UPDATE_ALL_AXES ahrs_update_mag_full(); @@ -302,6 +400,15 @@ void ahrs_update_mag(void) { #endif } +void ahrs_set_mag_gains(void) { + /* Complementary filter proportionnal gain = 2*omega*zeta */ + ahrs_impl.mag_kp = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * + AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY; + /* Complementary filter integral gain = omega^2 */ + ahrs_impl.mag_ki = ahrs_impl.mag_omega * ahrs_impl.mag_omega / + AHRS_MAG_CORRECT_FREQUENCY; +} + static inline void ahrs_update_mag_full(void) { @@ -314,14 +421,38 @@ static inline void ahrs_update_mag_full(void) { struct Int32Vect3 residual; INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu); - ahrs_impl.rate_correction.p += residual.x/32/16; - ahrs_impl.rate_correction.q += residual.y/32/16; - ahrs_impl.rate_correction.r += residual.z/32/16; + /* Complementary filter proportionnal gain. + * Kp = 2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY + * + * residual FRAC: 2 * MAG_FRAC = 22 + * rate_correction FRAC: RATE_FRAC = 12 + * FRAC conversion: 2^12 / 2^22 = 1/1024 + * + * inv_rate_gain = 1 / Kp / FRAC_conversion + * inv_rate_gain = 1024 / Kp + */ + const int32_t inv_rate_gain = (int32_t)(1024.0 / ahrs_impl.mag_kp); - ahrs_impl.high_rez_bias.p -= residual.x/32*1024; - ahrs_impl.high_rez_bias.q -= residual.y/32*1024; - ahrs_impl.high_rez_bias.r -= residual.z/32*1024; + ahrs_impl.rate_correction.p += residual.x / inv_rate_gain; + ahrs_impl.rate_correction.q += residual.y / inv_rate_gain; + ahrs_impl.rate_correction.r += residual.z / inv_rate_gain; + + /* Complementary filter integral gain + * Correct the gyro bias. + * Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY + * + * residual FRAC: 2* MAG_FRAC = 22 + * high_rez_bias FRAC: RATE_FRAC+28 = 40 + * FRAC conversion: 2^40 / 2^22 = 2^18 + * + * bias_gain = Ki * FRAC_conversion = Ki * 2^18 + */ + const int32_t bias_gain = (int32_t)(ahrs_impl.mag_ki * (1<<18)); + + ahrs_impl.high_rez_bias.p -= residual.x * bias_gain; + ahrs_impl.high_rez_bias.q -= residual.y * bias_gain; + ahrs_impl.high_rez_bias.r -= residual.z * bias_gain; INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); @@ -331,45 +462,61 @@ static inline void ahrs_update_mag_full(void) { static inline void ahrs_update_mag_2d(void) { + struct Int32Vect2 expected_ltp = {ahrs_impl.mag_h.x, ahrs_impl.mag_h.y}; + /* normalize expected ltp in 2D (x,y) */ + INT32_VECT2_NORMALIZE(expected_ltp, INT32_MAG_FRAC); + struct Int32RMat ltp_to_imu_rmat; INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); struct Int32Vect3 measured_ltp; INT32_RMAT_TRANSP_VMULT(measured_ltp, ltp_to_imu_rmat, imu.mag); + /* normalize measured ltp in 2D (x,y) */ + struct Int32Vect2 measured_ltp_2d = {measured_ltp.x, measured_ltp.y}; + INT32_VECT2_NORMALIZE(measured_ltp_2d, INT32_MAG_FRAC); + + /* residual_ltp FRAC: 2 * MAG_FRAC - 5 = 17 */ struct Int32Vect3 residual_ltp = { 0, 0, - (measured_ltp.x * ahrs_impl.mag_h.y - measured_ltp.y * ahrs_impl.mag_h.x)/(1<<5)}; + (measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x)/(1<<5)}; + struct Int32Vect3 residual_imu; INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp); - // residual_ltp FRAC = 2 * MAG_FRAC = 22 - // rate_correction FRAC = RATE_FRAC = 12 - // 2^12 / 2^22 * 2.5 = 1/410 + /* Complementary filter proportionnal gain. + * Kp = 2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY + * + * residual_imu FRAC = residual_ltp FRAC = 17 + * rate_correction FRAC: RATE_FRAC = 12 + * FRAC conversion: 2^12 / 2^17 = 1/32 + * + * inv_rate_gain = 1 / Kp / FRAC_conversion + * inv_rate_gain = 32 / Kp + */ + int32_t inv_rate_gain = (int32_t)(32.0 / ahrs_impl.mag_kp); - // ahrs_impl.rate_correction.p += residual_imu.x*(1<<5)/410; - // ahrs_impl.rate_correction.q += residual_imu.y*(1<<5)/410; - // ahrs_impl.rate_correction.r += residual_imu.z*(1<<5)/410; + ahrs_impl.rate_correction.p += (residual_imu.x / inv_rate_gain); + ahrs_impl.rate_correction.q += (residual_imu.y / inv_rate_gain); + ahrs_impl.rate_correction.r += (residual_imu.z / inv_rate_gain); - ahrs_impl.rate_correction.p += residual_imu.x/16; - ahrs_impl.rate_correction.q += residual_imu.y/16; - ahrs_impl.rate_correction.r += residual_imu.z/16; - - - // residual_ltp FRAC = 2 * MAG_FRAC = 22 - // high_rez_bias = RATE_FRAC+28 = 40 - // 2^40 / 2^22 * 2.5e-3 = 655 - - // ahrs_impl.high_rez_bias.p -= residual_imu.x*(1<<5)*655; - // ahrs_impl.high_rez_bias.q -= residual_imu.y*(1<<5)*655; - // ahrs_impl.high_rez_bias.r -= residual_imu.z*(1<<5)*655; - - ahrs_impl.high_rez_bias.p -= residual_imu.x*1024; - ahrs_impl.high_rez_bias.q -= residual_imu.y*1024; - ahrs_impl.high_rez_bias.r -= residual_imu.z*1024; + /* Complementary filter integral gain + * Correct the gyro bias. + * Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY + * + * residual_imu FRAC = residual_ltp FRAC = 17 + * high_rez_bias FRAC: RATE_FRAC+28 = 40 + * FRAC conversion: 2^40 / 2^17 = 2^23 + * + * bias_gain = Ki * FRAC_conversion = Ki * 2^23 + */ + int32_t bias_gain = (int32_t)(ahrs_impl.mag_ki * (1 << 23)); + ahrs_impl.high_rez_bias.p -= (residual_imu.x * bias_gain); + ahrs_impl.high_rez_bias.q -= (residual_imu.y * bias_gain); + ahrs_impl.high_rez_bias.r -= (residual_imu.z * bias_gain); INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); @@ -502,7 +649,7 @@ void ahrs_realign_heading(int32_t heading) { /* Rotate angles and rates from imu to body frame and set state */ -__attribute__ ((always_inline)) static inline void set_body_state_from_quat(void) { +static inline void set_body_state_from_quat(void) { /* Compute LTP to BODY quaternion */ struct Int32Quat ltp_to_body_quat; INT32_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat); @@ -527,5 +674,3 @@ __attribute__ ((always_inline)) static inline void set_body_state_from_quat(void /* Set state */ stateSetBodyRates_i(&body_rate); } - - diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h index b5e2ac3df3..e7c5fbce6d 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2011 The Paparazzi Team + * Copyright (C) 2008-2013 The Paparazzi Team * * This file is part of paparazzi. * @@ -28,29 +28,67 @@ * */ -#ifndef AHRS_INT_CMPL_H -#define AHRS_INT_CMPL_H +#ifndef AHRS_INT_CMPL_QUAT_H +#define AHRS_INT_CMPL_QUAT_H #include "subsystems/ahrs.h" #include "std.h" #include "math/pprz_algebra_int.h" -struct AhrsIntCmpl { +/** + * Ahrs implementation specifc values + */ +struct AhrsIntCmplQuat { struct Int32Rates gyro_bias; struct Int32Rates imu_rate; struct Int32Rates rate_correction; struct Int64Quat high_rez_quat; struct Int64Rates high_rez_bias; struct Int32Quat ltp_to_imu_quat; - struct Int32Vect3 mag_h; + struct Int32Vect3 mag_h; + int32_t ltp_vel_norm; bool_t ltp_vel_norm_valid; - bool_t correct_gravity; - bool_t use_gravity_heuristic; bool_t heading_aligned; + float weight; + float accel_inv_kp; + float accel_inv_ki; + float mag_kp; + float mag_ki; + + /* parameters/options that can be changed */ + /** enable gravity vector correction by removing centrifugal acceleration */ + bool_t correct_gravity; + + /** sets how strongly the gravity heuristic reduces accel correction. + * Set to zero in order to disable gravity heuristic. + */ + uint8_t gravity_heuristic_factor; + + /** filter cut-off frequency for correcting the attitude from accels. + * (pseudo-gravity measurement) + * only update through #ahrs_int_cmpl_quat_SetAccelOmega(omega) + */ + float accel_omega; + + /** filter damping for correcting the gyro-bias from accels. + * (pseudo-gravity measurement) + * only update through #ahrs_int_cmpl_quat_SetAccelZeta(zeta) + */ + float accel_zeta; + + /** filter cut-off frequency for correcting the attitude (heading) from magnetometer. + * only update through #ahrs_int_cmpl_quat_SetMagOmega(omega) + */ + float mag_omega; + + /** filter damping for correcting the gyro bias from magnetometer. + * only update through #ahrs_int_cmpl_quat_SetMagZeta(zeta) + */ + float mag_zeta; }; -extern struct AhrsIntCmpl ahrs_impl; +extern struct AhrsIntCmplQuat ahrs_impl; /** Update yaw based on a heading measurement. @@ -66,10 +104,37 @@ void ahrs_update_heading(int32_t heading); */ void ahrs_realign_heading(int32_t heading); + +/// update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta +extern void ahrs_set_accel_gains(void); + +static inline void ahrs_int_cmpl_quat_SetAccelOmega(float omega) { + ahrs_impl.accel_omega = omega; + ahrs_set_accel_gains(); +} + +static inline void ahrs_int_cmpl_quat_SetAccelZeta(float zeta) { + ahrs_impl.accel_zeta = zeta; + ahrs_set_accel_gains(); +} + +/// update pre-computed kp and ki gains from mag_omega and mag_zeta +extern void ahrs_set_mag_gains(void); + +static inline void ahrs_int_cmpl_quat_SetMagOmega(float omega) { + ahrs_impl.mag_omega = omega; + ahrs_set_mag_gains(); +} + +static inline void ahrs_int_cmpl_quat_SetMagZeta(float zeta) { + ahrs_impl.mag_zeta = zeta; + ahrs_set_mag_gains(); +} + + #ifdef AHRS_UPDATE_FW_ESTIMATOR extern float ins_roll_neutral; extern float ins_pitch_neutral; #endif - -#endif /* AHRS_INT_CMPL_H */ +#endif /* AHRS_INT_CMPL_QUAT_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 1fa57f3d44..7beb16e0d8 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" @@ -501,14 +500,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) @@ -532,7 +523,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 @@ -550,20 +541,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) { @@ -572,11 +556,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 e8727750c7..cf451b7e43 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 567b21b9f5..1044c02cd6 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,42 +45,50 @@ #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; -#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 */ -#if USE_VFF -int32_t ins_qfe; -bool_t ins_baro_initialised; -int32_t ins_baro_alt; +/** 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); -/* output */ -struct NedCoor_i ins_ltp_pos; -struct NedCoor_i ins_ltp_speed; -struct NedCoor_i ins_ltp_accel; +struct InsInt ins_impl; #if DOWNLINK #include "subsystems/datalink/telemetry.h" @@ -104,41 +115,48 @@ static void send_ins_ref(void) { } #endif -void ins_init() { +static void ins_init_origin_from_flightplan(void); +static void ins_ned_to_state(void); +#if USE_HFF +static void ins_update_from_hff(void); +#endif + + +void ins_init(void) { + #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); #if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "INS", send_ins); @@ -147,19 +165,23 @@ void ins_init() { #endif } -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() { @@ -169,116 +191,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/airborne/test/Makefile b/sw/airborne/test/Makefile index f8e05d1151..5938ed851a 100644 --- a/sw/airborne/test/Makefile +++ b/sw/airborne/test/Makefile @@ -4,7 +4,8 @@ Q=@ CC = gcc -CFLAGS = -std=c99 -I.. -I../../include -I../booz -I../../booz -Wall +CFLAGS = -std=c99 -I.. -I../../include -Wall +#CFLAGS += -DDEBUG LDFLAGS = -lm diff --git a/sw/airborne/test/test_geodetic.c b/sw/airborne/test/test_geodetic.c index e4c24a983e..51197828f5 100644 --- a/sw/airborne/test/test_geodetic.c +++ b/sw/airborne/test/test_geodetic.c @@ -20,6 +20,7 @@ static void test_lla_of_utm(void); static void test_floats(void); static void test_doubles(void); static void test_enu_of_ecef_int(void); +static void test_ecef_of_ned_int(void); static void test_ned_to_ecef_to_ned(void); static void test_enu_to_ecef_to_enu( void ); @@ -32,12 +33,16 @@ int main(int argc, char** argv) { //test_floats(); //test_doubles(); - test_lla_of_utm(); + //test_lla_of_utm(); + + //test_ned_to_ecef_to_ned(); + + //test_enu_to_ecef_to_enu(); + + test_ecef_of_ned_int(); //test_enu_of_ecef_int(); - // test_ned_to_ecef_to_ned(); - // test_enu_to_ecef_to_enu(); return 0; } @@ -48,7 +53,7 @@ static void test_lla_of_utm(void) { struct UtmCoor_d utm_d = { .east=348805.71, .north=4759354.89, .zone=31 }; struct LlaCoor_d lla_d; struct LlaCoor_d l_ref_d = {.lat=0.749999999392454875, - .lon=0.019999999054505127}; + .lon=0.019999999054505127}; lla_of_utm_d(&lla_d, &utm_d); printf(" lat=%.16f lon=%.16f\nref_lat=%.16f ref_lon=%.16f\n", lla_d.lat, lla_d.lon, l_ref_d.lat, l_ref_d.lon); @@ -58,7 +63,7 @@ static void test_lla_of_utm(void) { struct UtmCoor_f utm_f = { .east=348805.71, .north=4759354.89, .zone=31 }; struct LlaCoor_f lla_f; struct LlaCoor_f l_ref_f = {.lat=0.749999999392454875, - .lon=0.019999999054505127}; + .lon=0.019999999054505127}; lla_of_utm_f(&lla_f, &utm_f); printf(" lat=%.16f lon=%.16f\nref_lat=%.16f ref_lon=%.16f\n", lla_f.lat, lla_f.lon, l_ref_f.lat, l_ref_f.lon); @@ -78,15 +83,16 @@ static void test_floats(void) { struct LtpDef_f ltp_def; ltp_def_from_ecef_f(<p_def, &ref_coor); - printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), ltp_def.lla.alt); + printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), + ltp_def.lla.alt); struct EcefCoor_f my_ecef_point = ref_coor; struct EnuCoor_f my_enu_point; enu_of_ecef_point_f(&my_enu_point, <p_def, &my_ecef_point); printf("ecef to enu : (%f,%f,%f) -> (%f,%f,%f)\n", - my_ecef_point.x, my_ecef_point.y, my_ecef_point.z, - my_enu_point.x, my_enu_point.y, my_enu_point.z ); + my_ecef_point.x, my_ecef_point.y, my_ecef_point.z, + my_enu_point.x, my_enu_point.y, my_enu_point.z ); printf("\n"); } @@ -105,18 +111,91 @@ static void test_doubles(void) { struct LtpDef_d ltp_def; ltp_def_from_ecef_d(<p_def, &ref_coor); - printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), ltp_def.lla.alt); + printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), + ltp_def.lla.alt); struct EcefCoor_d my_ecef_point = ref_coor; struct EnuCoor_d my_enu_point; enu_of_ecef_point_d(&my_enu_point, <p_def, &my_ecef_point); printf("ecef to enu : (%f,%f,%f) -> (%f,%f,%f)\n", - my_ecef_point.x, my_ecef_point.y, my_ecef_point.z, - my_enu_point.x, my_enu_point.y, my_enu_point.z ); + my_ecef_point.x, my_ecef_point.y, my_ecef_point.z, + my_enu_point.x, my_enu_point.y, my_enu_point.z ); printf("\n"); } +static void test_ecef_of_ned_int(void) { + + printf("\n--- ecef_of_ned int ---\n"); + struct EcefCoor_d ref_coor_d = { 4624497.0 , 116475.0, 4376563.0}; + struct LtpDef_d ltp_def_d; + ltp_def_from_ecef_d(<p_def_d, &ref_coor_d); + + struct EcefCoor_i ref_coor_i = { rint(CM_OF_M(ref_coor_d.x)), + rint(CM_OF_M(ref_coor_d.y)), + rint(CM_OF_M(ref_coor_d.z))}; + printf("ecef0 : (%d,%d,%d)\n", ref_coor_i.x, ref_coor_i.y, ref_coor_i.z); + struct LtpDef_i ltp_def_i; + ltp_def_from_ecef_i(<p_def_i, &ref_coor_i); + + +#define STEP_NED 100.0 +#define RANGE_NED 10000. + double sum_err = 0; + struct FloatVect3 max_err; + FLOAT_VECT3_ZERO(max_err); + struct FloatVect3 offset; + for (offset.x=-RANGE_NED; offset.x<=RANGE_NED; offset.x+=STEP_NED) { + for (offset.y=-RANGE_NED; offset.y<=RANGE_NED; offset.y+=STEP_NED) { + for (offset.z=-RANGE_NED; offset.z<=RANGE_NED; offset.z+=STEP_NED) { + struct NedCoor_d my_ned_point_d; + VECT3_COPY(my_ned_point_d, offset); + struct EcefCoor_d my_ecef_point_d; + ecef_of_ned_point_d(&my_ecef_point_d, <p_def_d, &my_ned_point_d); +#if DEBUG + printf("ned to ecef double : (%.02f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n", + my_enu_point_f.x, my_enu_point_f.y, my_enu_point_f.z, + my_ecef_point_f.x, my_ecef_point_f.y, my_ecef_point_f.z); +#endif + + struct NedCoor_i my_ned_point_i; + POSITIONS_BFP_OF_REAL(my_ned_point_i, my_ned_point_d); + struct EcefCoor_i my_ecef_point_i; + ecef_of_ned_pos_i(&my_ecef_point_i, <p_def_i, &my_ned_point_i); + +#if DEBUG + //printf("def->ecef (%d,%d,%d)\n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z); + printf("ned to ecef int : (%.2f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n\n", + POS_BFP_OF_REAL(my_ned_point_i.x), + POS_BFP_OF_REAL(my_ned_point_i.y), + POS_BFP_OF_REAL(my_ned_point_i.z), + M_OF_CM((double)my_ecef_point_i.x), + M_OF_CM((double)my_ecef_point_i.y), + M_OF_CM((double)my_ecef_point_i.z)); +#endif + + float ex = my_ecef_point_d.x - M_OF_CM((double)my_ecef_point_i.x); + if (fabs(ex) > max_err.x) max_err.x = fabs(ex); + float ey = my_ecef_point_d.y - M_OF_CM((double)my_ecef_point_i.y); + if (fabs(ey) > max_err.y) max_err.y = fabs(ey); + float ez = my_ecef_point_d.z - M_OF_CM((double)my_ecef_point_i.z); + if (fabs(ez) > max_err.z) max_err.z = fabs(ez); + sum_err += ex*ex + ey*ey + ez*ez; + } + } + } + + double nb_samples = (2*RANGE_NED / STEP_NED + 1) * (2*RANGE_NED / STEP_NED + 1) * + (2*RANGE_NED / STEP_NED + 1); + + + printf("ecef_of_ned_pos int/float comparison:\n"); + printf("error max (%f,%f,%f) m\n", max_err.x, max_err.y, max_err.z ); + printf("error avg (%f ) m \n", sqrt(sum_err) / nb_samples ); + printf("\n"); + +} + static void test_enu_of_ecef_int(void) { @@ -126,15 +205,16 @@ static void test_enu_of_ecef_int(void) { ltp_def_from_ecef_f(<p_def_f, &ref_coor_f); struct EcefCoor_i ref_coor_i = { rint(CM_OF_M(ref_coor_f.x)), - rint(CM_OF_M(ref_coor_f.y)), - rint(CM_OF_M(ref_coor_f.z))}; + rint(CM_OF_M(ref_coor_f.y)), + rint(CM_OF_M(ref_coor_f.z))}; printf("ecef0 : (%d,%d,%d)\n", ref_coor_i.x, ref_coor_i.y, ref_coor_i.z); struct LtpDef_i ltp_def_i; ltp_def_from_ecef_i(<p_def_i, &ref_coor_i); - printf("lla0 : (%f deg, %f deg, %f m) (%f,%f,%f)\n", DegOfRad(ltp_def_f.lla.lat), DegOfRad(ltp_def_f.lla.lon), ltp_def_f.lla.alt, - DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lat)), - DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lon)), - M_OF_MM((double)ltp_def_i.lla.alt)); + printf("lla0 : float (%f deg, %f deg, %f m) , int (%f, %f, %f)\n", + DegOfRad(ltp_def_f.lla.lat), DegOfRad(ltp_def_f.lla.lon), ltp_def_f.lla.alt, + DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lat)), + DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lon)), + M_OF_MM((double)ltp_def_i.lla.alt)); #define STEP 1000. #define RANGE 100000. @@ -145,40 +225,40 @@ static void test_enu_of_ecef_int(void) { for (offset.x=-RANGE; offset.x<=RANGE; offset.x+=STEP) { for (offset.y=-RANGE; offset.y<=RANGE; offset.y+=STEP) { for (offset.z=-RANGE; offset.z<=RANGE; offset.z+=STEP) { - struct EcefCoor_f my_ecef_point_f = ref_coor_f; - VECT3_ADD(my_ecef_point_f, offset); - struct EnuCoor_f my_enu_point_f; - enu_of_ecef_point_f(&my_enu_point_f, <p_def_f, &my_ecef_point_f); + struct EcefCoor_f my_ecef_point_f = ref_coor_f; + VECT3_ADD(my_ecef_point_f, offset); + struct EnuCoor_f my_enu_point_f; + enu_of_ecef_point_f(&my_enu_point_f, <p_def_f, &my_ecef_point_f); #if DEBUG - printf("ecef to enu float : (%.02f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n", - my_ecef_point_f.x, my_ecef_point_f.y, my_ecef_point_f.z, - my_enu_point_f.x, my_enu_point_f.y, my_enu_point_f.z ); + printf("ecef to enu float : (%.02f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n", + my_ecef_point_f.x, my_ecef_point_f.y, my_ecef_point_f.z, + my_enu_point_f.x, my_enu_point_f.y, my_enu_point_f.z ); #endif - struct EcefCoor_i my_ecef_point_i = { rint(CM_OF_M(my_ecef_point_f.x)), - rint(CM_OF_M(my_ecef_point_f.y)), - rint(CM_OF_M(my_ecef_point_f.z))};; - struct EnuCoor_i my_enu_point_i; - enu_of_ecef_point_i(&my_enu_point_i, <p_def_i, &my_ecef_point_i); + struct EcefCoor_i my_ecef_point_i = { rint(CM_OF_M(my_ecef_point_f.x)), + rint(CM_OF_M(my_ecef_point_f.y)), + rint(CM_OF_M(my_ecef_point_f.z))};; + struct EnuCoor_i my_enu_point_i; + enu_of_ecef_point_i(&my_enu_point_i, <p_def_i, &my_ecef_point_i); #if DEBUG - // printf("def->ecef (%d,%d,%d)\n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z); - printf("ecef to enu int : (%.2f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n\n", - M_OF_CM((double)my_ecef_point_i.x), - M_OF_CM((double)my_ecef_point_i.y), - M_OF_CM((double)my_ecef_point_i.z), - M_OF_CM((double)my_enu_point_i.x), - M_OF_CM((double)my_enu_point_i.y), - M_OF_CM((double)my_enu_point_i.z)); + //printf("def->ecef (%d,%d,%d)\n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z); + printf("ecef to enu int : (%.2f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n\n", + M_OF_CM((double)my_ecef_point_i.x), + M_OF_CM((double)my_ecef_point_i.y), + M_OF_CM((double)my_ecef_point_i.z), + M_OF_CM((double)my_enu_point_i.x), + M_OF_CM((double)my_enu_point_i.y), + M_OF_CM((double)my_enu_point_i.z)); #endif - float ex = my_enu_point_f.x - M_OF_CM((double)my_enu_point_i.x); - if (fabs(ex) > max_err.x) max_err.x = fabs(ex); - float ey = my_enu_point_f.y - M_OF_CM((double)my_enu_point_i.y); - if (fabs(ey) > max_err.y) max_err.y = fabs(ey); - float ez = my_enu_point_f.z - M_OF_CM((double)my_enu_point_i.z); - if (fabs(ez) > max_err.z) max_err.z = fabs(ez); - sum_err += ex*ex + ey*ey + ez*ez; + float ex = my_enu_point_f.x - M_OF_CM((double)my_enu_point_i.x); + if (fabs(ex) > max_err.x) max_err.x = fabs(ex); + float ey = my_enu_point_f.y - M_OF_CM((double)my_enu_point_i.y); + if (fabs(ey) > max_err.y) max_err.y = fabs(ey); + float ez = my_enu_point_f.z - M_OF_CM((double)my_enu_point_i.z); + if (fabs(ez) > max_err.z) max_err.z = fabs(ez); + sum_err += ex*ex + ey*ey + ez*ez; } } } @@ -191,7 +271,6 @@ static void test_enu_of_ecef_int(void) { printf("error avg (%f ) m \n", sqrt(sum_err) / nb_samples ); printf("\n"); - } @@ -200,7 +279,8 @@ static void test_enu_of_ecef_int(void) { void test_ned_to_ecef_to_ned( void ) { -#if 0 + printf("\n--- NED -> ECEF -> NED in double ---\n"); + struct EcefCoor_d ref_coor = { 4624497.0 , 116475.0, 4376563.0}; printf("ecef0 : (%.02f,%.02f,%.02f)\n", ref_coor.x, ref_coor.y, ref_coor.z); @@ -211,18 +291,16 @@ void test_ned_to_ecef_to_ned( void ) { struct NedCoor_d ned_p1; ned_of_ecef_point_d(&ned_p1, <p_def, &ecef_p1); printf("ecef to ned : (%f,%f,%f) -> (%f,%f,%f)\n", - ecef_p1.x, ecef_p1.y, ecef_p1.z, - ned_p1.x, ned_p1.y, ned_p1.z ); + ecef_p1.x, ecef_p1.y, ecef_p1.z, + ned_p1.x, ned_p1.y, ned_p1.z ); struct EcefCoor_d ecef_p2; ecef_of_ned_point_d(&ecef_p2, <p_def, &ned_p1); printf("ned to ecef : (%f,%f,%f) -> (%f,%f,%f)\n", - ned_p1.x, ned_p1.y, ned_p1.z, - ecef_p2.x, ecef_p2.y, ecef_p2.z); + ned_p1.x, ned_p1.y, ned_p1.z, + ecef_p2.x, ecef_p2.y, ecef_p2.z); printf("\n"); -#endif - } @@ -233,6 +311,8 @@ void test_ned_to_ecef_to_ned( void ) { void test_enu_to_ecef_to_enu( void ) { + printf("\n--- ENU -> ECEF -> ENU in float ---\n"); + struct EcefCoor_f ref_coor = { 4624497.0 , 116475.0, 4376563.0}; printf("ecef0 : (%.02f,%.02f,%.02f)\n", ref_coor.x, ref_coor.y, ref_coor.z); @@ -243,19 +323,16 @@ void test_enu_to_ecef_to_enu( void ) { struct EnuCoor_f enu_p1; enu_of_ecef_point_f(&enu_p1, <p_def, &ecef_p1); printf("ecef to enu : (%f,%f,%f) -> (%f,%f,%f)\n", - ecef_p1.x, ecef_p1.y, ecef_p1.z, - enu_p1.x, enu_p1.y, enu_p1.z ); + ecef_p1.x, ecef_p1.y, ecef_p1.z, + enu_p1.x, enu_p1.y, enu_p1.z ); -#if 0 struct EcefCoor_f ecef_p2; ecef_of_enu_point_f(&ecef_p2, <p_def, &enu_p1); printf("enu to ecef : (%f,%f,%f) -> (%f,%f,%f)\n", - enu_p1.x, enu_p1.y, enu_p1.z, - ecef_p2.x, ecef_p2.y, ecef_p2.z); + enu_p1.x, enu_p1.y, enu_p1.z, + ecef_p2.x, ecef_p2.y, ecef_p2.z); printf("\n"); -#endif - } diff --git a/sw/in_progress/python/attitude_viz.py b/sw/in_progress/python/attitude_viz.py index ebc5074440..325c5d3b7f 100755 --- a/sw/in_progress/python/attitude_viz.py +++ b/sw/in_progress/python/attitude_viz.py @@ -18,353 +18,362 @@ import os _NAME = 'attitude_viz' + class TelemetryQuat: - def __init__(self, message_name, index, name, integer): - self.message_name = message_name - self.index = index - self.name = name - self.qi = 1 - self.qx = 0 - self.qy = 0 - self.qz = 0 - # optional scaling for fixed point telemetry - if integer: - self.scale = 0.00003051757812 - else: - self.scale = 1.0 + def __init__(self, message_name, index, name, integer): + self.message_name = message_name + self.index = index + self.name = name + self.qi = 1 + self.qx = 0 + self.qy = 0 + self.qz = 0 + # optional scaling for fixed point telemetry + if integer: + self.scale = 0.00003051757812 + else: + self.scale = 1.0 + class TelemetryValue: - def __init__(self, message_name, index, name, offset, scale, max): - self.message_name = message_name - self.index = index - self.name = name - self.offset = offset - self.scale = scale - self.max = max - self.value = 0 + def __init__(self, message_name, index, name, offset, scale, max): + self.message_name = message_name + self.index = index + self.name = name + self.offset = offset + self.scale = scale + self.max = max + self.value = 0 + class Visualization: - def __init__(self, parent): - self.quats = [] - self.graph_values = [] - self.throttle = 0.0 - self.mode = 0.0 - self.airspeed = 0.0 - self.display_list = None - self.display_dirty = True - self.rotate_theta = parent.rotate_theta - - for message_name, index, name, bfp in VEHICLE_QUATS: - self.quats.append(TelemetryQuat(message_name, index, name, bfp)) - for message_name, index, name, offset, scale, max in BAR_VALUES: - self.graph_values.append(TelemetryValue(message_name, index, name, offset, scale, max)) - - def onmsgproc(self, agent, *larg): - data = str(larg[0]).split(' ') - for telemetry_quat in self.quats: - if (telemetry_quat.message_name == data[1]): + def __init__(self, parent): + self.quats = [] + self.graph_values = [] + self.throttle = 0.0 + self.mode = 0.0 + self.airspeed = 0.0 + self.display_list = None self.display_dirty = True - telemetry_quat.qi = float(data[telemetry_quat.index + 0]) - telemetry_quat.qx = float(data[telemetry_quat.index + 1]) - telemetry_quat.qy = float(data[telemetry_quat.index + 2]) - telemetry_quat.qz = float(data[telemetry_quat.index + 3]) + self.rotate_theta = parent.rotate_theta - for graph_value in self.graph_values: - if (graph_value.message_name == data[1]): - self.display_dirty = True - graph_value.value = (float(data[graph_value.index + 0]) + graph_value.offset) / graph_value.scale + for message_name, index, name, bfp in VEHICLE_QUATS: + self.quats.append(TelemetryQuat(message_name, index, name, bfp)) + for message_name, index, name, offset, scale, max in BAR_VALUES: + self.graph_values.append(TelemetryValue(message_name, index, name, offset, scale, max)) - def DrawCircle(self, radius): - glBegin(GL_TRIANGLE_FAN) - glVertex3f(0, 0, 0) - for angle in range (0, 361, 12): - glVertex3f( math.sin(math.radians(angle)) * radius, math.cos(math.radians(angle)) * radius, 0) - glEnd() + def onmsgproc(self, agent, *larg): + data = str(larg[0]).split(' ') + for telemetry_quat in self.quats: + if telemetry_quat.message_name == data[1]: + self.display_dirty = True + telemetry_quat.qi = float(data[telemetry_quat.index + 0]) + telemetry_quat.qx = float(data[telemetry_quat.index + 1]) + telemetry_quat.qy = float(data[telemetry_quat.index + 2]) + telemetry_quat.qz = float(data[telemetry_quat.index + 3]) - # draw quad centered at origin, z = 0 - def DrawQuad(self, width, height): - glBegin (GL_QUADS) - glVertex3f( width, height, 0) - glVertex3f( -width, height, 0) - glVertex3f( -width, -height, 0) - glVertex3f( width, -height, 0) - glEnd() + for graph_value in self.graph_values: + if graph_value.message_name == data[1]: + self.display_dirty = True + graph_value.value = (float(data[graph_value.index + 0]) + graph_value.offset) / graph_value.scale - def DrawBox(self, width, height, depth): - glPushMatrix() - glTranslate(0, 0, depth) - self.DrawQuad(width, height) - glTranslate(0, 0, -2 * depth) - self.DrawQuad(width, height) - glPopMatrix() + def DrawCircle(self, radius): + glBegin(GL_TRIANGLE_FAN) + glVertex3f(0, 0, 0) + for angle in range(0, 361, 12): + glVertex3f(math.sin(math.radians(angle)) * radius, math.cos(math.radians(angle)) * radius, 0) + glEnd() - glPushMatrix() - glRotate(90, 1, 0, 0) - glTranslate(0, 0, height) - self.DrawQuad(width, depth) - glTranslate(0, 0, -2 * height) - self.DrawQuad(width, depth) - glPopMatrix() + # draw quad centered at origin, z = 0 + def DrawQuad(self, width, height): + glBegin(GL_QUADS) + glVertex3f(width, height, 0) + glVertex3f(-width, height, 0) + glVertex3f(-width, -height, 0) + glVertex3f(width, -height, 0) + glEnd() - glPushMatrix() - glRotate(90, 0, 1, 0) - glTranslate(0, 0, width) - self.DrawQuad(depth, height) - glTranslate(0, 0, -2 * width) - self.DrawQuad(depth, height) - glPopMatrix() - - def DrawVehicle(self, name): - wingspan = 2.7 - separation = 0.7 - chord = 0.35 - thickness = 0.08 - strutcount = 3 - discradius = 0.45 - discseparation = 0.01 - - #wings - glColor3f(0.1, 0.1, 0.9) - glPushMatrix() - glTranslate(0, 0, 0.05) - self.DrawBox(wingspan, chord, thickness) - glColor3f(0.0, 0.0, 0.0) - glTranslate(-wingspan, -0.2, thickness + 0.01) - glScale(0.004, 0.004, 0.004) - for c in name: - glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) - glPopMatrix() - - glPushMatrix() - glTranslate(0, 0, -0.05) - glColor3f(0.6, 0.6, 0.2) - self.DrawBox(wingspan, chord, thickness) - glColor3f(0.0, 0.0, 0.0) - glTranslate(wingspan, -0.2, -0.01 - thickness) - glScale(0.004, 0.004, 0.004) - glRotate(180, 0, 1, 0) - for c in name: - glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) - glPopMatrix() - - if self.display_list is None: - self.display_list = glGenLists(1) - glNewList(self.display_list, GL_COMPILE) - # struts - glColor3f(0.4, 0.4, 0.4) - glPushMatrix() - glTranslate(-wingspan/2, 0, separation/2) - glRotate(90, 0, 1, 0) - for x in range (0, strutcount-1): - self.DrawBox(separation/2, chord - .01, thickness) - glTranslate(0, 0, wingspan) - glTranslate(separation, 0, -5*wingspan/2) - for x in range (0, strutcount-1): - self.DrawBox(separation/2, chord - .01, thickness) - glTranslate(0, 0, 2*wingspan) - glPopMatrix() - - #rotors - glColor3f(0.9, 0.1, 0.1) - glPushMatrix() - glRotate(90, 1, 0, 0) - glTranslate(-wingspan/2, separation, -(chord + .01)) - for x in range (0, strutcount): - if (x != strutcount/2): - self.DrawCircle(discradius) - glTranslate(2 * wingspan/(strutcount + 1), 0, 0) - glPopMatrix() - - glPushMatrix() - glRotate(90, 1, 0, 0) - glTranslate(-wingspan, -separation, -(chord + .01)) - for x in range (0, strutcount): - if (x != strutcount/2): - self.DrawCircle(discradius) - glTranslate(2 * wingspan/(strutcount - 1), 0, 0) - glPopMatrix() - glEndList() - - glCallList(self.display_list) - - def DrawBar(self, name, value): - bar_height = 0.12 - bar_length = 3 - glPushMatrix() - glColor3f(0, 0, 0) - glTranslate(-bar_length, -0.09, 0.02) - glScale(0.0015, 0.0015, 0.0015) - for c in name: - glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) - glPopMatrix() - glColor3f(0.92, 0.92, 0.92) - glPushMatrix() - glTranslate(0, 0, 0) - self.DrawQuad(bar_length, bar_height) - glPopMatrix() - glPushMatrix() - glTranslate(bar_length * value - bar_length, 0, 0.01) - glColor3f(0.6, 0.6, 0.6) - self.DrawQuad(bar_length * value, bar_height) - glPopMatrix() - - def Draw(self): - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) - - glPushMatrix() - - height = 5 - - glDisable(GL_LIGHTING) - glPushMatrix() - for graph_value in self.graph_values: - self.DrawBar(graph_value.name % (graph_value.value), graph_value.value / graph_value.max) - glTranslate(0, 0.35, 0) - glPopMatrix() - glEnable(GL_LIGHTING) - - glTranslate(0, -height + (height / len(self.quats) + 1), 0) - for telemetry_quat in self.quats: - glPushMatrix() - try: - scaled_quat = [telemetry_quat.qi * telemetry_quat.scale, telemetry_quat.qx * telemetry_quat.scale, telemetry_quat.qy * telemetry_quat.scale, telemetry_quat.qz * telemetry_quat.scale] - glRotate(360 * math.acos(scaled_quat[0] ) / math.pi, scaled_quat[2], -scaled_quat[3], -scaled_quat[1]) - glRotate(self.rotate_theta, 1, 0, 0) - - self.DrawVehicle(telemetry_quat.name) - except Exception: - raise Exception - finally: + def DrawBox(self, width, height, depth): + glPushMatrix() + glTranslate(0, 0, depth) + self.DrawQuad(width, height) + glTranslate(0, 0, -2 * depth) + self.DrawQuad(width, height) glPopMatrix() - glTranslate(0, 2 * height / (len(self.quats)), 0) - glPopMatrix() + + glPushMatrix() + glRotate(90, 1, 0, 0) + glTranslate(0, 0, height) + self.DrawQuad(width, depth) + glTranslate(0, 0, -2 * height) + self.DrawQuad(width, depth) + glPopMatrix() + + glPushMatrix() + glRotate(90, 0, 1, 0) + glTranslate(0, 0, width) + self.DrawQuad(depth, height) + glTranslate(0, 0, -2 * width) + self.DrawQuad(depth, height) + glPopMatrix() + + def DrawVehicle(self, name): + wingspan = 2.7 + separation = 0.7 + chord = 0.35 + thickness = 0.08 + strutcount = 3 + discradius = 0.45 + discseparation = 0.01 + + #wings + glColor3f(0.1, 0.1, 0.9) + glPushMatrix() + glTranslate(0, 0, 0.05) + self.DrawBox(wingspan, chord, thickness) + glColor3f(0.0, 0.0, 0.0) + glTranslate(-wingspan, -0.2, thickness + 0.01) + glScale(0.004, 0.004, 0.004) + for c in name: + glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) + glPopMatrix() + + glPushMatrix() + glTranslate(0, 0, -0.05) + glColor3f(0.6, 0.6, 0.2) + self.DrawBox(wingspan, chord, thickness) + glColor3f(0.0, 0.0, 0.0) + glTranslate(wingspan, -0.2, -0.01 - thickness) + glScale(0.004, 0.004, 0.004) + glRotate(180, 0, 1, 0) + for c in name: + glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) + glPopMatrix() + + if self.display_list is None: + self.display_list = glGenLists(1) + glNewList(self.display_list, GL_COMPILE) + # struts + glColor3f(0.4, 0.4, 0.4) + glPushMatrix() + glTranslate(-wingspan / 2, 0, separation / 2) + glRotate(90, 0, 1, 0) + for x in range(0, strutcount - 1): + self.DrawBox(separation / 2, chord - .01, thickness) + glTranslate(0, 0, wingspan) + glTranslate(separation, 0, -5 * wingspan / 2) + for x in range(0, strutcount - 1): + self.DrawBox(separation / 2, chord - .01, thickness) + glTranslate(0, 0, 2 * wingspan) + glPopMatrix() + + #rotors + glColor3f(0.9, 0.1, 0.1) + glPushMatrix() + glRotate(90, 1, 0, 0) + glTranslate(-wingspan / 2, separation, -(chord + .01)) + for x in range(0, strutcount): + if (x != strutcount / 2): + self.DrawCircle(discradius) + glTranslate(2 * wingspan / (strutcount + 1), 0, 0) + glPopMatrix() + + glPushMatrix() + glRotate(90, 1, 0, 0) + glTranslate(-wingspan, -separation, -(chord + .01)) + for x in range(0, strutcount): + if (x != strutcount / 2): + self.DrawCircle(discradius) + glTranslate(2 * wingspan / (strutcount - 1), 0, 0) + glPopMatrix() + glEndList() + + glCallList(self.display_list) + + def DrawBar(self, name, value): + bar_height = 0.12 + bar_length = 3 + glPushMatrix() + glColor3f(0, 0, 0) + glTranslate(-bar_length, -0.09, 0.02) + glScale(0.0015, 0.0015, 0.0015) + for c in name: + glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) + glPopMatrix() + glColor3f(0.92, 0.92, 0.92) + glPushMatrix() + glTranslate(0, 0, 0) + self.DrawQuad(bar_length, bar_height) + glPopMatrix() + glPushMatrix() + glTranslate(bar_length * value - bar_length, 0, 0.01) + glColor3f(0.6, 0.6, 0.6) + self.DrawQuad(bar_length * value, bar_height) + glPopMatrix() + + def Draw(self): + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) + + glPushMatrix() + + height = 5 + + glDisable(GL_LIGHTING) + glPushMatrix() + for graph_value in self.graph_values: + self.DrawBar(graph_value.name % (graph_value.value), graph_value.value / graph_value.max) + glTranslate(0, 0.35, 0) + glPopMatrix() + glEnable(GL_LIGHTING) + + glTranslate(0, -height + (height / len(self.quats) + 1), 0) + for telemetry_quat in self.quats: + glPushMatrix() + try: + scaled_quat = [telemetry_quat.qi * telemetry_quat.scale, telemetry_quat.qx * telemetry_quat.scale, + telemetry_quat.qy * telemetry_quat.scale, telemetry_quat.qz * telemetry_quat.scale] + glRotate(360 * math.acos(scaled_quat[0]) / math.pi, scaled_quat[2], -scaled_quat[3], -scaled_quat[1]) + glRotate(self.rotate_theta, 1, 0, 0) + + self.DrawVehicle(telemetry_quat.name) + except Exception: + raise Exception + finally: + glPopMatrix() + glTranslate(0, 2 * height / (len(self.quats)), 0) + glPopMatrix() + class Visualizer: - def __init__(self, rotate_theta): - self.rotate_theta = rotate_theta - self.visualization = Visualization(self) + def __init__(self, rotate_theta): + self.rotate_theta = rotate_theta + self.visualization = Visualization(self) - # listen to Ivy - logging.getLogger('Ivy').setLevel(logging.WARN) - IvyInit(_NAME, - "", - 0, - lambda x, y: y, - lambda x, z: z - ) + # listen to Ivy + logging.getLogger('Ivy').setLevel(logging.WARN) + IvyInit(_NAME, + "", + 0, + lambda x, y: y, + lambda x, z: z) - if os.getenv('IVY_BUS') != None: - IvyStart(os.getenv('IVY_BUS')) - else: - if platform.system() == 'Darwin': - IvyStart("224.255.255.255:2010") - else: - IvyStart() + if os.getenv('IVY_BUS') is not None: + IvyStart(os.getenv('IVY_BUS')) + else: + if platform.system() == 'Darwin': + IvyStart("224.255.255.255:2010") + else: + IvyStart() - # list of all message names - messages = [] + # list of all message names + messages = [] - # append all message names - for vehicle_quat in VEHICLE_QUATS: - messages.append(vehicle_quat[0]) - for bar_value in BAR_VALUES: - messages.append(bar_value[0]) + # append all message names + for vehicle_quat in VEHICLE_QUATS: + messages.append(vehicle_quat[0]) + for bar_value in BAR_VALUES: + messages.append(bar_value[0]) - # bind to set of messages (ie, only bind each message once) - for message_name in set(messages): - bind_string = "(^.*" + message_name + ".*$)" - IvyBindMsg(self.visualization.onmsgproc, bind_string) + # bind to set of messages (ie, only bind each message once) + for message_name in set(messages): + bind_string = "(^.*" + message_name + ".*$)" + IvyBindMsg(self.visualization.onmsgproc, bind_string) - def Draw(self): - if self.visualization.display_dirty: - self.visualization.Draw() - self.visualization.display_dirty = False + def Draw(self): + if self.visualization.display_dirty: + self.visualization.Draw() + self.visualization.display_dirty = False + + def OnClose(self): + IvyStop() - def OnClose(self): - IvyStop() SCREEN_SIZE = (800, 800) + def resize(width, height): - glViewport(0, 0, width, height) - glMatrixMode(GL_PROJECTION) - glLoadIdentity() - gluPerspective(60.0, float(width/height), .1, 100.) - glMatrixMode(GL_MODELVIEW) - glLoadIdentity() + glViewport(0, 0, width, height) + glMatrixMode(GL_PROJECTION) + glLoadIdentity() + gluPerspective(60.0, float(width / height), .1, 100.) + glMatrixMode(GL_MODELVIEW) + glLoadIdentity() + def init(): - glutInit() - glEnable(GL_LINE_SMOOTH) - glEnable(GL_DEPTH_TEST) - glEnable(GL_LIGHTING) - glEnable(GL_LIGHT0) - glEnable(GL_BLEND) - glShadeModel (GL_SMOOTH) - glClearColor(1.0, 1.0, 1.0, 1.0) - glClearDepth(1.0) + glutInit() + glEnable(GL_LINE_SMOOTH) + glEnable(GL_DEPTH_TEST) + glEnable(GL_LIGHTING) + glEnable(GL_LIGHT0) + glEnable(GL_BLEND) + glShadeModel(GL_SMOOTH) + glClearColor(1.0, 1.0, 1.0, 1.0) + glClearDepth(1.0) - glPointSize(3.0) + glPointSize(3.0) - glMatrixMode(GL_PROJECTION) - glLoadIdentity() - gluPerspective(7.0, 1.0, 95.0, 105.0) + glMatrixMode(GL_PROJECTION) + glLoadIdentity() + gluPerspective(7.0, 1.0, 95.0, 105.0) - glMatrixMode(GL_MODELVIEW) + glMatrixMode(GL_MODELVIEW) - glLight(GL_LIGHT0, GL_POSITION, [5, 30, -20]) - glLight(GL_LIGHT0, GL_AMBIENT, [0.5, 0.5, 0.5]) - glLight(GL_LIGHT0, GL_SPECULAR, [0.0, 0.0, 0.0]) - glLight(GL_LIGHT0, GL_DIFFUSE, [0.8, 0.8, 0.8]) - glEnable(GL_COLOR_MATERIAL) - glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE) + glLight(GL_LIGHT0, GL_POSITION, [5, 30, -20]) + glLight(GL_LIGHT0, GL_AMBIENT, [0.5, 0.5, 0.5]) + glLight(GL_LIGHT0, GL_SPECULAR, [0.0, 0.0, 0.0]) + glLight(GL_LIGHT0, GL_DIFFUSE, [0.8, 0.8, 0.8]) + glEnable(GL_COLOR_MATERIAL) + glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE) + + glMatrixMode(GL_MODELVIEW) + glLoadIdentity() + gluLookAt(0.0, 0.0, 100.0, + 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0) - glMatrixMode(GL_MODELVIEW) - glLoadIdentity() - gluLookAt(0.0, 0.0, 100.0, - 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0) def run(): - global VEHICLE_QUATS, BAR_VALUES - VEHICLE_QUATS = [ ["AHRS_REF_QUAT", 6, "Estimate", True], ["AHRS_REF_QUAT", 2, "Reference", True]] - BAR_VALUES = [ ["ROTORCRAFT_RADIO_CONTROL", 5, "Throttle (%%) %i", 0, 100, 100] ] - window_title = "Attitude_Viz" - rotate_theta = -90 - try: - opts, args = getopt.getopt(sys.argv[1:], "t:r:", ["title", "rotate_theta"]) - for o, a in opts: - if o in ("-t", "--title"): - window_title = a - if o in ("-r", "--rotate_theta"): - rotate_theta = int(a) - except getopt.error as msg: - print(msg) - print("""usage: + global VEHICLE_QUATS, BAR_VALUES + VEHICLE_QUATS = [["AHRS_REF_QUAT", 6, "Estimate", True], ["AHRS_REF_QUAT", 2, "Reference", True]] + BAR_VALUES = [["ROTORCRAFT_RADIO_CONTROL", 5, "Throttle (%%) %i", 0, 100, 100]] + window_title = "Attitude_Viz" + rotate_theta = -90 + try: + opts, args = getopt.getopt(sys.argv[1:], "t:r:", ["title", "rotate_theta"]) + for o, a in opts: + if o in ("-t", "--title"): + window_title = a + if o in ("-r", "--rotate_theta"): + rotate_theta = int(a) + except getopt.error as msg: + print(msg) + print("""usage: -t, --title set window title -r, --rotate_theta rotate the quaternion by n degrees over the pitch axis (default: -90) """) - pygame.init() - screen = pygame.display.set_mode(SCREEN_SIZE, pygame.OPENGL|pygame.DOUBLEBUF) - #resize(*SCREEN_SIZE) - init() - visualizer = Visualizer(rotate_theta) + pygame.init() + screen = pygame.display.set_mode(SCREEN_SIZE, pygame.OPENGL | pygame.DOUBLEBUF) + #resize(*SCREEN_SIZE) + init() + visualizer = Visualizer(rotate_theta) + + try: + while True: + for event in pygame.event.get(): + if event.type == pygame.QUIT: + visualizer.OnClose() + return + if event.type == pygame.KEYUP and event.key == pygame.K_ESCAPE: + visualizer.OnClose() + return + visualizer.Draw() + pygame.display.flip() + time.sleep(.02) + except KeyboardInterrupt: + visualizer.OnClose() + return - try: - while True: - for event in pygame.event.get(): - if event.type == pygame.QUIT: - visualizer.OnClose() - return - if event.type == pygame.KEYUP and event.key == pygame.K_ESCAPE: - visualizer.OnClose() - return - visualizer.Draw() - pygame.display.flip() - time.sleep(.02) - except KeyboardInterrupt: - visualizer.OnClose() - return if __name__ == "__main__": - run() + run() diff --git a/sw/lib/ocaml/expr_syntax.ml b/sw/lib/ocaml/expr_syntax.ml index d94a1cfe71..269f959b85 100644 --- a/sw/lib/ocaml/expr_syntax.ml +++ b/sw/lib/ocaml/expr_syntax.ml @@ -39,22 +39,32 @@ type expression = let c_var_of_ident = fun x -> "_var_" ^ x -let rec sprint = function -Ident i when i.[0] = '$' -> sprintf "%s" (c_var_of_ident (String.sub i 1 (String.length i - 1))) - | Ident i -> sprintf "%s" i - | Int i -> sprintf "%d" i - | Float i -> sprintf "%f" i - | CallOperator (op, [e1;e2]) -> - sprintf "(%s%s%s)" (sprint e1) op (sprint e2) - | CallOperator (op, [e1]) -> - sprintf "%s(%s)" op (sprint e1) - | CallOperator (_,_) -> failwith "Operator should be binary or unary" - | Call (i, es) -> - let ses = List.map sprint es in - sprintf "%s(%s)" i (String.concat "," ses) - | Index (i,e) -> sprintf "%s[%s]" i (sprint e) - | Field (i,f) -> sprintf "%s.%s" i f - | Deref (e,f) -> sprintf "(%s)->%s" (sprint e) f +let sprint = fun ?call_assoc expr -> + let n, l = match call_assoc with + | None -> None, [] + | Some (n, l) -> Some n, l + in + let rec eval = function + | Ident i when i.[0] = '$' -> sprintf "%s" (c_var_of_ident (String.sub i 1 (String.length i - 1))) + | Ident i -> sprintf "%s" i + | Int i -> sprintf "%d" i + | Float i -> sprintf "%f" i + | CallOperator (op, [e1;e2]) -> + sprintf "(%s%s%s)" (eval e1) op (eval e2) + | CallOperator (op, [e1]) -> + sprintf "%s(%s)" op (eval e1) + | CallOperator (_,_) -> failwith "Operator should be binary or unary" + | Call (i, [Ident s]) when Some i = n -> + let index = try List.assoc s l with Not_found -> failwith (sprintf "Unknown block: '%s'" s) in + sprintf "%d" index + | Call (i, es) -> + let ses = List.map eval es in + sprintf "%s(%s)" i (String.concat "," ses) + | Index (i,e) -> sprintf "%s[%s]" i (eval e) + | Field (i,f) -> sprintf "%s.%s" i f + | Deref (e,f) -> sprintf "(%s)->%s" (eval e) f + in + eval expr (* Valid functions : FIXME *) let functions = [ diff --git a/sw/lib/ocaml/expr_syntax.mli b/sw/lib/ocaml/expr_syntax.mli index ee57fbe537..46c6f30c23 100644 --- a/sw/lib/ocaml/expr_syntax.mli +++ b/sw/lib/ocaml/expr_syntax.mli @@ -37,7 +37,8 @@ type expression = val c_var_of_ident : ident -> string (** Encapsulate a user ident into a C variable *) -val sprint : expression -> string +val sprint : ?call_assoc:(ident * (ident * int) list) -> expression -> string +(** call_assoc: call an optional association list function *) exception Unknown_ident of string exception Unknown_operator of string diff --git a/sw/lib/ocaml/latlong.ml b/sw/lib/ocaml/latlong.ml index 1e7e3d689a..5862626830 100644 --- a/sw/lib/ocaml/latlong.ml +++ b/sw/lib/ocaml/latlong.ml @@ -469,9 +469,16 @@ let bearing = fun geo1 geo2 -> ((Rad>>Deg)(atan2 dx dy), sqrt(dx*.dx+.dy*.dy)) -let leap_seconds = 15 (* http://www.leapsecond.com/java/gpsclock.htm *) +(** Offset between GPS and UTC times in seconds. + * Update when a new leap second is inserted and be careful about times in the + * past when this offset was different. + * Last leap second was inserted on June 30, 2012 at 23:59:60 UTC + * http://www.leapsecond.com/java/gpsclock.htm + *) +let leap_seconds = 16 -let gps_epoch = 315964800. (* In seconds, in the unix reference *) +(** Unix timestamp of the GPS epoch 1980-01-06 00:00:00 UTC *) +let gps_epoch = 315964800. let gps_tow_of_utc = fun ?wday hour min sec -> let wday = diff --git a/sw/simulator/fg.c b/sw/simulator/fg.c index 63493b2a30..19bd9c7a91 100644 --- a/sw/simulator/fg.c +++ b/sw/simulator/fg.c @@ -41,7 +41,7 @@ value fg_msg_native(value s, value lat, value lon, value z, value phi, value the msg.num_tanks = 1; msg.fuel_quantity[0] = 10.; - msg.cur_time = 3213092700ul;//time(NULL); + msg.cur_time = 3213092700ul+((uint32_t)((msg.longitude)*13578)); //time(NULL); msg.warp = 0; msg.ground_elev = 0.; 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/calibration/calibrate.py b/sw/tools/calibration/calibrate.py index c67aa7df0b..082545f995 100755 --- a/sw/tools/calibration/calibrate.py +++ b/sw/tools/calibration/calibrate.py @@ -30,6 +30,7 @@ from scipy import optimize import calibration_utils + def main(): usage = "usage: %prog [options] log_filename.data" + "\n" + "Run %prog --help to list the options." parser = OptionParser(usage) @@ -58,7 +59,7 @@ def main(): print(args[0] + " not found") sys.exit(1) ac_ids = calibration_utils.get_ids_in_log(filename) - if options.ac_id == None: + if options.ac_id is None: if len(ac_ids) == 1: options.ac_id = ac_ids[0] else: @@ -69,13 +70,13 @@ def main(): if options.sensor == "ACCEL": sensor_ref = 9.81 sensor_res = 10 - noise_window = 20; - noise_threshold = 40; + noise_window = 20 + noise_threshold = 40 elif options.sensor == "MAG": sensor_ref = 1. sensor_res = 11 - noise_window = 10; - noise_threshold = 1000; + noise_window = 10 + noise_threshold = 1000 if not filename.endswith(".data"): parser.error("Please specify a *.data log file") @@ -88,7 +89,7 @@ def main(): print("Error: found zero IMU_"+options.sensor+"_RAW measurements for aircraft with id "+options.ac_id+" in log file!") sys.exit(1) if options.verbose: - print("found "+str(len(measurements))+" records") + print("found "+str(len(measurements))+" records") # estimate the noise threshold # find the median of measurement vector lenght diff --git a/sw/tools/calibration/calibrate_gyro.py b/sw/tools/calibration/calibrate_gyro.py index 7f2970e0fd..77aac9f076 100755 --- a/sw/tools/calibration/calibrate_gyro.py +++ b/sw/tools/calibration/calibrate_gyro.py @@ -87,7 +87,7 @@ def main(): if options.verbose: print("reading file "+filename+" for aircraft "+str(options.ac_id)+" and turntable "+str(options.tt_id)) - samples = calibration_utils.read_turntable_log(options.ac_id, options.tt_id, filename, 1, 7) + samples = calibration_utils.read_turntable_log(options.ac_id, options.tt_id, filename, 1, 7) if len(samples) == 0: print("Error: found zero matching messages in log file!") @@ -106,13 +106,13 @@ def main(): parser.error("Specify a valid axis!") #Linear regression using stats.linregress - t = samples[:, 0] + t = samples[:, 0] xn = samples[:, axis_idx] - (a_s, b_s, r, tt, stderr)=stats.linregress(t, xn) + (a_s, b_s, r, tt, stderr) = stats.linregress(t, xn) print('Linear regression using stats.linregress') print(('regression: a=%.2f b=%.2f, std error= %.3f' % (a_s, b_s, stderr))) - print(('' % (b_s))); - print(('' % (pow(2, 12)/a_s))); + print(('' % (b_s))) + print(('' % (pow(2, 12)/a_s))) # # overlay fited value @@ -125,7 +125,7 @@ def main(): plot(samples[:, 1]) plot(samples[:, 2]) plot(samples[:, 3]) - legend(['p', 'q', 'r']); + legend(['p', 'q', 'r']) subplot(3, 1, 2) plot(samples[:, 0]) @@ -134,7 +134,7 @@ def main(): plot(samples[:, 0], samples[:, axis_idx], 'b.') plot(ovl_omega, ovl_adc, 'r') - show(); + show() if __name__ == "__main__": diff --git a/sw/tools/calibration/calibrate_mag_current.py b/sw/tools/calibration/calibrate_mag_current.py index d93f6cfc64..6779f0f9a1 100755 --- a/sw/tools/calibration/calibrate_mag_current.py +++ b/sw/tools/calibration/calibrate_mag_current.py @@ -49,7 +49,7 @@ def main(): print(args[0] + " not found") sys.exit(1) ac_ids = calibration_utils.get_ids_in_log(filename) - if options.ac_id == None: + if options.ac_id is None: if len(ac_ids) == 1: options.ac_id = ac_ids[0] else: @@ -57,10 +57,6 @@ def main(): if options.verbose: print("Using aircraft id "+options.ac_id) - sensor_ref = 1. - sensor_res = 11 - noise_window = 10; - noise_threshold = 1000; if not filename.endswith(".data"): parser.error("Please specify a *.data log file") @@ -73,7 +69,7 @@ def main(): print("Error: found zero IMU_MAG_CURRENT_CALIBRATION measurements for aircraft with id "+options.ac_id+" in log file!") sys.exit(1) if options.verbose: - print("found "+str(len(measurements))+" records") + print("found "+str(len(measurements))+" records") coefficient = calibration_utils.estimate_mag_current_relation(measurements) diff --git a/sw/tools/calibration/calibration_utils.py b/sw/tools/calibration/calibration_utils.py index 0cc8f54b75..4fc13aab6c 100644 --- a/sw/tools/calibration/calibration_utils.py +++ b/sw/tools/calibration/calibration_utils.py @@ -38,7 +38,7 @@ def get_ids_in_log(filename): line = f.readline().strip() if line == '': break - m=re.match(pattern, line) + m = re.match(pattern, line) if m: id = m.group(1) if not id in ids: @@ -56,7 +56,7 @@ def read_log(ac_id, filename, sensor): line = f.readline().strip() if line == '': break - m=re.match(pattern, line) + m = re.match(pattern, line) if m: list_meas.append([float(m.group(2)), float(m.group(3)), float(m.group(4))]) return scipy.array(list_meas) @@ -72,7 +72,7 @@ def read_log_mag_current(ac_id, filename): line = f.readline().strip() if line == '': break - m=re.match(pattern, line) + m = re.match(pattern, line) if m: list_meas.append([float(m.group(2)), float(m.group(3)), float(m.group(4)), float(m.group(5))]) return scipy.array(list_meas) @@ -106,8 +106,8 @@ def get_min_max_guess(meas, scale): # scale the set of measurements # def scale_measurements(meas, p): - l_comp = []; - l_norm = []; + l_comp = [] + l_norm = [] for m in meas[:,]: sm = (m - p[0:3])*p[3:6] l_comp.append(sm) @@ -120,7 +120,7 @@ def scale_measurements(meas, p): def estimate_mag_current_relation(meas): coefficient = [] for i in range(0,3): - gradient, intercept, r_value, p_value, std_err = stats.linregress(meas[:,3],meas[:,i]) + gradient, intercept, r_value, p_value, std_err = stats.linregress(meas[:,3], meas[:,i]) coefficient.append(gradient) return coefficient @@ -154,26 +154,26 @@ def plot_results(block, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sen title('Raw sensors') subplot(3, 2, 3) - plot(cp0[:, 0]); - plot(cp0[:, 1]); - plot(cp0[:, 2]); - plot(-sensor_ref*scipy.ones(len(flt_meas))); - plot(sensor_ref*scipy.ones(len(flt_meas))); + plot(cp0[:, 0]) + plot(cp0[:, 1]) + plot(cp0[:, 2]) + plot(-sensor_ref*scipy.ones(len(flt_meas))) + plot(sensor_ref*scipy.ones(len(flt_meas))) subplot(3, 2, 4) - plot(np0); - plot(sensor_ref*scipy.ones(len(flt_meas))); + plot(np0) + plot(sensor_ref*scipy.ones(len(flt_meas))) subplot(3, 2, 5) - plot(cp1[:, 0]); - plot(cp1[:, 1]); - plot(cp1[:, 2]); - plot(-sensor_ref*scipy.ones(len(flt_meas))); - plot(sensor_ref*scipy.ones(len(flt_meas))); + plot(cp1[:, 0]) + plot(cp1[:, 1]) + plot(cp1[:, 2]) + plot(-sensor_ref*scipy.ones(len(flt_meas))) + plot(sensor_ref*scipy.ones(len(flt_meas))) subplot(3, 2, 6) - plot(np1); - plot(sensor_ref*scipy.ones(len(flt_meas))); + plot(np1) + plot(sensor_ref*scipy.ones(len(flt_meas))) # if we want to have another plot we only draw the figure (non-blocking) # also in matplotlib before 1.0.0 there is only one call to show possible @@ -187,14 +187,14 @@ def plot_results(block, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sen # def plot_mag_3d(measured, calibrated, p): # set up points for sphere and ellipsoid wireframes - u=r_[0:2*pi:20j] - v=r_[0:pi:20j] - wx=outer(cos(u),sin(v)) - wy=outer(sin(u),sin(v)) - wz=outer(ones(size(u)),cos(v)) - ex=p[0]*ones(size(u)) + outer(cos(u),sin(v))/p[3] - ey=p[1]*ones(size(u)) + outer(sin(u),sin(v))/p[4] - ez=p[2]*ones(size(u)) + outer(ones(size(u)),cos(v))/p[5] + u = r_[0:2 * pi:20j] + v = r_[0:pi:20j] + wx = outer(cos(u), sin(v)) + wy = outer(sin(u), sin(v)) + wz = outer(ones(size(u)), cos(v)) + ex = p[0] * ones(size(u)) + outer(cos(u), sin(v)) / p[3] + ey = p[1] * ones(size(u)) + outer(sin(u), sin(v)) / p[4] + ez = p[2] * ones(size(u)) + outer(ones(size(u)), cos(v)) / p[5] # measurements mx = measured[:, 0] @@ -228,10 +228,10 @@ def plot_mag_3d(measured, calibrated, p): ax.plot_wireframe(ex, ey, ez, color='grey', alpha=0.5) # Create cubic bounding box to simulate equal aspect ratio - max_range = np.array([mx.max()-mx.min(), my.max()-my.min(), mz.max()-mz.min()]).max() - Xb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][0].flatten() + 0.5*(mx.max()+mx.min()) - Yb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][1].flatten() + 0.5*(my.max()+my.min()) - Zb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][2].flatten() + 0.5*(mz.max()+mz.min()) + max_range = np.array([mx.max() - mx.min(), my.max() - my.min(), mz.max() - mz.min()]).max() + Xb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][0].flatten() + 0.5 * (mx.max() + mx.min()) + Yb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][1].flatten() + 0.5 * (my.max() + my.min()) + Zb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][2].flatten() + 0.5 * (mz.max() + mz.min()) # uncomment following both lines to test the fake bounding box: #for xb, yb, zb in zip(Xb, Yb, Zb): # ax.plot([xb], [yb], [zb], 'r*') @@ -241,7 +241,6 @@ def plot_mag_3d(measured, calibrated, p): ax.set_ylabel('y') ax.set_zlabel('z') - if matplotlib.__version__.startswith('0'): ax = Axes3D(fig, rect=rect_r) else: @@ -273,10 +272,10 @@ def read_turntable_log(ac_id, tt_id, filename, _min, _max): line = f.readline().strip() if line == '': break - m=re.match(pattern_t, line) + m = re.match(pattern_t, line) if m: last_tt = float(m.group(2)) - m=re.match(pattern_g, line) + m = re.match(pattern_g, line) if m and last_tt and last_tt > _min and last_tt < _max: list_tt.append([last_tt, float(m.group(2)), float(m.group(3)), float(m.group(4))]) return scipy.array(list_tt) 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"; diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index 972704966b..d0f466edf2 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -36,6 +36,8 @@ let georef_of_xml = fun xml -> let sof = string_of_float let soi = string_of_int +let index_of_blocks = ref [] + let check_expressions = ref false let cm = fun x -> 100. *. x @@ -55,7 +57,7 @@ let parse = fun s -> | Expr_syntax.Unknown_function x -> unexpected "function" x end end; - Expr_syntax.sprint e + Expr_syntax.sprint ~call_assoc:("IndexOfBlock", !index_of_blocks) e let parsed_attrib = fun xml a -> parse (ExtXml.attrib xml a) @@ -126,8 +128,6 @@ let print_waypoint_lla = fun utm0 default_alt waypoint -> printf " {%d, %d, %.0f}, /* 1e7deg, 1e7deg, cm (hmsl=%.2fm) */ \\\n" (convert_angle wgs84.posn_lat) (convert_angle wgs84.posn_long) (100. *. float_of_string alt) (Egm96.of_wgs84 wgs84) -let index_of_blocks = ref [] - let get_index_block = fun x -> try List.assoc x !index_of_blocks @@ -481,6 +481,13 @@ let rec print_stage = fun index_of_waypoints x -> let statement = ExtXml.attrib x "fun" in lprintf "if (! (%s))\n" statement; lprintf " NextStageAndBreak();\n"; + begin + try + let c = parsed_attrib x "until" in + lprintf "if (%s) NextStageAndBreak();\n" c + with + ExtXml.Error _ -> () + end; lprintf "break;\n" | "survey_rectangle" -> let grid = parsed_attrib x "grid" @@ -495,6 +502,13 @@ let rec print_stage = fun index_of_waypoints x -> left (); stage (); lprintf "NavSurveyRectangle(%s, %s);\n" wp1 wp2; + begin + try + let c = parsed_attrib x "until" in + lprintf "if (%s) NextStageAndBreak();\n" c + with + ExtXml.Error _ -> () + end; lprintf "break;\n" | _s -> failwith "Unreachable" end; @@ -785,6 +799,9 @@ let () = lprintf "};\n"; Xml2h.define "NB_WAYPOINT" (string_of_int (List.length waypoints)); + Xml2h.define "FP_BLOCKS" "{ \\"; + List.iter (fun b -> printf " { \"%s\" }, \\\n" (ExtXml.attrib b "name")) blocks; + lprintf "};\n"; Xml2h.define "NB_BLOCK" (string_of_int (List.length blocks)); Xml2h.define "GROUND_ALT" (sof !ground_alt); @@ -794,15 +811,6 @@ let () = Xml2h.define "HOME_MODE_HEIGHT" (sof home_mode_height); Xml2h.define "MAX_DIST_FROM_HOME" (sof mdfh); - (** Print defines for blocks **) - lprintf "\n"; - let idx = ref 0 in - List.iter - (fun s -> - let v = ExtXml.attrib s "name" in - lprintf "#define BLOCK_%s %d\n" (Str.global_replace (Str.regexp "[\\. ]") "_" v) !idx; incr idx) blocks; - lprintf "\n"; - let index_of_waypoints = let i = ref (-1) in List.map (fun w -> incr i; (name_of w, !i)) waypoints in diff --git a/sw/tools/gen_settings.ml b/sw/tools/gen_settings.ml index 087d477f22..e959b8e400 100644 --- a/sw/tools/gen_settings.ml +++ b/sw/tools/gen_settings.ml @@ -69,14 +69,10 @@ let print_dl_settings = fun settings -> lprintf "\n"; (** Datalink knowing what settings mean **) - let idx = ref 0 in - lprintf "\n"; - List.iter - (fun s -> - let v = ExtXml.attrib s "var" in - lprintf "#define SETTINGS_%s %d\n" (Str.global_replace (Str.regexp "\\.") "_" v) !idx; incr idx) - settings; - lprintf "\n"; + Xml2h.define "SETTINGS" "{ \\"; + List.iter (fun b -> printf " { \"%s\" }, \\\n" (ExtXml.attrib b "var")) settings; + lprintf "};\n"; + Xml2h.define "NB_SETTING" (string_of_int (List.length settings)); (** Macro to call to set one variable *) lprintf "#define DlSetting(_idx, _value) { \\\n";